static void FindIncidentEdge(out FixedArray2 <ClipVertex> c,
PolygonShape poly1, ref XForm xf1, int edge1,
PolygonShape poly2, ref XForm xf2)
{
c = new FixedArray2 <ClipVertex>();
int count1 = poly1._vertexCount;
int count2 = poly2._vertexCount;
Debug.Assert(0 <= edge1 && edge1 < count1);
// Get the normal of the reference edge in poly2's frame.
Vector2 normal1 = MathUtils.MultiplyT(ref xf2.R, MathUtils.Multiply(ref xf1.R, poly1._normals[edge1]));
// Find the incident edge on poly2.
int index = 0;
float minDot = Settings.b2_FLT_MAX;
for (int i = 0; i < count2; ++i)
{
float dot = Vector2.Dot(normal1, poly2._normals[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;
var cv0 = c[0];
cv0.v = MathUtils.Multiply(ref xf2, poly2._vertices[i1]);
cv0.id.Features.ReferenceEdge = (byte)edge1;
cv0.id.Features.IncidentEdge = (byte)i1;
cv0.id.Features.IncidentVertex = 0;
c[0] = cv0;
var cv1 = c[1];
cv1.v = MathUtils.Multiply(ref xf2, poly2._vertices[i2]);
cv1.id.Features.ReferenceEdge = (byte)edge1;
cv1.id.Features.IncidentEdge = (byte)i2;
cv1.id.Features.IncidentVertex = 1;
c[1] = cv1;
}
/// Clipping for contact manifolds.
public static int ClipSegmentToLine(out FixedArray2 <ClipVertex> vOut, ref FixedArray2 <ClipVertex> vIn,
Vector2 normal, float offset)
{
vOut = new FixedArray2 <ClipVertex>();
// Start with no output points
int numOut = 0;
// Calculate the distance of end points to the line
float distance0 = Vector2.Dot(normal, vIn[0].v) - offset;
float distance1 = Vector2.Dot(normal, vIn[1].v) - offset;
// If the points are behind the plane
if (distance0 <= 0.0f)
{
vOut[numOut++] = vIn[0];
}
if (distance1 <= 0.0f)
{
vOut[numOut++] = vIn[1];
}
// If the points are on different sides of the plane
if (distance0 * distance1 < 0.0f)
{
// Find intersection point of edge and plane
float interp = distance0 / (distance0 - distance1);
var cv = vOut[numOut];
cv.v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);
if (distance0 > 0.0f)
{
cv.id = vIn[0].id;
}
else
{
cv.id = vIn[1].id;
}
vOut[numOut] = cv;
++numOut;
}
return(numOut);
}
public static void GetPointStates(out FixedArray2 <PointState> state1, out FixedArray2 <PointState> state2,
ref Manifold manifold1, ref Manifold manifold2)
{
state1 = new FixedArray2 <PointState>();
state2 = new FixedArray2 <PointState>();
// Detect persists and removes.
for (int i = 0; i < manifold1._pointCount; ++i)
{
ContactID id = manifold1._points[i].Id;
state1[i] = PointState.Remove;
for (int j = 0; j < manifold2._pointCount; ++j)
{
if (manifold2._points[j].Id.Key == id.Key)
{
state1[i] = PointState.Persist;
break;
}
}
}
// Detect persists and adds.
for (int i = 0; i < manifold2._pointCount; ++i)
{
ContactID id = manifold2._points[i].Id;
state2[i] = PointState.Add;
for (int j = 0; j < manifold1._pointCount; ++j)
{
if (manifold1._points[j].Id.Key == id.Key)
{
state2[i] = PointState.Persist;
break;
}
}
}
}
internal PositionSolverManifold(ref ContactConstraint cc)
{
_points = new FixedArray2 <Vector2>();
_separations = new FixedArray2 <float>();
_normal = new Vector2();
Debug.Assert(cc.pointCount > 0);
switch (cc.type)
{
case ManifoldType.Circles:
{
Vector2 pointA = cc.bodyA.GetWorldPoint(cc.localPoint);
Vector2 pointB = cc.bodyB.GetWorldPoint(cc.points[0].localPoint);
if (Vector2.DistanceSquared(pointA, pointB) > Settings.b2_FLT_EPSILON * Settings.b2_FLT_EPSILON)
{
_normal = pointB - pointA;
_normal.Normalize();
}
else
{
_normal = new Vector2(1.0f, 0.0f);
}
_points[0] = 0.5f * (pointA + pointB);
_separations[0] = Vector2.Dot(pointB - pointA, _normal) - cc.radius;
}
break;
case ManifoldType.FaceA:
{
_normal = cc.bodyA.GetWorldVector(cc.localPlaneNormal);
Vector2 planePoint = cc.bodyA.GetWorldPoint(cc.localPoint);
for (int i = 0; i < cc.pointCount; ++i)
{
Vector2 clipPoint = cc.bodyB.GetWorldPoint(cc.points[i].localPoint);
_separations[i] = Vector2.Dot(clipPoint - planePoint, _normal) - cc.radius;
_points[i] = clipPoint;
}
}
break;
case ManifoldType.FaceB:
{
_normal = cc.bodyB.GetWorldVector(cc.localPlaneNormal);
Vector2 planePoint = cc.bodyB.GetWorldPoint(cc.localPoint);
for (int i = 0; i < cc.pointCount; ++i)
{
Vector2 clipPoint = cc.bodyA.GetWorldPoint(cc.points[i].localPoint);
_separations[i] = Vector2.Dot(clipPoint - planePoint, _normal) - cc.radius;
_points[i] = clipPoint;
}
// Ensure normal points from A to B
_normal = -_normal;
}
break;
}
}
/// Evaluate the manifold with supplied transforms. This assumes
/// modest motion from the original state. This does not change the
/// point count, impulses, etc. The radii must come from the shapes
/// that generated the manifold.
public WorldManifold(ref Manifold manifold,
ref XForm xfA, float radiusA,
ref XForm xfB, float radiusB)
{
_normal = Vector2.Zero;
_points = new FixedArray2 <Vector2>();
if (manifold._pointCount == 0)
{
return;
}
switch (manifold._type)
{
case ManifoldType.Circles:
{
Vector2 pointA = MathUtils.Multiply(ref xfA, manifold._localPoint);
Vector2 pointB = MathUtils.Multiply(ref xfB, manifold._points[0].LocalPoint);
Vector2 normal = new Vector2(1.0f, 0.0f);
if (Vector2.DistanceSquared(pointA, pointB) > Settings.b2_FLT_EPSILON * Settings.b2_FLT_EPSILON)
{
normal = pointB - pointA;
normal.Normalize();
}
_normal = normal;
Vector2 cA = pointA + radiusA * normal;
Vector2 cB = pointB - radiusB * normal;
_points[0] = 0.5f * (cA + cB);
}
break;
case ManifoldType.FaceA:
{
Vector2 normal = MathUtils.Multiply(ref xfA.R, manifold._localPlaneNormal);
Vector2 planePoint = MathUtils.Multiply(ref xfA, manifold._localPoint);
// Ensure normal points from A to B.
_normal = normal;
for (int i = 0; i < manifold._pointCount; ++i)
{
Vector2 clipPoint = MathUtils.Multiply(ref xfB, manifold._points[i].LocalPoint);
Vector2 cA = clipPoint + (radiusA - Vector2.Dot(clipPoint - planePoint, normal)) * normal;
Vector2 cB = clipPoint - radiusB * normal;
_points[i] = 0.5f * (cA + cB);
}
}
break;
case ManifoldType.FaceB:
{
Vector2 normal = MathUtils.Multiply(ref xfB.R, manifold._localPlaneNormal);
Vector2 planePoint = MathUtils.Multiply(ref xfB, manifold._localPoint);
// Ensure normal points from A to B.
_normal = -normal;
for (int i = 0; i < manifold._pointCount; ++i)
{
Vector2 clipPoint = MathUtils.Multiply(ref xfA, manifold._points[i].LocalPoint);
Vector2 cA = clipPoint - radiusA * normal;
Vector2 cB = clipPoint + (radiusB - Vector2.Dot(clipPoint - planePoint, normal)) * normal;
_points[i] = 0.5f * (cA + cB);
}
}
break;
}
}
