public override bool TestPoint(Transform xf, Vec2 p)
{
Vec2 pLocal = pool1;
Vec2 temp = pool2;
pLocal.Set(p).SubLocal(xf.P);
Rot.MulTransUnsafe(xf.Q, pLocal, temp);
pLocal.Set(temp);
if (DEBUG)
{
Console.Out.WriteLine("--testPoint debug--");
Console.Out.WriteLine("Vertices: ");
for (int i = 0; i < VertexCount; ++i)
{
Console.Out.WriteLine(Vertices[i]);
}
Console.Out.WriteLine("pLocal: " + pLocal);
}
for (int i = 0; i < VertexCount; ++i)
{
temp.Set(pLocal).SubLocal(Vertices[i]);
float dot = Vec2.Dot(Normals[i], temp);
if (dot > 0.0f)
{
return(false);
}
}
return(true);
}
public float Evaluate(int indexA, int indexB, float t)
{
SweepA.GetTransform(xfa, t);
SweepB.GetTransform(xfb, t);
switch (Type)
{
case Type.Points:
{
Rot.MulTransUnsafe(xfa.Q, Axis, axisA);
Rot.MulTransUnsafe(xfb.Q, Axis.NegateLocal(), axisB);
Axis.NegateLocal();
localPointA.Set(ProxyA.GetVertex(indexA));
localPointB.Set(ProxyB.GetVertex(indexB));
Transform.MulToOutUnsafe(xfa, localPointA, pointA);
Transform.MulToOutUnsafe(xfb, localPointB, pointB);
float separation = Vec2.Dot(pointB.SubLocal(pointA), Axis);
return(separation);
}
case Type.FaceA:
{
// System.out.printf("We're faceA\n");
Rot.MulToOutUnsafe(xfa.Q, Axis, normal);
Transform.MulToOutUnsafe(xfa, LocalPoint, pointA);
Rot.MulTransUnsafe(xfb.Q, normal.NegateLocal(), axisB);
normal.NegateLocal();
localPointB.Set(ProxyB.GetVertex(indexB));
Transform.MulToOutUnsafe(xfb, localPointB, pointB);
float separation = Vec2.Dot(pointB.SubLocal(pointA), normal);
return(separation);
}
case Type.FaceB:
{
// System.out.printf("We're faceB\n");
Rot.MulToOutUnsafe(xfb.Q, Axis, normal);
Transform.MulToOutUnsafe(xfb, LocalPoint, pointB);
Rot.MulTransUnsafe(xfa.Q, normal.NegateLocal(), axisA);
normal.NegateLocal();
localPointA.Set(ProxyA.GetVertex(indexA));
Transform.MulToOutUnsafe(xfa, localPointA, pointA);
float separation = Vec2.Dot(pointA.SubLocal(pointB), normal);
return(separation);
}
default:
Debug.Assert(false);
return(0f);
}
}
public void GetLocalVectorToOutUnsafe(Vec2 worldVector, Vec2 result)
{
Rot.MulTransUnsafe(Xf.Q, worldVector, result);
}
// float FindMinSeparation(int* indexA, int* indexB, float t) const
public float FindMinSeparation(int[] indexes, float t)
{
SweepA.GetTransform(xfa, t);
SweepB.GetTransform(xfb, t);
switch (Type)
{
case Type.Points:
{
Rot.MulTransUnsafe(xfa.Q, Axis, axisA);
Rot.MulTransUnsafe(xfb.Q, Axis.NegateLocal(), axisB);
Axis.NegateLocal();
indexes[0] = ProxyA.GetSupport(axisA);
indexes[1] = ProxyB.GetSupport(axisB);
localPointA.Set(ProxyA.GetVertex(indexes[0]));
localPointB.Set(ProxyB.GetVertex(indexes[1]));
Transform.MulToOutUnsafe(xfa, localPointA, pointA);
Transform.MulToOutUnsafe(xfb, localPointB, pointB);
float separation = Vec2.Dot(pointB.SubLocal(pointA), Axis);
return(separation);
}
case Type.FaceA:
{
Rot.MulToOutUnsafe(xfa.Q, Axis, normal);
Transform.MulToOutUnsafe(xfa, LocalPoint, pointA);
Rot.MulTransUnsafe(xfb.Q, normal.NegateLocal(), axisB);
normal.NegateLocal();
indexes[0] = -1;
indexes[1] = ProxyB.GetSupport(axisB);
localPointB.Set(ProxyB.GetVertex(indexes[1]));
Transform.MulToOutUnsafe(xfb, localPointB, pointB);
float separation = Vec2.Dot(pointB.SubLocal(pointA), normal);
return(separation);
}
case Type.FaceB:
{
Rot.MulToOutUnsafe(xfb.Q, Axis, normal);
Transform.MulToOutUnsafe(xfb, LocalPoint, pointB);
Rot.MulTransUnsafe(xfa.Q, normal.NegateLocal(), axisA);
normal.NegateLocal();
indexes[1] = -1;
indexes[0] = ProxyA.GetSupport(axisA);
localPointA.Set(ProxyA.GetVertex(indexes[0]));
Transform.MulToOutUnsafe(xfa, localPointA, pointA);
float separation = Vec2.Dot(pointA.SubLocal(pointB), normal);
return(separation);
}
default:
Debug.Assert(false);
indexes[0] = -1;
indexes[1] = -1;
return(0f);
}
}
/// <summary>
/// Compute the closest points between two shapes. Supports any combination of: CircleShape and
/// PolygonShape. The simplex cache is input/output. On the first call set SimplexCache.count to
/// zero.
/// </summary>
/// <param name="output"></param>
/// <param name="cache"></param>
/// <param name="input"></param>
public void GetDistance(DistanceOutput output, SimplexCache cache, DistanceInput input)
{
GJK_CALLS++;
DistanceProxy proxyA = input.ProxyA;
DistanceProxy proxyB = input.ProxyB;
Transform transformA = input.TransformA;
Transform transformB = input.TransformB;
// Initialize the simplex.
simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB);
// Get simplex vertices as an array.
SimplexVertex[] vertices = simplex.Vertices;
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
// (pooled above)
simplex.GetClosestPoint(closestPoint);
float distanceSqr1 = closestPoint.LengthSquared();
// Main iteration loop
int iter = 0;
while (iter < GJK_MAX_ITERS)
{
// Copy simplex so we can identify duplicates.
int saveCount = simplex.Count;
for (int i = 0; i < saveCount; i++)
{
saveA[i] = vertices[i].IndexA;
saveB[i] = vertices[i].IndexB;
}
switch (simplex.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.Count == 3)
{
break;
}
// Compute closest point.
simplex.GetClosestPoint(closestPoint);
float distanceSqr2 = closestPoint.LengthSquared();
// ensure progress
if (distanceSqr2 >= distanceSqr1)
{
// break;
}
distanceSqr1 = distanceSqr2;
// get search direction;
simplex.GetSearchDirection(d);
// Ensure the search direction is numerically fit.
if (d.LengthSquared() < Settings.EPSILON * Settings.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;
}
/*
* SimplexVertex* vertex = vertices + simplex.m_count; vertex.indexA =
* proxyA.GetSupport(MulT(transformA.R, -d)); vertex.wA = Mul(transformA,
* proxyA.GetVertex(vertex.indexA)); Vec2 wBLocal; vertex.indexB =
* proxyB.GetSupport(MulT(transformB.R, d)); vertex.wB = Mul(transformB,
* proxyB.GetVertex(vertex.indexB)); vertex.w = vertex.wB - vertex.wA;
*/
// Compute a tentative new simplex vertex using support points.
SimplexVertex vertex = vertices[simplex.Count];
Rot.MulTransUnsafe(transformA.Q, d.NegateLocal(), temp);
vertex.IndexA = proxyA.GetSupport(temp);
Transform.MulToOutUnsafe(transformA, proxyA.GetVertex(vertex.IndexA), vertex.WA);
// Vec2 wBLocal;
Rot.MulTransUnsafe(transformB.Q, d.NegateLocal(), temp);
vertex.IndexB = proxyB.GetSupport(temp);
Transform.MulToOutUnsafe(transformB, proxyB.GetVertex(vertex.IndexB), vertex.WB);
vertex.W.Set(vertex.WB).SubLocal(vertex.WA);
// Iteration count is equated to the number of support point calls.
++iter;
++GJK_ITERS;
// 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.Count;
}
GJK_MAX_ITERS = MathUtils.Max(GJK_MAX_ITERS, iter);
// Prepare output.
simplex.GetWitnessPoints(output.PointA, output.PointB);
output.Distance = MathUtils.Distance(output.PointA, output.PointB);
output.Iterations = iter;
// Cache the simplex.
simplex.WriteCache(cache);
// Apply radii if requested.
if (input.UseRadii)
{
float rA = proxyA.Radius;
float rB = proxyB.Radius;
if (output.Distance > rA + rB && output.Distance > Settings.EPSILON)
{
// Shapes are still no overlapped.
// Move the witness points to the outer surface.
output.Distance -= (rA + rB);
normal.Set(output.PointB).SubLocal(output.PointA);
normal.Normalize();
temp.Set(normal).MulLocal(rA);
output.PointA.AddLocal(temp);
temp.Set(normal).MulLocal(rB);
output.PointB.SubLocal(temp);
}
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.AddLocal(output.PointB).MulLocal(.5f);
output.PointB.Set(output.PointA);
output.Distance = 0.0f;
}
}
}
