738 lines
16 KiB
C++
738 lines
16 KiB
C++
/*
|
|
* 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 = static_cast<const b2CircleShape*>(shape);
|
|
m_vertices = &circle->m_p;
|
|
m_count = 1;
|
|
m_radius = circle->m_radius;
|
|
}
|
|
break;
|
|
|
|
case b2Shape::e_polygon:
|
|
{
|
|
const b2PolygonShape* polygon = static_cast<const b2PolygonShape*>(shape);
|
|
m_vertices = polygon->m_vertices;
|
|
m_count = polygon->m_count;
|
|
m_radius = polygon->m_radius;
|
|
}
|
|
break;
|
|
|
|
case b2Shape::e_chain:
|
|
{
|
|
const b2ChainShape* chain = static_cast<const 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 = static_cast<const b2EdgeShape*>(shape);
|
|
m_vertices = &edge->m_vertex1;
|
|
m_count = 2;
|
|
m_radius = edge->m_radius;
|
|
}
|
|
break;
|
|
|
|
default:
|
|
b2Assert(false);
|
|
}
|
|
}
|
|
|
|
void b2DistanceProxy::Set(const b2Vec2* vertices, int32 count, float32 radius)
|
|
{
|
|
m_vertices = vertices;
|
|
m_count = count;
|
|
m_radius = radius;
|
|
}
|
|
|
|
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;
|
|
v->a = 1.0f;
|
|
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.0f;
|
|
|
|
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;
|
|
|
|
// 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;
|
|
}
|
|
|
|
// 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;
|
|
}
|
|
}
|
|
}
|
|
|
|
// GJK-raycast
|
|
// Algorithm by Gino van den Bergen.
|
|
// "Smooth Mesh Contacts with GJK" in Game Physics Pearls. 2010
|
|
bool b2ShapeCast(b2ShapeCastOutput * output, const b2ShapeCastInput * input)
|
|
{
|
|
output->iterations = 0;
|
|
output->lambda = 1.0f;
|
|
output->normal.SetZero();
|
|
output->point.SetZero();
|
|
|
|
const b2DistanceProxy* proxyA = &input->proxyA;
|
|
const b2DistanceProxy* proxyB = &input->proxyB;
|
|
|
|
float32 radiusA = b2Max(proxyA->m_radius, b2_polygonRadius);
|
|
float32 radiusB = b2Max(proxyB->m_radius, b2_polygonRadius);
|
|
float32 radius = radiusA + radiusB;
|
|
|
|
b2Transform xfA = input->transformA;
|
|
b2Transform xfB = input->transformB;
|
|
|
|
b2Vec2 r = input->translationB;
|
|
b2Vec2 n(0.0f, 0.0f);
|
|
float32 lambda = 0.0f;
|
|
|
|
// Initial simplex
|
|
b2Simplex simplex;
|
|
simplex.m_count = 0;
|
|
|
|
// Get simplex vertices as an array.
|
|
b2SimplexVertex* vertices = &simplex.m_v1;
|
|
|
|
// Get support point in -r direction
|
|
int32 indexA = proxyA->GetSupport(b2MulT(xfA.q, -r));
|
|
b2Vec2 wA = b2Mul(xfA, proxyA->GetVertex(indexA));
|
|
int32 indexB = proxyB->GetSupport(b2MulT(xfB.q, r));
|
|
b2Vec2 wB = b2Mul(xfB, proxyB->GetVertex(indexB));
|
|
b2Vec2 v = wA - wB;
|
|
|
|
// Sigma is the target distance between polygons
|
|
float32 sigma = b2Max(b2_polygonRadius, radius - b2_polygonRadius);
|
|
const float32 tolerance = 0.5f * b2_linearSlop;
|
|
|
|
// Main iteration loop.
|
|
const int32 k_maxIters = 20;
|
|
int32 iter = 0;
|
|
while (iter < k_maxIters && b2Abs(v.Length() - sigma) > tolerance)
|
|
{
|
|
b2Assert(simplex.m_count < 3);
|
|
|
|
output->iterations += 1;
|
|
|
|
// Support in direction -v (A - B)
|
|
indexA = proxyA->GetSupport(b2MulT(xfA.q, -v));
|
|
wA = b2Mul(xfA, proxyA->GetVertex(indexA));
|
|
indexB = proxyB->GetSupport(b2MulT(xfB.q, v));
|
|
wB = b2Mul(xfB, proxyB->GetVertex(indexB));
|
|
b2Vec2 p = wA - wB;
|
|
|
|
// -v is a normal at p
|
|
v.Normalize();
|
|
|
|
// Intersect ray with plane
|
|
float32 vp = b2Dot(v, p);
|
|
float32 vr = b2Dot(v, r);
|
|
if (vp - sigma > lambda * vr)
|
|
{
|
|
if (vr <= 0.0f)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
lambda = (vp - sigma) / vr;
|
|
if (lambda > 1.0f)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
n = -v;
|
|
simplex.m_count = 0;
|
|
}
|
|
|
|
// Reverse simplex since it works with B - A.
|
|
// Shift by lambda * r because we want the closest point to the current clip point.
|
|
// Note that the support point p is not shifted because we want the plane equation
|
|
// to be formed in unshifted space.
|
|
b2SimplexVertex* vertex = vertices + simplex.m_count;
|
|
vertex->indexA = indexB;
|
|
vertex->wA = wB + lambda * r;
|
|
vertex->indexB = indexA;
|
|
vertex->wB = wA;
|
|
vertex->w = vertex->wB - vertex->wA;
|
|
vertex->a = 1.0f;
|
|
simplex.m_count += 1;
|
|
|
|
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)
|
|
{
|
|
// Overlap
|
|
return false;
|
|
}
|
|
|
|
// Get search direction.
|
|
v = simplex.GetClosestPoint();
|
|
|
|
// Iteration count is equated to the number of support point calls.
|
|
++iter;
|
|
}
|
|
|
|
// Prepare output.
|
|
b2Vec2 pointA, pointB;
|
|
simplex.GetWitnessPoints(&pointB, &pointA);
|
|
|
|
if (v.LengthSquared() > 0.0f)
|
|
{
|
|
n = -v;
|
|
n.Normalize();
|
|
}
|
|
|
|
output->point = pointA + radiusA * n;
|
|
output->normal = n;
|
|
output->lambda = lambda;
|
|
output->iterations = iter;
|
|
return true;
|
|
}
|