public ContactConstraint() { for (int i = 0; i < Settings.MaxManifoldPoints; i++) { Points[i] = new ContactConstraintPoint(); } }
public void StoreImpulses() { for (int i = 0; i < _constraintCount; ++i) { ContactConstraint c = Constraints[i]; Manifold m = c.Manifold; for (int j = 0; j < c.PointCount; ++j) { ManifoldPoint pj = m.Points[j]; ContactConstraintPoint cp = c.Points[j]; pj.NormalImpulse = cp.NormalImpulse; pj.TangentImpulse = cp.TangentImpulse; m.Points[j] = pj; } c.Manifold = m; _contacts[i].Manifold = m; } }
public void WarmStart() { // Warm start. for (int i = 0; i < _constraintCount; ++i) { ContactConstraint c = Constraints[i]; float tangentx = c.Normal.Y; float tangenty = -c.Normal.X; for (int j = 0; j < c.PointCount; ++j) { ContactConstraintPoint ccp = c.Points[j]; float px = ccp.NormalImpulse * c.Normal.X + ccp.TangentImpulse * tangentx; float py = ccp.NormalImpulse * c.Normal.Y + ccp.TangentImpulse * tangenty; c.BodyA.AngularVelocityInternal -= c.BodyA.InvI * (ccp.rA.X * py - ccp.rA.Y * px); c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * px; c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * py; c.BodyB.AngularVelocityInternal += c.BodyB.InvI * (ccp.rB.X * py - ccp.rB.Y * px); c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * px; c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * py; } } }
public void Reset(Contact[] contacts, int contactCount, float impulseRatio, bool warmstarting) { _contacts = contacts; _constraintCount = contactCount; // grow the array if (Constraints == null || Constraints.Length < _constraintCount) { Constraints = new ContactConstraint[_constraintCount * 2]; for (int i = 0; i < Constraints.Length; i++) { Constraints[i] = new ContactConstraint(); } } // Initialize position independent portions of the constraints. for (int i = 0; i < _constraintCount; ++i) { Contact contact = contacts[i]; Fixture fixtureA = contact.FixtureA; Fixture fixtureB = contact.FixtureB; Shape shapeA = fixtureA.Shape; Shape shapeB = fixtureB.Shape; float radiusA = shapeA.Radius; float radiusB = shapeB.Radius; Body bodyA = fixtureA.Body; Body bodyB = fixtureB.Body; Manifold manifold = contact.Manifold; Debug.Assert(manifold.PointCount > 0); ContactConstraint cc = Constraints[i]; cc.Friction = Settings.MixFriction(fixtureA.Friction, fixtureB.Friction); cc.Restitution = Settings.MixRestitution(fixtureA.Restitution, fixtureB.Restitution); cc.BodyA = bodyA; cc.BodyB = bodyB; cc.Manifold = manifold; cc.Normal = Vector2.Zero; cc.PointCount = manifold.PointCount; cc.LocalNormal = manifold.LocalNormal; cc.LocalPoint = manifold.LocalPoint; cc.RadiusA = radiusA; cc.RadiusB = radiusB; cc.Type = manifold.Type; for (int j = 0; j < cc.PointCount; ++j) { ManifoldPoint cp = manifold.Points[j]; ContactConstraintPoint ccp = cc.Points[j]; if (warmstarting) { ccp.NormalImpulse = impulseRatio * cp.NormalImpulse; ccp.TangentImpulse = impulseRatio * cp.TangentImpulse; } else { ccp.NormalImpulse = 0.0f; ccp.TangentImpulse = 0.0f; } ccp.LocalPoint = cp.LocalPoint; ccp.rA = Vector2.Zero; ccp.rB = Vector2.Zero; ccp.NormalMass = 0.0f; ccp.TangentMass = 0.0f; ccp.VelocityBias = 0.0f; } cc.K.SetZero(); cc.NormalMass.SetZero(); } }
public void SolveVelocityConstraints() { for (int i = 0; i < _constraintCount; ++i) { ContactConstraint c = Constraints[i]; float wA = c.BodyA.AngularVelocityInternal; float wB = c.BodyB.AngularVelocityInternal; float tangentx = c.Normal.Y; float tangenty = -c.Normal.X; 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]; float lambda = ccp.TangentMass * -((c.BodyB.LinearVelocityInternal.X + (-wB * ccp.rB.Y) - c.BodyA.LinearVelocityInternal.X - (-wA * ccp.rA.Y)) * tangentx + (c.BodyB.LinearVelocityInternal.Y + (wB * ccp.rB.X) - c.BodyA.LinearVelocityInternal.Y - (wA * ccp.rA.X)) * tangenty); // MathUtils.Clamp the accumulated force float maxFriction = friction * ccp.NormalImpulse; float newImpulse = Math.Max(-maxFriction, Math.Min(ccp.TangentImpulse + lambda, maxFriction)); lambda = newImpulse - ccp.TangentImpulse; // Apply contact impulse float px = lambda * tangentx; float py = lambda * tangenty; c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * px; c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * py; wA -= c.BodyA.InvI * (ccp.rA.X * py - ccp.rA.Y * px); c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * px; c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * py; wB += c.BodyB.InvI * (ccp.rB.X * py - ccp.rB.Y * px); ccp.TangentImpulse = newImpulse; } // Solve normal constraints if (c.PointCount == 1) { ContactConstraintPoint ccp = c.Points[0]; // Relative velocity at contact // Compute normal impulse float lambda = -ccp.NormalMass * ((c.BodyB.LinearVelocityInternal.X + (-wB * ccp.rB.Y) - c.BodyA.LinearVelocityInternal.X - (-wA * ccp.rA.Y)) * c.Normal.X + (c.BodyB.LinearVelocityInternal.Y + (wB * ccp.rB.X) - c.BodyA.LinearVelocityInternal.Y - (wA * ccp.rA.X)) * c.Normal.Y - ccp.VelocityBias); // Clamp the accumulated impulse float newImpulse = Math.Max(ccp.NormalImpulse + lambda, 0.0f); lambda = newImpulse - ccp.NormalImpulse; // Apply contact impulse float px = lambda * c.Normal.X; float py = lambda * c.Normal.Y; c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * px; c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * py; wA -= c.BodyA.InvI * (ccp.rA.X * py - ccp.rA.Y * px); c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * px; c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * py; wB += c.BodyB.InvI * (ccp.rB.X * py - ccp.rB.Y * px); ccp.NormalImpulse = newImpulse; } 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]; float ax = cp1.NormalImpulse; float ay = cp2.NormalImpulse; Debug.Assert(ax >= 0.0f && ay >= 0.0f); // Relative velocity at contact // Compute normal velocity float vn1 = (c.BodyB.LinearVelocityInternal.X + (-wB * cp1.rB.Y) - c.BodyA.LinearVelocityInternal.X - (-wA * cp1.rA.Y)) * c.Normal.X + (c.BodyB.LinearVelocityInternal.Y + (wB * cp1.rB.X) - c.BodyA.LinearVelocityInternal.Y - (wA * cp1.rA.X)) * c.Normal.Y; float vn2 = (c.BodyB.LinearVelocityInternal.X + (-wB * cp2.rB.Y) - c.BodyA.LinearVelocityInternal.X - (-wA * cp2.rA.Y)) * c.Normal.X + (c.BodyB.LinearVelocityInternal.Y + (wB * cp2.rB.X) - c.BodyA.LinearVelocityInternal.Y - (wA * cp2.rA.X)) * c.Normal.Y; float bx = vn1 - cp1.VelocityBias - (c.K.Col1.X * ax + c.K.Col2.X * ay); float by = vn2 - cp2.VelocityBias - (c.K.Col1.Y * ax + c.K.Col2.Y * ay); float xx = -(c.NormalMass.Col1.X * bx + c.NormalMass.Col2.X * by); float xy = -(c.NormalMass.Col1.Y * bx + c.NormalMass.Col2.Y * by); while (true) { // // Case 1: vn = 0 // // 0 = A * x' + b' // // Solve for x': // // x' = - inv(A) * b' // if (xx >= 0.0f && xy >= 0.0f) { // Resubstitute for the incremental impulse float dx = xx - ax; float dy = xy - ay; // Apply incremental impulse float p1x = dx * c.Normal.X; float p1y = dx * c.Normal.Y; float p2x = dy * c.Normal.X; float p2y = dy * c.Normal.Y; float p12x = p1x + p2x; float p12y = p1y + p2y; c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * p12x; c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * p12y; wA -= c.BodyA.InvI * ((cp1.rA.X * p1y - cp1.rA.Y * p1x) + (cp2.rA.X * p2y - cp2.rA.Y * p2x)); c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * p12x; c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * p12y; wB += c.BodyB.InvI * ((cp1.rB.X * p1y - cp1.rB.Y * p1x) + (cp2.rB.X * p2y - cp2.rB.Y * p2x)); // Accumulate cp1.NormalImpulse = xx; cp2.NormalImpulse = xy; #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' // xx = -cp1.NormalMass * bx; xy = 0.0f; vn1 = 0.0f; vn2 = c.K.Col1.Y * xx + by; if (xx >= 0.0f && vn2 >= 0.0f) { // Resubstitute for the incremental impulse float dx = xx - ax; float dy = xy - ay; // Apply incremental impulse float p1x = dx * c.Normal.X; float p1y = dx * c.Normal.Y; float p2x = dy * c.Normal.X; float p2y = dy * c.Normal.Y; float p12x = p1x + p2x; float p12y = p1y + p2y; c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * p12x; c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * p12y; wA -= c.BodyA.InvI * ((cp1.rA.X * p1y - cp1.rA.Y * p1x) + (cp2.rA.X * p2y - cp2.rA.Y * p2x)); c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * p12x; c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * p12y; wB += c.BodyB.InvI * ((cp1.rB.X * p1y - cp1.rB.Y * p1x) + (cp2.rB.X * p2y - cp2.rB.Y * p2x)); // Accumulate cp1.NormalImpulse = xx; cp2.NormalImpulse = xy; #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' // xx = 0.0f; xy = -cp2.NormalMass * by; vn1 = c.K.Col2.X * xy + bx; vn2 = 0.0f; if (xy >= 0.0f && vn1 >= 0.0f) { // Resubstitute for the incremental impulse float dx = xx - ax; float dy = xy - ay; // Apply incremental impulse float p1x = dx * c.Normal.X; float p1y = dx * c.Normal.Y; float p2x = dy * c.Normal.X; float p2y = dy * c.Normal.Y; float p12x = p1x + p2x; float p12y = p1y + p2y; c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * p12x; c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * p12y; wA -= c.BodyA.InvI * ((cp1.rA.X * p1y - cp1.rA.Y * p1x) + (cp2.rA.X * p2y - cp2.rA.Y * p2x)); c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * p12x; c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * p12y; wB += c.BodyB.InvI * ((cp1.rB.X * p1y - cp1.rB.Y * p1x) + (cp2.rB.X * p2y - cp2.rB.Y * p2x)); // Accumulate cp1.NormalImpulse = xx; cp2.NormalImpulse = xy; #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; xx = 0.0f; xy = 0.0f; vn1 = bx; vn2 = by; if (vn1 >= 0.0f && vn2 >= 0.0f) { // Resubstitute for the incremental impulse float dx = xx - ax; float dy = xy - ay; // Apply incremental impulse float p1x = dx * c.Normal.X; float p1y = dx * c.Normal.Y; float p2x = dy * c.Normal.X; float p2y = dy * c.Normal.Y; float p12x = p1x + p2x; float p12y = p1y + p2y; c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * p12x; c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * p12y; wA -= c.BodyA.InvI * ((cp1.rA.X * p1y - cp1.rA.Y * p1x) + (cp2.rA.X * p2y - cp2.rA.Y * p2x)); c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * p12x; c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * p12y; wB += c.BodyB.InvI * ((cp1.rB.X * p1y - cp1.rB.Y * p1x) + (cp2.rB.X * p2y - cp2.rB.Y * p2x)); // Accumulate cp1.NormalImpulse = xx; cp2.NormalImpulse = xy; break; } // No solution, give up. This is hit sometimes, but it doesn't seem to matter. break; } } c.BodyA.AngularVelocityInternal = wA; c.BodyB.AngularVelocityInternal = wB; } }
public void InitializeVelocityConstraints() { for (int i = 0; i < _constraintCount; ++i) { ContactConstraint cc = Constraints[i]; float radiusA = cc.RadiusA; float radiusB = cc.RadiusB; Body bodyA = cc.BodyA; Body bodyB = cc.BodyB; Manifold manifold = cc.Manifold; Vector2 vA = bodyA.LinearVelocity; Vector2 vB = bodyB.LinearVelocity; float wA = bodyA.AngularVelocity; float wB = bodyB.AngularVelocity; Debug.Assert(manifold.PointCount > 0); FixedArray2 <Vector2> points; Collision.Collision.GetWorldManifold(ref manifold, ref bodyA.Xf, radiusA, ref bodyB.Xf, radiusB, out cc.Normal, out points); Vector2 tangent = new Vector2(cc.Normal.Y, -cc.Normal.X); for (int j = 0; j < cc.PointCount; ++j) { ContactConstraintPoint ccp = cc.Points[j]; ccp.rA = points[j] - bodyA.Sweep.C; ccp.rB = points[j] - bodyB.Sweep.C; 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; rnA *= rnA; rnB *= rnB; float kNormal = bodyA.InvMass + bodyB.InvMass + bodyA.InvI * rnA + bodyB.InvI * rnB; Debug.Assert(kNormal > Settings.Epsilon); ccp.NormalMass = 1.0f / kNormal; float rtA = ccp.rA.X * tangent.Y - ccp.rA.Y * tangent.X; float rtB = ccp.rB.X * tangent.Y - ccp.rB.Y * tangent.X; rtA *= rtA; rtB *= rtB; float kTangent = bodyA.InvMass + bodyB.InvMass + bodyA.InvI * rtA + bodyB.InvI * rtB; Debug.Assert(kTangent > Settings.Epsilon); ccp.TangentMass = 1.0f / kTangent; // Setup a velocity bias for restitution. ccp.VelocityBias = 0.0f; float vRel = cc.Normal.X * (vB.X + -wB * ccp.rB.Y - vA.X - -wA * ccp.rA.Y) + cc.Normal.Y * (vB.Y + wB * ccp.rB.X - vA.Y - wA * ccp.rA.X); if (vRel < -Settings.VelocityThreshold) { ccp.VelocityBias = -cc.Restitution * vRel; } } // 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 = ccp1.rA.X * cc.Normal.Y - ccp1.rA.Y * cc.Normal.X; float rn1B = ccp1.rB.X * cc.Normal.Y - ccp1.rB.Y * cc.Normal.X; float rn2A = ccp2.rA.X * cc.Normal.Y - ccp2.rA.Y * cc.Normal.X; float rn2B = ccp2.rB.X * cc.Normal.Y - ccp2.rB.Y * cc.Normal.X; 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.Col1.X = k11; cc.K.Col1.Y = k12; cc.K.Col2.X = k12; cc.K.Col2.Y = k22; float a = cc.K.Col1.X, b = cc.K.Col2.X, c = cc.K.Col1.Y, d = cc.K.Col2.Y; float det = a * d - b * c; if (det != 0.0f) { det = 1.0f / det; } cc.NormalMass.Col1.X = det * d; cc.NormalMass.Col1.Y = -det * c; cc.NormalMass.Col2.X = -det * b; cc.NormalMass.Col2.Y = det * a; } else { // The constraints are redundant, just use one. // TODO_ERIN use deepest? cc.PointCount = 1; } } } }