public ContactSolver() { PositionConstraints = new ContactPositionConstraint[INITIAL_NUM_CONSTRAINTS]; VelocityConstraints = new ContactVelocityConstraint[INITIAL_NUM_CONSTRAINTS]; for (int i = 0; i < INITIAL_NUM_CONSTRAINTS; i++) { PositionConstraints[i] = new ContactPositionConstraint(); VelocityConstraints[i] = new ContactVelocityConstraint(); } }
public void StoreImpulses() { for (int i = 0; i < Count; i++) { ContactVelocityConstraint vc = VelocityConstraints[i]; Manifold manifold = Contacts[vc.ContactIndex].Manifold; for (int j = 0; j < vc.PointCount; j++) { manifold.Points[j].NormalImpulse = vc.Points[j].NormalImpulse; manifold.Points[j].TangentImpulse = vc.Points[j].TangentImpulse; } } }
public void WarmStart() { // Warm start. for (int i = 0; i < Count; ++i) { ContactVelocityConstraint vc = VelocityConstraints[i]; int indexA = vc.IndexA; int indexB = vc.IndexB; float mA = vc.InvMassA; float iA = vc.InvIA; float mB = vc.InvMassB; float iB = vc.InvIB; int pointCount = vc.PointCount; Vec2 vA = Velocities[indexA].V; float wA = Velocities[indexA].W; Vec2 vB = Velocities[indexB].V; float wB = Velocities[indexB].W; Vec2 normal = vc.Normal; Vec2.CrossToOutUnsafe(normal, 1.0f, tangent); for (int j = 0; j < pointCount; ++j) { ContactVelocityConstraint.VelocityConstraintPoint vcp = vc.Points[j]; //Console.WriteLine("vcp normal impulse is " + vcp.normalImpulse); temp.Set(normal).MulLocal(vcp.NormalImpulse); P.Set(tangent).MulLocal(vcp.TangentImpulse).AddLocal(temp); wA -= iA * Vec2.Cross(vcp.RA, P); vA.SubLocal(temp.Set(P).MulLocal(mA)); //Debug.Assert(vA.x == 0); //Debug.Assert(wA == 0); wB += iB * Vec2.Cross(vcp.RB, P); vB.AddLocal(temp.Set(P).MulLocal(mB)); //Debug.Assert(vB.x == 0); //Debug.Assert(wB == 0); } Velocities[indexA].W = wA; Velocities[indexB].W = wB; //Console.WriteLine("Ending velocity for " + indexA + " is " + vA.x + "," + vA.y + " - " + wA); //Console.WriteLine("Ending velocity for " + indexB + " is " + vB.x + "," + vB.y + " - " + wB); } }
public void Init(ContactSolverDef def) { //Console.WriteLine("Initializing contact solver"); Step = def.Step; Count = def.Count; if (PositionConstraints.Length < Count) { ContactPositionConstraint[] old = PositionConstraints; PositionConstraints = new ContactPositionConstraint[MathUtils.Max(old.Length * 2, Count)]; Array.Copy(old, 0, PositionConstraints, 0, old.Length); for (int i = old.Length; i < PositionConstraints.Length; i++) { PositionConstraints[i] = new ContactPositionConstraint(); } } if (VelocityConstraints.Length < Count) { ContactVelocityConstraint[] old = VelocityConstraints; VelocityConstraints = new ContactVelocityConstraint[MathUtils.Max(old.Length * 2, Count)]; Array.Copy(old, 0, VelocityConstraints, 0, old.Length); for (int i = old.Length; i < VelocityConstraints.Length; i++) { VelocityConstraints[i] = new ContactVelocityConstraint(); } } Positions = def.Positions; Velocities = def.Velocities; Contacts = def.Contacts; for (int i = 0; i < Count; ++i) { //Console.WriteLine("contacts: " + m_count); 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; int pointCount = manifold.PointCount; Debug.Assert(pointCount > 0); ContactVelocityConstraint vc = VelocityConstraints[i]; vc.Friction = contact.Friction; vc.Restitution = contact.Restitution; vc.TangentSpeed = contact.TangentSpeed; vc.IndexA = bodyA.IslandIndex; vc.IndexB = bodyB.IslandIndex; vc.InvMassA = bodyA.InvMass; vc.InvMassB = bodyB.InvMass; vc.InvIA = bodyA.InvI; vc.InvIB = bodyB.InvI; vc.ContactIndex = i; vc.PointCount = pointCount; vc.K.SetZero(); vc.NormalMass.SetZero(); ContactPositionConstraint pc = PositionConstraints[i]; pc.IndexA = bodyA.IslandIndex; pc.IndexB = bodyB.IslandIndex; pc.InvMassA = bodyA.InvMass; pc.InvMassB = bodyB.InvMass; pc.LocalCenterA.Set(bodyA.Sweep.LocalCenter); pc.LocalCenterB.Set(bodyB.Sweep.LocalCenter); pc.InvIA = bodyA.InvI; pc.InvIB = bodyB.InvI; pc.LocalNormal.Set(manifold.LocalNormal); pc.LocalPoint.Set(manifold.LocalPoint); pc.PointCount = pointCount; pc.RadiusA = radiusA; pc.RadiusB = radiusB; pc.Type = manifold.Type; //Console.WriteLine("contact point count: " + pointCount); for (int j = 0; j < pointCount; j++) { ManifoldPoint cp = manifold.Points[j]; ContactVelocityConstraint.VelocityConstraintPoint vcp = vc.Points[j]; if (Step.WarmStarting) { //Debug.Assert(cp.normalImpulse == 0); //Console.WriteLine("contact normal impulse: " + cp.normalImpulse); vcp.NormalImpulse = Step.DtRatio * cp.NormalImpulse; vcp.TangentImpulse = Step.DtRatio * cp.TangentImpulse; } else { vcp.NormalImpulse = 0; vcp.TangentImpulse = 0; } vcp.RA.SetZero(); vcp.RB.SetZero(); vcp.NormalMass = 0; vcp.TangentMass = 0; vcp.VelocityBias = 0; pc.LocalPoints[j].Set(cp.LocalPoint); } } }
public void SolveVelocityConstraints() { for (int i = 0; i < Count; ++i) { ContactVelocityConstraint vc = VelocityConstraints[i]; int indexA = vc.IndexA; int indexB = vc.IndexB; float mA = vc.InvMassA; float mB = vc.InvMassB; float iA = vc.InvIA; float iB = vc.InvIB; int pointCount = vc.PointCount; Vec2 vA = Velocities[indexA].V; float wA = Velocities[indexA].W; Vec2 vB = Velocities[indexB].V; float wB = Velocities[indexB].W; //Debug.Assert(wA == 0); //Debug.Assert(wB == 0); Vec2 normal = vc.Normal; //Vec2.crossToOutUnsafe(normal, 1f, tangent); tangent.X = 1.0f * vc.Normal.Y; tangent.Y = (-1.0f) * vc.Normal.X; float friction = vc.Friction; Debug.Assert(pointCount == 1 || pointCount == 2); // Solve tangent constraints for (int j = 0; j < pointCount; ++j) { ContactVelocityConstraint.VelocityConstraintPoint vcp = vc.Points[j]; //Vec2.crossToOutUnsafe(wA, vcp.rA, temp); //Vec2.crossToOutUnsafe(wB, vcp.rB, dv); //dv.addLocal(vB).subLocal(vA).subLocal(temp); Vec2 a = vcp.RA; dv.X = (-wB) * vcp.RB.Y + vB.X - vA.X + wA * a.Y; dv.Y = wB * vcp.RB.X + vB.Y - vA.Y - wA * a.X; // Compute tangent force float vt = dv.X * tangent.X + dv.Y * tangent.Y - vc.TangentSpeed; float lambda = vcp.TangentMass * (-vt); // Clamp the accumulated force float maxFriction = friction * vcp.NormalImpulse; float newImpulse = MathUtils.Clamp(vcp.TangentImpulse + lambda, -maxFriction, maxFriction); lambda = newImpulse - vcp.TangentImpulse; vcp.TangentImpulse = newImpulse; // Apply contact impulse // Vec2 P = lambda * tangent; float Px = tangent.X * lambda; float Py = tangent.Y * lambda; // vA -= invMassA * P; vA.X -= Px * mA; vA.Y -= Py * mA; wA -= iA * (vcp.RA.X * Py - vcp.RA.Y * Px); // vB += invMassB * P; vB.X += Px * mB; vB.Y += Py * mB; wB += iB * (vcp.RB.X * Py - vcp.RB.Y * Px); //Console.WriteLine("tangent solve velocity (point "+j+") for " + indexA + " is " + vA.x + "," + vA.y + " rot " + wA); //Console.WriteLine("tangent solve velocity (point "+j+") for " + indexB + " is " + vB.x + "," + vB.y + " rot " + wB); } // Solve normal constraints if (vc.PointCount == 1) { ContactVelocityConstraint.VelocityConstraintPoint vcp = vc.Points[0]; Vec2 a1 = vcp.RA; // Relative velocity at contact //Vec2 dv = vB + Cross(wB, vcp.rB) - vA - Cross(wA, vcp.rA); //Vec2.crossToOut(wA, vcp.rA, temp1); //Vec2.crossToOut(wB, vcp.rB, dv); //dv.addLocal(vB).subLocal(vA).subLocal(temp1); dv.X = (-wB) * vcp.RB.Y + vB.X - vA.X + wA * a1.Y; dv.Y = wB * vcp.RB.X + vB.Y - vA.Y - wA * a1.X; // Compute normal impulse float vn = dv.X * normal.X + dv.Y * normal.Y; float lambda = (-vcp.NormalMass) * (vn - vcp.VelocityBias); // Clamp the accumulated impulse float a = vcp.NormalImpulse + lambda; float newImpulse = (a > 0.0f ? a : 0.0f); lambda = newImpulse - vcp.NormalImpulse; //Debug.Assert(newImpulse == 0); vcp.NormalImpulse = newImpulse; // Apply contact impulse float Px = normal.X * lambda; float Py = normal.Y * lambda; // vA -= invMassA * P; vA.X -= Px * mA; vA.Y -= Py * mA; wA -= iA * (vcp.RA.X * Py - vcp.RA.Y * Px); //Debug.Assert(vA.x == 0); // vB += invMassB * P; vB.X += Px * mB; vB.Y += Py * mB; wB += iB * (vcp.RB.X * Py - vcp.RB.Y * Px); //Debug.Assert(vB.x == 0); } 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 = 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; ContactVelocityConstraint.VelocityConstraintPoint cp1 = vc.Points[0]; ContactVelocityConstraint.VelocityConstraintPoint cp2 = vc.Points[1]; a.X = cp1.NormalImpulse; a.Y = cp2.NormalImpulse; Debug.Assert(a.X >= 0.0f && a.Y >= 0.0f); // Relative velocity at contact // Vec2 dv1 = vB + Cross(wB, cp1.rB) - vA - Cross(wA, cp1.rA); dv1.X = (-wB) * cp1.RB.Y + vB.X - vA.X + wA * cp1.RA.Y; dv1.Y = wB * cp1.RB.X + vB.Y - vA.Y - wA * cp1.RA.X; // Vec2 dv2 = vB + Cross(wB, cp2.rB) - vA - Cross(wA, cp2.rA); dv2.X = (-wB) * cp2.RB.Y + vB.X - vA.X + wA * cp2.RA.Y; dv2.Y = wB * cp2.RB.X + vB.Y - 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; b.X = vn1 - cp1.VelocityBias; b.Y = vn2 - cp2.VelocityBias; //Console.WriteLine("b is " + b.x + "," + b.y); // Compute b' Mat22 R = vc.K; b.X -= (R.Ex.X * a.X + R.Ey.X * a.Y); b.Y -= (R.Ex.Y * a.X + R.Ey.Y * a.Y); //Console.WriteLine("b' is " + b.x + "," + b.y); // final 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' // // Vec2 x = - Mul(c.normalMass, b); Mat22.MulToOutUnsafe(vc.NormalMass, b, x); x.MulLocal(-1); if (x.X >= 0.0f && x.Y >= 0.0f) { //Console.WriteLine("case 1"); // Get the incremental impulse // Vec2 d = x - a; d.Set(x).SubLocal(a); // Apply incremental impulse // Vec2 P1 = d.x * normal; // Vec2 P2 = d.y * normal; P1.Set(normal).MulLocal(d.X); P2.Set(normal).MulLocal(d.Y); /* * vA -= invMassA * (P1 + P2); wA -= invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2)); * * vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2)); */ temp1.Set(P1).AddLocal(P2); temp2.Set(temp1).MulLocal(mA); vA.SubLocal(temp2); temp2.Set(temp1).MulLocal(mB); vB.AddLocal(temp2); //Debug.Assert(vA.x == 0); //Debug.Assert(vB.x == 0); wA -= iA * (Vec2.Cross(cp1.RA, P1) + Vec2.Cross(cp2.RA, P2)); wB += iB * (Vec2.Cross(cp1.RB, P1) + Vec2.Cross(cp2.RB, P2)); // Accumulate cp1.NormalImpulse = x.X; cp2.NormalImpulse = x.Y; /* * #if B2_DEBUG_SOLVER == 1 // Postconditions dv1 = vB + Cross(wB, cp1.rB) - vA - * Cross(wA, cp1.rA); dv2 = vB + Cross(wB, cp2.rB) - vA - Cross(wA, cp2.rA); * * // Compute normal velocity vn1 = Dot(dv1, normal); vn2 = Dot(dv2, normal); * * Debug.Assert(Abs(vn1 - cp1.velocityBias) < k_errorTol); Debug.Assert(Abs(vn2 - cp2.velocityBias) * < k_errorTol); #endif */ if (DEBUG_SOLVER) { // Postconditions Vec2 _dv1 = vB.Add(Vec2.Cross(wB, cp1.RB).SubLocal(vA).SubLocal(Vec2.Cross(wA, cp1.RA))); Vec2 _dv2 = vB.Add(Vec2.Cross(wB, cp2.RB).SubLocal(vA).SubLocal(Vec2.Cross(wA, cp2.RA))); // Compute normal velocity vn1 = Vec2.Dot(_dv1, normal); vn2 = Vec2.Dot(_dv2, normal); Debug.Assert(MathUtils.Abs(vn1 - cp1.VelocityBias) < ERROR_TO_I); Debug.Assert(MathUtils.Abs(vn2 - cp2.VelocityBias) < ERROR_TO_I); } break; } // // Case 2: vn1 = 0 and x2 = 0 // // 0 = a11 * x1' + a12 * 0 + b1' // vn2 = a21 * x1' + a22 * 0 + ' // x.X = (-cp1.NormalMass) * b.X; x.Y = 0.0f; vn1 = 0.0f; vn2 = vc.K.Ex.Y * x.X + b.Y; if (x.X >= 0.0f && vn2 >= 0.0f) { //Console.WriteLine("case 2"); // Get the incremental impulse d.Set(x).SubLocal(a); // Apply incremental impulse // Vec2 P1 = d.x * normal; // Vec2 P2 = d.y * normal; P1.Set(normal).MulLocal(d.X); P2.Set(normal).MulLocal(d.Y); /* * Vec2 P1 = d.x * normal; Vec2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -= * invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2)); * * vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2)); */ temp1.Set(P1).AddLocal(P2); temp2.Set(temp1).MulLocal(mA); vA.SubLocal(temp2); temp2.Set(temp1).MulLocal(mB); vB.AddLocal(temp2); //Debug.Assert(vA.x == 0); //Debug.Assert(vB.x == 0); wA -= iA * (Vec2.Cross(cp1.RA, P1) + Vec2.Cross(cp2.RA, P2)); wB += iB * (Vec2.Cross(cp1.RB, P1) + Vec2.Cross(cp2.RB, P2)); // Accumulate //Debug.Assert(x.x == 0 && x.y == 0); cp1.NormalImpulse = x.X; cp2.NormalImpulse = x.Y; /* * #if B2_DEBUG_SOLVER == 1 // Postconditions dv1 = vB + Cross(wB, cp1.rB) - vA - * Cross(wA, cp1.rA); * * // Compute normal velocity vn1 = Dot(dv1, normal); * * Debug.Assert(Abs(vn1 - cp1.velocityBias) < k_errorTol); #endif */ if (DEBUG_SOLVER) { // Postconditions Vec2 _dv1 = vB.Add(Vec2.Cross(wB, cp1.RB).SubLocal(vA).SubLocal(Vec2.Cross(wA, cp1.RA))); // Compute normal velocity vn1 = Vec2.Dot(_dv1, normal); Debug.Assert(MathUtils.Abs(vn1 - cp1.VelocityBias) < ERROR_TO_I); } break; } // // Case 3: wB = 0 and x1 = 0 // // vn1 = a11 * 0 + a12 * x2' + b1' // 0 = a21 * 0 + a22 * x2' + ' // x.X = 0.0f; x.Y = (-cp2.NormalMass) * b.Y; vn1 = vc.K.Ey.X * x.Y + b.X; vn2 = 0.0f; if (x.Y >= 0.0f && vn1 >= 0.0f) { //Console.WriteLine("case 3"); // Resubstitute for the incremental impulse d.Set(x).SubLocal(a); // Apply incremental impulse /* * Vec2 P1 = d.x * normal; Vec2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -= * invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2)); * * vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2)); */ P1.Set(normal).MulLocal(d.X); P2.Set(normal).MulLocal(d.Y); temp1.Set(P1).AddLocal(P2); temp2.Set(temp1).MulLocal(mA); vA.SubLocal(temp2); temp2.Set(temp1).MulLocal(mB); vB.AddLocal(temp2); //Debug.Assert(vA.x == 0); //Debug.Assert(vB.x == 0); wA -= iA * (Vec2.Cross(cp1.RA, P1) + Vec2.Cross(cp2.RA, P2)); wB += iB * (Vec2.Cross(cp1.RB, P1) + Vec2.Cross(cp2.RB, P2)); // Accumulate //Debug.Assert(x.x == 0 && x.y == 0); cp1.NormalImpulse = x.X; cp2.NormalImpulse = x.Y; /* * #if B2_DEBUG_SOLVER == 1 // Postconditions dv2 = vB + Cross(wB, cp2.rB) - vA - * Cross(wA, cp2.rA); * * // Compute normal velocity vn2 = Dot(dv2, normal); * * Debug.Assert(Abs(vn2 - cp2.velocityBias) < k_errorTol); #endif */ if (DEBUG_SOLVER) { // Postconditions Vec2 _dv2 = vB.Add(Vec2.Cross(wB, cp2.RB).SubLocal(vA).SubLocal(Vec2.Cross(wA, cp2.RA))); // Compute normal velocity vn2 = Vec2.Dot(_dv2, normal); Debug.Assert(MathUtils.Abs(vn2 - cp2.VelocityBias) < ERROR_TO_I); } break; } // // Case 4: x1 = 0 and x2 = 0 // // vn1 = b1 // vn2 = ; x.X = 0.0f; x.Y = 0.0f; vn1 = b.X; vn2 = b.Y; if (vn1 >= 0.0f && vn2 >= 0.0f) { //Console.WriteLine("case 4"); // Resubstitute for the incremental impulse d.Set(x).SubLocal(a); // Apply incremental impulse /* * Vec2 P1 = d.x * normal; Vec2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -= * invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2)); * * vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2)); */ P1.Set(normal).MulLocal(d.X); P2.Set(normal).MulLocal(d.Y); temp1.Set(P1).AddLocal(P2); temp2.Set(temp1).MulLocal(mA); vA.SubLocal(temp2); temp2.Set(temp1).MulLocal(mB); vB.AddLocal(temp2); //Debug.Assert(vA.x == 0); //Debug.Assert(vB.x == 0); wA -= iA * (Vec2.Cross(cp1.RA, P1) + Vec2.Cross(cp2.RA, P2)); wB += iB * (Vec2.Cross(cp1.RB, P1) + Vec2.Cross(cp2.RB, P2)); // Accumulate //Debug.Assert(x.x == 0 && x.y == 0); 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; } } Velocities[indexA].V.Set(vA); Velocities[indexA].W = wA; Velocities[indexB].V.Set(vB); Velocities[indexB].W = wB; //Console.WriteLine("Ending velocity for " + indexA + " is " + vA.x + "," + vA.y + " rot " + wA); //Console.WriteLine("Ending velocity for " + indexB + " is " + vB.x + "," + vB.y + " rot " + wB); } }
public void InitializeVelocityConstraints() { //Console.WriteLine("Initializing velocity constraints for " + m_count + " contacts"); // Warm start. for (int i = 0; i < Count; ++i) { ContactVelocityConstraint vc = VelocityConstraints[i]; ContactPositionConstraint pc = PositionConstraints[i]; float radiusA = pc.RadiusA; float radiusB = pc.RadiusB; Manifold manifold = Contacts[vc.ContactIndex].Manifold; int indexA = vc.IndexA; int indexB = vc.IndexB; float mA = vc.InvMassA; float mB = vc.InvMassB; float iA = vc.InvIA; float iB = vc.InvIB; Vec2 localCenterA = pc.LocalCenterA; Vec2 localCenterB = pc.LocalCenterB; Vec2 cA = Positions[indexA].C; float aA = Positions[indexA].A; Vec2 vA = Velocities[indexA].V; float wA = Velocities[indexA].W; Vec2 cB = Positions[indexB].C; float aB = Positions[indexB].A; Vec2 vB = Velocities[indexB].V; float wB = Velocities[indexB].W; Debug.Assert(manifold.PointCount > 0); xfA.Q.Set(aA); xfB.Q.Set(aB); Rot.MulToOutUnsafe(xfA.Q, localCenterA, temp); xfA.P.Set(cA).SubLocal(temp); Rot.MulToOutUnsafe(xfB.Q, localCenterB, temp); xfB.P.Set(cB).SubLocal(temp); worldManifold.Initialize(manifold, xfA, radiusA, xfB, radiusB); vc.Normal.Set(worldManifold.Normal); int pointCount = vc.PointCount; for (int j = 0; j < pointCount; ++j) { ContactVelocityConstraint.VelocityConstraintPoint vcp = vc.Points[j]; vcp.RA.Set(worldManifold.Points[j]).SubLocal(cA); vcp.RB.Set(worldManifold.Points[j]).SubLocal(cB); float rnA = Vec2.Cross(vcp.RA, vc.Normal); float rnB = Vec2.Cross(vcp.RB, vc.Normal); float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB; vcp.NormalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; Vec2.CrossToOutUnsafe(vc.Normal, 1.0f, tangent); float rtA = Vec2.Cross(vcp.RA, tangent); float rtB = Vec2.Cross(vcp.RB, tangent); float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB; vcp.TangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f; // Setup a velocity bias for restitution. vcp.VelocityBias = 0.0f; Vec2.CrossToOutUnsafe(wB, vcp.RB, temp1); Vec2.CrossToOutUnsafe(wA, vcp.RA, temp2); temp.Set(vB).AddLocal(temp1).SubLocal(vA).SubLocal(temp2); float vRel = Vec2.Dot(vc.Normal, temp); if (vRel < -Settings.VELOCITY_THRESHOLD) { vcp.VelocityBias = (-vc.Restitution) * vRel; } } // If we have two points, then prepare the block solver. if (vc.PointCount == 2) { ContactVelocityConstraint.VelocityConstraintPoint vcp1 = vc.Points[0]; ContactVelocityConstraint.VelocityConstraintPoint vcp2 = vc.Points[1]; float rn1A = Vec2.Cross(vcp1.RA, vc.Normal); float rn1B = Vec2.Cross(vcp1.RB, vc.Normal); float rn2A = Vec2.Cross(vcp2.RA, vc.Normal); float rn2B = Vec2.Cross(vcp2.RB, vc.Normal); float k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B; float k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B; float k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B; if (k11 * k11 < MAX_CONDITION_NUMBER * (k11 * k22 - k12 * k12)) { // K is safe to invert. vc.K.Ex.Set(k11, k12); vc.K.Ey.Set(k12, k22); vc.K.InvertToOut(vc.NormalMass); } else { // The constraints are redundant, just use one. // TODO_ERIN use deepest? vc.PointCount = 1; } } } }
public void Report(ContactVelocityConstraint[] constraints) { if (Listener == null) { return; } for (int i = 0; i < ContactCount; ++i) { Contact c = Contacts[i]; ContactVelocityConstraint vc = constraints[i]; impulse.Count = vc.PointCount; for (int j = 0; j < vc.PointCount; ++j) { impulse.NormalImpulses[j] = vc.Points[j].NormalImpulse; impulse.TangentImpulses[j] = vc.Points[j].TangentImpulse; } Listener.PostSolve(c, impulse); } }
public void init(ContactSolverDef def) { //Console.WriteLine("Initializing contact solver"); m_step = def.step; m_count = def.count; if (m_positionConstraints.Length < m_count) { ContactPositionConstraint[] old = m_positionConstraints; m_positionConstraints = new ContactPositionConstraint[MathUtils.max(old.Length * 2, m_count)]; Array.Copy(old, 0, m_positionConstraints, 0, old.Length); for (int i = old.Length; i < m_positionConstraints.Length; i++) { m_positionConstraints[i] = new ContactPositionConstraint(); } } if (m_velocityConstraints.Length < m_count) { ContactVelocityConstraint[] old = m_velocityConstraints; m_velocityConstraints = new ContactVelocityConstraint[MathUtils.max(old.Length * 2, m_count)]; Array.Copy(old, 0, m_velocityConstraints, 0, old.Length); for (int i = old.Length; i < m_velocityConstraints.Length; i++) { m_velocityConstraints[i] = new ContactVelocityConstraint(); } } m_positions = def.positions; m_velocities = def.velocities; m_contacts = def.contacts; for (int i = 0; i < m_count; ++i) { //Console.WriteLine("contacts: " + m_count); Contact contact = m_contacts[i]; Fixture fixtureA = contact.m_fixtureA; Fixture fixtureB = contact.m_fixtureB; Shape shapeA = fixtureA.Shape; Shape shapeB = fixtureB.Shape; float radiusA = shapeA.m_radius; float radiusB = shapeB.m_radius; Body bodyA = fixtureA.Body; Body bodyB = fixtureB.Body; Manifold manifold = contact.Manifold; int pointCount = manifold.pointCount; Debug.Assert(pointCount > 0); ContactVelocityConstraint vc = m_velocityConstraints[i]; vc.friction = contact.m_friction; vc.restitution = contact.m_restitution; vc.tangentSpeed = contact.m_tangentSpeed; vc.indexA = bodyA.m_islandIndex; vc.indexB = bodyB.m_islandIndex; vc.invMassA = bodyA.m_invMass; vc.invMassB = bodyB.m_invMass; vc.invIA = bodyA.m_invI; vc.invIB = bodyB.m_invI; vc.contactIndex = i; vc.pointCount = pointCount; vc.K.setZero(); vc.normalMass.setZero(); ContactPositionConstraint pc = m_positionConstraints[i]; pc.indexA = bodyA.m_islandIndex; pc.indexB = bodyB.m_islandIndex; pc.invMassA = bodyA.m_invMass; pc.invMassB = bodyB.m_invMass; pc.localCenterA.set_Renamed(bodyA.m_sweep.localCenter); pc.localCenterB.set_Renamed(bodyB.m_sweep.localCenter); pc.invIA = bodyA.m_invI; pc.invIB = bodyB.m_invI; pc.localNormal.set_Renamed(manifold.localNormal); pc.localPoint.set_Renamed(manifold.localPoint); pc.pointCount = pointCount; pc.radiusA = radiusA; pc.radiusB = radiusB; pc.type = manifold.type; //Console.WriteLine("contact point count: " + pointCount); for (int j = 0; j < pointCount; j++) { ManifoldPoint cp = manifold.points[j]; ContactVelocityConstraint.VelocityConstraintPoint vcp = vc.points[j]; if (m_step.warmStarting) { //Debug.Assert(cp.normalImpulse == 0); //Console.WriteLine("contact normal impulse: " + cp.normalImpulse); vcp.normalImpulse = m_step.dtRatio * cp.normalImpulse; vcp.tangentImpulse = m_step.dtRatio * cp.tangentImpulse; } else { vcp.normalImpulse = 0; vcp.tangentImpulse = 0; } vcp.rA.setZero(); vcp.rB.setZero(); vcp.normalMass = 0; vcp.tangentMass = 0; vcp.velocityBias = 0; pc.localPoints[j].set_Renamed(cp.localPoint); } } }
public virtual void report(ContactVelocityConstraint[] constraints) { if (m_listener == null) { return; } for (int i = 0; i < m_contactCount; ++i) { Contact c = m_contacts[i]; ContactVelocityConstraint vc = constraints[i]; impulse.count = vc.pointCount; for (int j = 0; j < vc.pointCount; ++j) { impulse.normalImpulses[j] = vc.points[j].normalImpulse; impulse.tangentImpulses[j] = vc.points[j].tangentImpulse; } m_listener.postSolve(c, impulse); } }