public static b2SimplexCache Create()
{
b2SimplexCache obj = new b2SimplexCache();
obj.Defaults();
return(obj);
}
public void WriteCache(ref b2SimplexCache cache)
{
cache.metric = GetMetric();
cache.count = m_count;
for (int i = 0; i < m_count; ++i)
{
cache.indexA[i] = m_vertices[i].indexA;
cache.indexB[i] = m_vertices[i].indexB;
}
}
public void WriteCache(b2SimplexCache cache)
{
cache.metric = GetMetric();
cache.count = (uint)(m_count);
b2SimplexVertex[] vertices = m_vertices;
for (int i = 0; i < m_count; i++)
{
cache.indexA[i] = (int)((uint)(vertices[i].indexA));
cache.indexB[i] = (int)((uint)(vertices[i].indexB));
}
}
public void ReadCache(b2SimplexCache cache,
b2DistanceProxy proxyA, b2Transform transformA,
b2DistanceProxy proxyB, b2Transform transformB)
{
b2Settings.b2Assert(0 <= cache.count && cache.count <= 3);
b2Vec2 wALocal;
b2Vec2 wBLocal;
// Copy data from cache.
m_count = (int)cache.count;
b2SimplexVertex[] vertices = m_vertices;
for (int i = 0; i < m_count; i++)
{
b2SimplexVertex v = vertices[i];
v.indexA = cache.indexA[i];
v.indexB = cache.indexB[i];
wALocal = proxyA.GetVertex(v.indexA);
wBLocal = proxyB.GetVertex(v.indexB);
v.wA = b2Math.MulX(transformA, wALocal);
v.wB = b2Math.MulX(transformB, wBLocal);
v.w = b2Math.SubtractVV(v.wB, v.wA);
v.a = 0;
}
// Compute the new simplex metric, if it substantially different than
// old metric then flush the simplex
if (m_count > 1)
{
float metric1 = cache.metric;
float metric2 = GetMetric();
if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < float.MinValue)
{
// 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;
wALocal = proxyA.GetVertex(0);
wBLocal = proxyB.GetVertex(0);
v.wA = b2Math.MulX(transformA, wALocal);
v.wB = b2Math.MulX(transformB, wBLocal);
v.w = b2Math.SubtractVV(v.wB, v.wA);
m_count = 1;
}
}
public void ReadCache(ref b2SimplexCache cache,
ref b2DistanceProxy proxyA, ref b2Transform transformA,
ref b2DistanceProxy proxyB, ref b2Transform transformB)
{
Debug.Assert(cache.count <= 3);
// Copy data from cache.
m_count = (int)cache.count;
m_vertices = new b2SimplexVertex[3];
for (int i = 0; i < m_count; ++i)
{
b2SimplexVertex v;
v.indexA = (int)cache.indexA[i];
v.indexB = (int)cache.indexB[i];
b2Vec2 wALocal = proxyA.GetVertex(v.indexA);
b2Vec2 wBLocal = proxyB.GetVertex(v.indexB);
v.wA = b2Math.b2Mul(transformA, wALocal);
v.wB = b2Math.b2Mul(transformB, wBLocal);
v.w = v.wB - v.wA;
v.a = 0.0f;
m_vertices[i] = v;
}
// Compute the new simplex metric, if it is substantially different than
// old metric then flush the simplex.
if (m_count > 1)
{
float metric1 = cache.metric;
float metric2 = GetMetric();
if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < b2Settings.b2_epsilon)
{
// Reset the simplex.
m_count = 0;
}
}
// If the cache is empty or invalid ...
if (m_count == 0)
{
b2SimplexVertex v;
v.indexA = 0;
v.indexB = 0;
b2Vec2 wALocal = proxyA.GetVertex(0);
b2Vec2 wBLocal = proxyB.GetVertex(0);
v.wA = b2Math.b2Mul(transformA, wALocal);
v.wB = b2Math.b2Mul(transformB, wBLocal);
v.w = v.wB - v.wA;
v.a = 0.0f;
m_count = 1;
m_vertices[0] = v;
}
}
/// Determine if two generic shapes overlap.
public static bool b2TestOverlap(b2Shape shapeA, int indexA,
b2Shape shapeB, int indexB,
ref b2Transform xfA, ref b2Transform xfB)
{
b2DistanceInput input = b2DistanceInput.Create();
input.proxyA = b2DistanceProxy.Create(shapeA, indexA);
input.proxyB = b2DistanceProxy.Create(shapeB, indexB);
input.transformA = xfA;
input.transformB = xfB;
input.useRadii = true;
b2SimplexCache cache = b2SimplexCache.Create();
b2DistanceOutput output = new b2DistanceOutput();
b2Simplex.b2Distance(ref output, ref cache, ref input);
// Console.WriteLine("{2} vs {3}: distance={0} after {1} iters", output.distance, output.iterations, shapeA.ShapeType, shapeB.ShapeType);
return output.distance < 10.0f * b2Settings.b2_epsilon;
}
public void SolveTOI(b2TimeStep subStep, int toiIndexA, int toiIndexB)
{
Debug.Assert(toiIndexA < m_bodyCount);
Debug.Assert(toiIndexB < m_bodyCount);
// Initialize the body state.
for (int i = 0; i < m_bodyCount; ++i)
{
b2Body b = m_bodies[i];
m_positions[i].c = b.Sweep.c;
m_positions[i].a = b.Sweep.a;
m_velocities[i].v = b.LinearVelocity;
m_velocities[i].w = b.AngularVelocity;
}
b2ContactSolverDef contactSolverDef;
contactSolverDef.contacts = m_contacts;
contactSolverDef.count = m_contactCount;
contactSolverDef.step = subStep;
contactSolverDef.positions = m_positions;
contactSolverDef.velocities = m_velocities;
b2ContactSolver contactSolver = new b2ContactSolver(contactSolverDef);
// Solve position constraints.
for (int i = 0; i < subStep.positionIterations; ++i)
{
bool contactsOkay = contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB);
if (contactsOkay)
{
break;
}
}
#if false
// Is the new position really safe?
for (int i = 0; i < m_contactCount; ++i)
{
b2Contact c = m_contacts[i];
b2Fixture fA = c.GetFixtureA();
b2Fixture fB = c.GetFixtureB();
b2Body bA = fA.Body;
b2Body bB = fB.Body;
int indexA = c.GetChildIndexA();
int indexB = c.GetChildIndexB();
b2DistanceInput input = new b2DistanceInput();
input.proxyA.Set(fA.Shape, indexA);
input.proxyB.Set(fB.Shape, indexB);
input.transformA = bA.Transform;
input.transformB = bB.Transform;
input.useRadii = false;
b2DistanceOutput output;
b2SimplexCache cache = new b2SimplexCache();
cache.count = 0;
output = b2Distance(cache, input);
if (output.distance == 0 || cache.count == 3)
{
cache.count += 0;
}
}
#endif
// Leap of faith to new safe state.
m_bodies[toiIndexA].Sweep.c0 = m_positions[toiIndexA].c;
m_bodies[toiIndexA].Sweep.a0 = m_positions[toiIndexA].a;
m_bodies[toiIndexB].Sweep.c0 = m_positions[toiIndexB].c;
m_bodies[toiIndexB].Sweep.a0 = m_positions[toiIndexB].a;
// No warm starting is needed for TOI events because warm
// starting impulses were applied in the discrete solver.
contactSolver.InitializeVelocityConstraints();
// Solve velocity constraints.
for (int i = 0; i < subStep.velocityIterations; ++i)
{
contactSolver.SolveVelocityConstraints();
}
// Don't store the TOI contact forces for warm starting
// because they can be quite large.
float h = subStep.dt;
// Integrate positions
for (int i = 0; i < m_bodyCount; ++i)
{
b2Vec2 c = m_positions[i].c;
float a = m_positions[i].a;
b2Vec2 v = m_velocities[i].v;
float w = m_velocities[i].w;
// Check for large velocities
b2Vec2 translation = h * v;
if (b2Math.b2Dot(translation, translation) > b2Settings.b2_maxTranslationSquared)
{
float ratio = b2Settings.b2_maxTranslation / translation.Length();
v *= ratio;
}
float rotation = h * w;
if (rotation * rotation > b2Settings.b2_maxRotationSquared)
{
float ratio = b2Settings.b2_maxRotation / Math.Abs(rotation);
w *= ratio;
}
// Integrate
c += h * v;
a += h * w;
m_positions[i].c = c;
m_positions[i].a = a;
m_velocities[i].v = v;
m_velocities[i].w = w;
// Sync bodies
b2Body body = m_bodies[i];
body.Sweep.c = c;
body.Sweep.a = a;
body.LinearVelocity = v;
body.AngularVelocity = w;
body.SynchronizeTransform();
}
Report(contactSolver.m_velocityConstraints);
}
// CCD via the local separating axis method. This seeks progression
// by computing the largest time at which separation is maintained.
public static void Compute(out b2TOIOutput output, ref b2TOIInput input)
{
++b2_toiCalls;
output.state = b2ImpactState.e_unknown;
output.t = input.tMax;
b2DistanceProxy proxyA = input.proxyA;
b2DistanceProxy proxyB = input.proxyB;
b2Sweep sweepA = input.sweepA;
b2Sweep sweepB = input.sweepB;
// Large rotations can make the root finder fail, so we normalize the
// sweep angles.
sweepA.Normalize();
sweepB.Normalize();
float tMax = input.tMax;
float totalRadius = proxyA.Radius + proxyB.Radius;
float target = Math.Max(b2Settings.b2_linearSlop, totalRadius - 3.0f * b2Settings.b2_linearSlop);
float tolerance = 0.25f * b2Settings.b2_linearSlop;
Debug.Assert(target > tolerance);
float t1 = 0.0f;
int k_maxIterations = 20; // TODO_ERIN b2Settings
int iter = 0;
// Prepare input for distance query.
b2SimplexCache cache = _cache;
b2DistanceInput distanceInput;
distanceInput.proxyA = input.proxyA;
distanceInput.proxyB = input.proxyB;
distanceInput.useRadii = false;
// The outer loop progressively attempts to compute new separating axes.
// This loop terminates when an axis is repeated (no progress is made).
while (true)
{
// Get the distance between shapes. We can also use the results
// to get a separating axis.
sweepA.GetTransform(out distanceInput.transformA, t1);
sweepB.GetTransform(out distanceInput.transformB, t1);
b2DistanceOutput distanceOutput;
b2Simplex.b2Distance(out distanceOutput, ref cache, ref distanceInput);
// If the shapes are overlapped, we give up on continuous collision.
if (distanceOutput.distance <= 0.0f)
{
// Failure!
output.state = b2ImpactState.e_overlapped;
output.t = 0.0f;
break;
}
if (distanceOutput.distance < target + tolerance)
{
// Victory!
output.state = b2ImpactState.e_touching;
output.t = t1;
break;
}
// Initialize the separating axis.
b2SeparationFunction fcn = new b2SeparationFunction();
fcn.Initialize(ref cache, proxyA, ref sweepA, proxyB, ref sweepB, t1,
ref distanceInput.transformA, ref distanceInput.transformB);
#if false
// Dump the curve seen by the root finder
{
int N = 100;
float dx = 1.0f / N;
float xs[N + 1];
// TODO_ERIN might not need to return the separation
public float Initialize(ref b2SimplexCache cache,
b2DistanceProxy proxyA, ref b2Sweep sweepA,
b2DistanceProxy proxyB, ref b2Sweep sweepB,
float t1, ref b2Transform xfA, ref b2Transform xfB)
{
m_proxyA = proxyA;
m_proxyB = proxyB;
int count = cache.count;
Debug.Assert(0 < count && count < 3);
m_sweepA = sweepA;
m_sweepB = sweepB;
if (count == 1)
{
m_type = SeparationType.e_points;
b2Vec2 localPointA = m_proxyA.m_vertices[(int)cache.indexA[0]];
b2Vec2 localPointB = m_proxyB.m_vertices[(int)cache.indexB[0]];
float pointAx = (xfA.q.c * localPointA.x - xfA.q.s * localPointA.y) + xfA.p.x;
float pointAy = (xfA.q.s * localPointA.x + xfA.q.c * localPointA.y) + xfA.p.y;
float pointBx = (xfB.q.c * localPointB.x - xfB.q.s * localPointB.y) + xfB.p.x;
float pointBy = (xfB.q.s * localPointB.x + xfB.q.c * localPointB.y) + xfB.p.y;
m_axis.x = pointBx - pointAx;
m_axis.y = pointBy - pointAy;
float s = m_axis.Normalize();
return(s);
}
else if (cache.indexA[0] == cache.indexA[1])
{
// Two points on B and one on A.
m_type = SeparationType.e_faceB;
b2Vec2 localPointB1 = proxyB.m_vertices[(int)cache.indexB[0]];
b2Vec2 localPointB2 = proxyB.m_vertices[(int)cache.indexB[1]];
float b21x = localPointB2.x - localPointB1.x;
float b21y = localPointB2.y - localPointB1.y;
m_axis.x = -b21y;
m_axis.y = b21x;
// m_axis = b2Math.b2Cross(localPointB2 - localPointB1, 1.0f);
m_axis.Normalize();
float normalx = xfB.q.c * m_axis.x - xfB.q.s * m_axis.y;
float normaly = xfB.q.s * m_axis.x + xfB.q.c * m_axis.y;
m_localPoint.x = 0.5f * (localPointB1.x + localPointB2.x);
m_localPoint.y = 0.5f * (localPointB1.y + localPointB2.y);
float pointBx = (xfB.q.c * m_localPoint.x - xfB.q.s * m_localPoint.y) + xfB.p.x;
float pointBy = (xfB.q.s * m_localPoint.x + xfB.q.c * m_localPoint.y) + xfB.p.y;
b2Vec2 localPointA = proxyA.m_vertices[(int)cache.indexA[0]];
float pointAx = (xfA.q.c * localPointA.x - xfA.q.s * localPointA.y) + xfA.p.x;
float pointAy = (xfA.q.s * localPointA.x + xfA.q.c * localPointA.y) + xfA.p.y;
float aminusbx = pointAx - pointBx;
float aminusby = pointAy - pointBy;
float s = aminusbx * normalx + aminusby * normaly;
if (s < 0.0f)
{
m_axis.x = -m_axis.x;
m_axis.y = -m_axis.y;
s = -s;
}
return(s);
}
else
{
// Two points on A and one or two points on B.
m_type = SeparationType.e_faceA;
b2Vec2 localPointA1 = m_proxyA.m_vertices[cache.indexA[0]];
b2Vec2 localPointA2 = m_proxyA.m_vertices[cache.indexA[1]];
float a2minusa1x = localPointA2.x - localPointA1.x;
float a2minusa1y = localPointA2.y - localPointA1.y;
//m_axis = a2minusa1.UnitCross();// b2Math.b2Cross(localPointA2 - localPointA1, 1.0f);
m_axis.x = a2minusa1y;
m_axis.y = -a2minusa1x;
m_axis.Normalize();
float normalx = xfA.q.c * m_axis.x - xfA.q.s * m_axis.y;
float normaly = xfA.q.s * m_axis.x + xfA.q.c * m_axis.y;
m_localPoint.x = 0.5f * (localPointA1.x + localPointA2.x);
m_localPoint.y = 0.5f * (localPointA1.y + localPointA2.y);
float pointAx = (xfA.q.c * m_localPoint.x - xfA.q.s * m_localPoint.y) + xfA.p.x;
float pointAy = (xfA.q.s * m_localPoint.x + xfA.q.c * m_localPoint.y) + xfA.p.y;
b2Vec2 localPointB = m_proxyB.m_vertices[cache.indexB[0]];
float pointBx = (xfB.q.c * localPointB.x - xfB.q.s * localPointB.y) + xfB.p.x;
float pointBy = (xfB.q.s * localPointB.x + xfB.q.c * localPointB.y) + xfB.p.y;
float bminusax = pointBx - pointAx;
float bminusay = pointBy - pointAy;
float s = bminusax * normalx + bminusay * normaly;
if (s < 0.0f)
{
m_axis.x = -m_axis.x;
m_axis.y = -m_axis.y;
s = -s;
}
return(s);
}
}
// TODO_ERIN might not need to return the separation
public float Initialize(ref b2SimplexCache cache,
b2DistanceProxy proxyA, ref b2Sweep sweepA,
b2DistanceProxy proxyB, ref b2Sweep sweepB,
float t1, ref b2Transform xfA, ref b2Transform xfB)
{
m_proxyA = proxyA;
m_proxyB = proxyB;
int count = cache.count;
Debug.Assert(0 < count && count < 3);
m_sweepA = sweepA;
m_sweepB = sweepB;
if (count == 1)
{
m_type = SeparationType.e_points;
b2Vec2 localPointA = m_proxyA.m_vertices[(int)cache.indexA[0]];
b2Vec2 localPointB = m_proxyB.m_vertices[(int)cache.indexB[0]];
float pointAx = (xfA.q.c * localPointA.x - xfA.q.s * localPointA.y) + xfA.p.x;
float pointAy = (xfA.q.s * localPointA.x + xfA.q.c * localPointA.y) + xfA.p.y;
float pointBx = (xfB.q.c * localPointB.x - xfB.q.s * localPointB.y) + xfB.p.x;
float pointBy = (xfB.q.s * localPointB.x + xfB.q.c * localPointB.y) + xfB.p.y;
m_axis.x = pointBx - pointAx;
m_axis.y = pointBy - pointAy;
float s = m_axis.Normalize();
return s;
}
else if (cache.indexA[0] == cache.indexA[1])
{
// Two points on B and one on A.
m_type = SeparationType.e_faceB;
b2Vec2 localPointB1 = proxyB.m_vertices[(int)cache.indexB[0]];
b2Vec2 localPointB2 = proxyB.m_vertices[(int)cache.indexB[1]];
float b21x = localPointB2.x - localPointB1.x;
float b21y = localPointB2.y - localPointB1.y;
m_axis.x = -b21y;
m_axis.y = b21x;
// m_axis = b2Math.b2Cross(localPointB2 - localPointB1, 1.0f);
m_axis.Normalize();
float normalx = xfB.q.c * m_axis.x - xfB.q.s * m_axis.y;
float normaly = xfB.q.s * m_axis.x + xfB.q.c * m_axis.y;
m_localPoint.x = 0.5f * (localPointB1.x + localPointB2.x);
m_localPoint.y = 0.5f * (localPointB1.y + localPointB2.y);
float pointBx = (xfB.q.c * m_localPoint.x - xfB.q.s * m_localPoint.y) + xfB.p.x;
float pointBy = (xfB.q.s * m_localPoint.x + xfB.q.c * m_localPoint.y) + xfB.p.y;
b2Vec2 localPointA = proxyA.m_vertices[(int)cache.indexA[0]];
float pointAx = (xfA.q.c * localPointA.x - xfA.q.s * localPointA.y) + xfA.p.x;
float pointAy = (xfA.q.s * localPointA.x + xfA.q.c * localPointA.y) + xfA.p.y;
float aminusbx = pointAx - pointBx;
float aminusby = pointAy - pointBy;
float s = aminusbx * normalx + aminusby * normaly;
if (s < 0.0f)
{
m_axis.x = -m_axis.x;
m_axis.y = -m_axis.y;
s = -s;
}
return s;
}
else
{
// Two points on A and one or two points on B.
m_type = SeparationType.e_faceA;
b2Vec2 localPointA1 = m_proxyA.m_vertices[cache.indexA[0]];
b2Vec2 localPointA2 = m_proxyA.m_vertices[cache.indexA[1]];
float a2minusa1x = localPointA2.x - localPointA1.x;
float a2minusa1y = localPointA2.y - localPointA1.y;
//m_axis = a2minusa1.UnitCross();// b2Math.b2Cross(localPointA2 - localPointA1, 1.0f);
m_axis.x = a2minusa1y;
m_axis.y = -a2minusa1x;
m_axis.Normalize();
float normalx = xfA.q.c * m_axis.x - xfA.q.s * m_axis.y;
float normaly = xfA.q.s * m_axis.x + xfA.q.c * m_axis.y;
m_localPoint.x = 0.5f * (localPointA1.x + localPointA2.x);
m_localPoint.y = 0.5f * (localPointA1.y + localPointA2.y);
float pointAx = (xfA.q.c * m_localPoint.x - xfA.q.s * m_localPoint.y) + xfA.p.x;
float pointAy = (xfA.q.s * m_localPoint.x + xfA.q.c * m_localPoint.y) + xfA.p.y;
b2Vec2 localPointB = m_proxyB.m_vertices[cache.indexB[0]];
float pointBx = (xfB.q.c * localPointB.x - xfB.q.s * localPointB.y) + xfB.p.x;
float pointBy = (xfB.q.s * localPointB.x + xfB.q.c * localPointB.y) + xfB.p.y;
float bminusax = pointBx - pointAx;
float bminusay = pointBy - pointAy;
float s = bminusax * normalx + bminusay * normaly;
if (s < 0.0f)
{
m_axis.x = -m_axis.x;
m_axis.y = -m_axis.y;
s = -s;
}
return s;
}
}
// TODO_ERIN might not need to return the separation
public float Initialize(ref b2SimplexCache cache,
ref b2DistanceProxy proxyA, ref b2Sweep sweepA,
ref b2DistanceProxy proxyB, ref b2Sweep sweepB,
float t1)
{
m_proxyA = proxyA;
m_proxyB = proxyB;
int count = cache.count;
Debug.Assert(0 < count && count < 3);
m_sweepA = sweepA;
m_sweepB = sweepB;
b2Transform xfA = b2Transform.Default, xfB = b2Transform.Default;
m_sweepA.GetTransform(ref xfA, t1);
m_sweepB.GetTransform(ref xfB, t1);
if (count == 1)
{
m_type = SeparationType.e_points;
b2Vec2 localPointA = m_proxyA.GetVertex((int)cache.indexA[0]);
b2Vec2 localPointB = m_proxyB.GetVertex((int)cache.indexB[0]);
b2Vec2 pointA = b2Math.b2Mul(xfA, localPointA);
b2Vec2 pointB = b2Math.b2Mul(xfB, localPointB);
m_axis = pointB - pointA;
float s = m_axis.Normalize();
return s;
}
else if (cache.indexA[0] == cache.indexA[1])
{
// Two points on B and one on A.
m_type = SeparationType.e_faceB;
b2Vec2 localPointB1 = proxyB.GetVertex((int)cache.indexB[0]);
b2Vec2 localPointB2 = proxyB.GetVertex((int)cache.indexB[1]);
m_axis = b2Math.b2Cross(localPointB2 - localPointB1, 1.0f);
m_axis.Normalize();
b2Vec2 normal = b2Math.b2Mul(xfB.q, m_axis);
m_localPoint = 0.5f * (localPointB1 + localPointB2);
b2Vec2 pointB = b2Math.b2Mul(xfB, m_localPoint);
b2Vec2 localPointA = proxyA.GetVertex((int)cache.indexA[0]);
b2Vec2 pointA = b2Math.b2Mul(xfA, localPointA);
float s = b2Math.b2Dot(pointA - pointB, normal);
if (s < 0.0f)
{
m_axis = -m_axis;
s = -s;
}
return s;
}
else
{
// Two points on A and one or two points on B.
m_type = SeparationType.e_faceA;
b2Vec2 localPointA1 = m_proxyA.GetVertex(cache.indexA[0]);
b2Vec2 localPointA2 = m_proxyA.GetVertex(cache.indexA[1]);
m_axis = b2Math.b2Cross(localPointA2 - localPointA1, 1.0f);
m_axis.Normalize();
b2Vec2 normal = b2Math.b2Mul(xfA.q, m_axis);
m_localPoint = 0.5f * (localPointA1 + localPointA2);
b2Vec2 pointA = b2Math.b2Mul(xfA, m_localPoint);
b2Vec2 localPointB = m_proxyB.GetVertex(cache.indexB[0]);
b2Vec2 pointB = b2Math.b2Mul(xfB, localPointB);
float s = b2Math.b2Dot(pointB - pointA, normal);
if (s < 0.0f)
{
m_axis = -m_axis;
s = -s;
}
return s;
}
}
public static b2SimplexCache Create()
{
b2SimplexCache obj = new b2SimplexCache();
obj.Defaults();
return (obj);
}
public static void b2Distance(out b2DistanceOutput output, ref b2SimplexCache cache, ref b2DistanceInput input)
{
++b2DistanceProxy.b2_gjkCalls;
b2DistanceProxy proxyA = input.proxyA;
b2DistanceProxy proxyB = input.proxyB;
// Initialize the simplex.
b2Simplex simplex = new b2Simplex();
simplex.ReadCache(ref cache, proxyA, ref input.transformA, proxyB, ref input.transformB);
// Get simplex vertices as an array.
b2SimplexVertex[] vertices = b2Simplex.m_vertices;
int k_maxIters = 20;
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
int[] saveA = _saveA;
int[] saveB = _saveB;
int saveCount = 0;
b2Vec2 closestPoint;
simplex.GetClosestPoint(out closestPoint);
float distanceSqr1 = closestPoint.LengthSquared;
float distanceSqr2 = distanceSqr1;
// Console.WriteLine("Closest Point={0},{1}, distance={2}", closestPoint.x, closestPoint.y, distanceSqr1);
// Main iteration loop.
#region Main Iteration Loop
int iter = 0;
while (iter < k_maxIters)
{
// Copy simplex so we can identify duplicates.
saveCount = simplex.m_count;
for (int 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:
Debug.Assert(false);
break;
}
// 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(out p);
distanceSqr2 = p.x * p.x + p.y * p.y;
// Ensure progress
if (distanceSqr2 >= distanceSqr1)
{
//break;
}
distanceSqr1 = distanceSqr2;
// Get search direction.
b2Vec2 d;
simplex.GetSearchDirection(out d);
// Ensure the search direction is numerically fit.
if ((d.x * d.x + d.y * d.y) < b2Settings.b2_epsilon * b2Settings.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];
var q = input.transformA.q;
b2Vec2 b;
b.x = q.c * -d.x + q.s * -d.y;
b.y = -q.s * -d.x + q.c * -d.y;
vertex.indexA = proxyA.GetSupport(ref b);
var vA = proxyA.m_vertices[vertex.indexA];
vertex.wA.x = (q.c * vA.x - q.s * vA.y) + input.transformA.p.x;
vertex.wA.y = (q.s * vA.x + q.c * vA.y) + input.transformA.p.y;
// b2Vec2 wBLocal = new b2Vec2();
q = input.transformB.q;
b.x = q.c * d.x + q.s * d.y;
b.y = -q.s * d.x + q.c * d.y;
vertex.indexB = proxyB.GetSupport(ref b);
var vB = proxyB.m_vertices[vertex.indexB];
vertex.wB.x = (input.transformB.q.c * vB.x - input.transformB.q.s * vB.y) + input.transformB.p.x;
vertex.wB.y = (input.transformB.q.s * vB.x + input.transformB.q.c * vB.y) + input.transformB.p.y;
vertex.w.x = vertex.wB.x - vertex.wA.x;
vertex.w.y = vertex.wB.y - vertex.wA.y;
// Iteration count is equated to the number of support point calls.
++iter;
++b2DistanceProxy.b2_gjkIters;
// Check for duplicate support points. This is the main termination criteria.
bool duplicate = false;
for (int 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;
}
#endregion
b2DistanceProxy.b2_gjkMaxIters = Math.Max(b2DistanceProxy.b2_gjkMaxIters, iter);
// Prepare output.
simplex.GetWitnessPoints(out output.pointA, out output.pointB);
output.distance = b2Math.b2Distance(ref output.pointA, ref output.pointB);
output.iterations = iter;
// Cache the simplex.
simplex.WriteCache(ref cache);
// Apply radii if requested.
if (input.useRadii)
{
float rA = proxyA.Radius;
float rB = proxyB.Radius;
if (output.distance > rA + rB && output.distance > b2Settings.b2_epsilon)
{
// Shapes are still not overlapped.
// Move the witness points to the outer surface.
output.distance -= rA + rB;
b2Vec2 normal;
normal.x = output.pointB.x - output.pointA.x;
normal.y = output.pointB.y - output.pointA.y;
normal.Normalize();
output.pointA.x += rA * normal.x;
output.pointA += rA * normal;
output.pointB.x -= rB * normal.x;
output.pointB.y -= rB * normal.y;
}
else
{
// Shapes are overlapped when radii are considered.
// Move the witness points to the middle.
b2Vec2 p;
p.x = 0.5f * (output.pointA.x + output.pointB.x);
p.y = 0.5f * (output.pointA.y + output.pointB.y);
output.pointA = p;
output.pointB = p;
output.distance = 0.0f;
}
}
}
// TODO_ERIN might not need to return the separation
public float Initialize(ref b2SimplexCache cache,
b2DistanceProxy proxyA, ref b2Sweep sweepA,
b2DistanceProxy proxyB, ref b2Sweep sweepB,
float t1)
{
m_proxyA = proxyA;
m_proxyB = proxyB;
int count = cache.count;
Debug.Assert(0 < count && count < 3);
m_sweepA = sweepA;
m_sweepB = sweepB;
b2Transform xfA, xfB;
m_sweepA.GetTransform(out xfA, t1);
m_sweepB.GetTransform(out xfB, t1);
if (count == 1)
{
m_type = SeparationType.e_points;
b2Vec2 localPointA = m_proxyA.GetVertex((int)cache.indexA[0]);
b2Vec2 localPointB = m_proxyB.GetVertex((int)cache.indexB[0]);
b2Vec2 pointA = b2Math.b2Mul(xfA, localPointA);
b2Vec2 pointB = b2Math.b2Mul(xfB, localPointB);
m_axis = pointB - pointA;
float s = m_axis.Normalize();
return s;
}
else if (cache.indexA[0] == cache.indexA[1])
{
// Two points on B and one on A.
m_type = SeparationType.e_faceB;
b2Vec2 localPointB1 = proxyB.GetVertex((int)cache.indexB[0]);
b2Vec2 localPointB2 = proxyB.GetVertex((int)cache.indexB[1]);
float b21x = localPointB2.x - localPointB1.x;
float b21y = localPointB2.y - localPointB1.y;
m_axis.x = -b21y;
m_axis.y = b21x;
// m_axis = b2Math.b2Cross(localPointB2 - localPointB1, 1.0f);
m_axis.Normalize();
b2Vec2 normal = b2Math.b2Mul(xfB.q, m_axis);
m_localPoint = 0.5f * (localPointB1 + localPointB2);
b2Vec2 pointB = b2Math.b2Mul(xfB, m_localPoint);
b2Vec2 localPointA = proxyA.GetVertex((int)cache.indexA[0]);
b2Vec2 pointA = b2Math.b2Mul(xfA, localPointA);
b2Vec2 aminusb = pointA - pointB;
float s = b2Math.b2Dot(ref aminusb, ref normal);
if (s < 0.0f)
{
m_axis = -m_axis;
s = -s;
}
return s;
}
else
{
// Two points on A and one or two points on B.
m_type = SeparationType.e_faceA;
b2Vec2 localPointA1 = m_proxyA.GetVertex(cache.indexA[0]);
b2Vec2 localPointA2 = m_proxyA.GetVertex(cache.indexA[1]);
b2Vec2 a2minusa1 = localPointA2 - localPointA1;
m_axis = a2minusa1.UnitCross();// b2Math.b2Cross(localPointA2 - localPointA1, 1.0f);
m_axis.Normalize();
b2Vec2 normal = b2Math.b2Mul(xfA.q, m_axis);
m_localPoint = 0.5f * (localPointA1 + localPointA2);
b2Vec2 pointA = b2Math.b2Mul(xfA, m_localPoint);
b2Vec2 localPointB = m_proxyB.GetVertex(cache.indexB[0]);
b2Vec2 pointB = b2Math.b2Mul(xfB, localPointB);
b2Vec2 bminusa = pointB - pointA;
float s = b2Math.b2Dot(ref bminusa, ref normal);
if (s < 0.0f)
{
m_axis = -m_axis;
s = -s;
}
return s;
}
}
private void ReadCache(ref b2SimplexCache cache,
b2DistanceProxy proxyA, ref b2Transform transformA,
b2DistanceProxy proxyB, ref b2Transform transformB)
{
Debug.Assert(cache.count <= 3);
// Copy data from cache.
m_count = (int)cache.count;
for (int i = 0; i < m_count; ++i)
{
b2SimplexVertex v = m_vertices[i];
v.indexA = (int)cache.indexA[i];
v.indexB = (int)cache.indexB[i];
b2Vec2 wALocal = proxyA.m_vertices[v.indexA];
b2Vec2 wBLocal = proxyB.m_vertices[v.indexB];
v.wA.x = (transformA.q.c * wALocal.x - transformA.q.s * wALocal.y) + transformA.p.x;
v.wA.y = (transformA.q.s * wALocal.x + transformA.q.c * wALocal.y) + transformA.p.y;
v.wB.x = (transformB.q.c * wBLocal.x - transformB.q.s * wBLocal.y) + transformB.p.x;
v.wB.y = (transformB.q.s * wBLocal.x + transformB.q.c * wBLocal.y) + transformB.p.y;
v.w.x = v.wB.x - v.wA.x;
v.w.y = v.wB.y - v.wA.y;
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)
{
float metric1 = cache.metric;
float metric2 = GetMetric();
if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < b2Settings.b2_epsilon)
{
// Reset the simplex.
m_count = 0;
}
}
// If the cache is empty or invalid ...
if (m_count == 0)
{
b2SimplexVertex v = m_vertices_0;
v.indexA = 0;
v.indexB = 0;
b2Vec2 wALocal = proxyA.m_vertices[0];
b2Vec2 wBLocal = proxyB.m_vertices[0];
v.wA.x = (transformA.q.c * wALocal.x - transformA.q.s * wALocal.y) + transformA.p.x;
v.wA.y = (transformA.q.s * wALocal.x + transformA.q.c * wALocal.y) + transformA.p.y;
v.wB.x = (transformB.q.c * wBLocal.x - transformB.q.s * wBLocal.y) + transformB.p.x;
v.wB.y = (transformB.q.s * wBLocal.x + transformB.q.c * wBLocal.y) + transformB.p.y;
v.w.x = v.wB.x - v.wA.x;
v.w.y = v.wB.y - v.wA.y;
v.a = 0.0f;
m_count = 1;
}
}
public static void b2Distance(ref b2DistanceOutput output, ref b2SimplexCache cache, ref b2DistanceInput input)
{
++b2DistanceProxy.b2_gjkCalls;
b2DistanceProxy proxyA = input.proxyA.Copy();
b2DistanceProxy proxyB = input.proxyB.Copy();
b2Transform transformA = input.transformA;
b2Transform transformB = input.transformB;
// Initialize the simplex.
b2Simplex simplex = new b2Simplex();
simplex.ReadCache(ref cache, ref proxyA, ref transformA, ref proxyB, ref transformB);
// Get simplex vertices as an array.
b2SimplexVertex[] vertices = new b2SimplexVertex[] { simplex.m_vertices[0], simplex.m_vertices[1], simplex.m_vertices[2] };
int k_maxIters = 20;
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
int[] saveA = new int[3];
int[] saveB = new int[3];
int saveCount = 0;
b2Vec2 closestPoint = simplex.GetClosestPoint();
float distanceSqr1 = closestPoint.LengthSquared();
float distanceSqr2 = distanceSqr1;
// Console.WriteLine("Closest Point={0},{1}, distance={2}", closestPoint.x, closestPoint.y, distanceSqr1);
// Main iteration loop.
#region Main Iteration Loop
int iter = 0;
while (iter < k_maxIters)
{
// Copy simplex so we can identify duplicates.
saveCount = simplex.m_count;
for (int 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:
Debug.Assert(false);
break;
}
// 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() < b2Settings.b2_epsilon * b2Settings.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(b2Math.b2MulT(transformA.q, -d));
vertex.wA = b2Math.b2Mul(transformA, proxyA.GetVertex(vertex.indexA));
// b2Vec2 wBLocal = new b2Vec2();
vertex.indexB = proxyB.GetSupport(b2Math.b2MulT(transformB.q, d));
vertex.wB = b2Math.b2Mul(transformB, proxyB.GetVertex(vertex.indexB));
vertex.w = vertex.wB - vertex.wA;
vertices[simplex.m_count] = vertex;
// Iteration count is equated to the number of support point calls.
++iter;
++b2DistanceProxy.b2_gjkIters;
// Check for duplicate support points. This is the main termination criteria.
bool duplicate = false;
for (int 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;
}
#endregion
b2DistanceProxy.b2_gjkMaxIters = Math.Max(b2DistanceProxy.b2_gjkMaxIters, iter);
// Prepare output.
simplex.GetWitnessPoints(ref output.pointA, ref output.pointB);
output.distance = b2Math.b2Distance(output.pointA, output.pointB);
output.iterations = iter;
// Cache the simplex.
simplex.WriteCache(ref cache);
// Apply radii if requested.
if (input.useRadii)
{
float rA = proxyA.Radius;
float rB = proxyB.Radius;
if (output.distance > rA + rB && output.distance > b2Settings.b2_epsilon)
{
// Shapes are still not 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;
}
}
// Copy back the vertex changes because they are structs, but in C++ land they are reference values
simplex.m_vertices[0] = vertices[0];
simplex.m_vertices[1] = vertices[1];
simplex.m_vertices[2] = vertices[2];
}
public void ReadCache(ref b2SimplexCache cache,
ref b2DistanceProxy proxyA, ref b2Transform transformA,
ref b2DistanceProxy proxyB, ref b2Transform transformB)
{
Debug.Assert(cache.count <= 3);
// Copy data from cache.
m_count = (int)cache.count;
m_vertices = new b2SimplexVertex[3];
for (int i = 0; i < m_count; ++i)
{
b2SimplexVertex v;
v.indexA = (int)cache.indexA[i];
v.indexB = (int)cache.indexB[i];
b2Vec2 wALocal = proxyA.GetVertex(v.indexA);
b2Vec2 wBLocal = proxyB.GetVertex(v.indexB);
v.wA = b2Math.b2Mul(transformA, wALocal);
v.wB = b2Math.b2Mul(transformB, wBLocal);
v.w = v.wB - v.wA;
v.a = 0.0f;
m_vertices[i] = v;
}
// Compute the new simplex metric, if it is substantially different than
// old metric then flush the simplex.
if (m_count > 1)
{
float metric1 = cache.metric;
float metric2 = GetMetric();
if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < b2Settings.b2_epsilon)
{
// Reset the simplex.
m_count = 0;
}
}
// If the cache is empty or invalid ...
if (m_count == 0)
{
b2SimplexVertex v;
v.indexA = 0;
v.indexB = 0;
b2Vec2 wALocal = proxyA.GetVertex(0);
b2Vec2 wBLocal = proxyB.GetVertex(0);
v.wA = b2Math.b2Mul(transformA, wALocal);
v.wB = b2Math.b2Mul(transformB, wBLocal);
v.w = v.wB - v.wA;
v.a = 0.0f;
m_count = 1;
m_vertices[0] = v;
}
}
private void ReadCache(ref b2SimplexCache cache,
b2DistanceProxy proxyA, ref b2Transform transformA,
b2DistanceProxy proxyB, ref b2Transform transformB)
{
Debug.Assert(cache.count <= 3);
// Copy data from cache.
m_count = (int)cache.count;
for (int i = 0; i < m_count; ++i)
{
b2SimplexVertex v = m_vertices[i];
v.indexA = (int)cache.indexA[i];
v.indexB = (int)cache.indexB[i];
b2Vec2 wALocal = proxyA.m_vertices[v.indexA];
b2Vec2 wBLocal = proxyB.m_vertices[v.indexB];
v.wA.x = (transformA.q.c * wALocal.x - transformA.q.s * wALocal.y) + transformA.p.x;
v.wA.y = (transformA.q.s * wALocal.x + transformA.q.c * wALocal.y) + transformA.p.y;
v.wB.x = (transformB.q.c * wBLocal.x - transformB.q.s * wBLocal.y) + transformB.p.x;
v.wB.y = (transformB.q.s * wBLocal.x + transformB.q.c * wBLocal.y) + transformB.p.y;
v.w.x = v.wB.x - v.wA.x;
v.w.y = v.wB.y - v.wA.y;
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)
{
float metric1 = cache.metric;
float metric2 = GetMetric();
if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < b2Settings.b2_epsilon)
{
// Reset the simplex.
m_count = 0;
}
}
// If the cache is empty or invalid ...
if (m_count == 0)
{
b2SimplexVertex v = m_vertices_0;
v.indexA = 0;
v.indexB = 0;
b2Vec2 wALocal = proxyA.m_vertices[0];
b2Vec2 wBLocal = proxyB.m_vertices[0];
v.wA.x = (transformA.q.c * wALocal.x - transformA.q.s * wALocal.y) + transformA.p.x;
v.wA.y = (transformA.q.s * wALocal.x + transformA.q.c * wALocal.y) + transformA.p.y;
v.wB.x = (transformB.q.c * wBLocal.x - transformB.q.s * wBLocal.y) + transformB.p.x;
v.wB.y = (transformB.q.s * wBLocal.x + transformB.q.c * wBLocal.y) + transformB.p.y;
v.w.x = v.wB.x - v.wA.x;
v.w.y = v.wB.y - v.wA.y;
v.a = 0.0f;
m_count = 1;
}
}
// TODO_ERIN might not need to return the separation
public float Initialize(ref b2SimplexCache cache,
ref b2DistanceProxy proxyA, ref b2Sweep sweepA,
ref b2DistanceProxy proxyB, ref b2Sweep sweepB,
float t1)
{
m_proxyA = proxyA;
m_proxyB = proxyB;
int count = cache.count;
Debug.Assert(0 < count && count < 3);
m_sweepA = sweepA;
m_sweepB = sweepB;
b2Transform xfA = b2Transform.Create(), xfB = b2Transform.Create();
m_sweepA.GetTransform(ref xfA, t1);
m_sweepB.GetTransform(ref xfB, t1);
if (count == 1)
{
m_type = SeparationType.e_points;
b2Vec2 localPointA = m_proxyA.GetVertex((int)cache.indexA[0]);
b2Vec2 localPointB = m_proxyB.GetVertex((int)cache.indexB[0]);
b2Vec2 pointA = b2Math.b2Mul(xfA, localPointA);
b2Vec2 pointB = b2Math.b2Mul(xfB, localPointB);
m_axis = pointB - pointA;
float s = m_axis.Normalize();
return(s);
}
else if (cache.indexA[0] == cache.indexA[1])
{
// Two points on B and one on A.
m_type = SeparationType.e_faceB;
b2Vec2 localPointB1 = proxyB.GetVertex((int)cache.indexB[0]);
b2Vec2 localPointB2 = proxyB.GetVertex((int)cache.indexB[1]);
m_axis = b2Math.b2Cross(localPointB2 - localPointB1, 1.0f);
m_axis.Normalize();
b2Vec2 normal = b2Math.b2Mul(xfB.q, m_axis);
m_localPoint = 0.5f * (localPointB1 + localPointB2);
b2Vec2 pointB = b2Math.b2Mul(xfB, m_localPoint);
b2Vec2 localPointA = proxyA.GetVertex((int)cache.indexA[0]);
b2Vec2 pointA = b2Math.b2Mul(xfA, localPointA);
float s = b2Math.b2Dot(pointA - pointB, normal);
if (s < 0.0f)
{
m_axis = -m_axis;
s = -s;
}
return(s);
}
else
{
// Two points on A and one or two points on B.
m_type = SeparationType.e_faceA;
b2Vec2 localPointA1 = m_proxyA.GetVertex(cache.indexA[0]);
b2Vec2 localPointA2 = m_proxyA.GetVertex(cache.indexA[1]);
m_axis = b2Math.b2Cross(localPointA2 - localPointA1, 1.0f);
m_axis.Normalize();
b2Vec2 normal = b2Math.b2Mul(xfA.q, m_axis);
m_localPoint = 0.5f * (localPointA1 + localPointA2);
b2Vec2 pointA = b2Math.b2Mul(xfA, m_localPoint);
b2Vec2 localPointB = m_proxyB.GetVertex(cache.indexB[0]);
b2Vec2 pointB = b2Math.b2Mul(xfB, localPointB);
float s = b2Math.b2Dot(pointB - pointA, normal);
if (s < 0.0f)
{
m_axis = -m_axis;
s = -s;
}
return(s);
}
}
public void WriteCache(ref b2SimplexCache cache)
{
cache.metric = GetMetric();
cache.count = m_count;
for (int i = 0; i < m_count; ++i)
{
cache.indexA[i] = m_vertices[i].indexA;
cache.indexB[i] = m_vertices[i].indexB;
}
}
public static void b2Distance(ref b2DistanceOutput output, ref b2SimplexCache cache, ref b2DistanceInput input)
{
++b2DistanceProxy.b2_gjkCalls;
b2DistanceProxy proxyA = input.proxyA.Copy();
b2DistanceProxy proxyB = input.proxyB.Copy();
b2Transform transformA = input.transformA;
b2Transform transformB = input.transformB;
// Initialize the simplex.
b2Simplex simplex = new b2Simplex();
simplex.ReadCache(ref cache, ref proxyA, ref transformA, ref proxyB, ref transformB);
// Get simplex vertices as an array.
b2SimplexVertex[] vertices = new b2SimplexVertex[] { simplex.m_vertices[0], simplex.m_vertices[1], simplex.m_vertices[2] };
int k_maxIters = 20;
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
int[] saveA = new int[3];
int[] saveB = new int[3];
int saveCount = 0;
b2Vec2 closestPoint = simplex.GetClosestPoint();
float distanceSqr1 = closestPoint.LengthSquared();
float distanceSqr2 = distanceSqr1;
// Console.WriteLine("Closest Point={0},{1}, distance={2}", closestPoint.x, closestPoint.y, distanceSqr1);
// Main iteration loop.
#region Main Iteration Loop
int iter = 0;
while (iter < k_maxIters)
{
// Copy simplex so we can identify duplicates.
saveCount = simplex.m_count;
for (int 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:
Debug.Assert(false);
break;
}
// 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() < b2Settings.b2_epsilon * b2Settings.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(b2Math.b2MulT(transformA.q, -d));
vertex.wA = b2Math.b2Mul(transformA, proxyA.GetVertex(vertex.indexA));
// b2Vec2 wBLocal = new b2Vec2();
vertex.indexB = proxyB.GetSupport(b2Math.b2MulT(transformB.q, d));
vertex.wB = b2Math.b2Mul(transformB, proxyB.GetVertex(vertex.indexB));
vertex.w = vertex.wB - vertex.wA;
vertices[simplex.m_count] = vertex;
// Iteration count is equated to the number of support point calls.
++iter;
++b2DistanceProxy.b2_gjkIters;
// Check for duplicate support points. This is the main termination criteria.
bool duplicate = false;
for (int 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;
}
#endregion
b2DistanceProxy.b2_gjkMaxIters = Math.Max(b2DistanceProxy.b2_gjkMaxIters, iter);
// Prepare output.
simplex.GetWitnessPoints(ref output.pointA, ref output.pointB);
output.distance = b2Math.b2Distance(output.pointA, output.pointB);
output.iterations = iter;
// Cache the simplex.
simplex.WriteCache(ref cache);
// Apply radii if requested.
if (input.useRadii)
{
float rA = proxyA.Radius;
float rB = proxyB.Radius;
if (output.distance > rA + rB && output.distance > b2Settings.b2_epsilon)
{
// Shapes are still not 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;
}
}
// Copy back the vertex changes because they are structs, but in C++ land they are reference values
simplex.m_vertices[0] = vertices[0];
simplex.m_vertices[1] = vertices[1];
simplex.m_vertices[2] = vertices[2];
}
public static void b2Distance(out b2DistanceOutput output, ref b2SimplexCache cache, ref b2DistanceInput input)
{
++b2DistanceProxy.b2_gjkCalls;
b2DistanceProxy proxyA = input.proxyA;
b2DistanceProxy proxyB = input.proxyB;
// Initialize the simplex.
b2Simplex simplex = new b2Simplex();
simplex.ReadCache(ref cache, proxyA, ref input.transformA, proxyB, ref input.transformB);
// Get simplex vertices as an array.
b2SimplexVertex[] vertices = b2Simplex.m_vertices;
int k_maxIters = 20;
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
int[] saveA = _saveA;
int[] saveB = _saveB;
int saveCount = 0;
b2Vec2 closestPoint;
simplex.GetClosestPoint(out closestPoint);
float distanceSqr1 = closestPoint.LengthSquared;
float distanceSqr2 = distanceSqr1;
// Console.WriteLine("Closest Point={0},{1}, distance={2}", closestPoint.x, closestPoint.y, distanceSqr1);
// Main iteration loop.
#region Main Iteration Loop
int iter = 0;
while (iter < k_maxIters)
{
// Copy simplex so we can identify duplicates.
saveCount = simplex.m_count;
for (int 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:
Debug.Assert(false);
break;
}
// 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(out p);
distanceSqr2 = p.x * p.x + p.y * p.y;
// Ensure progress
if (distanceSqr2 >= distanceSqr1)
{
//break;
}
distanceSqr1 = distanceSqr2;
// Get search direction.
b2Vec2 d;
simplex.GetSearchDirection(out d);
// Ensure the search direction is numerically fit.
if ((d.x * d.x + d.y * d.y) < b2Settings.b2_epsilon * b2Settings.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];
var q = input.transformA.q;
b2Vec2 b;
b.x = q.c * -d.x + q.s * -d.y;
b.y = -q.s * -d.x + q.c * -d.y;
vertex.indexA = proxyA.GetSupport(ref b);
var vA = proxyA.m_vertices[vertex.indexA];
vertex.wA.x = (q.c * vA.x - q.s * vA.y) + input.transformA.p.x;
vertex.wA.y = (q.s * vA.x + q.c * vA.y) + input.transformA.p.y;
// b2Vec2 wBLocal = new b2Vec2();
q = input.transformB.q;
b.x = q.c * d.x + q.s * d.y;
b.y = -q.s * d.x + q.c * d.y;
vertex.indexB = proxyB.GetSupport(ref b);
var vB = proxyB.m_vertices[vertex.indexB];
vertex.wB.x = (input.transformB.q.c * vB.x - input.transformB.q.s * vB.y) + input.transformB.p.x;
vertex.wB.y = (input.transformB.q.s * vB.x + input.transformB.q.c * vB.y) + input.transformB.p.y;
vertex.w.x = vertex.wB.x - vertex.wA.x;
vertex.w.y = vertex.wB.y - vertex.wA.y;
// Iteration count is equated to the number of support point calls.
++iter;
++b2DistanceProxy.b2_gjkIters;
// Check for duplicate support points. This is the main termination criteria.
bool duplicate = false;
for (int 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;
}
#endregion
b2DistanceProxy.b2_gjkMaxIters = Math.Max(b2DistanceProxy.b2_gjkMaxIters, iter);
// Prepare output.
simplex.GetWitnessPoints(out output.pointA, out output.pointB);
output.distance = b2Math.b2Distance(ref output.pointA, ref output.pointB);
output.iterations = iter;
// Cache the simplex.
simplex.WriteCache(ref cache);
// Apply radii if requested.
if (input.useRadii)
{
float rA = proxyA.Radius;
float rB = proxyB.Radius;
if (output.distance > rA + rB && output.distance > b2Settings.b2_epsilon)
{
// Shapes are still not overlapped.
// Move the witness points to the outer surface.
output.distance -= rA + rB;
b2Vec2 normal;
normal.x = output.pointB.x - output.pointA.x;
normal.y = output.pointB.y - output.pointA.y;
normal.Normalize();
output.pointA.x += rA * normal.x;
output.pointA += rA * normal;
output.pointB.x -= rB * normal.x;
output.pointB.y -= rB * normal.y;
}
else
{
// Shapes are overlapped when radii are considered.
// Move the witness points to the middle.
b2Vec2 p;
p.x = 0.5f * (output.pointA.x + output.pointB.x);
p.y = 0.5f * (output.pointA.y + output.pointB.y);
output.pointA = p;
output.pointB = p;
output.distance = 0.0f;
}
}
}
public void Initialize(b2SimplexCache cache,
b2DistanceProxy proxyA, b2Transform transformA,
b2DistanceProxy proxyB, b2Transform transformB)
{
m_proxyA = proxyA;
m_proxyB = proxyB;
int count = (int)cache.count;
b2Settings.b2Assert(0 < count && count < 3);
b2Vec2 localPointA = null;
b2Vec2 localPointA1;
b2Vec2 localPointA2;
b2Vec2 localPointB = null;
b2Vec2 localPointB1;
b2Vec2 localPointB2;
float pointAX;
float pointAY;
float pointBX;
float pointBY;
float normalX;
float normalY;
b2Mat22 tMat;
b2Vec2 tVec;
float s;
float sgn;
if (count == 1)
{
m_type = e_points;
localPointA = m_proxyA.GetVertex(cache.indexA[0]);
localPointB = m_proxyB.GetVertex(cache.indexB[0]);
//pointA = b2Math.b2MulX(transformA, localPointA);
tVec = localPointA;
tMat = transformA.R;
pointAX = transformA.position.x + (tMat.col1.x * tVec.x + tMat.col2.x * tVec.y);
pointAY = transformA.position.y + (tMat.col1.y * tVec.x + tMat.col2.y * tVec.y);
//pointB = b2Math.b2MulX(transformB, localPointB);
tVec = localPointB;
tMat = transformB.R;
pointBX = transformB.position.x + (tMat.col1.x * tVec.x + tMat.col2.x * tVec.y);
pointBY = transformB.position.y + (tMat.col1.y * tVec.x + tMat.col2.y * tVec.y);
//m_axis = b2Math.SubtractVV(pointB, pointA);
m_axis.x = pointBX - pointAX;
m_axis.y = pointBY - pointAY;
m_axis.Normalize();
}
else if (cache.indexB[0] == cache.indexB[1])
{
// Two points on A and one on B
m_type = e_faceA;
localPointA1 = m_proxyA.GetVertex(cache.indexA[0]);
localPointA2 = m_proxyA.GetVertex(cache.indexA[1]);
localPointB = m_proxyB.GetVertex(cache.indexB[0]);
m_localPoint.x = 0.5f * (localPointA1.x + localPointA2.x);
m_localPoint.y = 0.5f * (localPointA1.y + localPointA2.y);
m_axis = b2Math.CrossVF(b2Math.SubtractVV(localPointA2, localPointA1), 1.0f);
m_axis.Normalize();
//normal = b2Math.b2MulMV(transformA.R, m_axis);
tVec = m_axis;
tMat = transformA.R;
normalX = tMat.col1.x * tVec.x + tMat.col2.x * tVec.y;
normalY = tMat.col1.y * tVec.x + tMat.col2.y * tVec.y;
//pointA = b2Math.b2MulX(transformA, m_localPoint);
tVec = m_localPoint;
tMat = transformA.R;
pointAX = transformA.position.x + (tMat.col1.x * tVec.x + tMat.col2.x * tVec.y);
pointAY = transformA.position.y + (tMat.col1.y * tVec.x + tMat.col2.y * tVec.y);
//pointB = b2Math.b2MulX(transformB, localPointB);
tVec = localPointB;
tMat = transformB.R;
pointBX = transformB.position.x + (tMat.col1.x * tVec.x + tMat.col2.x * tVec.y);
pointBY = transformB.position.y + (tMat.col1.y * tVec.x + tMat.col2.y * tVec.y);
//float32 s = b2Dot(pointB - pointA, normal);
s = (pointBX - pointAX) * normalX + (pointBY - pointAY) * normalY;
if (s < 0.0f)
{
m_axis.NegativeSelf();
}
}
else if (cache.indexA[0] == cache.indexA[0])
{
// Two points on B and one on A
m_type = e_faceB;
localPointB1 = m_proxyB.GetVertex(cache.indexB[0]);
localPointB2 = m_proxyB.GetVertex(cache.indexB[1]);
localPointA = m_proxyA.GetVertex(cache.indexA[0]);
m_localPoint.x = 0.5f * (localPointB1.x + localPointB2.x);
m_localPoint.y = 0.5f * (localPointB1.y + localPointB2.y);
m_axis = b2Math.CrossVF(b2Math.SubtractVV(localPointB2, localPointB1), 1.0f);
m_axis.Normalize();
//normal = b2Math.b2MulMV(transformB.R, m_axis);
tVec = m_axis;
tMat = transformB.R;
normalX = tMat.col1.x * tVec.x + tMat.col2.x * tVec.y;
normalY = tMat.col1.y * tVec.x + tMat.col2.y * tVec.y;
//pointB = b2Math.b2MulX(transformB, m_localPoint);
tVec = m_localPoint;
tMat = transformB.R;
pointBX = transformB.position.x + (tMat.col1.x * tVec.x + tMat.col2.x * tVec.y);
pointBY = transformB.position.y + (tMat.col1.y * tVec.x + tMat.col2.y * tVec.y);
//pointA = b2Math.b2MulX(transformA, localPointA);
tVec = localPointA;
tMat = transformA.R;
pointAX = transformA.position.x + (tMat.col1.x * tVec.x + tMat.col2.x * tVec.y);
pointAY = transformA.position.y + (tMat.col1.y * tVec.x + tMat.col2.y * tVec.y);
//float32 s = b2Dot(pointA - pointB, normal);
s = (pointAX - pointBX) * normalX + (pointAY - pointBY) * normalY;
if (s < 0.0f)
{
m_axis.NegativeSelf();
}
}
else
{
// Two points on B and two points on A.
// The faces are parallel.
localPointA1 = m_proxyA.GetVertex(cache.indexA[0]);
localPointA2 = m_proxyA.GetVertex(cache.indexA[1]);
localPointB1 = m_proxyB.GetVertex(cache.indexB[0]);
localPointB2 = m_proxyB.GetVertex(cache.indexB[1]);
b2Vec2 pA = b2Math.MulX(transformA, localPointA);
b2Vec2 dA = b2Math.MulMV(transformA.R, b2Math.SubtractVV(localPointA2, localPointA1));
b2Vec2 pB = b2Math.MulX(transformB, localPointB);
b2Vec2 dB = b2Math.MulMV(transformB.R, b2Math.SubtractVV(localPointB2, localPointB1));
float a = dA.x * dA.x + dA.y * dA.y;
float e = dB.x * dB.x + dB.y * dB.y;
b2Vec2 r = b2Math.SubtractVV(dB, dA);
float c = dA.x * r.x + dA.y * r.y;
float f = dB.x * r.x + dB.y * r.y;
float b = dA.x * dB.x + dA.y * dB.y;
float denom = a * e - b * b;
s = 0.0f;
if (denom != 0.0f)
{
s = b2Math.Clamp((b * f - c * e) / denom, 0.0f, 1.0f);
}
float t = (b * s + f) / e;
if (t < 0.0f)
{
t = 0.0f;
s = b2Math.Clamp((b - c) / a, 0.0f, 1.0f);
}
//b2Vec2 localPointA = localPointA1 + s * (localPointA2 - localPointA1);
localPointA = new b2Vec2();
localPointA.x = localPointA1.x + s * (localPointA2.x - localPointA1.x);
localPointA.y = localPointA1.y + s * (localPointA2.y - localPointA1.y);
//b2Vec2 localPointB = localPointB1 + s * (localPointB2 - localPointB1);
localPointB = new b2Vec2();
localPointB.x = localPointB1.x + s * (localPointB2.x - localPointB1.x);
localPointB.y = localPointB1.y + s * (localPointB2.y - localPointB1.y);
if (s == 0.0f || s == 1.0f)
{
m_type = e_faceB;
m_axis = b2Math.CrossVF(b2Math.SubtractVV(localPointB2, localPointB1), 1.0f);
m_axis.Normalize();
m_localPoint = localPointB;
//normal = b2Math.b2MulMV(transformB.R, m_axis);
tVec = m_axis;
tMat = transformB.R;
normalX = tMat.col1.x * tVec.x + tMat.col2.x * tVec.y;
normalY = tMat.col1.y * tVec.x + tMat.col2.y * tVec.y;
//pointB = b2Math.b2MulX(transformB, m_localPoint);
tVec = m_localPoint;
tMat = transformB.R;
pointBX = transformB.position.x + (tMat.col1.x * tVec.x + tMat.col2.x * tVec.y);
pointBY = transformB.position.y + (tMat.col1.y * tVec.x + tMat.col2.y * tVec.y);
//pointA = b2Math.b2MulX(transformA, localPointA);
tVec = localPointA;
tMat = transformA.R;
pointAX = transformA.position.x + (tMat.col1.x * tVec.x + tMat.col2.x * tVec.y);
pointAY = transformA.position.y + (tMat.col1.y * tVec.x + tMat.col2.y * tVec.y);
//float32 sgn = b2Dot(pointA - pointB, normal);
sgn = (pointAX - pointBX) * normalX + (pointAY - pointBY) * normalY;
if (s < 0.0f)
{
m_axis.NegativeSelf();
}
}
else
{
m_type = e_faceA;
m_axis = b2Math.CrossVF(b2Math.SubtractVV(localPointA2, localPointA1), 1.0f);
m_localPoint = localPointA;
//normal = b2Math.b2MulMV(transformA.R, m_axis);
tVec = m_axis;
tMat = transformA.R;
normalX = tMat.col1.x * tVec.x + tMat.col2.x * tVec.y;
normalY = tMat.col1.y * tVec.x + tMat.col2.y * tVec.y;
//pointA = b2Math.b2MulX(transformA, m_localPoint);
tVec = m_localPoint;
tMat = transformA.R;
pointAX = transformA.position.x + (tMat.col1.x * tVec.x + tMat.col2.x * tVec.y);
pointAY = transformA.position.y + (tMat.col1.y * tVec.x + tMat.col2.y * tVec.y);
//pointB = b2Math.b2MulX(transformB, localPointB);
tVec = localPointB;
tMat = transformB.R;
pointBX = transformB.position.x + (tMat.col1.x * tVec.x + tMat.col2.x * tVec.y);
pointBY = transformB.position.y + (tMat.col1.y * tVec.x + tMat.col2.y * tVec.y);
//float32 sgn = b2Dot(pointB - pointA, normal);
sgn = (pointBX - pointAX) * normalX + (pointBY - pointAY) * normalY;
if (s < 0.0f)
{
m_axis.NegativeSelf();
}
}
}
}
public static void Distance(b2DistanceOutput output, b2SimplexCache cache, b2DistanceInput input)
{
++b2_gjkCalls;
b2DistanceProxy proxyA = input.proxyA;
b2DistanceProxy proxyB = input.proxyB;
b2Transform transformA = input.transformA;
b2Transform transformB = input.transformB;
// Initialize the simplex
b2Simplex simplex = s_simplex;
simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB);
// Get simplex vertices as an vector.
b2SimplexVertex[] vertices = simplex.m_vertices;
const int k_maxIters = 20;
// These store the vertices of the last simplex so that we
// can check for duplicates and preven cycling
int[] saveA = s_saveA;
int[] saveB = s_saveB;
int saveCount = 0;
b2Vec2 closestPoint = simplex.GetClosestPoint();
float distanceSqr1 = closestPoint.LengthSquared();
float distanceSqr2 = distanceSqr1;
int i;
b2Vec2 p;
// Main iteration loop
int iter = 0;
while (iter < k_maxIters)
{
// Copy the simplex so that we can identify duplicates
saveCount = simplex.m_count;
for (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:
* b2Settings.b2Assert(false);
* }*/
if (simplex.m_count == 1)
{
}
else if (simplex.m_count == 2)
{
simplex.Solve2();
}
else if (simplex.m_count == 3)
{
simplex.Solve3();
}
else
{
b2Settings.b2Assert(false);
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex.m_count == 3)
{
break;
}
// Compute the closest point.
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() < 0.0f /*Number.MinValue * Number.MinValue*/)
{
// 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 very 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 = (int)proxyA.GetSupport(b2Math.MulTMV(transformA.R, d.GetNegative()));
vertex.wA = b2Math.MulX(transformA, proxyA.GetVertex(vertex.indexA));
vertex.indexB = (int)proxyB.GetSupport(b2Math.MulTMV(transformB.R, d));
vertex.wB = b2Math.MulX(transformB, proxyB.GetVertex(vertex.indexB));
vertex.w = b2Math.SubtractVV(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 (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 exist to avoid cycling
if (duplicate)
{
break;
}
// New vertex is ok and needed.
++simplex.m_count;
}
b2_gjkMaxIters = (int)b2Math.Max(b2_gjkMaxIters, iter);
// Prepare output
simplex.GetWitnessPoints(output.pointA, output.pointB);
output.distance = b2Math.SubtractVV(output.pointA, output.pointB).Length();
output.iterations = iter;
// Cache the simplex
simplex.WriteCache(cache);
// Apply radii if requested.
if (input.useRadii)
{
float rA = proxyA.m_radius;
float rB = proxyB.m_radius;
if (output.distance > rA + rB && output.distance > float.MinValue)
{
// Shapes are still not overlapped.
// Move the witness points to the outer surface.
output.distance -= rA + rB;
b2Vec2 normal = b2Math.SubtractVV(output.pointB, output.pointA);
normal.Normalize();
output.pointA.x += rA * normal.x;
output.pointA.y += rA * normal.y;
output.pointB.x -= rB * normal.x;
output.pointB.y -= rB * normal.y;
}
else
{
// Shapes are overlapped when radii are considered.
// Move the witness points to the middle.
p = new b2Vec2();
p.x = 0.5f * (output.pointA.x + output.pointB.x);
p.y = 0.5f * (output.pointA.y + output.pointB.y);
output.pointA.x = output.pointB.x = p.x;
output.pointA.y = output.pointB.y = p.y;
output.distance = 0.0f;
}
}
}
// TODO_ERIN might not need to return the separation
public float Initialize(ref b2SimplexCache cache,
b2DistanceProxy proxyA, ref b2Sweep sweepA,
b2DistanceProxy proxyB, ref b2Sweep sweepB,
float t1)
{
m_proxyA = proxyA;
m_proxyB = proxyB;
int count = cache.count;
Debug.Assert(0 < count && count < 3);
m_sweepA = sweepA;
m_sweepB = sweepB;
b2Transform xfA, xfB;
m_sweepA.GetTransform(out xfA, t1);
m_sweepB.GetTransform(out xfB, t1);
if (count == 1)
{
m_type = SeparationType.e_points;
b2Vec2 localPointA = m_proxyA.GetVertex((int)cache.indexA[0]);
b2Vec2 localPointB = m_proxyB.GetVertex((int)cache.indexB[0]);
b2Vec2 pointA = b2Math.b2Mul(xfA, localPointA);
b2Vec2 pointB = b2Math.b2Mul(xfB, localPointB);
m_axis = pointB - pointA;
float s = m_axis.Normalize();
return(s);
}
else if (cache.indexA[0] == cache.indexA[1])
{
// Two points on B and one on A.
m_type = SeparationType.e_faceB;
b2Vec2 localPointB1 = proxyB.GetVertex((int)cache.indexB[0]);
b2Vec2 localPointB2 = proxyB.GetVertex((int)cache.indexB[1]);
float b21x = localPointB2.x - localPointB1.x;
float b21y = localPointB2.y - localPointB1.y;
m_axis.x = -b21y;
m_axis.y = b21x;
// m_axis = b2Math.b2Cross(localPointB2 - localPointB1, 1.0f);
m_axis.Normalize();
b2Vec2 normal = b2Math.b2Mul(xfB.q, m_axis);
m_localPoint = 0.5f * (localPointB1 + localPointB2);
b2Vec2 pointB = b2Math.b2Mul(xfB, m_localPoint);
b2Vec2 localPointA = proxyA.GetVertex((int)cache.indexA[0]);
b2Vec2 pointA = b2Math.b2Mul(xfA, localPointA);
b2Vec2 aminusb = pointA - pointB;
float s = b2Math.b2Dot(ref aminusb, ref normal);
if (s < 0.0f)
{
m_axis = -m_axis;
s = -s;
}
return(s);
}
else
{
// Two points on A and one or two points on B.
m_type = SeparationType.e_faceA;
b2Vec2 localPointA1 = m_proxyA.GetVertex(cache.indexA[0]);
b2Vec2 localPointA2 = m_proxyA.GetVertex(cache.indexA[1]);
b2Vec2 a2minusa1 = localPointA2 - localPointA1;
m_axis = a2minusa1.UnitCross();// b2Math.b2Cross(localPointA2 - localPointA1, 1.0f);
m_axis.Normalize();
b2Vec2 normal = b2Math.b2Mul(xfA.q, m_axis);
m_localPoint = 0.5f * (localPointA1 + localPointA2);
b2Vec2 pointA = b2Math.b2Mul(xfA, m_localPoint);
b2Vec2 localPointB = m_proxyB.GetVertex(cache.indexB[0]);
b2Vec2 pointB = b2Math.b2Mul(xfB, localPointB);
b2Vec2 bminusa = pointB - pointA;
float s = b2Math.b2Dot(ref bminusa, ref normal);
if (s < 0.0f)
{
m_axis = -m_axis;
s = -s;
}
return(s);
}
}
