/// <summary>Initialize position dependent portions of the velocity constraints.</summary> public void InitializeVelocityConstraints() { 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; Vector2 localCenterA = pc.LocalCenterA; Vector2 localCenterB = pc.LocalCenterB; Vector2 cA = positions[indexA].C; float aA = positions[indexA].A; Vector2 vA = velocities[indexA].V; float wA = velocities[indexA].W; Vector2 cB = positions[indexB].C; float aB = positions[indexB].A; Vector2 vB = velocities[indexB].V; float wB = velocities[indexB].W; Debug.Assert(manifold.PointCount > 0); Transform xfA = new Transform(); Transform xfB = new Transform(); xfA.Q.Set(aA); xfB.Q.Set(aB); xfA.P = cA - MathUtils.Mul(xfA.Q, localCenterA); xfB.P = cB - MathUtils.Mul(xfB.Q, localCenterB); WorldManifold.Initialize(ref manifold, ref xfA, radiusA, ref xfB, radiusB, out Vector2 normal, out FixedArray2 <Vector2> points, out _); vc.Normal = normal; int pointCount = vc.PointCount; for (int j = 0; j < pointCount; ++j) { VelocityConstraintPoint vcp = vc.Points[j]; vcp.RA = points[j] - cA; vcp.RB = points[j] - cB; float rnA = MathUtils.Cross(vcp.RA, vc.Normal); float rnB = MathUtils.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; Vector2 tangent = MathUtils.Cross(vc.Normal, 1.0f); float rtA = MathUtils.Cross(vcp.RA, tangent); float rtB = MathUtils.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; float vRel = MathUtils.Dot(vc.Normal, vB + MathUtils.Cross(wB, vcp.RB) - vA - MathUtils.Cross(wA, vcp.RA)); if (vRel < -vc.Threshold) { vcp.VelocityBias = -vc.Restitution * vRel; } } // If we have two points, then prepare the block solver. if (vc.PointCount == 2 && Settings.BlockSolve) { VelocityConstraintPoint vcp1 = vc.Points[0]; VelocityConstraintPoint vcp2 = vc.Points[1]; float rn1A = MathUtils.Cross(vcp1.RA, vc.Normal); float rn1B = MathUtils.Cross(vcp1.RB, vc.Normal); float rn2A = MathUtils.Cross(vcp2.RA, vc.Normal); float rn2B = MathUtils.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; // Ensure a reasonable condition number. const float kMaxConditionNumber = 1000.0f; if (k11 * k11 < kMaxConditionNumber * (k11 * k22 - k12 * k12)) { // K is safe to invert. vc.K.Ex = new Vector2(k11, k12); vc.K.Ey = new Vector2(k12, k22); vc.NormalMass = vc.K.Inverse; } else { // The constraints are redundant, just use one. // TODO_ERIN use deepest? vc.PointCount = 1; } } } }
// Sequential position solver for position constraints. /// <summary> /// Describes whether this instance solve toi position constraints /// </summary> /// <param name="toiIndexA">The toi index</param> /// <param name="toiIndexB">The toi index</param> /// <returns>The bool</returns> public bool SolveToiPositionConstraints(int toiIndexA, int toiIndexB) { float minSeparation = 0.0f; for (int i = 0; i < count; ++i) { ContactPositionConstraint pc = positionConstraints[i]; int indexA = pc.IndexA; int indexB = pc.IndexB; Vector2 localCenterA = pc.LocalCenterA; Vector2 localCenterB = pc.LocalCenterB; int pointCount = pc.PointCount; float mA = 0.0f; float iA = 0.0f; if (indexA == toiIndexA || indexA == toiIndexB) { mA = pc.InvMassA; iA = pc.InvIa; } float mB = 0.0f; float iB = 0.0f; if (indexB == toiIndexA || indexB == toiIndexB) { mB = pc.InvMassB; iB = pc.InvIb; } Vector2 cA = positions[indexA].C; float aA = positions[indexA].A; Vector2 cB = positions[indexB].C; float aB = positions[indexB].A; // Solve normal constraints for (int j = 0; j < pointCount; ++j) { Transform xfA = new Transform(); Transform xfB = new Transform(); xfA.Q.Set(aA); xfB.Q.Set(aB); xfA.P = cA - MathUtils.Mul(xfA.Q, localCenterA); xfB.P = cB - MathUtils.Mul(xfB.Q, localCenterB); PositionSolverManifold.Initialize(pc, ref xfA, ref xfB, j, out Vector2 normal, out Vector2 point, out float separation); Vector2 rA = point - cA; Vector2 rB = point - cB; // Track max constraint error. minSeparation = Math.Min(minSeparation, separation); // Prevent large corrections and allow slop. float c = MathUtils.Clamp(Settings.ToiBaumgarte * (separation + Settings.LinearSlop), -Settings.MaxLinearCorrection, 0.0f); // Compute the effective mass. float rnA = MathUtils.Cross(rA, normal); float rnB = MathUtils.Cross(rB, normal); float k = mA + mB + iA * rnA * rnA + iB * rnB * rnB; // Compute normal impulse float impulse = k > 0.0f ? -c / k : 0.0f; Vector2 p = impulse * normal; cA -= mA * p; aA -= iA * MathUtils.Cross(rA, p); cB += mB * p; aB += iB * MathUtils.Cross(rB, p); } positions[indexA].C = cA; positions[indexA].A = aA; positions[indexB].C = cB; positions[indexB].A = aB; } // We can't expect minSpeparation >= -b2_linearSlop because we don't // push the separation above -b2_linearSlop. return(minSeparation >= -1.5f * Settings.LinearSlop); }
/// <summary> /// Resets the step /// </summary> /// <param name="step">The step</param> /// <param name="count">The count</param> /// <param name="contacts">The contacts</param> /// <param name="positions">The positions</param> /// <param name="velocities">The velocities</param> public void Reset(TimeStep step, int count, Contact[] contacts, Position[] positions, Velocity[] velocities) { this.step = step; this.count = count; this.positions = positions; this.velocities = velocities; this.contacts = contacts; // grow the array if (VelocityConstraints == null || VelocityConstraints.Length < count) { VelocityConstraints = new ContactVelocityConstraint[count * 2]; positionConstraints = new ContactPositionConstraint[count * 2]; for (int i = 0; i < VelocityConstraints.Length; i++) { VelocityConstraints[i] = new ContactVelocityConstraint(); } for (int i = 0; i < positionConstraints.Length; i++) { positionConstraints[i] = new ContactPositionConstraint(); } } // Initialize position independent portions of the constraints. for (int i = 0; i < this.count; ++i) { Contact contact = contacts[i]; Fixture fixtureA = contact.FixtureA; Fixture fixtureB = contact.FixtureB; Shape shapeA = fixtureA.Shape; Shape shapeB = fixtureB.Shape; float radiusA = shapeA.RadiusPrivate; float radiusB = shapeB.RadiusPrivate; 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.Threshold = contact.RestitutionThreshold; 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 = bodyA.Sweep.LocalCenter; pc.LocalCenterB = bodyB.Sweep.LocalCenter; pc.InvIa = bodyA.InvI; pc.InvIb = bodyB.InvI; pc.LocalNormal = manifold.LocalNormal; pc.LocalPoint = manifold.LocalPoint; pc.PointCount = pointCount; pc.RadiusA = radiusA; pc.RadiusB = radiusB; pc.Type = manifold.Type; for (int j = 0; j < pointCount; ++j) { ManifoldPoint cp = manifold.Points[j]; VelocityConstraintPoint vcp = vc.Points[j]; if (step.WarmStarting) { vcp.NormalImpulse = this.step.DeltaTimeRatio * cp.NormalImpulse; vcp.TangentImpulse = this.step.DeltaTimeRatio * cp.TangentImpulse; } else { vcp.NormalImpulse = 0.0f; vcp.TangentImpulse = 0.0f; } vcp.RA = Vector2.Zero; vcp.RB = Vector2.Zero; vcp.NormalMass = 0.0f; vcp.TangentMass = 0.0f; vcp.VelocityBias = 0.0f; pc.LocalPoints[j] = cp.LocalPoint; } } }