public void ReadCache(SimplexCache cache,
DistanceProxy proxyA, Transform transformA,
DistanceProxy proxyB, Transform transformB)
{
Utilities.Assert(cache.count <= 3);
// Copy data from cache.
m_count = cache.count;
SimplexVertex[] vertices = this.verticies;
for (int i = 0; i < m_count; ++i)
{
SimplexVertex v = vertices[i];
v.indexA = cache.indexA[i];
v.indexB = cache.indexB[i];
Vec2 wALocal = proxyA.GetVertex(v.indexA);
Vec2 wBLocal = proxyB.GetVertex(v.indexB);
v.wA = Utilities.Mul(transformA, wALocal);
v.wB = Utilities.Mul(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)
{
float metric1 = cache.metric;
float metric2 = GetMetric();
if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Single.Epsilon)
{
// Reset the simplex.
m_count = 0;
}
}
// If the cache is empty or invalid ...
if (m_count == 0)
{
SimplexVertex v = vertices[0];
v.indexA = 0;
v.indexB = 0;
Vec2 wALocal = proxyA.GetVertex(0);
Vec2 wBLocal = proxyB.GetVertex(0);
v.wA = Utilities.Mul(transformA, wALocal);
v.wB = Utilities.Mul(transformB, wBLocal);
v.w = v.wB - v.wA;
v.a = 1.0f;
m_count = 1;
}
}
public void ReadCache(SimplexCache cache,
DistanceProxy proxyA, Transform transformA,
DistanceProxy proxyB, Transform transformB)
{
Utilities.Assert(cache.count <= 3);
// Copy data from cache.
m_count = cache.count;
SimplexVertex[] vertices = this.verticies;
for (int i = 0; i < m_count; ++i)
{
SimplexVertex v = vertices[i];
v.indexA = cache.indexA[i];
v.indexB = cache.indexB[i];
Vec2 wALocal = proxyA.GetVertex(v.indexA);
Vec2 wBLocal = proxyB.GetVertex(v.indexB);
v.wA = Utilities.Mul(transformA, wALocal);
v.wB = Utilities.Mul(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)
{
float metric1 = cache.metric;
float metric2 = GetMetric();
if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Single.Epsilon)
{
// Reset the simplex.
m_count = 0;
}
}
// If the cache is empty or invalid ...
if (m_count == 0)
{
SimplexVertex v = vertices[0];
v.indexA = 0;
v.indexB = 0;
Vec2 wALocal = proxyA.GetVertex(0);
Vec2 wBLocal = proxyB.GetVertex(0);
v.wA = Utilities.Mul(transformA, wALocal);
v.wB = Utilities.Mul(transformB, wBLocal);
v.w = v.wB - v.wA;
v.a = 1.0f;
m_count = 1;
}
}
internal void ReadCache(ref SimplexCache cache,
ref DistanceProxy proxyA, ref Transform transformA,
ref DistanceProxy proxyB, ref Transform transformB)
{
//Debug.Assert(cache.count <= 3);
// Copy data from cache.
_count = cache.count;
for (int i = 0; i < _count; ++i)
{
SimplexVertex v = _v[i];
v.indexA = cache.indexA[i];
v.indexB = cache.indexB[i];
Vector2 wALocal = proxyA.GetVertex(v.indexA);
Vector2 wBLocal = proxyB.GetVertex(v.indexB);
v.wA = MathUtils.Multiply(ref transformA, wALocal);
v.wB = MathUtils.Multiply(ref transformB, wBLocal);
v.w = v.wB - v.wA;
v.a = 0.0f;
_v[i] = v;
}
// Compute the new simplex metric, if it is substantially different than
// old metric then flush the simplex.
if (_count > 1)
{
float metric1 = cache.metric;
float metric2 = GetMetric();
if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Settings.b2_epsilon)
{
// Reset the simplex.
_count = 0;
}
}
// If the cache is empty or invalid ...
if (_count == 0)
{
SimplexVertex v = _v[0];
v.indexA = 0;
v.indexB = 0;
Vector2 wALocal = proxyA.GetVertex(0);
Vector2 wBLocal = proxyB.GetVertex(0);
v.wA = MathUtils.Multiply(ref transformA, wALocal);
v.wB = MathUtils.Multiply(ref transformB, wBLocal);
v.w = v.wB - v.wA;
_v[0] = v;
_count = 1;
}
}
// TODO_ERIN might not need to return the separation
float Initialize(SimplexCache cache,
DistanceProxy proxyA, Sweep sweepA,
DistanceProxy proxyB, Sweep sweepB,
float t1)
{
m_proxyA = proxyA;
m_proxyB = proxyB;
int count = cache.count;
Utilities.Assert(0 < count && count < 3);
m_sweepA = sweepA;
m_sweepB = sweepB;
Transform xfA, xfB;
m_sweepA.GetTransform(out xfA, t1);
m_sweepB.GetTransform(out xfB, t1);
if (count == 1)
{
m_type = SeparationType.e_points;
Vec2 localPointA = m_proxyA.GetVertex(cache.indexA[0]);
Vec2 localPointB = m_proxyB.GetVertex(cache.indexB[0]);
Vec2 pointA = Utilities.Mul(xfA, localPointA);
Vec2 pointB = Utilities.Mul(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;
Vec2 localPointB1 = proxyB.GetVertex(cache.indexB[0]);
Vec2 localPointB2 = proxyB.GetVertex(cache.indexB[1]);
m_axis = Utilities.Cross(localPointB2 - localPointB1, 1.0f);
m_axis.Normalize();
Vec2 normal = Utilities.Mul(xfB.q, m_axis);
m_localPoint = 0.5f * (localPointB1 + localPointB2);
Vec2 pointB = Utilities.Mul(xfB, m_localPoint);
Vec2 localPointA = proxyA.GetVertex(cache.indexA[0]);
Vec2 pointA = Utilities.Mul(xfA, localPointA);
float s = Utilities.Dot(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;
Vec2 localPointA1 = m_proxyA.GetVertex(cache.indexA[0]);
Vec2 localPointA2 = m_proxyA.GetVertex(cache.indexA[1]);
m_axis = Utilities.Cross(localPointA2 - localPointA1, 1.0f);
m_axis.Normalize();
Vec2 normal = Utilities.Mul(xfA.q, m_axis);
m_localPoint = 0.5f * (localPointA1 + localPointA2);
Vec2 pointA = Utilities.Mul(xfA, m_localPoint);
Vec2 localPointB = m_proxyB.GetVertex(cache.indexB[0]);
Vec2 pointB = Utilities.Mul(xfB, localPointB);
float s = Utilities.Dot(pointB - pointA, normal);
if (s < 0.0f)
{
m_axis = -m_axis;
s = -s;
}
return(s);
}
}
public SeparationFunction(ref SimplexCache cache,
ref DistanceProxy proxyA, ref Sweep sweepA,
ref DistanceProxy proxyB, ref Sweep sweepB,
float t1)
{
_localPoint = Vector2.zero;
_proxyA = proxyA;
_proxyB = proxyB;
int count = cache.count;
//Debug.Assert(0 < count && count < 3);
_sweepA = sweepA;
_sweepB = sweepB;
Transform xfA, xfB;
_sweepA.GetTransform(out xfA, t1);
_sweepB.GetTransform(out xfB, t1);
if (count == 1)
{
_type = SeparationFunctionType.Points;
Vector2 localPointA = _proxyA.GetVertex(cache.indexA[0]);
Vector2 localPointB = _proxyB.GetVertex(cache.indexB[0]);
Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA);
Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB);
_axis = pointB - pointA;
_axis.Normalize();
return;
}
else if (cache.indexA[0] == cache.indexA[1])
{
// Two points on B and one on A.
_type = SeparationFunctionType.FaceB;
Vector2 localPointB1 = proxyB.GetVertex(cache.indexB[0]);
Vector2 localPointB2 = proxyB.GetVertex(cache.indexB[1]);
_axis = MathUtils.Cross(localPointB2 - localPointB1, 1.0f);
_axis.Normalize();
Vector2 normal = MathUtils.Multiply(ref xfB.R, _axis);
_localPoint = 0.5f * (localPointB1 + localPointB2);
Vector2 pointB = MathUtils.Multiply(ref xfB, _localPoint);
Vector2 localPointA = proxyA.GetVertex(cache.indexA[0]);
Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA);
float s = Vector2.Dot(pointA - pointB, normal);
if (s < 0.0f)
{
_axis = -_axis;
s = -s;
}
return;
}
else
{
// Two points on A and one or two points on B.
_type = SeparationFunctionType.FaceA;
Vector2 localPointA1 = _proxyA.GetVertex(cache.indexA[0]);
Vector2 localPointA2 = _proxyA.GetVertex(cache.indexA[1]);
_axis = MathUtils.Cross(localPointA2 - localPointA1, 1.0f);
_axis.Normalize();
Vector2 normal = MathUtils.Multiply(ref xfA.R, _axis);
_localPoint = 0.5f * (localPointA1 + localPointA2);
Vector2 pointA = MathUtils.Multiply(ref xfA, _localPoint);
Vector2 localPointB = _proxyB.GetVertex(cache.indexB[0]);
Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB);
float s = Vector2.Dot(pointB - pointA, normal);
if (s < 0.0f)
{
_axis = -_axis;
s = -s;
}
return;
}
}
/// Compute the closest points between two shapes. Supports any combination of:
/// CircleShape, PolygonShape, EdgeShape. The simplex cache is input/output.
/// On the first call set SimplexCache.count to zero.
public static void Distance(out DistanceOutput output,
SimplexCache cache,
DistanceInput input)
{
++_gjkCalls;
DistanceProxy proxyA = input.proxyA;
DistanceProxy proxyB = input.proxyB;
Transform transformA = input.transformA;
Transform transformB = input.transformB;
// Initialize the simplex.
Simplex simplex = new Simplex();
simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB);
// Get simplex vertices as an array.
SimplexVertex[] vertices = simplex.verticies;
const 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;
float distanceSqr1 = Single.MaxValue;
float distanceSqr2 = distanceSqr1;
// 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:
Utilities.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.
Vec2 p = simplex.GetClosestPoint();
distanceSqr2 = p.LengthSquared();
// Ensure progress
if (distanceSqr2 >= distanceSqr1)
{
//break;
}
distanceSqr1 = distanceSqr2;
// Get search direction.
Vec2 d = simplex.GetSearchDirection();
// Ensure the search direction is numerically fit.
if (d.LengthSquared() < Single.Epsilon * Single.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.
SimplexVertex vertex = vertices[simplex.m_count];
vertex.indexA = proxyA.GetSupport(Utilities.MulT(transformA.q, -d));
vertex.wA = Utilities.Mul(transformA, proxyA.GetVertex(vertex.indexA));
Vec2 wBLocal;
vertex.indexB = proxyB.GetSupport(Utilities.MulT(transformB.q, d));
vertex.wB = Utilities.Mul(transformB, proxyB.GetVertex(vertex.indexB));
vertex.w = vertex.wB - vertex.wA;
// Iteration count is equated to the number of support point calls.
++iter;
++_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;
}
_gjkMaxIters = Math.Max(_gjkMaxIters, iter);
// Prepare output.
simplex.GetWitnessPoints(out output.pointA, out output.pointB);
output.distance = Utilities.Distance(output.pointA, output.pointB);
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 > Single.Epsilon)
{
// Shapes are still no overlapped.
// Move the witness points to the outer surface.
output.distance -= rA + rB;
Vec2 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.
Vec2 p = 0.5f * (output.pointA + output.pointB);
output.pointA = p;
output.pointB = p;
output.distance = 0.0f;
}
}
}
/// Compute the upper bound on time before two shapes penetrate. Time is represented as
/// a fraction between [0,tMax]. This uses a swept separating axis and may miss some intermediate,
/// non-tunneling collision. If you change the time interval, you should call this function
/// again.
/// Note: use Distance to compute the contact point and normal at the time of impact.
// CCD via the local separating axis method. This seeks progression
// by computing the largest time at which separation is maintained.
public static void TimeOfImpact(out TOIOutput output, TOIInput input)
{
Timer timer = new Timer();
++_toiCalls;
output.state = TOIOutput.State.e_unknown;
output.t = input.tMax;
DistanceProxy proxyA = input.proxyA;
DistanceProxy proxyB = input.proxyB;
Sweep sweepA = input.sweepA;
Sweep 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.m_radius + proxyB.m_radius;
float target = Math.Max(Settings._linearSlop, totalRadius - 3.0f * Settings._linearSlop);
float tolerance = 0.25f * Settings._linearSlop;
Utilities.Assert(target > tolerance);
float t1 = 0.0f;
const int k_maxIterations = 20; // TODO_ERIN Settings
int iter = 0;
// Prepare input for distance query.
SimplexCache cache = new SimplexCache();
cache.count = 0;
DistanceInput 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).
for (;;)
{
Transform xfA, xfB;
sweepA.GetTransform(out xfA, t1);
sweepB.GetTransform(out xfB, t1);
// Get the distance between shapes. We can also use the results
// to get a separating axis.
distanceInput.transformA = xfA;
distanceInput.transformB = xfB;
DistanceOutput distanceOutput;
Utilities.Distance(out distanceOutput, cache, distanceInput);
// If the shapes are overlapped, we give up on continuous collision.
if (distanceOutput.distance <= 0.0f)
{
// Failure!
output.state = TOIOutput.State.e_overlapped;
output.t = 0.0f;
break;
}
if (distanceOutput.distance < target + tolerance)
{
// Victory!
output.state = TOIOutput.State.e_touching;
output.t = t1;
break;
}
// Initialize the separating axis.
throw new NotImplementedException();
// SeparationFunction fcn;
// fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1);
//#if ZERO
// // Dump the curve seen by the root finder
// {
// const int N = 100;
// float dx = 1.0f / N;
// float xs[N+1];
// float fs[N+1];
// float x = 0.0f;
// for (int i = 0; i <= N; ++i)
// {
// sweepA.GetTransform(out xfA, x);
// sweepB.GetTransform(out xfB, x);
// float f = fcn.Evaluate(xfA, xfB) - target;
// printf("%g %g\n", x, f);
// xs[i] = x;
// fs[i] = f;
// x += dx;
// }
// }
//#endif
// // Compute the TOI on the separating axis. We do this by successively
// // resolving the deepest point. This loop is bounded by the number of vertices.
// bool done = false;
// float t2 = tMax;
// int pushBackIter = 0;
// for (;;)
// {
// // Find the deepest point at t2. Store the witness point indices.
// int indexA, indexB;
// float s2 = fcn.FindMinSeparation(&indexA, &indexB, t2);
// // Is the final configuration separated?
// if (s2 > target + tolerance)
// {
// // Victory!
// output.state = TOIOutput.State.e_separated;
// output.t = tMax;
// done = true;
// break;
// }
// // Has the separation reached tolerance?
// if (s2 > target - tolerance)
// {
// // Advance the sweeps
// t1 = t2;
// break;
// }
// // Compute the initial separation of the witness points.
// float s1 = fcn.Evaluate(indexA, indexB, t1);
// // Check for initial overlap. This might happen if the root finder
// // runs out of iterations.
// if (s1 < target - tolerance)
// {
// output.state = TOIOutput.State.e_failed;
// output.t = t1;
// done = true;
// break;
// }
// // Check for touching
// if (s1 <= target + tolerance)
// {
// // Victory! t1 should hold the TOI (could be 0.0).
// output.state = TOIOutput.State.e_touching;
// output.t = t1;
// done = true;
// break;
// }
// // Compute 1D root of: f(x) - target = 0
// int rootIterCount = 0;
// float a1 = t1, a2 = t2;
// for (;;)
// {
// // Use a mix of the secant rule and bisection.
// float t;
// if (rootIterCount & 1)
// {
// // Secant rule to improve convergence.
// t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
// }
// else
// {
// // Bisection to guarantee progress.
// t = 0.5f * (a1 + a2);
// }
// ++rootIterCount;
// ++_toiRootIters;
// float s = fcn.Evaluate(indexA, indexB, t);
// if (Math.Abs(s - target) < tolerance)
// {
// // t2 holds a tentative value for t1
// t2 = t;
// break;
// }
// // Ensure we continue to bracket the root.
// if (s > target)
// {
// a1 = t;
// s1 = s;
// }
// else
// {
// a2 = t;
// s2 = s;
// }
// if (rootIterCount == 50)
// {
// break;
// }
// }
// _toiMaxRootIters = Math.Max(_toiMaxRootIters, rootIterCount);
// ++pushBackIter;
// if (pushBackIter == Settings._maxPolygonVertices)
// {
// break;
// }
// }
// ++iter;
// ++_toiIters;
// if (done)
// {
// break;
// }
// if (iter == k_maxIterations)
// {
// // Root finder got stuck. Semi-victory.
// output.state = TOIOutput.State.e_failed;
// output.t = t1;
// break;
// }
}
_toiMaxIters = Math.Max(_toiMaxIters, iter);
float time = timer.GetMilliseconds();
_toiMaxTime = Math.Max(_toiMaxTime, time);
_toiTime += time;
}
// TODO_ERIN might not need to return the separation
float Initialize(SimplexCache cache,
DistanceProxy proxyA, Sweep sweepA,
DistanceProxy proxyB, Sweep sweepB,
float t1)
{
m_proxyA = proxyA;
m_proxyB = proxyB;
int count = cache.count;
Utilities.Assert(0 < count && count < 3);
m_sweepA = sweepA;
m_sweepB = sweepB;
Transform xfA, xfB;
m_sweepA.GetTransform(out xfA, t1);
m_sweepB.GetTransform(out xfB, t1);
if (count == 1)
{
m_type = SeparationType.e_points;
Vec2 localPointA = m_proxyA.GetVertex(cache.indexA[0]);
Vec2 localPointB = m_proxyB.GetVertex(cache.indexB[0]);
Vec2 pointA = Utilities.Mul(xfA, localPointA);
Vec2 pointB = Utilities.Mul(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;
Vec2 localPointB1 = proxyB.GetVertex(cache.indexB[0]);
Vec2 localPointB2 = proxyB.GetVertex(cache.indexB[1]);
m_axis = Utilities.Cross(localPointB2 - localPointB1, 1.0f);
m_axis.Normalize();
Vec2 normal = Utilities.Mul(xfB.q, m_axis);
m_localPoint = 0.5f * (localPointB1 + localPointB2);
Vec2 pointB = Utilities.Mul(xfB, m_localPoint);
Vec2 localPointA = proxyA.GetVertex(cache.indexA[0]);
Vec2 pointA = Utilities.Mul(xfA, localPointA);
float s = Utilities.Dot(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;
Vec2 localPointA1 = m_proxyA.GetVertex(cache.indexA[0]);
Vec2 localPointA2 = m_proxyA.GetVertex(cache.indexA[1]);
m_axis = Utilities.Cross(localPointA2 - localPointA1, 1.0f);
m_axis.Normalize();
Vec2 normal = Utilities.Mul(xfA.q, m_axis);
m_localPoint = 0.5f * (localPointA1 + localPointA2);
Vec2 pointA = Utilities.Mul(xfA, m_localPoint);
Vec2 localPointB = m_proxyB.GetVertex(cache.indexB[0]);
Vec2 pointB = Utilities.Mul(xfB, localPointB);
float s = Utilities.Dot(pointB - pointA, normal);
if (s < 0.0f)
{
m_axis = -m_axis;
s = -s;
}
return s;
}
}