internal override bool SolvePositionConstraints(SolverData data)
{
Vec2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
Vec2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
Rot qA = new Rot(aA);
Rot qB = new Rot(aB);
Vec2 rA = Utilities.Mul(qA, m_localAnchorA - m_localCenterA);
Vec2 rB = Utilities.Mul(qB, m_localAnchorB - m_localCenterB);
Vec2 u = cB + rB - cA - rA;
float length = u.Normalize();
float C = length - m_maxLength;
C = Utilities.Clamp(C, 0.0f, Settings._maxLinearCorrection);
float impulse = -m_mass * C;
Vec2 P = impulse * u;
cA -= m_invMassA * P;
aA -= m_invIA * Utilities.Cross(rA, P);
cB += m_invMassB * P;
aB += m_invIB * Utilities.Cross(rB, P);
data.positions[m_indexA].c = cA;
data.positions[m_indexA].a = aA;
data.positions[m_indexB].c = cB;
data.positions[m_indexB].a = aB;
return(length - m_maxLength < Settings._linearSlop);
}
public Prismatic() {
Body ground = null;
{
BodyDef bd = new BodyDef();
ground = m_world.CreateBody(bd);
EdgeShape shape = new EdgeShape();
shape.Set(new Vec2(-40.0f, 0.0f), new Vec2(40.0f, 0.0f));
shape.Density = 0;
ground.CreateFixture(shape);
}
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(2.0f, 0.5f);
shape.Density = 5;
BodyDef bd = new BodyDef();
bd.type = BodyType._dynamicBody;
bd.Position.Set(-10.0f, 10.0f);
bd.angle = 0.5f * (float)Math.PI;
bd.allowSleep = false;
Body body = m_world.CreateBody(bd);
body.CreateFixture(shape);
PrismaticJointDef pjd = new PrismaticJointDef();
// Bouncy limit
Vec2 axis = new Vec2(2.0f, 1.0f);
axis.Normalize();
pjd.Initialize(ground, body, new Vec2(0.0f, 0.0f), axis);
// Non-bouncy limit
//pjd.Initialize(ground, body, new Vec2(-10.0f, 10.0f), new Vec2(1.0f, 0.0f));
pjd.motorSpeed = 10.0f;
pjd.maxMotorForce = 10000.0f;
pjd.enableMotor = true;
pjd.lowerTranslation = 0.0f;
pjd.upperTranslation = 20.0f;
pjd.enableLimit = true;
m_joint = (PrismaticJoint)m_world.CreateJoint(pjd);
}
}
internal override bool SolvePositionConstraints(SolverData data)
{
if (m_frequencyHz > 0.0f)
{
// There is no position correction for soft distance constraints.
return(true);
}
Vec2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
Vec2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
Rot qA = new Rot(aA); Rot qB = new Rot(aB);
Vec2 rA = Utilities.Mul(qA, m_localAnchorA - m_localCenterA);
Vec2 rB = Utilities.Mul(qB, m_localAnchorB - m_localCenterB);
Vec2 u = cB + rB - cA - rA;
float length = u.Normalize();
float C = length - m_length;
C = Utilities.Clamp(C, -Settings._maxLinearCorrection, Settings._maxLinearCorrection);
float impulse = -m_mass * C;
Vec2 P = impulse * u;
cA -= m_invMassA * P;
aA -= m_invIA * Utilities.Cross(rA, P);
cB += m_invMassB * P;
aB += m_invIB * Utilities.Cross(rB, P);
data.positions[m_indexA].c = cA;
data.positions[m_indexA].a = aA;
data.positions[m_indexB].c = cB;
data.positions[m_indexB].a = aB;
return(Math.Abs(C) < Settings._linearSlop);
}
/// Compute the collision manifold between an edge and a circle.
public static void CollideEdgeAndCircle(out Manifold manifold,
EdgeShape edgeA, Transform xfA,
CircleShape circleB, Transform xfB){
manifold = new Manifold();
// Compute circle in frame of edge
Vec2 Q = Utilities.MulT(xfA, Utilities.Mul(xfB, circleB.m_p));
Vec2 A = edgeA.m_vertex1, B = edgeA.m_vertex2;
Vec2 e = B - A;
// Barycentric coordinates
float u = Utilities.Dot(e, B - Q);
float v = Utilities.Dot(e, Q - A);
float radius = edgeA.m_radius + circleB.m_radius;
ContactFeature cf;
cf.indexB = 0;
cf.typeB = ContactFeature.FeatureType.e_vertex;
// Region A
if (v <= 0.0f)
{
Vec2 P = A;
Vec2 d = Q - P;
float dd = Utilities.Dot(d, d);
if (dd > radius * radius)
{
return;
}
// Is there an edge connected to A?
if (edgeA.m_hasVertex0)
{
Vec2 A1 = edgeA.m_vertex0;
Vec2 B1 = A;
Vec2 e1 = B1 - A1;
float u1 = Utilities.Dot(e1, B1 - Q);
// Is the circle in Region AB of the previous edge?
if (u1 > 0.0f)
{
return;
}
}
cf.indexA = 0;
cf.typeA = ContactFeature.FeatureType.e_vertex;
manifold.points.Clear();
manifold.points.Add(new ManifoldPoint());
manifold.type = Manifold.ManifoldType.e_circles;
manifold.localNormal.SetZero();
manifold.localPoint = P;
manifold.points[0].id.key = 0;
manifold.points[0].id.cf = cf;
manifold.points[0].localPoint = circleB.m_p;
return;
}
// Region B
if (u <= 0.0f)
{
Vec2 P = B;
Vec2 d = Q - P;
float dd = Utilities.Dot(d, d);
if (dd > radius * radius)
{
return;
}
// Is there an edge connected to B?
if (edgeA.m_hasVertex3)
{
Vec2 B2 = edgeA.m_vertex3;
Vec2 A2 = B;
Vec2 e2 = B2 - A2;
float v2 = Utilities.Dot(e2, Q - A2);
// Is the circle in Region AB of the next edge?
if (v2 > 0.0f)
{
return;
}
}
cf.indexA = 1;
cf.typeA = ContactFeature.FeatureType.e_vertex;
manifold.points.Clear();
manifold.points.Add(new ManifoldPoint());
manifold.type = Manifold.ManifoldType.e_circles;
manifold.localNormal.SetZero();
manifold.localPoint = P;
manifold.points[0].id.key = 0;
manifold.points[0].id.cf = cf;
manifold.points[0].localPoint = circleB.m_p;
return;
}
// Region AB
float den = Utilities.Dot(e, e);
Utilities.Assert(den > 0.0f);
Vec2 Pb = (1.0f / den) * (u * A + v * B);
Vec2 db = Q - Pb;
float ddb = Utilities.Dot(db, db);
if (ddb > radius * radius)
{
return;
}
Vec2 n = new Vec2(-e.Y, e.X);
if (Utilities.Dot(n, Q - A) < 0.0f)
{
n.Set(-n.X, -n.Y);
}
n.Normalize();
cf.indexA = 0;
cf.typeA = ContactFeature.FeatureType.e_face;
manifold.points.Clear();
manifold.points.Add(new ManifoldPoint());
manifold.type = Manifold.ManifoldType.e_faceA;
manifold.localNormal = n;
manifold.localPoint = A;
manifold.points[0].id.key = 0;
manifold.points[0].id.cf = cf;
manifold.points[0].localPoint = circleB.m_p;
}
/// Compute the collision manifold between an edge and a circle.
public static void CollideEdgeAndCircle(out Manifold manifold,
EdgeShape edgeA, Transform xfA,
CircleShape circleB, Transform xfB)
{
manifold = new Manifold();
// Compute circle in frame of edge
Vec2 Q = Utilities.MulT(xfA, Utilities.Mul(xfB, circleB.m_p));
Vec2 A = edgeA.m_vertex1, B = edgeA.m_vertex2;
Vec2 e = B - A;
// Barycentric coordinates
float u = Utilities.Dot(e, B - Q);
float v = Utilities.Dot(e, Q - A);
float radius = edgeA.m_radius + circleB.m_radius;
ContactFeature cf;
cf.indexB = 0;
cf.typeB = ContactFeature.FeatureType.e_vertex;
// Region A
if (v <= 0.0f)
{
Vec2 P = A;
Vec2 d = Q - P;
float dd = Utilities.Dot(d, d);
if (dd > radius * radius)
{
return;
}
// Is there an edge connected to A?
if (edgeA.m_hasVertex0)
{
Vec2 A1 = edgeA.m_vertex0;
Vec2 B1 = A;
Vec2 e1 = B1 - A1;
float u1 = Utilities.Dot(e1, B1 - Q);
// Is the circle in Region AB of the previous edge?
if (u1 > 0.0f)
{
return;
}
}
cf.indexA = 0;
cf.typeA = ContactFeature.FeatureType.e_vertex;
manifold.points.Clear();
manifold.points.Add(new ManifoldPoint());
manifold.type = Manifold.ManifoldType.e_circles;
manifold.localNormal.SetZero();
manifold.localPoint = P;
manifold.points[0].id.key = 0;
manifold.points[0].id.cf = cf;
manifold.points[0].localPoint = circleB.m_p;
return;
}
// Region B
if (u <= 0.0f)
{
Vec2 P = B;
Vec2 d = Q - P;
float dd = Utilities.Dot(d, d);
if (dd > radius * radius)
{
return;
}
// Is there an edge connected to B?
if (edgeA.m_hasVertex3)
{
Vec2 B2 = edgeA.m_vertex3;
Vec2 A2 = B;
Vec2 e2 = B2 - A2;
float v2 = Utilities.Dot(e2, Q - A2);
// Is the circle in Region AB of the next edge?
if (v2 > 0.0f)
{
return;
}
}
cf.indexA = 1;
cf.typeA = ContactFeature.FeatureType.e_vertex;
manifold.points.Clear();
manifold.points.Add(new ManifoldPoint());
manifold.type = Manifold.ManifoldType.e_circles;
manifold.localNormal.SetZero();
manifold.localPoint = P;
manifold.points[0].id.key = 0;
manifold.points[0].id.cf = cf;
manifold.points[0].localPoint = circleB.m_p;
return;
}
// Region AB
float den = Utilities.Dot(e, e);
Utilities.Assert(den > 0.0f);
Vec2 Pb = (1.0f / den) * (u * A + v * B);
Vec2 db = Q - Pb;
float ddb = Utilities.Dot(db, db);
if (ddb > radius * radius)
{
return;
}
Vec2 n = new Vec2(-e.Y, e.X);
if (Utilities.Dot(n, Q - A) < 0.0f)
{
n.Set(-n.X, -n.Y);
}
n.Normalize();
cf.indexA = 0;
cf.typeA = ContactFeature.FeatureType.e_face;
manifold.points.Clear();
manifold.points.Add(new ManifoldPoint());
manifold.type = Manifold.ManifoldType.e_faceA;
manifold.localNormal = n;
manifold.localPoint = A;
manifold.points[0].id.key = 0;
manifold.points[0].id.cf = cf;
manifold.points[0].localPoint = circleB.m_p;
}
// Find edge normal of max separation on A - return if separating axis is found
// Find edge normal of max separation on B - return if separation axis is found
// Choose reference edge as min(minA, minB)
// Find incident edge
// Clip
// The normal points from 1 to 2
/// Compute the collision manifold between two polygons.
public static void CollidePolygons(out Manifold manifold,
PolygonShape polyA, Transform xfA,
PolygonShape polyB, Transform xfB)
{
manifold = new Manifold();
float totalRadius = polyA.m_radius + polyB.m_radius;
int edgeA = 0;
float separationA = FindMaxSeparation(out edgeA, polyA, xfA, polyB, xfB);
if (separationA > totalRadius)
{
return;
}
int edgeB = 0;
float separationB = FindMaxSeparation(out edgeB, polyB, xfB, polyA, xfA);
if (separationB > totalRadius)
{
return;
}
PolygonShape poly1; // reference polygon
PolygonShape poly2; // incident polygon
Transform xf1, xf2;
int edge1; // reference edge
bool flip;
const float k_relativeTol = 0.98f;
const float k_absoluteTol = 0.001f;
if (separationB > k_relativeTol * separationA + k_absoluteTol)
{
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = edgeB;
manifold.type = Manifold.ManifoldType.e_faceB;
flip = true;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
manifold.type = Manifold.ManifoldType.e_faceA;
flip = false;
}
ClipVertex[] incidentEdge = new ClipVertex[2];
FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
int count1 = poly1.m_count;
Vec2[] vertices1 = poly1.m_vertices;
int iv1 = edge1;
int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
Vec2 v11 = vertices1[iv1];
Vec2 v12 = vertices1[iv2];
Vec2 localTangent = v12 - v11;
localTangent.Normalize();
Vec2 localNormal = Utilities.Cross(localTangent, 1.0f);
Vec2 planePoint = 0.5f * (v11 + v12);
Vec2 tangent = Utilities.Mul(xf1.q, localTangent);
Vec2 normal = Utilities.Cross(tangent, 1.0f);
v11 = Utilities.Mul(xf1, v11);
v12 = Utilities.Mul(xf1, v12);
// Face offset.
float frontOffset = Utilities.Dot(normal, v11);
// Side offsets, extended by polytope skin thickness.
float sideOffset1 = -Utilities.Dot(tangent, v11) + totalRadius;
float sideOffset2 = Utilities.Dot(tangent, v12) + totalRadius;
// Clip incident edge against extruded edge1 side edges.
ClipVertex[] clipPoints1 = new ClipVertex[2];
ClipVertex[] clipPoints2 = new ClipVertex[2];
int np;
// Clip to box side 1
np = ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, iv1);
if (np < 2)
{
return;
}
// Clip to negative box side 1
np = ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
manifold.localNormal = localNormal;
manifold.localPoint = planePoint;
manifold.points.Clear();
for (int i = 0; i < Settings._maxManifoldPoints; ++i)
{
float separation = Utilities.Dot(normal, clipPoints2[i].v) - frontOffset;
if (separation <= totalRadius)
{
ManifoldPoint cp = new ManifoldPoint();
cp.localPoint = Utilities.MulT(xf2, clipPoints2[i].v);
cp.id = clipPoints2[i].id;
if (flip)
{
// Swap features
ContactFeature cf = cp.id.cf;
cp.id.cf.indexA = cf.indexB;
cp.id.cf.indexB = cf.indexA;
cp.id.cf.typeA = cf.typeB;
cp.id.cf.typeB = cf.typeA;
}
manifold.points.Add(cp);
}
}
}
// 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
public void Collide(out Manifold manifold, EdgeShape edgeA, Transform xfA, PolygonShape polygonB, Transform xfB)
{
manifold = new Manifold();
m_xf = Utilities.MulT(xfA, xfB);
m_centroidB = Utilities.Mul(m_xf, polygonB.m_centroid);
m_v0 = edgeA.m_vertex0;
m_v1 = edgeA.m_vertex1;
m_v2 = edgeA.m_vertex2;
m_v3 = edgeA.m_vertex3;
bool hasVertex0 = edgeA.m_hasVertex0;
bool hasVertex3 = edgeA.m_hasVertex3;
Vec2 edge1 = m_v2 - m_v1;
edge1.Normalize();
m_normal1.Set(edge1.Y, -edge1.X);
float offset1 = Utilities.Dot(m_normal1, m_centroidB - m_v1);
float offset0 = 0.0f, offset2 = 0.0f;
bool convex1 = false, convex2 = false;
// Is there a preceding edge?
if (hasVertex0)
{
Vec2 edge0 = m_v1 - m_v0;
edge0.Normalize();
m_normal0.Set(edge0.Y, -edge0.X);
convex1 = Utilities.Cross(edge0, edge1) >= 0.0f;
offset0 = Utilities.Dot(m_normal0, m_centroidB - m_v0);
}
// Is there a following edge?
if (hasVertex3)
{
Vec2 edge2 = m_v3 - m_v2;
edge2.Normalize();
m_normal2.Set(edge2.Y, -edge2.X);
convex2 = Utilities.Cross(edge1, edge2) > 0.0f;
offset2 = Utilities.Dot(m_normal2, m_centroidB - m_v2);
}
// Determine front or back collision. Determine collision normal limits.
if (hasVertex0 && hasVertex3)
{
if (convex1 && convex2)
{
m_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal0;
m_upperLimit = m_normal2;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = -m_normal1;
}
}
else if (convex1)
{
m_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal0;
m_upperLimit = m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal2;
m_upperLimit = -m_normal1;
}
}
else if (convex2)
{
m_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = m_normal2;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = -m_normal0;
}
}
else
{
m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal2;
m_upperLimit = -m_normal0;
}
}
}
else if (hasVertex0)
{
if (convex1)
{
m_front = offset0 >= 0.0f || offset1 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal0;
m_upperLimit = -m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = -m_normal1;
}
}
else
{
m_front = offset0 >= 0.0f && offset1 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = -m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = -m_normal0;
}
}
}
else if (hasVertex3)
{
if (convex2)
{
m_front = offset1 >= 0.0f || offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = m_normal2;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = m_normal1;
}
}
else
{
m_front = offset1 >= 0.0f && offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal2;
m_upperLimit = m_normal1;
}
}
}
else
{
m_front = offset1 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = -m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = m_normal1;
}
}
// Get polygonB in frameA
m_polygonB.count = polygonB.m_count;
for (int i = 0; i < polygonB.m_count; ++i)
{
m_polygonB.vertices[i] = Utilities.Mul(m_xf, polygonB.m_vertices[i]);
m_polygonB.normals[i] = Utilities.Mul(m_xf.q, polygonB.m_normals[i]);
}
m_radius = 2.0f * Settings._polygonRadius;
manifold.points.Clear();
EPAxis edgeAxis = ComputeEdgeSeparation();
// If no valid normal can be found than this edge should not collide.
if (edgeAxis.type == EPAxisType.e_unknown)
{
return;
}
if (edgeAxis.separation > m_radius)
{
return;
}
EPAxis polygonAxis = ComputePolygonSeparation();
if (polygonAxis.type != EPAxisType.e_unknown && polygonAxis.separation > m_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.e_unknown)
{
primaryAxis = edgeAxis;
}
else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol)
{
primaryAxis = polygonAxis;
}
else
{
primaryAxis = edgeAxis;
}
ClipVertex[] ie = new ClipVertex[2];
ReferenceFace rf = new ReferenceFace();
if (primaryAxis.type == EPAxisType.e_edgeA)
{
manifold.type = Manifold.ManifoldType.e_faceA;
// Search for the polygon normal that is most anti-parallel to the edge normal.
int bestIndex = 0;
float bestValue = Utilities.Dot(m_normal, m_polygonB.normals[0]);
for (int i = 1; i < m_polygonB.count; ++i)
{
float value = Utilities.Dot(m_normal, m_polygonB.normals[i]);
if (value < bestValue)
{
bestValue = value;
bestIndex = i;
}
}
int i1 = bestIndex;
int i2 = i1 + 1 < m_polygonB.count ? i1 + 1 : 0;
ie[0].v = m_polygonB.vertices[i1];
ie[0].id.cf.indexA = 0;
ie[0].id.cf.indexB = (byte)i1;
ie[0].id.cf.typeA = ContactFeature.FeatureType.e_face;
ie[0].id.cf.typeB = ContactFeature.FeatureType.e_vertex;
ie[1].v = m_polygonB.vertices[i2];
ie[1].id.cf.indexA = 0;
ie[1].id.cf.indexB = (byte)i2;
ie[1].id.cf.typeA = ContactFeature.FeatureType.e_face;
ie[1].id.cf.typeB = ContactFeature.FeatureType.e_vertex;
if (m_front)
{
rf.i1 = 0;
rf.i2 = 1;
rf.v1 = m_v1;
rf.v2 = m_v2;
rf.normal = m_normal1;
}
else
{
rf.i1 = 1;
rf.i2 = 0;
rf.v1 = m_v2;
rf.v2 = m_v1;
rf.normal = -m_normal1;
}
}
else
{
manifold.type = Manifold.ManifoldType.e_faceB;
ie[0].v = m_v1;
ie[0].id.cf.indexA = 0;
ie[0].id.cf.indexB = (byte)primaryAxis.index;
ie[0].id.cf.typeA = ContactFeature.FeatureType.e_vertex;
ie[0].id.cf.typeB = ContactFeature.FeatureType.e_face;
ie[1].v = m_v2;
ie[1].id.cf.indexA = 0;
ie[1].id.cf.indexB = (byte)primaryAxis.index;
ie[1].id.cf.typeA = ContactFeature.FeatureType.e_vertex;
ie[1].id.cf.typeB = ContactFeature.FeatureType.e_face;
rf.i1 = primaryAxis.index;
rf.i2 = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0;
rf.v1 = m_polygonB.vertices[rf.i1];
rf.v2 = m_polygonB.vertices[rf.i2];
rf.normal = m_polygonB.normals[rf.i1];
}
rf.sideNormal1.Set(rf.normal.Y, -rf.normal.X);
rf.sideNormal2 = -rf.sideNormal1;
rf.sideOffset1 = Utilities.Dot(rf.sideNormal1, rf.v1);
rf.sideOffset2 = Utilities.Dot(rf.sideNormal2, rf.v2);
// Clip incident edge against extruded edge1 side edges.
ClipVertex[] clipPoints1 = new ClipVertex[2];
ClipVertex[] clipPoints2 = new ClipVertex[2];
int np;
// Clip to box side 1
np = Collision.ClipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, rf.i1);
if (np < Settings._maxManifoldPoints)
{
return;
}
// Clip to negative box side 1
np = Collision.ClipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2);
if (np < Settings._maxManifoldPoints)
{
return;
}
// Now clipPoints2 contains the clipped points.
if (primaryAxis.type == EPAxisType.e_edgeA)
{
manifold.localNormal = rf.normal;
manifold.localPoint = rf.v1;
}
else
{
manifold.localNormal = polygonB.m_normals[rf.i1];
manifold.localPoint = polygonB.m_vertices[rf.i1];
}
manifold.points.Clear();
for (int i = 0; i < Settings._maxManifoldPoints; ++i)
{
float separation;
separation = Utilities.Dot(rf.normal, clipPoints2[i].v - rf.v1);
if (separation <= m_radius)
{
ManifoldPoint cp = new ManifoldPoint();
if (primaryAxis.type == EPAxisType.e_edgeA)
{
cp.localPoint = Utilities.MulT(m_xf, clipPoints2[i].v);
cp.id = clipPoints2[i].id;
}
else
{
cp.localPoint = clipPoints2[i].v;
cp.id.cf.typeA = clipPoints2[i].id.cf.typeB;
cp.id.cf.typeB = clipPoints2[i].id.cf.typeA;
cp.id.cf.indexA = clipPoints2[i].id.cf.indexB;
cp.id.cf.indexB = clipPoints2[i].id.cf.indexA;
}
manifold.points.Add(cp);
}
}
}
/// 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;
}
}
}
