public void SolveVelocityConstraints() { // Here be dragons for (var i = 0; i < _contactCount; ++i) { var velocityConstraint = _velocityConstraints[i]; var indexA = velocityConstraint.IndexA; var indexB = velocityConstraint.IndexB; var mA = velocityConstraint.InvMassA; var iA = velocityConstraint.InvIA; var mB = velocityConstraint.InvMassB; var iB = velocityConstraint.InvIB; var pointCount = velocityConstraint.PointCount; var vA = _linearVelocities[indexA]; var wA = _angularVelocities[indexA]; var vB = _linearVelocities[indexB]; var wB = _angularVelocities[indexB]; var normal = velocityConstraint.Normal; var tangent = Vector2.Cross(normal, 1.0f); var friction = velocityConstraint.Friction; DebugTools.Assert(pointCount == 1 || pointCount == 2); // Solve tangent constraints first because non-penetration is more important // than friction. for (var j = 0; j < pointCount; ++j) { VelocityConstraintPoint velConstraintPoint = velocityConstraint.Points[j]; // Relative velocity at contact var dv = vB + Vector2.Cross(wB, velConstraintPoint.RelativeVelocityB) - vA - Vector2.Cross(wA, velConstraintPoint.RelativeVelocityA); // Compute tangent force float vt = Vector2.Dot(dv, tangent) - velocityConstraint.TangentSpeed; float lambda = velConstraintPoint.TangentMass * (-vt); // b2Clamp the accumulated force var maxFriction = friction * velConstraintPoint.NormalImpulse; var newImpulse = Math.Clamp(velConstraintPoint.TangentImpulse + lambda, -maxFriction, maxFriction); lambda = newImpulse - velConstraintPoint.TangentImpulse; velConstraintPoint.TangentImpulse = newImpulse; // Apply contact impulse Vector2 P = tangent * lambda; vA -= P * mA; wA -= iA * Vector2.Cross(velConstraintPoint.RelativeVelocityA, P); vB += P * mB; wB += iB * Vector2.Cross(velConstraintPoint.RelativeVelocityB, P); } // Solve normal constraints if (velocityConstraint.PointCount == 1) { VelocityConstraintPoint vcp = velocityConstraint.Points[0]; // Relative velocity at contact Vector2 dv = vB + Vector2.Cross(wB, vcp.RelativeVelocityB) - vA - Vector2.Cross(wA, vcp.RelativeVelocityA); // Compute normal impulse float vn = Vector2.Dot(dv, normal); float lambda = -vcp.NormalMass * (vn - vcp.VelocityBias); // b2Clamp the accumulated impulse float newImpulse = Math.Max(vcp.NormalImpulse + lambda, 0.0f); lambda = newImpulse - vcp.NormalImpulse; vcp.NormalImpulse = newImpulse; // Apply contact impulse Vector2 P = normal * lambda; vA -= P * mA; wA -= iA * Vector2.Cross(vcp.RelativeVelocityA, P); vB += P * mB; wB += iB * Vector2.Cross(vcp.RelativeVelocityB, P); } 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 = vn0 - 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 = a + d // // a := old total impulse // x := new total impulse // d := incremental impulse // // For the current iteration we extend the formula for the incremental impulse // to compute the new total impulse: // // vn = A * d + b // = A * (x - a) + b // = A * x + b - A * a // = A * x + b' // b' = b - A * a; VelocityConstraintPoint cp1 = velocityConstraint.Points[0]; VelocityConstraintPoint cp2 = velocityConstraint.Points[1]; Vector2 a = new Vector2(cp1.NormalImpulse, cp2.NormalImpulse); DebugTools.Assert(a.X >= 0.0f && a.Y >= 0.0f); // Relative velocity at contact Vector2 dv1 = vB + Vector2.Cross(wB, cp1.RelativeVelocityB) - vA - Vector2.Cross(wA, cp1.RelativeVelocityA); Vector2 dv2 = vB + Vector2.Cross(wB, cp2.RelativeVelocityB) - vA - Vector2.Cross(wA, cp2.RelativeVelocityA); // Compute normal velocity float vn1 = Vector2.Dot(dv1, normal); float vn2 = Vector2.Dot(dv2, normal); Vector2 b = new Vector2 { X = vn1 - cp1.VelocityBias, Y = vn2 - cp2.VelocityBias }; // Compute b' b -= Transform.Mul(velocityConstraint.K, a); //const float k_errorTol = 1e-3f; //B2_NOT_USED(k_errorTol); for (; ;) { // // Case 1: vn = 0 // // 0 = A * x + b' // // Solve for x: // // x = - inv(A) * b' // Vector2 x = -Transform.Mul(velocityConstraint.NormalMass, b); if (x.X >= 0.0f && x.Y >= 0.0f) { // Get the incremental impulse Vector2 d = x - a; // Apply incremental impulse Vector2 P1 = normal * d.X; Vector2 P2 = normal * d.Y; vA -= (P1 + P2) * mA; wA -= iA * (Vector2.Cross(cp1.RelativeVelocityA, P1) + Vector2.Cross(cp2.RelativeVelocityA, P2)); vB += (P1 + P2) * mB; wB += iB * (Vector2.Cross(cp1.RelativeVelocityB, P1) + Vector2.Cross(cp2.RelativeVelocityB, P2)); // Accumulate cp1.NormalImpulse = x.X; cp2.NormalImpulse = x.Y; 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 = velocityConstraint.K[0].Y * x.X + b.Y; if (x.X >= 0.0f && vn2 >= 0.0f) { // Get the incremental impulse Vector2 d = x - a; // Apply incremental impulse Vector2 P1 = normal * d.X; Vector2 P2 = normal * d.Y; vA -= (P1 + P2) * mA; wA -= iA * (Vector2.Cross(cp1.RelativeVelocityA, P1) + Vector2.Cross(cp2.RelativeVelocityA, P2)); vB += (P1 + P2) * mB; wB += iB * (Vector2.Cross(cp1.RelativeVelocityB, P1) + Vector2.Cross(cp2.RelativeVelocityB, P2)); // Accumulate cp1.NormalImpulse = x.X; cp2.NormalImpulse = x.Y; 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 = velocityConstraint.K[1].X * x.Y + b.X; vn2 = 0.0f; if (x.Y >= 0.0f && vn1 >= 0.0f) { // Resubstitute for the incremental impulse Vector2 d = x - a; // Apply incremental impulse Vector2 P1 = normal * d.X; Vector2 P2 = normal * d.Y; vA -= (P1 + P2) * mA; wA -= iA * (Vector2.Cross(cp1.RelativeVelocityA, P1) + Vector2.Cross(cp2.RelativeVelocityA, P2)); vB += (P1 + P2) * mB; wB += iB * (Vector2.Cross(cp1.RelativeVelocityB, P1) + Vector2.Cross(cp2.RelativeVelocityB, P2)); // Accumulate cp1.NormalImpulse = x.X; cp2.NormalImpulse = x.Y; 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) { // Resubstitute for the incremental impulse Vector2 d = x - a; // Apply incremental impulse Vector2 P1 = normal * d.X; Vector2 P2 = normal * d.Y; vA -= (P1 + P2) * mA; wA -= iA * (Vector2.Cross(cp1.RelativeVelocityA, P1) + Vector2.Cross(cp2.RelativeVelocityA, P2)); vB += (P1 + P2) * mB; wB += iB * (Vector2.Cross(cp1.RelativeVelocityB, P1) + Vector2.Cross(cp2.RelativeVelocityB, P2)); // 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; } } _linearVelocities[indexA] = vA; _angularVelocities[indexA] = wA; _linearVelocities[indexB] = vB; _angularVelocities[indexB] = wB; } }
public void InitializeVelocityConstraints() { Span <Vector2> points = stackalloc Vector2[2]; for (var i = 0; i < _contactCount; ++i) { var velocityConstraint = _velocityConstraints[i]; var positionConstraint = _positionConstraints[i]; var radiusA = positionConstraint.RadiusA; var radiusB = positionConstraint.RadiusB; var manifold = _contacts[velocityConstraint.ContactIndex].Manifold; var indexA = velocityConstraint.IndexA; var indexB = velocityConstraint.IndexB; var invMassA = velocityConstraint.InvMassA; var invMassB = velocityConstraint.InvMassB; var invIA = velocityConstraint.InvIA; var invIB = velocityConstraint.InvIB; var localCenterA = positionConstraint.LocalCenterA; var localCenterB = positionConstraint.LocalCenterB; var centerA = _positions[indexA]; var angleA = _angles[indexA]; var linVelocityA = _linearVelocities[indexA]; var angVelocityA = _angularVelocities[indexA]; var centerB = _positions[indexB]; var angleB = _angles[indexB]; var linVelocityB = _linearVelocities[indexB]; var angVelocityB = _angularVelocities[indexB]; DebugTools.Assert(manifold.PointCount > 0); Transform xfA = new Transform(angleA); Transform xfB = new Transform(angleB); xfA.Position = centerA - Transform.Mul(xfA.Quaternion2D, localCenterA); xfB.Position = centerB - Transform.Mul(xfB.Quaternion2D, localCenterB); Vector2 normal; InitializeManifold(ref manifold, xfA, xfB, radiusA, radiusB, out normal, points); velocityConstraint.Normal = normal; int pointCount = velocityConstraint.PointCount; for (int j = 0; j < pointCount; ++j) { VelocityConstraintPoint vcp = velocityConstraint.Points[j]; vcp.RelativeVelocityA = points[j] - centerA; vcp.RelativeVelocityB = points[j] - centerB; float rnA = Vector2.Cross(vcp.RelativeVelocityA, velocityConstraint.Normal); float rnB = Vector2.Cross(vcp.RelativeVelocityB, velocityConstraint.Normal); float kNormal = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB; vcp.NormalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; Vector2 tangent = Vector2.Cross(velocityConstraint.Normal, 1.0f); float rtA = Vector2.Cross(vcp.RelativeVelocityA, tangent); float rtB = Vector2.Cross(vcp.RelativeVelocityB, tangent); float kTangent = invMassA + invMassB + invIA * rtA * rtA + invIB * rtB * rtB; vcp.TangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f; // Setup a velocity bias for restitution. vcp.VelocityBias = 0.0f; float vRel = Vector2.Dot(velocityConstraint.Normal, linVelocityB + Vector2.Cross(angVelocityB, vcp.RelativeVelocityB) - linVelocityA - Vector2.Cross(angVelocityA, vcp.RelativeVelocityA)); if (vRel < -_velocityThreshold) { vcp.VelocityBias = -velocityConstraint.Restitution * vRel; } } // If we have two points, then prepare the block solver. if (velocityConstraint.PointCount == 2) { var vcp1 = velocityConstraint.Points[0]; var vcp2 = velocityConstraint.Points[1]; var rn1A = Vector2.Cross(vcp1.RelativeVelocityA, velocityConstraint.Normal); var rn1B = Vector2.Cross(vcp1.RelativeVelocityB, velocityConstraint.Normal); var rn2A = Vector2.Cross(vcp2.RelativeVelocityA, velocityConstraint.Normal); var rn2B = Vector2.Cross(vcp2.RelativeVelocityB, velocityConstraint.Normal); var k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B; var k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B; var k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B; // Ensure a reasonable condition number. const float k_maxConditionNumber = 1000.0f; if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) { // K is safe to invert. velocityConstraint.K[0] = new Vector2(k11, k12); velocityConstraint.K[1] = new Vector2(k12, k22); velocityConstraint.NormalMass = velocityConstraint.K.Inverse(); } else { // The constraints are redundant, just use one. // TODO_ERIN use deepest? velocityConstraint.PointCount = 1; } } } }