internal float Evaluate(Transform TransformA, Transform TransformB)
{
switch (FaceType)
{
case Type.Points:
{
Vector2 axisA = TransformA.InverseTransformDirection(Axis);
Vector2 axisB = TransformB.InverseTransformDirection(-Axis);
Vector2 localPointA = ShapeA.GetSupportVertex(axisA);
Vector2 localPointB = ShapeB.GetSupportVertex(axisB);
Vector2 pointA = TransformA.TransformPoint(localPointA);
Vector2 pointB = TransformB.TransformPoint(localPointB);
float separation = Vector2.Dot(pointB - pointA, Axis);
return(separation);
}
case Type.FaceA:
{
Vector2 normal = TransformA.TransformDirection(Axis);
Vector2 pointA = TransformA.TransformPoint(LocalPoint);
Vector2 axisB = TransformB.InverseTransformDirection(-normal);
Vector2 localPointB = ShapeB.GetSupportVertex(axisB);
Vector2 pointB = TransformB.TransformPoint(localPointB);
float separation = Vector2.Dot(pointB - pointA, normal);
return(separation);
}
case Type.FaceB:
{
Vector2 normal = TransformB.TransformDirection(Axis);
Vector2 pointB = TransformB.TransformPoint(LocalPoint);
Vector2 axisA = TransformA.InverseTransformDirection(-normal);
Vector2 localPointA = ShapeA.GetSupportVertex(axisA);
Vector2 pointA = TransformA.TransformPoint(localPointA);
float separation = Vector2.Dot(pointA - pointB, normal);
return(separation);
}
default:
Box2DNetDebug.Assert(false);
return(0.0f);
}
}
public override bool TestPoint(Transform xf, Vector2 p)
{
Vector2 pLocal = xf.InverseTransformDirection(p - xf.position);
int vc = _vertexCount;
for (int i = 0; i < vc; ++i)
{
float dot = Vector2.Dot(_normals[i], pLocal - _vertices[i]);
if (dot > 0.0f)
{
return(false);
}
}
return(true);
}
public static void FindIncidentEdge(out ClipVertex[] c, PolygonShape poly1, Transform xf1, int edge1, PolygonShape poly2, Transform xf2)
{
int count1 = poly1._vertexCount;
Vector2[] normals1 = poly1._normals;
int count2 = poly2._vertexCount;
Vector2[] vertices2 = poly2._vertices;
Vector2[] normals2 = poly2._normals;
Box2DNetDebug.Assert(0 <= edge1 && edge1 < count1);
// Get the normal of the reference edge in poly2's frame.
Vector2 normal1 = xf2.InverseTransformDirection(xf1.TransformDirection(normals1[edge1]));
// Find the incident edge on poly2.
int index = 0;
float minDot = Settings.FLT_MAX;
for (int i = 0; i < count2; ++i)
{
float dot = Vector2.Dot(normal1, normals2[i]);
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
// Build the clip vertices for the incident edge.
int i1 = index;
int i2 = i1 + 1 < count2 ? i1 + 1 : 0;
c = new ClipVertex[2];
c[0].V = Common.Math.Mul(xf2, vertices2[i1]);
c[0].ID.Features.ReferenceEdge = (byte)edge1;
c[0].ID.Features.IncidentEdge = (byte)i1;
c[0].ID.Features.IncidentVertex = 0;
c[1].V = Common.Math.Mul(xf2, vertices2[i2]);
c[1].ID.Features.ReferenceEdge = (byte)edge1;
c[1].ID.Features.IncidentEdge = (byte)i2;
c[1].ID.Features.IncidentVertex = 1;
}
/// <summary>
/// Find the separation between poly1 and poly2 for a give edge normal on poly1.
/// </summary>
public static float EdgeSeparation(PolygonShape poly1, Transform xf1, int edge1, PolygonShape poly2, Transform xf2)
{
int count1 = poly1._vertexCount;
Vector2[] vertices1 = poly1._vertices;
Vector2[] normals1 = poly1._normals;
int count2 = poly2._vertexCount;
Vector2[] vertices2 = poly2._vertices;
//This fixed it, not sure why it was broken
if (normals1.Length - 1 <= edge1 || edge1 < 0)
{
return(0);
}
// Convert normal from poly1's frame into poly2's frame.
Vector2 normal1World = xf1.TransformDirection(normals1[edge1]);
Vector2 normal1 = xf2.InverseTransformDirection(normal1World);
// Find support vertex on poly2 for -normal.
int index = 0;
float minDot = Common.Settings.FLT_MAX;
for (int i = 0; i < count2; ++i)
{
float dot = Vector2.Dot(vertices2[i], normal1);
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
Vector2 v1 = xf1.TransformPoint(vertices1[edge1]);
Vector2 v2 = xf2.TransformPoint(vertices2[index]);
float separation = Vector2.Dot(v2 - v1, normal1World);
return(separation);
}
static void Distance(out DistanceOutput output, ref SimplexCache cache, ref DistanceInput input, Shape shapeA, Shape shapeB)
{
output = new DistanceOutput();
Transform transformA = input.TransformA;
Transform transformB = input.TransformB;
// Initialize the simplex.
Simplex simplex = new Simplex();
#if ALLOWUNSAFE
fixed(SimplexCache *sPtr = &cache)
{
simplex.ReadCache(sPtr, shapeA, transformA, shapeB, transformB);
}
#else
simplex.ReadCache(cache, shapeA, transformA, shapeB, transformB);
#endif
// Get simplex vertices as an array.
#if ALLOWUNSAFE
SimplexVertex *vertices = &simplex._v1;
#else
SimplexVertex[] vertices = new SimplexVertex[] { simplex._v1, simplex._v2, simplex._v3 };
#endif
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
#if ALLOWUNSAFE
int *lastA = stackalloc int[4], lastB = stackalloc int[4];
#else
int[] lastA = new int[4];
int[] lastB = new int[4];
#endif // ALLOWUNSAFE
int lastCount;
// Main iteration loop.
int iter = 0;
const int k_maxIterationCount = 20;
while (iter < k_maxIterationCount)
{
// Copy simplex so we can identify duplicates.
lastCount = simplex._count;
int i;
for (i = 0; i < lastCount; ++i)
{
lastA[i] = vertices[i].indexA;
lastB[i] = vertices[i].indexB;
}
switch (simplex._count)
{
case 1:
break;
case 2:
simplex.Solve2();
break;
case 3:
simplex.Solve3();
break;
default:
break;
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex._count == 3)
{
break;
}
// Compute closest point.
Vector2 p = simplex.GetClosestPoint();
float distanceSqr = p.LengthSquared();
// Ensure the search direction is numerically fit.
if (distanceSqr < Common.Settings.FLT_EPSILON_SQUARED)
{
// 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.
#if ALLOWUNSAFE
SimplexVertex *vertex = vertices + simplex._count;
vertex->indexA = shapeA.GetSupport(transformA.InverseTransformDirection(p));
vertex->wA = transformA.TransformPoint(shapeA.GetVertex(vertex->indexA));
//Vec2 wBLocal;
vertex->indexB = shapeB.GetSupport(transformB.InverseTransformDirection(-p));
vertex->wB = transformB.TransformPoint(shapeB.GetVertex(vertex->indexB));
vertex->w = vertex->wB - vertex->wA;
#else
SimplexVertex vertex = vertices[simplex._count - 1];
vertex.indexA = shapeA.GetSupport(transformA.InverseTransformDirection(p));
vertex.wA = transformA.TransformPoint(shapeA.GetVertex(vertex.indexA));
//Vec2 wBLocal;
vertex.indexB = shapeB.GetSupport(transformB.InverseTransformDirection(-p));
vertex.wB = transformB.TransformPoint(shapeB.GetVertex(vertex.indexB));
vertex.w = vertex.wB - vertex.wA;
#endif // ALLOWUNSAFE
// Iteration count is equated to the number of support point calls.
++iter;
// Check for convergence.
#if ALLOWUNSAFE
float lowerBound = Vector2.Dot(p, vertex->w);
#else
float lowerBound = Vector2.Dot(p, vertex.w);
#endif
float upperBound = distanceSqr;
const float k_relativeTolSqr = 0.01f * 0.01f; // 1:100
if (upperBound - lowerBound <= k_relativeTolSqr * upperBound)
{
// Converged!
break;
}
// Check for duplicate support points.
bool duplicate = false;
for (i = 0; i < lastCount; ++i)
{
#if ALLOWUNSAFE
if (vertex->indexA == lastA[i] && vertex->indexB == lastB[i])
#else
if (vertex.indexA == lastA[i] && vertex.indexB == lastB[i])
#endif
{
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;
}
#if ALLOWUNSAFE
fixed(DistanceOutput *doPtr = &output)
{
// Prepare output.
simplex.GetWitnessPoints(&doPtr->PointA, &doPtr->PointB);
doPtr->Distance = Vector2.Distance(doPtr->PointA, doPtr->PointB);
doPtr->Iterations = iter;
}
fixed(SimplexCache *sPtr = &cache)
{
// Cache the simplex.
simplex.WriteCache(sPtr);
}
#else
// Prepare output.
simplex.GetWitnessPoints(ref output.PointA, ref output.PointB);
output.Distance = Box2DNet.Common.Math.Distance(output.PointA, output.PointB);
output.Iterations = iter;
// Cache the simplex.
simplex.WriteCache(cache);
#endif
// Apply radii if requested.
if (input.UseRadii)
{
float rA = shapeA._radius;
float rB = shapeB._radius;
if (output.Distance > rA + rB && output.Distance > Common.Settings.FLT_EPSILON)
{
// Shapes are still no overlapped.
// Move the witness points to the outer surface.
output.Distance -= rA + rB;
Vector2 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.
Vector2 p = 0.5f * (output.PointA + output.PointB);
output.PointA = p;
output.PointB = p;
output.Distance = 0.0f;
}
}
}
public override float ComputeSubmergedArea(Vector2 normal, float offset, Transform xf, out Vector2 c)
{
//Transform plane into shape co-ordinates
Vector2 normalL = xf.InverseTransformDirection(normal);
float offsetL = offset - Vector2.Dot(normal, xf.position);
float[] depths = new float[Common.Settings.MaxPolygonVertices];
int diveCount = 0;
int intoIndex = -1;
int outoIndex = -1;
bool lastSubmerged = false;
int i;
for (i = 0; i < _vertexCount; i++)
{
depths[i] = Vector2.Dot(normalL, _vertices[i]) - offsetL;
bool isSubmerged = depths[i] < -Common.Settings.FLT_EPSILON;
if (i > 0)
{
if (isSubmerged)
{
if (!lastSubmerged)
{
intoIndex = i - 1;
diveCount++;
}
}
else
{
if (lastSubmerged)
{
outoIndex = i - 1;
diveCount++;
}
}
}
lastSubmerged = isSubmerged;
}
switch (diveCount)
{
case 0:
if (lastSubmerged)
{
//Completely submerged
MassData md;
ComputeMass(out md, 1f);
c = xf.TransformPoint(md.Center);
return(md.Mass);
}
else
{
//Completely dry
c = new Vector2();
return(0);
}
break;
case 1:
if (intoIndex == -1)
{
intoIndex = _vertexCount - 1;
}
else
{
outoIndex = _vertexCount - 1;
}
break;
}
int intoIndex2 = (intoIndex + 1) % _vertexCount;
int outoIndex2 = (outoIndex + 1) % _vertexCount;
float intoLambda = (0 - depths[intoIndex]) / (depths[intoIndex2] - depths[intoIndex]);
float outoLambda = (0 - depths[outoIndex]) / (depths[outoIndex2] - depths[outoIndex]);
Vector2 intoVec = new Vector2(_vertices[intoIndex].X * (1 - intoLambda) + _vertices[intoIndex2].X * intoLambda,
_vertices[intoIndex].Y * (1 - intoLambda) + _vertices[intoIndex2].Y * intoLambda);
Vector2 outoVec = new Vector2(_vertices[outoIndex].X * (1 - outoLambda) + _vertices[outoIndex2].X * outoLambda,
_vertices[outoIndex].Y * (1 - outoLambda) + _vertices[outoIndex2].Y * outoLambda);
//Initialize accumulator
float area = 0;
Vector2 center = Vector2.Zero;
Vector2 p2 = _vertices[intoIndex2];
Vector2 p3;
const float k_inv3 = 1.0f / 3.0f;
//An awkward loop from intoIndex2+1 to outIndex2
i = intoIndex2;
while (i != outoIndex2)
{
i = (i + 1) % _vertexCount;
if (i == outoIndex2)
{
p3 = outoVec;
}
else
{
p3 = _vertices[i];
}
//Add the triangle formed by intoVec,p2,p3
{
Vector2 e1 = p2 - intoVec;
Vector2 e2 = p3 - intoVec;
float D = e1.Cross(e2);
float triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
center += triangleArea * k_inv3 * (intoVec + p2 + p3);
}
//
p2 = p3;
}
//Normalize and Transform centroid
center *= 1.0f / area;
c = xf.TransformPoint(center);
return(area);
}
public override SegmentCollide TestSegment(Transform xf, out float lambda, out Vector2 normal, Segment segment, float maxLambda)
{
lambda = 0f;
normal = Vector2.Zero;
float lower = 0.0f, upper = maxLambda;
Vector2 p1 = xf.InverseTransformDirection(segment.P1 - xf.position);
Vector2 p2 = xf.InverseTransformDirection(segment.P2 - xf.position);
Vector2 d = p2 - p1;
int index = -1;
for (int i = 0; i < _vertexCount; ++i)
{
// p = p1 + a * d
// dot(normal, p - v) = 0
// dot(normal, p1 - v) + a * dot(normal, d) = 0
float numerator = Vector2.Dot(_normals[i], _vertices[i] - p1);
float denominator = Vector2.Dot(_normals[i], d);
if (denominator == 0.0f)
{
if (numerator < 0.0f)
{
return(SegmentCollide.MissCollide);
}
}
else
{
// Note: we want this predicate without division:
// lower < numerator / denominator, where denominator < 0
// Since denominator < 0, we have to flip the inequality:
// lower < numerator / denominator <==> denominator * lower > numerator.
if (denominator < 0.0f && numerator < lower * denominator)
{
// Increase lower.
// The segment enters this half-space.
lower = numerator / denominator;
index = i;
}
else if (denominator > 0.0f && numerator < upper * denominator)
{
// Decrease upper.
// The segment exits this half-space.
upper = numerator / denominator;
}
}
if (upper < lower)
{
return(SegmentCollide.MissCollide);
}
}
if (index >= 0)
{
lambda = lower;
normal = xf.TransformDirection(_normals[index]);
return(SegmentCollide.HitCollide);
}
lambda = 0f;
return(SegmentCollide.StartInsideCollide);
}
/// <summary>
/// Find the max separation between poly1 and poly2 using edge normals from poly1.
/// </summary>
public static float FindMaxSeparation(ref int edgeIndex, PolygonShape poly1, Transform xf1, PolygonShape poly2, Transform xf2)
{
int count1 = poly1._vertexCount;
Vector2[] normals1 = poly1._normals;
// Vector pointing from the centroid of poly1 to the centroid of poly2.
Vector2 d = xf2.TransformPoint(poly2._centroid) - xf1.TransformPoint(poly2._centroid);
Vector2 dLocal1 = xf1.InverseTransformDirection(d);
// Find edge normal on poly1 that has the largest projection onto d.
int edge = 0;
float maxDot = -Common.Settings.FLT_MAX;
for (int i = 0; i < count1; ++i)
{
float dot = Vector2.Dot(normals1[i], dLocal1);
if (dot > maxDot)
{
maxDot = dot;
edge = i;
}
}
// Get the separation for the edge normal.
float s = Collision.EdgeSeparation(poly1, xf1, edge, poly2, xf2);
// Check the separation for the previous edge normal.
int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
float sPrev = Collision.EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
// Check the separation for the next edge normal.
int nextEdge = edge + 1 < count1 ? edge + 1 : 0;
float sNext = Collision.EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2);
// Find the best edge and the search direction.
int bestEdge;
float bestSeparation;
int increment;
if (sPrev > s && sPrev > sNext)
{
increment = -1;
bestEdge = prevEdge;
bestSeparation = sPrev;
}
else if (sNext > s)
{
increment = 1;
bestEdge = nextEdge;
bestSeparation = sNext;
}
else
{
edgeIndex = edge;
return(s);
}
// Perform a local search for the best edge normal.
for (; ;)
{
if (increment == -1)
{
edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
}
else
{
edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
}
s = Collision.EdgeSeparation(poly1, xf1, edge, poly2, xf2);
if (s > bestSeparation)
{
bestEdge = edge;
bestSeparation = s;
}
else
{
break;
}
}
edgeIndex = bestEdge;
return(bestSeparation);
}
/// <summary>
/// Gets a local vector given a world vector.
/// </summary>
/// <param name="worldVector">A vector in world coordinates.</param>
/// <returns>Return the corresponding local vector.</returns>
public Vector2 GetLocalVector(Vector2 worldVector)
{
return(_xf.InverseTransformDirection(worldVector));
}
