/* * Copyright (c) 2007-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages * arising from the use of this software. * Permission is granted to anyone to use this software for any purpose, * including commercial applications, and to alter it and redistribute it * freely, subject to the following restrictions: * 1. The origin of this software must not be misrepresented; you must not * claim that you wrote the original software. If you use this software * in a product, an acknowledgment in the product documentation would be * appreciated but is not required. * 2. Altered source versions must be plainly marked as such, and must not be * misrepresented as being the original software. * 3. This notice may not be removed or altered from any source distribution. */ #include <Box2D/Collision/b2Distance.h> #include <Box2D/Collision/Shapes/b2CircleShape.h> #include <Box2D/Collision/Shapes/b2EdgeShape.h> #include <Box2D/Collision/Shapes/b2ChainShape.h> #include <Box2D/Collision/Shapes/b2PolygonShape.h> // GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates. int32 b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters; void b2DistanceProxy::Set(const b2Shape* shape, int32 index) { switch (shape->GetType()) { case b2Shape::e_circle: { const b2CircleShape* circle = (b2CircleShape*)shape; m_vertices = &circle->m_p; m_count = 1; m_radius = circle->m_radius; } break; case b2Shape::e_polygon: { const b2PolygonShape* polygon = (b2PolygonShape*)shape; m_vertices = polygon->m_vertices; m_count = polygon->m_vertexCount; m_radius = polygon->m_radius; } break; case b2Shape::e_chain: { const b2ChainShape* chain = (b2ChainShape*)shape; b2Assert(0 <= index && index < chain->m_count); m_buffer[0] = chain->m_vertices[index]; if (index + 1 < chain->m_count) { m_buffer[1] = chain->m_vertices[index + 1]; } else { m_buffer[1] = chain->m_vertices[0]; } m_vertices = m_buffer; m_count = 2; m_radius = chain->m_radius; } break; case b2Shape::e_edge: { const b2EdgeShape* edge = (b2EdgeShape*)shape; m_vertices = &edge->m_vertex1; m_count = 2; m_radius = edge->m_radius; } break; default: b2Assert(false); } } struct b2SimplexVertex { b2Vec2 wA; // support point in proxyA b2Vec2 wB; // support point in proxyB b2Vec2 w; // wB - wA float32 a; // barycentric coordinate for closest point int32 indexA; // wA index int32 indexB; // wB index }; struct b2Simplex { void ReadCache( const b2SimplexCache* cache, const b2DistanceProxy* proxyA, const b2Transform& transformA, const b2DistanceProxy* proxyB, const b2Transform& transformB) { b2Assert(cache->count <= 3); // Copy data from cache. m_count = cache->count; b2SimplexVertex* vertices = &m_v1; for (int32 i = 0; i < m_count; ++i) { b2SimplexVertex* v = vertices + i; v->indexA = cache->indexA[i]; v->indexB = cache->indexB[i]; b2Vec2 wALocal = proxyA->GetVertex(v->indexA); b2Vec2 wBLocal = proxyB->GetVertex(v->indexB); v->wA = b2Mul(transformA, wALocal); v->wB = b2Mul(transformB, wBLocal); v->w = v->wB - v->wA; v->a = 0.0f; } // Compute the new simplex metric, if it is substantially different than // old metric then flush the simplex. if (m_count > 1) { float32 metric1 = cache->metric; float32 metric2 = GetMetric(); if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < b2_epsilon) { // Reset the simplex. m_count = 0; } } // If the cache is empty or invalid ... if (m_count == 0) { b2SimplexVertex* v = vertices + 0; v->indexA = 0; v->indexB = 0; b2Vec2 wALocal = proxyA->GetVertex(0); b2Vec2 wBLocal = proxyB->GetVertex(0); v->wA = b2Mul(transformA, wALocal); v->wB = b2Mul(transformB, wBLocal); v->w = v->wB - v->wA; m_count = 1; } } void WriteCache(b2SimplexCache* cache) const { cache->metric = GetMetric(); cache->count = uint16(m_count); const b2SimplexVertex* vertices = &m_v1; for (int32 i = 0; i < m_count; ++i) { cache->indexA[i] = uint8(vertices[i].indexA); cache->indexB[i] = uint8(vertices[i].indexB); } } b2Vec2 GetSearchDirection() const { switch (m_count) { case 1: return -m_v1.w; case 2: { b2Vec2 e12 = m_v2.w - m_v1.w; float32 sgn = b2Cross(e12, -m_v1.w); if (sgn > 0.0f) { // Origin is left of e12. return b2Cross(1.0f, e12); } else { // Origin is right of e12. return b2Cross(e12, 1.0f); } } default: b2Assert(false); return b2Vec2_zero; } } b2Vec2 GetClosestPoint() const { switch (m_count) { case 0: b2Assert(false); return b2Vec2_zero; case 1: return m_v1.w; case 2: return m_v1.a * m_v1.w + m_v2.a * m_v2.w; case 3: return b2Vec2_zero; default: b2Assert(false); return b2Vec2_zero; } } void GetWitnessPoints(b2Vec2* pA, b2Vec2* pB) const { switch (m_count) { case 0: b2Assert(false); break; case 1: *pA = m_v1.wA; *pB = m_v1.wB; break; case 2: *pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA; *pB = m_v1.a * m_v1.wB + m_v2.a * m_v2.wB; break; case 3: *pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA + m_v3.a * m_v3.wA; *pB = *pA; break; default: b2Assert(false); break; } } float32 GetMetric() const { switch (m_count) { case 0: b2Assert(false); return 0.0; case 1: return 0.0f; case 2: return b2Distance(m_v1.w, m_v2.w); case 3: return b2Cross(m_v2.w - m_v1.w, m_v3.w - m_v1.w); default: b2Assert(false); return 0.0f; } } void Solve2(); void Solve3(); b2SimplexVertex m_v1, m_v2, m_v3; int32 m_count; }; // Solve a line segment using barycentric coordinates. // // p = a1 * w1 + a2 * w2 // a1 + a2 = 1 // // The vector from the origin to the closest point on the line is // perpendicular to the line. // e12 = w2 - w1 // dot(p, e) = 0 // a1 * dot(w1, e) + a2 * dot(w2, e) = 0 // // 2-by-2 linear system // [1 1 ][a1] = [1] // [w1.e12 w2.e12][a2] = [0] // // Define // d12_1 = dot(w2, e12) // d12_2 = -dot(w1, e12) // d12 = d12_1 + d12_2 // // Solution // a1 = d12_1 / d12 // a2 = d12_2 / d12 void b2Simplex::Solve2() { b2Vec2 w1 = m_v1.w; b2Vec2 w2 = m_v2.w; b2Vec2 e12 = w2 - w1; // w1 region float32 d12_2 = -b2Dot(w1, e12); if (d12_2 <= 0.0f) { // a2 <= 0, so we clamp it to 0 m_v1.a = 1.0f; m_count = 1; return; } // w2 region float32 d12_1 = b2Dot(w2, e12); if (d12_1 <= 0.0f) { // a1 <= 0, so we clamp it to 0 m_v2.a = 1.0f; m_count = 1; m_v1 = m_v2; return; } // Must be in e12 region. float32 inv_d12 = 1.0f / (d12_1 + d12_2); m_v1.a = d12_1 * inv_d12; m_v2.a = d12_2 * inv_d12; m_count = 2; } // Possible regions: // - points[2] // - edge points[0]-points[2] // - edge points[1]-points[2] // - inside the triangle void b2Simplex::Solve3() { b2Vec2 w1 = m_v1.w; b2Vec2 w2 = m_v2.w; b2Vec2 w3 = m_v3.w; // Edge12 // [1 1 ][a1] = [1] // [w1.e12 w2.e12][a2] = [0] // a3 = 0 b2Vec2 e12 = w2 - w1; float32 w1e12 = b2Dot(w1, e12); float32 w2e12 = b2Dot(w2, e12); float32 d12_1 = w2e12; float32 d12_2 = -w1e12; // Edge13 // [1 1 ][a1] = [1] // [w1.e13 w3.e13][a3] = [0] // a2 = 0 b2Vec2 e13 = w3 - w1; float32 w1e13 = b2Dot(w1, e13); float32 w3e13 = b2Dot(w3, e13); float32 d13_1 = w3e13; float32 d13_2 = -w1e13; // Edge23 // [1 1 ][a2] = [1] // [w2.e23 w3.e23][a3] = [0] // a1 = 0 b2Vec2 e23 = w3 - w2; float32 w2e23 = b2Dot(w2, e23); float32 w3e23 = b2Dot(w3, e23); float32 d23_1 = w3e23; float32 d23_2 = -w2e23; // Triangle123 float32 n123 = b2Cross(e12, e13); float32 d123_1 = n123 * b2Cross(w2, w3); float32 d123_2 = n123 * b2Cross(w3, w1); float32 d123_3 = n123 * b2Cross(w1, w2); // w1 region if (d12_2 <= 0.0f && d13_2 <= 0.0f) { m_v1.a = 1.0f; m_count = 1; return; } // e12 if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f) { float32 inv_d12 = 1.0f / (d12_1 + d12_2); m_v1.a = d12_1 * inv_d12; m_v2.a = d12_2 * inv_d12; m_count = 2; return; } // e13 if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f) { float32 inv_d13 = 1.0f / (d13_1 + d13_2); m_v1.a = d13_1 * inv_d13; m_v3.a = d13_2 * inv_d13; m_count = 2; m_v2 = m_v3; return; } // w2 region if (d12_1 <= 0.0f && d23_2 <= 0.0f) { m_v2.a = 1.0f; m_count = 1; m_v1 = m_v2; return; } // w3 region if (d13_1 <= 0.0f && d23_1 <= 0.0f) { m_v3.a = 1.0f; m_count = 1; m_v1 = m_v3; return; } // e23 if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f) { float32 inv_d23 = 1.0f / (d23_1 + d23_2); m_v2.a = d23_1 * inv_d23; m_v3.a = d23_2 * inv_d23; m_count = 2; m_v1 = m_v3; return; } // Must be in triangle123 float32 inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3); m_v1.a = d123_1 * inv_d123; m_v2.a = d123_2 * inv_d123; m_v3.a = d123_3 * inv_d123; m_count = 3; } void b2Distance(b2DistanceOutput* output, b2SimplexCache* cache, const b2DistanceInput* input) { ++b2_gjkCalls; const b2DistanceProxy* proxyA = &input->proxyA; const b2DistanceProxy* proxyB = &input->proxyB; b2Transform transformA = input->transformA; b2Transform transformB = input->transformB; // Initialize the simplex. b2Simplex simplex; simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB); // Get simplex vertices as an array. b2SimplexVertex* vertices = &simplex.m_v1; const int32 k_maxIters = 20; // These store the vertices of the last simplex so that we // can check for duplicates and prevent cycling. int32 saveA[3], saveB[3]; int32 saveCount = 0; b2Vec2 closestPoint = simplex.GetClosestPoint(); float32 distanceSqr1 = closestPoint.LengthSquared(); float32 distanceSqr2 = distanceSqr1; // Main iteration loop. int32 iter = 0; while (iter < k_maxIters) { // Copy simplex so we can identify duplicates. saveCount = simplex.m_count; for (int32 i = 0; i < saveCount; ++i) { saveA[i] = vertices[i].indexA; saveB[i] = vertices[i].indexB; } switch (simplex.m_count) { case 1: break; case 2: simplex.Solve2(); break; case 3: simplex.Solve3(); break; default: b2Assert(false); } // If we have 3 points, then the origin is in the corresponding triangle. if (simplex.m_count == 3) { break; } // Compute closest point. b2Vec2 p = simplex.GetClosestPoint(); distanceSqr2 = p.LengthSquared(); // Ensure progress if (distanceSqr2 >= distanceSqr1) { //break; } distanceSqr1 = distanceSqr2; // Get search direction. b2Vec2 d = simplex.GetSearchDirection(); // Ensure the search direction is numerically fit. if (d.LengthSquared() < b2_epsilon * b2_epsilon) { // The origin is probably contained by a line segment // or triangle. Thus the shapes are overlapped. // We can't return zero here even though there may be overlap. // In case the simplex is a point, segment, or triangle it is difficult // to determine if the origin is contained in the CSO or very close to it. break; } // Compute a tentative new simplex vertex using support points. b2SimplexVertex* vertex = vertices + simplex.m_count; vertex->indexA = proxyA->GetSupport(b2MulT(transformA.q, -d)); vertex->wA = b2Mul(transformA, proxyA->GetVertex(vertex->indexA)); b2Vec2 wBLocal; vertex->indexB = proxyB->GetSupport(b2MulT(transformB.q, d)); vertex->wB = b2Mul(transformB, proxyB->GetVertex(vertex->indexB)); vertex->w = vertex->wB - vertex->wA; // Iteration count is equated to the number of support point calls. ++iter; ++b2_gjkIters; // Check for duplicate support points. This is the main termination criteria. bool duplicate = false; for (int32 i = 0; i < saveCount; ++i) { if (vertex->indexA == saveA[i] && vertex->indexB == saveB[i]) { duplicate = true; break; } } // If we found a duplicate support point we must exit to avoid cycling. if (duplicate) { break; } // New vertex is ok and needed. ++simplex.m_count; } b2_gjkMaxIters = b2Max(b2_gjkMaxIters, iter); // Prepare output. simplex.GetWitnessPoints(&output->pointA, &output->pointB); output->distance = b2Distance(output->pointA, output->pointB); output->iterations = iter; // Cache the simplex. simplex.WriteCache(cache); // Apply radii if requested. if (input->useRadii) { float32 rA = proxyA->m_radius; float32 rB = proxyB->m_radius; if (output->distance > rA + rB && output->distance > b2_epsilon) { // Shapes are still no overlapped. // Move the witness points to the outer surface. output->distance -= rA + rB; b2Vec2 normal = output->pointB - output->pointA; normal.Normalize(); output->pointA += rA * normal; output->pointB -= rB * normal; } else { // Shapes are overlapped when radii are considered. // Move the witness points to the middle. b2Vec2 p = 0.5f * (output->pointA + output->pointB); output->pointA = p; output->pointB = p; output->distance = 0.0f; } } }