// Compute contact points for edge versus circle. // This accounts for edge connectivity. public static void CollideEdgeAndCircle(ref Manifold manifold, EdgeShape edgeA, ref Transform xfA, CircleShape circleB, ref Transform xfB) { manifold._pointCount = 0; // Compute circle in frame of edge Vector2 Q = MathUtils.MultiplyT(ref xfA, MathUtils.Multiply(ref xfB, circleB._p)); Vector2 A = edgeA._vertex1, B = edgeA._vertex2; Vector2 e = B - A; // Barycentric coordinates float u = Vector2.Dot(e, B - Q); float v = Vector2.Dot(e, Q - A); float radius = edgeA._radius + circleB._radius; ContactFeature cf; cf.indexB = 0; cf.typeB = (byte)ContactFeatureType.Vertex; Vector2 P, d; // Region A if (v <= 0.0f) { P = A; d = Q - P; float dd = Vector2.Dot(d, d); if (dd > radius * radius) { return; } // Is there an edge connected to A? if (edgeA._hasVertex0) { Vector2 A1 = edgeA._vertex0; Vector2 B1 = A; Vector2 e1 = B1 - A1; float 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 = (byte)ContactFeatureType.Vertex; manifold._pointCount = 1; manifold._type = ManifoldType.Circles; manifold._localNormal = Vector2.Zero; manifold._localPoint = P; var mp = new ManifoldPoint(); mp.Id.Key = 0; mp.Id.Features = cf; mp.LocalPoint = circleB._p; manifold._points[0] = mp; return; } // Region B if (u <= 0.0f) { P = B; d = Q - P; float dd = Vector2.Dot(d, d); if (dd > radius * radius) { return; } // Is there an edge connected to B? if (edgeA._hasVertex3) { Vector2 B2 = edgeA._vertex3; Vector2 A2 = B; Vector2 e2 = B2 - A2; float 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 = P; var mp = new ManifoldPoint(); mp.Id.Key = 0; mp.Id.Features = cf; mp.LocalPoint = circleB._p; manifold._points[0] = mp; return; } // Region AB float den = Vector2.Dot(e, e); Debug.Assert(den > 0.0f); P = (1.0f / den) * (u * A + v * B); d = Q - P; float dd2 = Vector2.Dot(d, d); if (dd2 > radius * radius) { return; } Vector2 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 = (byte)ContactFeatureType.Face; manifold._pointCount = 1; manifold._type = ManifoldType.FaceA; manifold._localNormal = n; manifold._localPoint = A; var mp2 = new ManifoldPoint(); mp2.Id.Key = 0; mp2.Id.Features = cf; mp2.LocalPoint = circleB._p; manifold._points[0] = mp2; }
/// <summary> /// Compute the collision manifold between two polygons. /// </summary> /// <param name="manifold"></param> /// <param name="polyA"></param> /// <param name="xfA"></param> /// <param name="polyB"></param> /// <param name="xfB"></param> public static void CollidePolygons(ref Manifold manifold, PolygonShape polyA, ref Transform xfA, PolygonShape polyB, ref Transform xfB) { manifold._pointCount = 0; float totalRadius = polyA._radius + polyB._radius; int edgeA = 0; float separationA = FindMaxSeparation(out edgeA, polyA, ref xfA, polyB, ref xfB); if (separationA > totalRadius) { return; } int edgeB = 0; float separationB = FindMaxSeparation(out edgeB, polyB, ref xfB, polyA, ref xfA); if (separationB > totalRadius) { return; } PolygonShape poly1; // reference polygon PolygonShape poly2; // incident polygon Transform xf1, xf2; int edge1; // reference edge byte 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 = ManifoldType.FaceB; flip = 1; } else { poly1 = polyA; poly2 = polyB; xf1 = xfA; xf2 = xfB; edge1 = edgeA; manifold._type = ManifoldType.FaceA; flip = 0; } FixedArray2 <ClipVertex> incidentEdge; FindIncidentEdge(out incidentEdge, poly1, ref xf1, edge1, poly2, ref xf2); int count1 = poly1._vertexCount; Vector2 v11 = poly1._vertices[edge1]; Vector2 v12 = edge1 + 1 < count1 ? poly1._vertices[edge1 + 1] : poly1._vertices[0]; Vector2 localTangent = v12 - v11; localTangent.Normalize(); Vector2 localNormal = MathUtils.Cross(localTangent, 1.0f); Vector2 planePoint = 0.5f * (v11 + v12); Vector2 tangent = MathUtils.Multiply(ref xf1.R, localTangent); Vector2 normal = MathUtils.Cross(tangent, 1.0f); v11 = MathUtils.Multiply(ref xf1, v11); v12 = MathUtils.Multiply(ref xf1, v12); // Face offset. float frontOffset = Vector2.Dot(normal, v11); // Side offsets, extended by polytope skin thickness. float sideOffset1 = -Vector2.Dot(tangent, v11) + totalRadius; float sideOffset2 = Vector2.Dot(tangent, v12) + totalRadius; // Clip incident edge against extruded edge1 side edges. FixedArray2 <ClipVertex> clipPoints1; FixedArray2 <ClipVertex> clipPoints2; int np; // Clip to box side 1 np = ClipSegmentToLine(out clipPoints1, ref incidentEdge, -tangent, sideOffset1); if (np < 2) { return; } // Clip to negative box side 1 np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, tangent, sideOffset2); if (np < 2) { return; } // Now clipPoints2 contains the clipped points. manifold._localNormal = localNormal; manifold._localPoint = planePoint; int pointCount = 0; for (int i = 0; i < Settings.b2_maxManifoldPoints; ++i) { float separation = Vector2.Dot(normal, clipPoints2[i].v) - frontOffset; if (separation <= totalRadius) { ManifoldPoint cp = manifold._points[pointCount]; cp.LocalPoint = MathUtils.MultiplyT(ref xf2, clipPoints2[i].v); cp.Id = clipPoints2[i].id; cp.Id.Features.Flip = flip; manifold._points[pointCount] = cp; ++pointCount; } } manifold._pointCount = pointCount; }
// Update the contact manifold and touching status. // Note: do not assume the fixture AABBs are overlapping or are valid. internal void Update(IContactListener listener) { Manifold oldManifold = _manifold; // Re-enable this contact. _flags |= ContactFlags.Enabled; bool touching = false; bool wasTouching = (_flags & ContactFlags.Touching) == ContactFlags.Touching; bool sensorA = _fixtureA.IsSensor(); bool sensorB = _fixtureB.IsSensor(); bool sensor = sensorA || sensorB; Body bodyA = _fixtureA.GetBody(); Body bodyB = _fixtureB.GetBody(); Transform xfA; bodyA.GetTransform(out xfA); Transform xfB; bodyB.GetTransform(out xfB); // Is this contact a sensor? if (sensor) { Shape shapeA = _fixtureA.GetShape(); Shape shapeB = _fixtureB.GetShape(); touching = AABB.TestOverlap(shapeA, _indexA, shapeB, _indexB, ref xfA, ref xfB); // Sensors don't generate manifolds. _manifold._pointCount = 0; } else { Evaluate(ref _manifold, ref xfA, ref xfB); touching = _manifold._pointCount > 0; // Match old contact ids to new contact ids and copy the // stored impulses to warm start the solver. for (int i = 0; i < _manifold._pointCount; ++i) { ManifoldPoint mp2 = _manifold._points[i]; mp2.NormalImpulse = 0.0f; mp2.TangentImpulse = 0.0f; ContactID id2 = mp2.Id; bool found = false; for (int j = 0; j < oldManifold._pointCount; ++j) { ManifoldPoint mp1 = oldManifold._points[j]; if (mp1.Id.Key == id2.Key) { mp2.NormalImpulse = mp1.NormalImpulse; mp2.TangentImpulse = mp1.TangentImpulse; found = true; break; } } if (found == false) { mp2.NormalImpulse = 0.0f; mp2.TangentImpulse = 0.0f; } _manifold._points[i] = mp2; } if (touching != wasTouching) { bodyA.SetAwake(true); bodyB.SetAwake(true); } } if (touching) { _flags |= ContactFlags.Touching; } else { _flags &= ~ContactFlags.Touching; } if (wasTouching == false && touching == true && null != listener) { listener.BeginContact(this); } if (wasTouching == true && touching == false && null != listener) { listener.EndContact(this); } if (sensor == false && null != listener) { listener.PreSolve(this, ref oldManifold); } }
public void Reset(Contact[] contacts, int contactCount, float impulseRatio) { _contacts = contacts; _constraintCount = contactCount; // grow the array if (_constraints == null || _constraints.Length < _constraintCount) { _constraints = new ContactConstraint[_constraintCount * 2]; } for (int i = 0; i < _constraintCount; ++i) { Contact contact = contacts[i]; Fixture fixtureA = contact._fixtureA; Fixture fixtureB = contact._fixtureB; Shape shapeA = fixtureA.GetShape(); Shape shapeB = fixtureB.GetShape(); float radiusA = shapeA._radius; float radiusB = shapeB._radius; Body bodyA = fixtureA.GetBody(); Body bodyB = fixtureB.GetBody(); Manifold manifold; contact.GetManifold(out manifold); float friction = Settings.b2MixFriction(fixtureA.GetFriction(), fixtureB.GetFriction()); float restitution = Settings.b2MixRestitution(fixtureA.GetRestitution(), fixtureB.GetRestitution()); Vector2 vA = bodyA._linearVelocity; Vector2 vB = bodyB._linearVelocity; float wA = bodyA._angularVelocity; float wB = bodyB._angularVelocity; Debug.Assert(manifold._pointCount > 0); WorldManifold worldManifold = new WorldManifold(ref manifold, ref bodyA._xf, radiusA, ref bodyB._xf, radiusB); ContactConstraint cc = _constraints[i]; cc.bodyA = bodyA; cc.bodyB = bodyB; cc.manifold = manifold; cc.normal = worldManifold._normal; cc.pointCount = manifold._pointCount; cc.friction = friction; cc.localNormal = manifold._localNormal; cc.localPoint = manifold._localPoint; cc.radius = radiusA + radiusB; cc.type = manifold._type; for (int j = 0; j < cc.pointCount; ++j) { ManifoldPoint cp = manifold._points[j]; ContactConstraintPoint ccp = cc.points[j]; ccp.normalImpulse = impulseRatio * cp.NormalImpulse; ccp.tangentImpulse = impulseRatio * cp.TangentImpulse; ccp.localPoint = cp.LocalPoint; ccp.rA = worldManifold._points[j] - bodyA._sweep.c; ccp.rB = worldManifold._points[j] - bodyB._sweep.c; #if MATH_OVERLOADS float rnA = MathUtils.Cross(ccp.rA, cc.normal); float rnB = MathUtils.Cross(ccp.rB, cc.normal); #else float rnA = ccp.rA.X * cc.normal.Y - ccp.rA.Y * cc.normal.X; float rnB = ccp.rB.X * cc.normal.Y - ccp.rB.Y * cc.normal.X; #endif rnA *= rnA; rnB *= rnB; float kNormal = bodyA._invMass + bodyB._invMass + bodyA._invI * rnA + bodyB._invI * rnB; Debug.Assert(kNormal > Settings.b2_epsilon); ccp.normalMass = 1.0f / kNormal; #if MATH_OVERLOADS Vector2 tangent = MathUtils.Cross(cc.normal, 1.0f); float rtA = MathUtils.Cross(ccp.rA, tangent); float rtB = MathUtils.Cross(ccp.rB, tangent); #else Vector2 tangent = new Vector2(cc.normal.Y, -cc.normal.X); float rtA = ccp.rA.X * tangent.Y - ccp.rA.Y * tangent.X; float rtB = ccp.rB.X * tangent.Y - ccp.rB.Y * tangent.X; #endif rtA *= rtA; rtB *= rtB; float kTangent = bodyA._invMass + bodyB._invMass + bodyA._invI * rtA + bodyB._invI * rtB; Debug.Assert(kTangent > Settings.b2_epsilon); ccp.tangentMass = 1.0f / kTangent; // Setup a velocity bias for restitution. ccp.velocityBias = 0.0f; float vRel = Vector2.Dot(cc.normal, vB + MathUtils.Cross(wB, ccp.rB) - vA - MathUtils.Cross(wA, ccp.rA)); if (vRel < -Settings.b2_velocityThreshold) { ccp.velocityBias = -restitution * vRel; } cc.points[j] = ccp; } // If we have two points, then prepare the block solver. if (cc.pointCount == 2) { ContactConstraintPoint ccp1 = cc.points[0]; ContactConstraintPoint ccp2 = cc.points[1]; float invMassA = bodyA._invMass; float invIA = bodyA._invI; float invMassB = bodyB._invMass; float invIB = bodyB._invI; float rn1A = MathUtils.Cross(ccp1.rA, cc.normal); float rn1B = MathUtils.Cross(ccp1.rB, cc.normal); float rn2A = MathUtils.Cross(ccp2.rA, cc.normal); float rn2B = MathUtils.Cross(ccp2.rB, cc.normal); float k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B; float k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B; float k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B; // Ensure a reasonable condition number. const float k_maxConditionNumber = 100.0f; if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) { // K is safe to invert. cc.K = new Mat22(new Vector2(k11, k12), new Vector2(k12, k22)); cc.normalMass = cc.K.GetInverse(); } else { // The constraints are redundant, just use one. // TODO_ERIN use deepest? cc.pointCount = 1; } } _constraints[i] = cc; } }