b2Distance.cpp
上传用户:gb3593
上传日期:2022-01-07
资源大小:3028k
文件大小:13k
- /*
- * Copyright (c) 2007-2009 Erin Catto http://www.gphysics.com
- *
- * 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/b2PolygonShape.h>
- // GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates.
- int32 b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters;
- void b2DistanceProxy::Set(const b2Shape* shape)
- {
- 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;
- 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.R, -d));
- vertex->wA = b2Mul(transformA, proxyA->GetVertex(vertex->indexA));
- b2Vec2 wBLocal;
- vertex->indexB = proxyB->GetSupport(b2MulT(transformB.R, 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;
- }
- }
- }