public static void Initialize(ContactPositionConstraint pc, Transform xfA, Transform xfB, int index,
out Vector2 normal, out Vector2 point, out float separation)
{
Debug.Assert(pc.PointCount > 0);
switch (pc.Type)
{
case ManifoldType.Circles:
{
var pointA = MathUtils.Mul(ref xfA, pc.LocalPoint);
var pointB = MathUtils.Mul(ref xfB, pc.LocalPoints[0]);
normal = pointB - pointA;
//Velcro: Fix to handle zero normalization
if (normal != Vector2.zero)
{
normal.Normalize();
}
point = 0.5f * (pointA + pointB);
separation = Vector2.Dot(pointB - pointA, normal) - pc.RadiusA - pc.RadiusB;
}
break;
case ManifoldType.FaceA:
{
normal = MathUtils.Mul(xfA.q, pc.LocalNormal);
var planePoint = MathUtils.Mul(ref xfA, pc.LocalPoint);
var clipPoint = MathUtils.Mul(ref xfB, pc.LocalPoints[index]);
separation = Vector2.Dot(clipPoint - planePoint, normal) - pc.RadiusA - pc.RadiusB;
point = clipPoint;
}
break;
case ManifoldType.FaceB:
{
normal = MathUtils.Mul(xfB.q, pc.LocalNormal);
var planePoint = MathUtils.Mul(ref xfB, pc.LocalPoint);
var clipPoint = MathUtils.Mul(ref xfA, pc.LocalPoints[index]);
separation = Vector2.Dot(clipPoint - planePoint, normal) - pc.RadiusA - pc.RadiusB;
point = clipPoint;
// Ensure normal points from A to B
normal = -normal;
}
break;
default:
normal = Vector2.zero;
point = Vector2.zero;
separation = 0;
break;
}
}
/// <summary>
/// Get the interpolated transform at a specific time.
/// </summary>
/// <param name="xfb">The transform.</param>
/// <param name="beta">beta is a factor in [0,1], where 0 indicates alpha0.</param>
public void GetTransform(out Transform xfb, float beta)
{
xfb = new Transform();
xfb.p.x = (1.0f - beta) * C0.x + beta * C.x;
xfb.p.y = (1.0f - beta) * C0.y + beta * C.y;
var angle = (1.0f - beta) * A0 + beta * A;
xfb.q.Set(angle);
// Shift to origin
xfb.p -= MathUtils.Mul(xfb.q, LocalCenter);
}
/// <summary>
/// Test overlap between the two shapes.
/// </summary>
/// <param name="shapeA">The first shape.</param>
/// <param name="indexA">The index for the first shape.</param>
/// <param name="shapeB">The second shape.</param>
/// <param name="indexB">The index for the second shape.</param>
/// <param name="xfA">The transform for the first shape.</param>
/// <param name="xfB">The transform for the seconds shape.</param>
/// <returns></returns>
public static bool TestOverlap(Shape shapeA, int indexA, Shape shapeB, int indexB, ref Transform xfA,
ref Transform xfB)
{
var input = new DistanceInput();
input.ProxyA = new DistanceProxy(shapeA, indexA);
input.ProxyB = new DistanceProxy(shapeB, indexB);
input.TransformA = xfA;
input.TransformB = xfB;
input.UseRadii = true;
SimplexCache cache;
DistanceOutput output;
DistanceGJK.ComputeDistance(ref input, out output, out cache);
return(output.Distance < 10.0f * Settings.Epsilon);
}
public static void Collide(ref Manifold manifold, EdgeShape edgeA, ref Transform xfA, PolygonShape polygonB,
ref Transform xfB)
{
// Algorithm:
// 1. Classify v1 and v2
// 2. Classify polygon centroid as front or back
// 3. Flip normal if necessary
// 4. Initialize normal range to [-pi, pi] about face normal
// 5. Adjust normal range according to adjacent edges
// 6. Visit each separating axes, only accept axes within the range
// 7. Return if _any_ axis indicates separation
// 8. Clip
bool front;
Vector2 lowerLimit, upperLimit;
Vector2 normal;
var normal0 = Vector2.zero;
var normal2 = Vector2.zero;
var xf = MathUtils.MulT(xfA, xfB);
var centroidB = MathUtils.Mul(ref xf, polygonB.MassData.Centroid);
var v0 = edgeA.Vertex0;
var v1 = edgeA._vertex1;
var v2 = edgeA._vertex2;
var v3 = edgeA.Vertex3;
var hasVertex0 = edgeA.HasVertex0;
var hasVertex3 = edgeA.HasVertex3;
var edge1 = v2 - v1;
edge1.Normalize();
var normal1 = new Vector2(edge1.y, -edge1.x);
var offset1 = Vector2.Dot(normal1, centroidB - v1);
float offset0 = 0.0f, offset2 = 0.0f;
bool convex1 = false, convex2 = false;
// Is there a preceding edge?
if (hasVertex0)
{
var edge0 = v1 - v0;
edge0.Normalize();
normal0 = new Vector2(edge0.y, -edge0.x);
convex1 = MathUtils.Cross(edge0, edge1) >= 0.0f;
offset0 = Vector2.Dot(normal0, centroidB - v0);
}
// Is there a following edge?
if (hasVertex3)
{
var edge2 = v3 - v2;
edge2.Normalize();
normal2 = new Vector2(edge2.y, -edge2.x);
convex2 = MathUtils.Cross(edge1, edge2) > 0.0f;
offset2 = Vector2.Dot(normal2, centroidB - v2);
}
// Determine front or back collision. Determine collision normal limits.
if (hasVertex0 && hasVertex3)
{
if (convex1 && convex2)
{
front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;
if (front)
{
normal = normal1;
lowerLimit = normal0;
upperLimit = normal2;
}
else
{
normal = -normal1;
lowerLimit = -normal1;
upperLimit = -normal1;
}
}
else if (convex1)
{
front = offset0 >= 0.0f || offset1 >= 0.0f && offset2 >= 0.0f;
if (front)
{
normal = normal1;
lowerLimit = normal0;
upperLimit = normal1;
}
else
{
normal = -normal1;
lowerLimit = -normal2;
upperLimit = -normal1;
}
}
else if (convex2)
{
front = offset2 >= 0.0f || offset0 >= 0.0f && offset1 >= 0.0f;
if (front)
{
normal = normal1;
lowerLimit = normal1;
upperLimit = normal2;
}
else
{
normal = -normal1;
lowerLimit = -normal1;
upperLimit = -normal0;
}
}
else
{
front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
if (front)
{
normal = normal1;
lowerLimit = normal1;
upperLimit = normal1;
}
else
{
normal = -normal1;
lowerLimit = -normal2;
upperLimit = -normal0;
}
}
}
else if (hasVertex0)
{
if (convex1)
{
front = offset0 >= 0.0f || offset1 >= 0.0f;
if (front)
{
normal = normal1;
lowerLimit = normal0;
upperLimit = -normal1;
}
else
{
normal = -normal1;
lowerLimit = normal1;
upperLimit = -normal1;
}
}
else
{
front = offset0 >= 0.0f && offset1 >= 0.0f;
if (front)
{
normal = normal1;
lowerLimit = normal1;
upperLimit = -normal1;
}
else
{
normal = -normal1;
lowerLimit = normal1;
upperLimit = -normal0;
}
}
}
else if (hasVertex3)
{
if (convex2)
{
front = offset1 >= 0.0f || offset2 >= 0.0f;
if (front)
{
normal = normal1;
lowerLimit = -normal1;
upperLimit = normal2;
}
else
{
normal = -normal1;
lowerLimit = -normal1;
upperLimit = normal1;
}
}
else
{
front = offset1 >= 0.0f && offset2 >= 0.0f;
if (front)
{
normal = normal1;
lowerLimit = -normal1;
upperLimit = normal1;
}
else
{
normal = -normal1;
lowerLimit = -normal2;
upperLimit = normal1;
}
}
}
else
{
front = offset1 >= 0.0f;
if (front)
{
normal = normal1;
lowerLimit = -normal1;
upperLimit = -normal1;
}
else
{
normal = -normal1;
lowerLimit = normal1;
upperLimit = normal1;
}
}
// Get polygonB in frameA
var normals = new Vector2[Settings.MaxPolygonVertices];
var vertices = new Vector2[Settings.MaxPolygonVertices];
var count = polygonB.Vertices.Count;
for (var i = 0; i < polygonB.Vertices.Count; ++i)
{
vertices[i] = MathUtils.Mul(ref xf, polygonB.Vertices[i]);
normals[i] = MathUtils.Mul(xf.q, polygonB.Normals[i]);
}
var radius = polygonB.Radius + edgeA.Radius;
manifold.PointCount = 0;
//Velcro: ComputeEdgeSeparation() was manually inlined here
EPAxis edgeAxis;
edgeAxis.Type = EPAxisType.EdgeA;
edgeAxis.Index = front ? 0 : 1;
edgeAxis.Separation = Settings.MaxFloat;
for (var i = 0; i < count; ++i)
{
var s = Vector2.Dot(normal, vertices[i] - v1);
if (s < edgeAxis.Separation)
{
edgeAxis.Separation = s;
}
}
// If no valid normal can be found than this edge should not collide.
if (edgeAxis.Type == EPAxisType.Unknown)
{
return;
}
if (edgeAxis.Separation > radius)
{
return;
}
//Velcro: ComputePolygonSeparation() was manually inlined here
EPAxis polygonAxis;
polygonAxis.Type = EPAxisType.Unknown;
polygonAxis.Index = -1;
polygonAxis.Separation = -Settings.MaxFloat;
var perp = new Vector2(-normal.y, normal.x);
for (var i = 0; i < count; ++i)
{
var n = -normals[i];
var s1 = Vector2.Dot(n, vertices[i] - v1);
var s2 = Vector2.Dot(n, vertices[i] - v2);
var s = Mathf.Min(s1, s2);
if (s > radius)
{
// No collision
polygonAxis.Type = EPAxisType.EdgeB;
polygonAxis.Index = i;
polygonAxis.Separation = s;
break;
}
// Adjacency
if (Vector2.Dot(n, perp) >= 0.0f)
{
if (Vector2.Dot(n - upperLimit, normal) < -Settings.AngularSlop)
{
continue;
}
}
else
{
if (Vector2.Dot(n - lowerLimit, normal) < -Settings.AngularSlop)
{
continue;
}
}
if (s > polygonAxis.Separation)
{
polygonAxis.Type = EPAxisType.EdgeB;
polygonAxis.Index = i;
polygonAxis.Separation = s;
}
}
if (polygonAxis.Type != EPAxisType.Unknown && polygonAxis.Separation > radius)
{
return;
}
// Use hysteresis for jitter reduction.
const float k_relativeTol = 0.98f;
const float k_absoluteTol = 0.001f;
EPAxis primaryAxis;
if (polygonAxis.Type == EPAxisType.Unknown)
{
primaryAxis = edgeAxis;
}
else if (polygonAxis.Separation > k_relativeTol * edgeAxis.Separation + k_absoluteTol)
{
primaryAxis = polygonAxis;
}
else
{
primaryAxis = edgeAxis;
}
var ie = new FixedArray2 <ClipVertex>();
ReferenceFace rf;
if (primaryAxis.Type == EPAxisType.EdgeA)
{
manifold.Type = ManifoldType.FaceA;
// Search for the polygon normal that is most anti-parallel to the edge normal.
var bestIndex = 0;
var bestValue = Vector2.Dot(normal, normals[0]);
for (var i = 1; i < count; ++i)
{
var value = Vector2.Dot(normal, normals[i]);
if (value < bestValue)
{
bestValue = value;
bestIndex = i;
}
}
var i1 = bestIndex;
var i2 = i1 + 1 < count ? i1 + 1 : 0;
ie.Value0.V = vertices[i1];
ie.Value0.ID.ContactFeature.IndexA = 0;
ie.Value0.ID.ContactFeature.IndexB = (byte)i1;
ie.Value0.ID.ContactFeature.TypeA = ContactFeatureType.Face;
ie.Value0.ID.ContactFeature.TypeB = ContactFeatureType.Vertex;
ie.Value1.V = vertices[i2];
ie.Value1.ID.ContactFeature.IndexA = 0;
ie.Value1.ID.ContactFeature.IndexB = (byte)i2;
ie.Value1.ID.ContactFeature.TypeA = ContactFeatureType.Face;
ie.Value1.ID.ContactFeature.TypeB = ContactFeatureType.Vertex;
if (front)
{
rf.i1 = 0;
rf.i2 = 1;
rf.v1 = v1;
rf.v2 = v2;
rf.Normal = normal1;
}
else
{
rf.i1 = 1;
rf.i2 = 0;
rf.v1 = v2;
rf.v2 = v1;
rf.Normal = -normal1;
}
}
else
{
manifold.Type = ManifoldType.FaceB;
ie.Value0.V = v1;
ie.Value0.ID.ContactFeature.IndexA = 0;
ie.Value0.ID.ContactFeature.IndexB = (byte)primaryAxis.Index;
ie.Value0.ID.ContactFeature.TypeA = ContactFeatureType.Vertex;
ie.Value0.ID.ContactFeature.TypeB = ContactFeatureType.Face;
ie.Value1.V = v2;
ie.Value1.ID.ContactFeature.IndexA = 0;
ie.Value1.ID.ContactFeature.IndexB = (byte)primaryAxis.Index;
ie.Value1.ID.ContactFeature.TypeA = ContactFeatureType.Vertex;
ie.Value1.ID.ContactFeature.TypeB = ContactFeatureType.Face;
rf.i1 = primaryAxis.Index;
rf.i2 = rf.i1 + 1 < count ? rf.i1 + 1 : 0;
rf.v1 = vertices[rf.i1];
rf.v2 = vertices[rf.i2];
rf.Normal = normals[rf.i1];
}
rf.SideNormal1 = new Vector2(rf.Normal.y, -rf.Normal.x);
rf.SideNormal2 = -rf.SideNormal1;
rf.SideOffset1 = Vector2.Dot(rf.SideNormal1, rf.v1);
rf.SideOffset2 = Vector2.Dot(rf.SideNormal2, rf.v2);
// Clip incident edge against extruded edge1 side edges.
FixedArray2 <ClipVertex> clipPoints1;
FixedArray2 <ClipVertex> clipPoints2;
int np;
// Clip to box side 1
np = Collision.ClipSegmentToLine(out clipPoints1, ref ie, rf.SideNormal1, rf.SideOffset1, rf.i1);
if (np < Settings.MaxManifoldPoints)
{
return;
}
// Clip to negative box side 1
np = Collision.ClipSegmentToLine(out clipPoints2, ref clipPoints1, rf.SideNormal2, rf.SideOffset2, rf.i2);
if (np < Settings.MaxManifoldPoints)
{
return;
}
// Now clipPoints2 contains the clipped points.
if (primaryAxis.Type == EPAxisType.EdgeA)
{
manifold.LocalNormal = rf.Normal;
manifold.LocalPoint = rf.v1;
}
else
{
manifold.LocalNormal = polygonB.Normals[rf.i1];
manifold.LocalPoint = polygonB.Vertices[rf.i1];
}
var pointCount = 0;
for (var i = 0; i < Settings.MaxManifoldPoints; ++i)
{
var separation = Vector2.Dot(rf.Normal, clipPoints2[i].V - rf.v1);
if (separation <= radius)
{
var cp = manifold.Points[pointCount];
if (primaryAxis.Type == EPAxisType.EdgeA)
{
cp.LocalPoint = MathUtils.MulT(ref xf, clipPoints2[i].V);
cp.Id = clipPoints2[i].ID;
}
else
{
cp.LocalPoint = clipPoints2[i].V;
cp.Id.ContactFeature.TypeA = clipPoints2[i].ID.ContactFeature.TypeB;
cp.Id.ContactFeature.TypeB = clipPoints2[i].ID.ContactFeature.TypeA;
cp.Id.ContactFeature.IndexA = clipPoints2[i].ID.ContactFeature.IndexB;
cp.Id.ContactFeature.IndexB = clipPoints2[i].ID.ContactFeature.IndexA;
}
manifold.Points[pointCount] = cp;
++pointCount;
}
}
manifold.PointCount = pointCount;
}
/// <summary>
/// Compute contact points for edge versus circle.
/// This accounts for edge connectivity.
/// </summary>
/// <param name="manifold">The manifold.</param>
/// <param name="edgeA">The edge A.</param>
/// <param name="transformA">The transform A.</param>
/// <param name="circleB">The circle B.</param>
/// <param name="transformB">The transform B.</param>
public static void CollideEdgeAndCircle(ref Manifold manifold, EdgeShape edgeA, ref Transform transformA,
CircleShape circleB, ref Transform transformB)
{
manifold.PointCount = 0;
// Compute circle in frame of edge
var Q = MathUtils.MulT(ref transformA, MathUtils.Mul(ref transformB, ref circleB._position));
Vector2 A = edgeA.Vertex1, B = edgeA.Vertex2;
var e = B - A;
// Barycentric coordinates
var u = Vector2.Dot(e, B - Q);
var v = Vector2.Dot(e, Q - A);
var radius = edgeA.Radius + circleB.Radius;
ContactFeature cf;
cf.IndexB = 0;
cf.TypeB = ContactFeatureType.Vertex;
// Region A
if (v <= 0.0f)
{
var P1 = A;
var d1 = Q - P1;
var dd1 = Vector2.Dot(d1, d1);
if (dd1 > radius * radius)
{
return;
}
// Is there an edge connected to A?
if (edgeA.HasVertex0)
{
var A1 = edgeA.Vertex0;
var B1 = A;
var e1 = B1 - A1;
var u1 = Vector2.Dot(e1, B1 - Q);
// Is the circle in Region AB of the previous edge?
if (u1 > 0.0f)
{
return;
}
}
cf.IndexA = 0;
cf.TypeA = ContactFeatureType.Vertex;
manifold.PointCount = 1;
manifold.Type = ManifoldType.Circles;
manifold.LocalNormal = Vector2.zero;
manifold.LocalPoint = P1;
manifold.Points.Value0.Id.Key = 0;
manifold.Points.Value0.Id.ContactFeature = cf;
manifold.Points.Value0.LocalPoint = circleB.Position;
return;
}
// Region B
if (u <= 0.0f)
{
var P2 = B;
var d2 = Q - P2;
var dd2 = Vector2.Dot(d2, d2);
if (dd2 > radius * radius)
{
return;
}
// Is there an edge connected to B?
if (edgeA.HasVertex3)
{
var B2 = edgeA.Vertex3;
var A2 = B;
var e2 = B2 - A2;
var v2 = Vector2.Dot(e2, Q - A2);
// Is the circle in Region AB of the next edge?
if (v2 > 0.0f)
{
return;
}
}
cf.IndexA = 1;
cf.TypeA = (byte)ContactFeatureType.Vertex;
manifold.PointCount = 1;
manifold.Type = ManifoldType.Circles;
manifold.LocalNormal = Vector2.zero;
manifold.LocalPoint = P2;
manifold.Points.Value0.Id.Key = 0;
manifold.Points.Value0.Id.ContactFeature = cf;
manifold.Points.Value0.LocalPoint = circleB.Position;
return;
}
// Region AB
var den = Vector2.Dot(e, e);
Debug.Assert(den > 0.0f);
var P = 1.0f / den * (u * A + v * B);
var d = Q - P;
var dd = Vector2.Dot(d, d);
if (dd > radius * radius)
{
return;
}
var n = new Vector2(-e.y, e.x);
if (Vector2.Dot(n, Q - A) < 0.0f)
{
n = new Vector2(-n.x, -n.y);
}
n.Normalize();
cf.IndexA = 0;
cf.TypeA = ContactFeatureType.Face;
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalNormal = n;
manifold.LocalPoint = A;
manifold.Points.Value0.Id.Key = 0;
manifold.Points.Value0.Id.ContactFeature = cf;
manifold.Points.Value0.LocalPoint = circleB.Position;
}
/// <summary>
/// Collides and edge and a polygon, taking into account edge adjacency.
/// </summary>
public static void CollideEdgeAndPolygon(ref Manifold manifold, EdgeShape edgeA, ref Transform xfA,
PolygonShape polygonB, ref Transform xfB)
{
EPCollider.Collide(ref manifold, edgeA, ref xfA, polygonB, ref xfB);
}
