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; } } }