From 373fc134edad97171b72fd2ad33950d083b6698b Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Sun, 25 Feb 2024 23:02:46 -0800 Subject: [PATCH 1/2] fix shape cast initial overlap and tolerance improve shape cast sample --- samples/sample_collision.cpp | 117 ++++++++++++++++++++++++++++------- src/distance.c | 66 +++++++++++--------- 2 files changed, 132 insertions(+), 51 deletions(-) diff --git a/samples/sample_collision.cpp b/samples/sample_collision.cpp index 7686d6f2..b05937bc 100644 --- a/samples/sample_collision.cpp +++ b/samples/sample_collision.cpp @@ -1598,14 +1598,14 @@ class RayCastWorld : public Sample { const char* castTypes[] = {"Ray", "Circle", "Capsule", "Polygon"}; int castType = int(m_castType); - if (ImGui::Combo("Cast Type", &castType, castTypes, IM_ARRAYSIZE(castTypes))) + if (ImGui::Combo("Type", &castType, castTypes, IM_ARRAYSIZE(castTypes))) { m_castType = CastType(castType); } if (m_castType != e_rayCast) { - ImGui::SliderFloat("radius", &m_castRadius, 0.0f, 2.0f, "%.1f"); + ImGui::SliderFloat("Radius", &m_castRadius, 0.0f, 2.0f, "%.1f"); } const char* modes[] = {"Any", "Closest", "Multiple", "Sorted"}; @@ -3274,10 +3274,11 @@ class ShapeCast : public Sample if (settings.restart == false) { g_camera.m_center = {-1.5f, 1.0f}; - g_camera.m_zoom = 0.12f; + g_camera.m_zoom = 0.2f; } #if 1 + // box swept against a triangle m_vAs[0] = {-0.5f, 1.0f}; m_vAs[1] = {0.5f, 1.0f}; m_vAs[2] = {0.0f, 0.0f}; @@ -3297,36 +3298,56 @@ class ShapeCast : public Sample m_transformB.q = b2Rot_identity; m_translationB = {8.0f, 0.0f}; #elif 0 - m_vAs[0].Set(0.0f, 0.0f); + // A point swept against a box + m_vAs[0] = {-0.5f, -0.5f}; + m_vAs[1] = {0.5f, -0.5f}; + m_vAs[2] = {0.5f, 0.5f}; + m_vAs[3] = {-0.5f, 0.5f}; + m_countA = 4; + m_radiusA = 0.0f; + + m_vBs[0] = {0.0f, 0.0f}; + m_countB = 1; + m_radiusB = 0.0f; + + m_transformA.p = {0.0f, 0.0f}; + m_transformA.q = b2Rot_identity; + m_transformB.p = {-1.0f, 0.0f}; + m_transformB.q = b2Rot_identity; + m_translationB = {1.0f, 0.0f}; +#elif 0 + m_vAs[0] = {0.0f, 0.0f}; m_countA = 1; m_radiusA = 0.5f; - m_vBs[0].Set(0.0f, 0.0f); + m_vBs[0] = {0.0f, 0.0f}; m_countB = 1; m_radiusB = 0.5f; - m_transformA.p.Set(0.0f, 0.25f); - m_transformA.q.SetIdentity(); - m_transformB.p.Set(-4.0f, 0.0f); - m_transformB.q.SetIdentity(); - m_translationB.Set(8.0f, 0.0f); + m_transformA.p = {0.0f, 0.25f}; + m_transformA.q = b2Rot_identity; + m_transformB.p = {-4.0f, 0.0f}; + m_transformB.q = b2Rot_identity; + m_translationB = {8.0f, 0.0f}; #else - m_vAs[0].Set(0.0f, 0.0f); - m_vAs[1].Set(2.0f, 0.0f); + m_vAs[0] = {0.0f, 0.0f}; + m_vAs[1] = {2.0f, 0.0f}; m_countA = 2; - m_radiusA = b2_polygonRadius; + m_radiusA = 0.0f; - m_vBs[0].Set(0.0f, 0.0f); + m_vBs[0] = {0.0f, 0.0f}; m_countB = 1; m_radiusB = 0.25f; // Initial overlap - m_transformA.p.Set(0.0f, 0.0f); - m_transformA.q.SetIdentity(); - m_transformB.p.Set(-0.244360745f, 0.05999358f); - m_transformB.q.SetIdentity(); - m_translationB.Set(0.0f, 0.0399999991f); + m_transformA.p = b2Vec2_zero; + m_transformA.q = b2Rot_identity; + m_transformB.p = {-0.244360745f, 0.05999358f}; + m_transformB.q = b2Rot_identity; + m_translationB = {0.0f, 0.0399999991f}; #endif + + m_rayDrag = false; } static Sample* Create(Settings& settings) @@ -3334,16 +3355,42 @@ class ShapeCast : public Sample return new ShapeCast(settings); } + void MouseDown(b2Vec2 p, int button, int mods) override + { + if (button == GLFW_MOUSE_BUTTON_1) + { + m_transformB.p = p; + m_rayDrag = true; + } + } + + void MouseUp(b2Vec2, int button) override + { + if (button == GLFW_MOUSE_BUTTON_1) + { + m_rayDrag = false; + } + } + + void MouseMove(b2Vec2 p) override + { + if (m_rayDrag) + { + m_translationB = b2Sub(p, m_transformB.p); + } + } + void Step(Settings& settings) override { Sample::Step(settings); - b2ShapeCastPairInput input; + b2ShapeCastPairInput input = {0}; input.proxyA = b2MakeProxy(m_vAs, m_countA, m_radiusA); input.proxyB = b2MakeProxy(m_vBs, m_countB, m_radiusB); input.transformA = m_transformA; input.transformB = m_transformB; input.translationB = m_translationB; + input.maxFraction = 1.0f; b2CastOutput output = b2ShapeCast(&input); @@ -3374,7 +3421,14 @@ class ShapeCast : public Sample if (m_countA == 1) { - g_draw.DrawCircle(vertices[0], m_radiusA, {0.9f, 0.9f, 0.9f, 1.0f}); + if (m_radiusA > 0.0f) + { + g_draw.DrawCircle(vertices[0], m_radiusA, {0.9f, 0.9f, 0.9f, 1.0f}); + } + else + { + g_draw.DrawPoint(vertices[0], 5.0f, {0.9f, 0.9f, 0.9f, 1.0f}); + } } else { @@ -3388,7 +3442,14 @@ class ShapeCast : public Sample if (m_countB == 1) { - g_draw.DrawCircle(vertices[0], m_radiusB, {0.5f, 0.9f, 0.5f, 1.0f}); + if (m_radiusB > 0.0f) + { + g_draw.DrawCircle(vertices[0], m_radiusB, {0.5f, 0.9f, 0.5f, 1.0f}); + } + else + { + g_draw.DrawPoint(vertices[0], 5.0f, {0.5f, 0.9f, 0.5f, 1.0f}); + } } else { @@ -3402,7 +3463,14 @@ class ShapeCast : public Sample if (m_countB == 1) { - g_draw.DrawCircle(vertices[0], m_radiusB, {0.5f, 0.7f, 0.9f, 1.0f}); + if (m_radiusB > 0.0f) + { + g_draw.DrawCircle(vertices[0], m_radiusB, {0.5f, 0.7f, 0.9f, 1.0f}); + } + else + { + g_draw.DrawPoint(vertices[0], 5.0f, {0.5f, 0.7f, 0.9f, 1.0f}); + } } else { @@ -3416,6 +3484,8 @@ class ShapeCast : public Sample b2Vec2 p2 = b2MulAdd(p1, 1.0f, output.normal); g_draw.DrawSegment(p1, p2, {0.9f, 0.3f, 0.3f, 1.0f}); } + + g_draw.DrawSegment(m_transformB.p, b2Add(m_transformB.p, m_translationB), {0.9f, 0.9f, 0.9f, 1.0f}); } b2Vec2 m_vAs[b2_maxPolygonVertices]; @@ -3429,6 +3499,7 @@ class ShapeCast : public Sample b2Transform m_transformA; b2Transform m_transformB; b2Vec2 m_translationB; + bool m_rayDrag; }; static int sampleShapeCast = RegisterSample("Collision", "Shape Cast", ShapeCast::Create); diff --git a/src/distance.c b/src/distance.c index 5734cd30..16c28e78 100644 --- a/src/distance.c +++ b/src/distance.c @@ -658,21 +658,32 @@ b2DistanceOutput b2ShapeDistance(b2DistanceCache* cache, const b2DistanceInput* // GJK-raycast // Algorithm by Gino van den Bergen. // "Smooth Mesh Contacts with GJK" in Game Physics Pearls. 2010 -// TODO_ERIN this is failing when used to raycast a box +// #todo this is failing when used to raycast a box b2CastOutput b2ShapeCast(const b2ShapeCastPairInput* input) { b2CastOutput output = {0}; + output.fraction = input->maxFraction; - const b2DistanceProxy* proxyA = &input->proxyA; - const b2DistanceProxy* proxyB = &input->proxyB; - - float radius = proxyA->radius + proxyB->radius; + b2DistanceProxy proxyA = input->proxyA; b2Transform xfA = input->transformA; b2Transform xfB = input->transformB; + b2Transform xf = b2InvMulTransforms(xfA, xfB); - b2Vec2 r = input->translationB; - b2Vec2 n = b2Vec2_zero; + // Put proxyB in proxyA's frame to reduce round-off error + b2DistanceProxy proxyB; + proxyB.count = input->proxyB.count; + proxyB.radius = input->proxyB.radius; + B2_ASSERT(proxyB.count <= b2_maxPolygonVertices); + + for (int i = 0; i < proxyB.count; ++i) + { + proxyB.vertices[i] = b2TransformPoint(xf, input->proxyB.vertices[i]); + } + + float radius = proxyA.radius + proxyB.radius; + + b2Vec2 r = b2RotateVector(xf.q, input->translationB); float lambda = 0.0f; float maxFraction = input->maxFraction; @@ -683,11 +694,11 @@ b2CastOutput b2ShapeCast(const b2ShapeCastPairInput* input) // Get simplex vertices as an array. b2SimplexVertex* vertices[] = {&simplex.v1, &simplex.v2, &simplex.v3}; - // Get support point in -r direction - int32_t indexA = b2FindSupport(proxyA, b2InvRotateVector(xfA.q, b2Neg(r))); - b2Vec2 wA = b2TransformPoint(xfA, proxyA->vertices[indexA]); - int32_t indexB = b2FindSupport(proxyB, b2InvRotateVector(xfB.q, r)); - b2Vec2 wB = b2TransformPoint(xfB, proxyB->vertices[indexB]); + // Get an initial point in A - B + int32_t indexA = b2FindSupport(&proxyA, b2Neg(r)); + b2Vec2 wA = proxyA.vertices[indexA]; + int32_t indexB = b2FindSupport(&proxyB, r); + b2Vec2 wB = proxyB.vertices[indexB]; b2Vec2 v = b2Sub(wA, wB); // Sigma is the target distance between proxies @@ -696,20 +707,20 @@ b2CastOutput b2ShapeCast(const b2ShapeCastPairInput* input) // Main iteration loop. const int32_t k_maxIters = 20; int32_t iter = 0; - while (iter < k_maxIters && b2Length(v) > sigma) + while (iter < k_maxIters && b2Length(v) > sigma + 0.5f * b2_linearSlop) { B2_ASSERT(simplex.count < 3); output.iterations += 1; // Support in direction -v (A - B) - indexA = b2FindSupport(proxyA, b2InvRotateVector(xfA.q, b2Neg(v))); - wA = b2TransformPoint(xfA, proxyA->vertices[indexA]); - indexB = b2FindSupport(proxyB, b2InvRotateVector(xfB.q, v)); - wB = b2TransformPoint(xfB, proxyB->vertices[indexB]); + indexA = b2FindSupport(&proxyA, b2Neg(v)); + wA = proxyA.vertices[indexA]; + indexB = b2FindSupport(&proxyB, v); + wB = proxyB.vertices[indexB]; b2Vec2 p = b2Sub(wA, wB); - // -v is a normal at p + // -v is a normal at p, normalize to work with sigma v = b2Normalize(v); // Intersect ray with plane @@ -719,16 +730,18 @@ b2CastOutput b2ShapeCast(const b2ShapeCastPairInput* input) { if (vr <= 0.0f) { + // miss return output; } lambda = (vp - sigma) / vr; if (lambda > maxFraction) { + // too far return output; } - n = (b2Vec2){-v.x, -v.y}; + // reset the simplex simplex.count = 0; } @@ -776,7 +789,7 @@ b2CastOutput b2ShapeCast(const b2ShapeCastPairInput* input) ++iter; } - if (iter == 0) + if (iter == 0 || lambda == 0.0f) { // Initial overlap return output; @@ -786,14 +799,11 @@ b2CastOutput b2ShapeCast(const b2ShapeCastPairInput* input) b2Vec2 pointA, pointB; b2ComputeSimplexWitnessPoints(&pointB, &pointA, &simplex); - if (b2Dot(v, v) > 0.0f) - { - n = b2Normalize(b2Neg(v)); - } + b2Vec2 n = b2Normalize(b2Neg(v)); + b2Vec2 point = {pointA.x + proxyA.radius * n.x, pointA.y + proxyA.radius * n.y}; - float radiusA = proxyA->radius; - output.point = (b2Vec2){pointA.x + radiusA * n.x, pointA.y + radiusA * n.y}; - output.normal = n; + output.point = b2TransformPoint(xfA, point); + output.normal = b2RotateVector(xfA.q, n); output.fraction = lambda; output.iterations = iter; output.hit = true; @@ -1043,7 +1053,7 @@ b2TOIOutput b2TimeOfImpact(const b2TOIInput* input) float tMax = input->tMax; float totalRadius = proxyA->radius + proxyB->radius; - float target = B2_MAX(b2_linearSlop, totalRadius + b2_linearSlop); + float target = B2_MAX(b2_linearSlop, totalRadius - b2_linearSlop); float tolerance = 0.25f * b2_linearSlop; B2_ASSERT(target > tolerance); From 3a4cb593430c475bf8475c861d24e448c6fbf05b Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Sun, 25 Feb 2024 23:19:30 -0800 Subject: [PATCH 2/2] added speculative collision failure case --- samples/sample_continuous.cpp | 48 +++++++++++++++++++++++++++++++++++ 1 file changed, 48 insertions(+) diff --git a/samples/sample_continuous.cpp b/samples/sample_continuous.cpp index 56eaa622..dc0cfba0 100644 --- a/samples/sample_continuous.cpp +++ b/samples/sample_continuous.cpp @@ -609,3 +609,51 @@ class GhostCollision : public Sample }; static int sampleGhostCollision = RegisterSample("Continuous", "Ghost Collision", GhostCollision::Create); + +// Speculative collision failure case suggested by Dirk Gregorius +class SpeculativeFail : public Sample +{ +public: + SpeculativeFail(Settings& settings) + : Sample(settings) + { + if (settings.restart == false) + { + g_camera.m_center = {1.0f, 5.0f}; + g_camera.m_zoom = 0.25f; + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody(m_worldId, &bodyDef); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Segment segment = {{-10.0f, 0.0f}, {10.0f, 0.0f}}; + b2CreateSegmentShape(groundId, &shapeDef, &segment); + + b2Vec2 points[5] = {{-2.0f, 4.0f}, {2.0f, 4.0f}, {2.0f, 4.1f}, {-0.5f, 4.2f}, {-2.0f, 4.2f}}; + b2Hull hull = b2ComputeHull(points, 5); + b2Polygon poly = b2MakePolygon(&hull, 0.0f); + b2CreatePolygonShape(groundId, &shapeDef, &poly); + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = {0.0f, 8.0f}; + bodyDef.linearVelocity = {0.0f, -100.0f}; + b2BodyId bodyId = b2CreateBody(m_worldId, &bodyDef); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Polygon box = b2MakeBox(2.0f, 0.05f); + b2CreatePolygonShape(bodyId, &shapeDef, &box); + } + } + + static Sample* Create(Settings& settings) + { + return new SpeculativeFail(settings); + } +}; + +static int sampleSpeculativeFail = RegisterSample("Continuous", "Speculative Fail", SpeculativeFail::Create);