public void WarmStart() { // Warm start. for (int i = 0; i < _constraintCount; ++i) { ContactConstraint c = _constraints[i]; Body bodyA = c.bodyA; Body bodyB = c.bodyB; float invMassA = bodyA._invMass; float invIA = bodyA._invI; float invMassB = bodyB._invMass; float invIB = bodyB._invI; Vector2 normal = c.normal; #if MATH_OVERLOADS Vector2 tangent = MathUtils.Cross(normal, 1.0f); #else Vector2 tangent = new Vector2(normal.y, -normal.x); #endif for (int j = 0; j < c.pointCount; ++j) { ContactConstraintPoint ccp = c.points[j]; #if MATH_OVERLOADS Vector2 P = ccp.normalImpulse * normal + ccp.tangentImpulse * tangent; bodyA._angularVelocity -= invIA * MathUtils.Cross(ccp.rA, P); bodyA._linearVelocity -= invMassA * P; bodyB._angularVelocity += invIB * MathUtils.Cross(ccp.rB, P); bodyB._linearVelocity += invMassB * P; #else Vector2 P = new Vector2(ccp.normalImpulse * normal.x + ccp.tangentImpulse * tangent.x, ccp.normalImpulse * normal.y + ccp.tangentImpulse * tangent.y); bodyA._angularVelocity -= invIA * (ccp.rA.x * P.y - ccp.rA.y * P.x); bodyA._linearVelocity.x -= invMassA * P.x; bodyA._linearVelocity.y -= invMassA * P.y; bodyB._angularVelocity += invIB * (ccp.rB.x * P.y - ccp.rB.y * P.x); bodyB._linearVelocity.x += invMassB * P.x; bodyB._linearVelocity.y += invMassB * P.y; #endif c.points[j] = ccp; } _constraints[i] = c; } }
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; } }
public void SolveVelocityConstraints() { for (int i = 0; i < _constraintCount; ++i) { ContactConstraint c = _constraints[i]; Body bodyA = c.bodyA; Body bodyB = c.bodyB; float wA = bodyA._angularVelocity; float wB = bodyB._angularVelocity; Vector2 vA = bodyA._linearVelocity; Vector2 vB = bodyB._linearVelocity; float invMassA = bodyA._invMass; float invIA = bodyA._invI; float invMassB = bodyB._invMass; float invIB = bodyB._invI; Vector2 normal = c.normal; #if MATH_OVERLOADS Vector2 tangent = zoomEngine.Physics.Common.Math.Cross(normal, 1.0f); #else Vector2 tangent = new Vector2(normal.y, -normal.x); #endif float friction = c.friction; //Debug.Assert(c.pointCount == 1 || c.pointCount == 2); // Solve tangent constraints for (int j = 0; j < c.pointCount; ++j) { ContactConstraintPoint ccp = c.points[j]; #if MATH_OVERLOADS // Relative velocity at contact Vector2 dv = vB + MathUtils.Cross(wB, ccp.rB) - vA - MathUtils.Cross(wA, ccp.rA); // Compute tangent force float vt = Vector2.Dot(dv, tangent); #else // Relative velocity at contact Vector2 dv = new Vector2(vB.x + (-wB * ccp.rB.y) - vA.x - (-wA * ccp.rA.y), vB.y + (wB * ccp.rB.x) - vA.y - (wA * ccp.rA.x)); // Compute tangent force float vt = dv.x * tangent.x + dv.y * tangent.y; #endif float lambda = ccp.tangentMass * (-vt); // MathUtils.Clamp the accumulated force float maxFriction = friction * ccp.normalImpulse; float newImpulse = MathUtils.Clamp(ccp.tangentImpulse + lambda, -maxFriction, maxFriction); lambda = newImpulse - ccp.tangentImpulse; #if MATH_OVERLOADS // Apply contact impulse Vector2 P = lambda * tangent; vA -= invMassA * P; wA -= invIA * MathUtils.Cross(ccp.rA, P); vB += invMassB * P; wB += invIB * MathUtils.Cross(ccp.rB, P); #else // Apply contact impulse Vector2 P = new Vector2(lambda * tangent.x, lambda * tangent.y); vA.x -= invMassA * P.x; vA.y -= invMassA * P.y; wA -= invIA * (ccp.rA.x * P.y - ccp.rA.y * P.x); vB.x += invMassB * P.x; vB.y += invMassB * P.y; wB += invIB * (ccp.rB.x * P.y - ccp.rB.y * P.x); #endif ccp.tangentImpulse = newImpulse; c.points[j] = ccp; } // Solve normal constraints if (c.pointCount == 1) { ContactConstraintPoint ccp = c.points[0]; #if MATH_OVERLOADS // Relative velocity at contact Vector2 dv = vB + MathUtils.Cross(wB, ccp.rB) - vA - MathUtils.Cross(wA, ccp.rA); // Compute normal impulse float vn = Vector2.Dot(dv, normal); float lambda = -ccp.normalMass * (vn - ccp.velocityBias); // MathUtils.Clamp the accumulated impulse float newImpulse = Math.Max(ccp.normalImpulse + lambda, 0.0f); lambda = newImpulse - ccp.normalImpulse; // Apply contact impulse Vector2 P = lambda * normal; vA -= invMassA * P; wA -= invIA * MathUtils.Cross(ccp.rA, P); vB += invMassB * P; wB += invIB * MathUtils.Cross(ccp.rB, P); #else // Relative velocity at contact Vector2 dv = new Vector2(vB.x + (-wB * ccp.rB.y) - vA.x - (-wA * ccp.rA.y), vB.y + (wB * ccp.rB.x) - vA.y - (wA * ccp.rA.x)); // Compute normal impulse float vn = dv.x * normal.x + dv.y * normal.y; float lambda = -ccp.normalMass * (vn - ccp.velocityBias); // Clamp the accumulated impulse float newImpulse = Math.Max(ccp.normalImpulse + lambda, 0.0f); lambda = newImpulse - ccp.normalImpulse; // Apply contact impulse var P = new Vector2(lambda * normal.x, lambda * normal.y); vA.x -= invMassA * P.x; vA.y -= invMassA * P.y; wA -= invIA * (ccp.rA.x * P.y - ccp.rA.y * P.x); vB.x += invMassB * P.x; vB.y += invMassB * P.y; wB += invIB * (ccp.rB.x * P.y - ccp.rB.y * P.x); #endif ccp.normalImpulse = newImpulse; c.points[0] = ccp; } else { // Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite). // Build the mini LCP for this contact patch // // vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2 // // A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n ) // b = vn_0 - velocityBias // // The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i // implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases // vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid // solution that satisfies the problem is chosen. // // In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires // that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i). // // Substitute: // // x = x' - a // // Plug into above equation: // // vn = A * x + b // = A * (x' - a) + b // = A * x' + b - A * a // = A * x' + b' // b' = b - A * a; ContactConstraintPoint cp1 = c.points[0]; ContactConstraintPoint cp2 = c.points[1]; Vector2 a = new Vector2(cp1.normalImpulse, cp2.normalImpulse); //Debug.Assert(a.x >= 0.0f && a.y >= 0.0f); #if MATH_OVERLOADS // Relative velocity at contact Vector2 dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA); Vector2 dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA); // Compute normal velocity float vn1 = Vector2.Dot(dv1, normal); float vn2 = Vector2.Dot(dv2, normal); Vector2 b = new Vector2(vn1 - cp1.velocityBias, vn2 - cp2.velocityBias); b -= MathUtils.Multiply(ref c.K, a); #else // Relative velocity at contact Vector2 dv1 = new Vector2(vB.x + (-wB * cp1.rB.y) - vA.x - (-wA * cp1.rA.y), vB.y + (wB * cp1.rB.x) - vA.y - (wA * cp1.rA.x)); Vector2 dv2 = new Vector2(vB.x + (-wB * cp2.rB.y) - vA.x - (-wA * cp2.rA.y), vB.y + (wB * cp2.rB.x) - vA.y - (wA * cp2.rA.x)); // Compute normal velocity float vn1 = dv1.x * normal.x + dv1.y * normal.y; float vn2 = dv2.x * normal.x + dv2.y * normal.y; Vector2 b = new Vector2(vn1 - cp1.velocityBias, vn2 - cp2.velocityBias); b -= MathUtils.Multiply(ref c.K, a); // Inlining didn't help for the multiply. #endif while (true) { // // Case 1: vn = 0 // // 0 = A * x' + b' // // Solve for x': // // x' = - inv(A) * b' // Vector2 x = -MathUtils.Multiply(ref c.normalMass, b); if (x.x >= 0.0f && x.y >= 0.0f) { #if MATH_OVERLOADS // Resubstitute for the incremental impulse Vector2 d = x - a; // Apply incremental impulse Vector2 P1 = d.x * normal; Vector2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -= invIA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2)); vB += invMassB * (P1 + P2); wB += invIB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2)); #else // Resubstitute for the incremental impulse Vector2 d = new Vector2(x.x - a.x, x.y - a.y); // Apply incremental impulse Vector2 P1 = new Vector2(d.x * normal.x, d.x * normal.y); Vector2 P2 = new Vector2(d.y * normal.x, d.y * normal.y); Vector2 P12 = new Vector2(P1.x + P2.x, P1.y + P2.y); vA.x -= invMassA * P12.x; vA.y -= invMassA * P12.y; wA -= invIA * ((cp1.rA.x * P1.y - cp1.rA.y * P1.x) + (cp2.rA.x * P2.y - cp2.rA.y * P2.x)); vB.x += invMassB * P12.x; vB.y += invMassB * P12.y; wB += invIB * ((cp1.rB.x * P1.y - cp1.rB.y * P1.x) + (cp2.rB.x * P2.y - cp2.rB.y * P2.x)); #endif // Accumulate cp1.normalImpulse = x.x; cp2.normalImpulse = x.y; #if B2_DEBUG_SOLVER float k_errorTol = 1e-3f; // Postconditions dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA); dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA); // Compute normal velocity vn1 = Vector2.Dot(dv1, normal); vn2 = Vector2.Dot(dv2, normal); //Debug.Assert(MathUtils.Abs(vn1 - cp1.velocityBias) < k_errorTol); //Debug.Assert(MathUtils.Abs(vn2 - cp2.velocityBias) < k_errorTol); #endif break; } // // Case 2: vn1 = 0 and x2 = 0 // // 0 = a11 * x1' + a12 * 0 + b1' // vn2 = a21 * x1' + a22 * 0 + b2' // x.x = -cp1.normalMass * b.x; x.y = 0.0f; vn1 = 0.0f; vn2 = c.K.col1.y * x.x + b.y; if (x.x >= 0.0f && vn2 >= 0.0f) { #if MATH_OVERLOADS // Resubstitute for the incremental impulse Vector2 d = x - a; // Apply incremental impulse Vector2 P1 = d.x * normal; Vector2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -= invIA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2)); vB += invMassB * (P1 + P2); wB += invIB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2)); #else // Resubstitute for the incremental impulse Vector2 d = new Vector2(x.x - a.x, x.y - a.y); // Apply incremental impulse Vector2 P1 = new Vector2(d.x * normal.x, d.x * normal.y); Vector2 P2 = new Vector2(d.y * normal.x, d.y * normal.y); Vector2 P12 = new Vector2(P1.x + P2.x, P1.y + P2.y); vA.x -= invMassA * P12.x; vA.y -= invMassA * P12.y; wA -= invIA * ((cp1.rA.x * P1.y - cp1.rA.y * P1.x) + (cp2.rA.x * P2.y - cp2.rA.y * P2.x)); vB.x += invMassB * P12.x; vB.y += invMassB * P12.y; wB += invIB * ((cp1.rB.x * P1.y - cp1.rB.y * P1.x) + (cp2.rB.x * P2.y - cp2.rB.y * P2.x)); #endif // Accumulate cp1.normalImpulse = x.x; cp2.normalImpulse = x.y; #if B2_DEBUG_SOLVER // Postconditions dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA); // Compute normal velocity vn1 = Vector2.Dot(dv1, normal); //Debug.Assert(MathUtils.Abs(vn1 - cp1.velocityBias) < k_errorTol); #endif break; } // // Case 3: vn2 = 0 and x1 = 0 // // vn1 = a11 * 0 + a12 * x2' + b1' // 0 = a21 * 0 + a22 * x2' + b2' // x.x = 0.0f; x.y = -cp2.normalMass * b.y; vn1 = c.K.col2.x * x.y + b.x; vn2 = 0.0f; if (x.y >= 0.0f && vn1 >= 0.0f) { #if MATH_OVERLOADS // Resubstitute for the incremental impulse Vector2 d = x - a; // Apply incremental impulse Vector2 P1 = d.x * normal; Vector2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -= invIA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2)); vB += invMassB * (P1 + P2); wB += invIB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2)); #else // Resubstitute for the incremental impulse Vector2 d = new Vector2(x.x - a.x, x.y - a.y); // Apply incremental impulse Vector2 P1 = new Vector2(d.x * normal.x, d.x * normal.y); Vector2 P2 = new Vector2(d.y * normal.x, d.y * normal.y); Vector2 P12 = new Vector2(P1.x + P2.x, P1.y + P2.y); vA.x -= invMassA * P12.x; vA.y -= invMassA * P12.y; wA -= invIA * ((cp1.rA.x * P1.y - cp1.rA.y * P1.x) + (cp2.rA.x * P2.y - cp2.rA.y * P2.x)); vB.x += invMassB * P12.x; vB.y += invMassB * P12.y; wB += invIB * ((cp1.rB.x * P1.y - cp1.rB.y * P1.x) + (cp2.rB.x * P2.y - cp2.rB.y * P2.x)); #endif // Accumulate cp1.normalImpulse = x.x; cp2.normalImpulse = x.y; #if B2_DEBUG_SOLVER // Postconditions dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA); // Compute normal velocity vn2 = Vector2.Dot(dv2, normal); //Debug.Assert(MathUtils.Abs(vn2 - cp2.velocityBias) < k_errorTol); #endif break; } // // Case 4: x1 = 0 and x2 = 0 // // vn1 = b1 // vn2 = b2; x.x = 0.0f; x.y = 0.0f; vn1 = b.x; vn2 = b.y; if (vn1 >= 0.0f && vn2 >= 0.0f) { #if MATH_OVERLOADS // Resubstitute for the incremental impulse Vector2 d = x - a; // Apply incremental impulse Vector2 P1 = d.x * normal; Vector2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -= invIA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2)); vB += invMassB * (P1 + P2); wB += invIB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2)); #else // Resubstitute for the incremental impulse Vector2 d = new Vector2(x.x - a.x, x.y - a.y); // Apply incremental impulse Vector2 P1 = new Vector2(d.x * normal.x, d.x * normal.y); Vector2 P2 = new Vector2(d.y * normal.x, d.y * normal.y); Vector2 P12 = new Vector2(P1.x + P2.x, P1.y + P2.y); vA.x -= invMassA * P12.x; vA.y -= invMassA * P12.y; wA -= invIA * ((cp1.rA.x * P1.y - cp1.rA.y * P1.x) + (cp2.rA.x * P2.y - cp2.rA.y * P2.x)); vB.x += invMassB * P12.x; vB.y += invMassB * P12.y; wB += invIB * ((cp1.rB.x * P1.y - cp1.rB.y * P1.x) + (cp2.rB.x * P2.y - cp2.rB.y * P2.x)); #endif // Accumulate cp1.normalImpulse = x.x; cp2.normalImpulse = x.y; break; } // No solution, give up. This is hit sometimes, but it doesn't seem to matter. break; } c.points[0] = cp1; c.points[1] = cp2; } _constraints[i] = c; bodyA._linearVelocity = vA; bodyA._angularVelocity = wA; bodyB._linearVelocity = vB; bodyB._angularVelocity = wB; } }