/* * Farseer Physics Engine based on Box2D.XNA port: * Copyright (c) 2010 Ian Qvist * * Box2D.XNA port of Box2D: * Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler * * Original source Box2D: * Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages * arising from the use of this software. * Permission is granted to anyone to use this software for any purpose, * including commercial applications, and to alter it and redistribute it * freely, subject to the following restrictions: * 1. The origin of this software must not be misrepresented; you must not * claim that you wrote the original software. If you use this software * in a product, an acknowledgment in the product documentation would be * appreciated but is not required. * 2. Altered source versions must be plainly marked as such, and must not be * misrepresented as being the original software. * 3. This notice may not be removed or altered from any source distribution. */ using System; using System.Diagnostics; using FarseerPhysics.Collision; using FarseerPhysics.Collision.Shapes; using FarseerPhysics.Common; using Microsoft.Xna.Framework; namespace FarseerPhysics.Dynamics.Contacts { public sealed class ContactConstraintPoint { public Vector2 LocalPoint; public float NormalImpulse; public float NormalMass; public float TangentImpulse; public float TangentMass; public float VelocityBias; public Vector2 rA; public Vector2 rB; } public sealed class ContactConstraint { public Body BodyA; public Body BodyB; public float Friction; public Mat22 K; public Vector2 LocalNormal; public Vector2 LocalPoint; public Manifold Manifold; public Vector2 Normal; public Mat22 NormalMass; public int PointCount; public ContactConstraintPoint[] Points = new ContactConstraintPoint[Settings.MaxPolygonVertices]; public float RadiusA; public float RadiusB; public float Restitution; public ManifoldType Type; public ContactConstraint() { for (int i = 0; i < Settings.MaxManifoldPoints; i++) { Points[i] = new ContactConstraintPoint(); } } } public class ContactSolver { public ContactConstraint[] Constraints; private int _constraintCount; // collection can be bigger. private Contact[] _contacts; 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 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 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; } } } } 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 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 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 bool SolvePositionConstraints(float baumgarte) { float minSeparation = 0.0f; for (int i = 0; i < _constraintCount; ++i) { ContactConstraint c = Constraints[i]; Body bodyA = c.BodyA; Body bodyB = c.BodyB; float invMassA = bodyA.Mass * bodyA.InvMass; float invIA = bodyA.Mass * bodyA.InvI; float invMassB = bodyB.Mass * bodyB.InvMass; float invIB = bodyB.Mass * bodyB.InvI; // Solve normal constraints for (int j = 0; j < c.PointCount; ++j) { Vector2 normal; Vector2 point; float separation; Solve(c, j, out normal, out point, out separation); float rax = point.X - bodyA.Sweep.C.X; float ray = point.Y - bodyA.Sweep.C.Y; float rbx = point.X - bodyB.Sweep.C.X; float rby = point.Y - bodyB.Sweep.C.Y; // Track max constraint error. minSeparation = Math.Min(minSeparation, separation); // Prevent large corrections and allow slop. float C = Math.Max(-Settings.MaxLinearCorrection, Math.Min(baumgarte * (separation + Settings.LinearSlop), 0.0f)); // Compute the effective mass. float rnA = rax * normal.Y - ray * normal.X; float rnB = rbx * normal.Y - rby * normal.X; float K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB; // Compute normal impulse float impulse = K > 0.0f ? -C / K : 0.0f; float px = impulse * normal.X; float py = impulse * normal.Y; bodyA.Sweep.C.X -= invMassA * px; bodyA.Sweep.C.Y -= invMassA * py; bodyA.Sweep.A -= invIA * (rax * py - ray * px); bodyB.Sweep.C.X += invMassB * px; bodyB.Sweep.C.Y += invMassB * py; bodyB.Sweep.A += invIB * (rbx * py - rby * px); bodyA.SynchronizeTransform(); bodyB.SynchronizeTransform(); } } // We can't expect minSpeparation >= -Settings.b2_linearSlop because we don't // push the separation above -Settings.b2_linearSlop. return minSeparation >= -1.5f * Settings.LinearSlop; } private static void Solve(ContactConstraint cc, int index, out Vector2 normal, out Vector2 point, out float separation) { Debug.Assert(cc.PointCount > 0); normal = Vector2.Zero; switch (cc.Type) { case ManifoldType.Circles: { Vector2 pointA = cc.BodyA.GetWorldPoint(ref cc.LocalPoint); Vector2 pointB = cc.BodyB.GetWorldPoint(ref cc.Points[0].LocalPoint); float a = (pointA.X - pointB.X) * (pointA.X - pointB.X) + (pointA.Y - pointB.Y) * (pointA.Y - pointB.Y); if (a > Settings.Epsilon * Settings.Epsilon) { Vector2 normalTmp = pointB - pointA; float factor = 1f / (float)Math.Sqrt(normalTmp.X * normalTmp.X + normalTmp.Y * normalTmp.Y); normal.X = normalTmp.X * factor; normal.Y = normalTmp.Y * factor; } else { normal.X = 1; normal.Y = 0; } point = 0.5f * (pointA + pointB); separation = (pointB.X - pointA.X) * normal.X + (pointB.Y - pointA.Y) * normal.Y - cc.RadiusA - cc.RadiusB; } break; case ManifoldType.FaceA: { normal = cc.BodyA.GetWorldVector(ref cc.LocalNormal); Vector2 planePoint = cc.BodyA.GetWorldPoint(ref cc.LocalPoint); Vector2 clipPoint = cc.BodyB.GetWorldPoint(ref cc.Points[index].LocalPoint); separation = (clipPoint.X - planePoint.X) * normal.X + (clipPoint.Y - planePoint.Y) * normal.Y - cc.RadiusA - cc.RadiusB; point = clipPoint; } break; case ManifoldType.FaceB: { normal = cc.BodyB.GetWorldVector(ref cc.LocalNormal); Vector2 planePoint = cc.BodyB.GetWorldPoint(ref cc.LocalPoint); Vector2 clipPoint = cc.BodyA.GetWorldPoint(ref cc.Points[index].LocalPoint); separation = (clipPoint.X - planePoint.X) * normal.X + (clipPoint.Y - planePoint.Y) * normal.Y - cc.RadiusA - cc.RadiusB; point = clipPoint; // Ensure normal points from A to B normal = -normal; } break; default: point = Vector2.Zero; separation = 0.0f; break; } } } }