public void WarmStart() { for (int i = 0; i < this._count; i++) { ContactVelocityConstraint contactVelocityConstraint = this._velocityConstraints[i]; int indexA = contactVelocityConstraint.indexA; int indexB = contactVelocityConstraint.indexB; FP invMassA = contactVelocityConstraint.invMassA; FP invIA = contactVelocityConstraint.invIA; FP invMassB = contactVelocityConstraint.invMassB; FP invIB = contactVelocityConstraint.invIB; int pointCount = contactVelocityConstraint.pointCount; TSVector2 tSVector = this._velocities[indexA].v; FP fP = this._velocities[indexA].w; TSVector2 tSVector2 = this._velocities[indexB].v; FP fP2 = this._velocities[indexB].w; TSVector2 normal = contactVelocityConstraint.normal; TSVector2 value = MathUtils.Cross(normal, 1f); for (int j = 0; j < pointCount; j++) { VelocityConstraintPoint velocityConstraintPoint = contactVelocityConstraint.points[j]; TSVector2 tSVector3 = velocityConstraintPoint.normalImpulse * normal + velocityConstraintPoint.tangentImpulse * value; fP -= invIA * MathUtils.Cross(velocityConstraintPoint.rA, tSVector3); tSVector -= invMassA * tSVector3; fP2 += invIB * MathUtils.Cross(velocityConstraintPoint.rB, tSVector3); tSVector2 += invMassB * tSVector3; } this._velocities[indexA].v = tSVector; this._velocities[indexA].w = fP; this._velocities[indexB].v = tSVector2; this._velocities[indexB].w = fP2; } }
public void StoreImpulses() { for (int i = 0; i < this._count; i++) { ContactVelocityConstraint contactVelocityConstraint = this._velocityConstraints[i]; Manifold manifold = this._contacts[contactVelocityConstraint.contactIndex].Manifold; for (int j = 0; j < contactVelocityConstraint.pointCount; j++) { ManifoldPoint value = manifold.Points[j]; value.NormalImpulse = contactVelocityConstraint.points[j].normalImpulse; value.TangentImpulse = contactVelocityConstraint.points[j].tangentImpulse; manifold.Points[j] = value; } this._contacts[contactVelocityConstraint.contactIndex].Manifold = manifold; } }
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) { ManifoldPoint point = manifold.Points[j]; point.NormalImpulse = vc.points[j].normalImpulse; point.TangentImpulse = vc.points[j].tangentImpulse; manifold.Points[j] = point; } _contacts[vc.contactIndex].Manifold = manifold; } }
public void WarmStart() { // Warm start. for (int i = 0; i < _count; ++i) { ContactVelocityConstraint vc = _velocityConstraints[i]; int indexA = vc.indexA; int indexB = vc.indexB; FP mA = vc.invMassA; FP iA = vc.invIA; FP mB = vc.invMassB; FP iB = vc.invIB; int pointCount = vc.pointCount; TSVector2 vA = _velocities[indexA].v; FP wA = _velocities[indexA].w; TSVector2 vB = _velocities[indexB].v; FP wB = _velocities[indexB].w; TSVector2 normal = vc.normal; TSVector2 tangent = MathUtils.Cross(normal, 1.0f); for (int j = 0; j < pointCount; ++j) { VelocityConstraintPoint vcp = vc.points[j]; TSVector2 P = vcp.normalImpulse * normal + vcp.tangentImpulse * tangent; wA -= iA * MathUtils.Cross(vcp.rA, P); vA -= mA * P; wB += iB * MathUtils.Cross(vcp.rB, P); vB += mB * P; } _velocities[indexA].v = vA; _velocities[indexA].w = wA; _velocities[indexB].v = vB; _velocities[indexB].w = wB; } }
private void PostSolve(Contact contact, ContactVelocityConstraint impulse) { if (!Broken) { if (Parts.Contains(contact.FixtureA) || Parts.Contains(contact.FixtureB)) { FP maxImpulse = 0.0f; int count = contact.Manifold.PointCount; for (int i = 0; i < count; ++i) { maxImpulse = TrueSync.TSMath.Max(maxImpulse, impulse.points[i].normalImpulse); } if (maxImpulse > Strength) { // Flag the body for breaking. _break = true; } } } }
private void PostSolve(Contact contact, ContactVelocityConstraint impulse) { bool flag = !this.Broken; if (flag) { bool flag2 = this.Parts.Contains(contact.FixtureA) || this.Parts.Contains(contact.FixtureB); if (flag2) { FP fP = 0f; int pointCount = contact.Manifold.PointCount; for (int i = 0; i < pointCount; i++) { fP = TSMath.Max(fP, impulse.points[i].normalImpulse); } bool flag3 = fP > this.Strength; if (flag3) { this._break = true; } } } }
public void SolveVelocityConstraints() { for (int i = 0; i < this._count; i++) { ContactVelocityConstraint contactVelocityConstraint = this._velocityConstraints[i]; int indexA = contactVelocityConstraint.indexA; int indexB = contactVelocityConstraint.indexB; FP invMassA = contactVelocityConstraint.invMassA; FP invIA = contactVelocityConstraint.invIA; FP invMassB = contactVelocityConstraint.invMassB; FP invIB = contactVelocityConstraint.invIB; int pointCount = contactVelocityConstraint.pointCount; TSVector2 tSVector = this._velocities[indexA].v; FP fP = this._velocities[indexA].w; TSVector2 tSVector2 = this._velocities[indexB].v; FP fP2 = this._velocities[indexB].w; TSVector2 normal = contactVelocityConstraint.normal; TSVector2 tSVector3 = MathUtils.Cross(normal, 1f); FP friction = contactVelocityConstraint.friction; Debug.Assert(pointCount == 1 || pointCount == 2); for (int j = 0; j < pointCount; j++) { VelocityConstraintPoint velocityConstraintPoint = contactVelocityConstraint.points[j]; TSVector2 value = tSVector2 + MathUtils.Cross(fP2, velocityConstraintPoint.rB) - tSVector - MathUtils.Cross(fP, velocityConstraintPoint.rA); FP x = TSVector2.Dot(value, tSVector3) - contactVelocityConstraint.tangentSpeed; FP fP3 = velocityConstraintPoint.tangentMass * -x; FP fP4 = friction * velocityConstraintPoint.normalImpulse; FP fP5 = MathUtils.Clamp(velocityConstraintPoint.tangentImpulse + fP3, -fP4, fP4); fP3 = fP5 - velocityConstraintPoint.tangentImpulse; velocityConstraintPoint.tangentImpulse = fP5; TSVector2 tSVector4 = fP3 * tSVector3; tSVector -= invMassA * tSVector4; fP -= invIA * MathUtils.Cross(velocityConstraintPoint.rA, tSVector4); tSVector2 += invMassB * tSVector4; fP2 += invIB * MathUtils.Cross(velocityConstraintPoint.rB, tSVector4); } bool flag = contactVelocityConstraint.pointCount == 1; if (flag) { VelocityConstraintPoint velocityConstraintPoint2 = contactVelocityConstraint.points[0]; TSVector2 value2 = tSVector2 + MathUtils.Cross(fP2, velocityConstraintPoint2.rB) - tSVector - MathUtils.Cross(fP, velocityConstraintPoint2.rA); FP x2 = TSVector2.Dot(value2, normal); FP fP6 = -velocityConstraintPoint2.normalMass * (x2 - velocityConstraintPoint2.velocityBias); FP fP7 = TSMath.Max(velocityConstraintPoint2.normalImpulse + fP6, 0f); fP6 = fP7 - velocityConstraintPoint2.normalImpulse; velocityConstraintPoint2.normalImpulse = fP7; TSVector2 tSVector5 = fP6 * normal; tSVector -= invMassA * tSVector5; fP -= invIA * MathUtils.Cross(velocityConstraintPoint2.rA, tSVector5); tSVector2 += invMassB * tSVector5; fP2 += invIB * MathUtils.Cross(velocityConstraintPoint2.rB, tSVector5); } else { VelocityConstraintPoint velocityConstraintPoint3 = contactVelocityConstraint.points[0]; VelocityConstraintPoint velocityConstraintPoint4 = contactVelocityConstraint.points[1]; TSVector2 tSVector6 = new TSVector2(velocityConstraintPoint3.normalImpulse, velocityConstraintPoint4.normalImpulse); Debug.Assert(tSVector6.x >= 0f && tSVector6.y >= 0f); TSVector2 value3 = tSVector2 + MathUtils.Cross(fP2, velocityConstraintPoint3.rB) - tSVector - MathUtils.Cross(fP, velocityConstraintPoint3.rA); TSVector2 value4 = tSVector2 + MathUtils.Cross(fP2, velocityConstraintPoint4.rB) - tSVector - MathUtils.Cross(fP, velocityConstraintPoint4.rA); FP x3 = TSVector2.Dot(value3, normal); FP x4 = TSVector2.Dot(value4, normal); TSVector2 tSVector7 = new TSVector2 { x = x3 - velocityConstraintPoint3.velocityBias, y = x4 - velocityConstraintPoint4.velocityBias } -MathUtils.Mul(ref contactVelocityConstraint.K, tSVector6); TSVector2 tSVector8 = -MathUtils.Mul(ref contactVelocityConstraint.normalMass, tSVector7); bool flag2 = tSVector8.x >= 0f && tSVector8.y >= 0f; if (flag2) { TSVector2 tSVector9 = tSVector8 - tSVector6; TSVector2 tSVector10 = tSVector9.x * normal; TSVector2 tSVector11 = tSVector9.y * normal; tSVector -= invMassA * (tSVector10 + tSVector11); fP -= invIA * (MathUtils.Cross(velocityConstraintPoint3.rA, tSVector10) + MathUtils.Cross(velocityConstraintPoint4.rA, tSVector11)); tSVector2 += invMassB * (tSVector10 + tSVector11); fP2 += invIB * (MathUtils.Cross(velocityConstraintPoint3.rB, tSVector10) + MathUtils.Cross(velocityConstraintPoint4.rB, tSVector11)); velocityConstraintPoint3.normalImpulse = tSVector8.x; velocityConstraintPoint4.normalImpulse = tSVector8.y; } else { tSVector8.x = -velocityConstraintPoint3.normalMass * tSVector7.x; tSVector8.y = 0f; x3 = 0f; x4 = contactVelocityConstraint.K.ex.y * tSVector8.x + tSVector7.y; bool flag3 = tSVector8.x >= 0f && x4 >= 0f; if (flag3) { TSVector2 tSVector12 = tSVector8 - tSVector6; TSVector2 tSVector13 = tSVector12.x * normal; TSVector2 tSVector14 = tSVector12.y * normal; tSVector -= invMassA * (tSVector13 + tSVector14); fP -= invIA * (MathUtils.Cross(velocityConstraintPoint3.rA, tSVector13) + MathUtils.Cross(velocityConstraintPoint4.rA, tSVector14)); tSVector2 += invMassB * (tSVector13 + tSVector14); fP2 += invIB * (MathUtils.Cross(velocityConstraintPoint3.rB, tSVector13) + MathUtils.Cross(velocityConstraintPoint4.rB, tSVector14)); velocityConstraintPoint3.normalImpulse = tSVector8.x; velocityConstraintPoint4.normalImpulse = tSVector8.y; } else { tSVector8.x = 0f; tSVector8.y = -velocityConstraintPoint4.normalMass * tSVector7.y; x3 = contactVelocityConstraint.K.ey.x * tSVector8.y + tSVector7.x; x4 = 0f; bool flag4 = tSVector8.y >= 0f && x3 >= 0f; if (flag4) { TSVector2 tSVector15 = tSVector8 - tSVector6; TSVector2 tSVector16 = tSVector15.x * normal; TSVector2 tSVector17 = tSVector15.y * normal; tSVector -= invMassA * (tSVector16 + tSVector17); fP -= invIA * (MathUtils.Cross(velocityConstraintPoint3.rA, tSVector16) + MathUtils.Cross(velocityConstraintPoint4.rA, tSVector17)); tSVector2 += invMassB * (tSVector16 + tSVector17); fP2 += invIB * (MathUtils.Cross(velocityConstraintPoint3.rB, tSVector16) + MathUtils.Cross(velocityConstraintPoint4.rB, tSVector17)); velocityConstraintPoint3.normalImpulse = tSVector8.x; velocityConstraintPoint4.normalImpulse = tSVector8.y; } else { tSVector8.x = 0f; tSVector8.y = 0f; x3 = tSVector7.x; x4 = tSVector7.y; bool flag5 = x3 >= 0f && x4 >= 0f; if (flag5) { TSVector2 tSVector18 = tSVector8 - tSVector6; TSVector2 tSVector19 = tSVector18.x * normal; TSVector2 tSVector20 = tSVector18.y * normal; tSVector -= invMassA * (tSVector19 + tSVector20); fP -= invIA * (MathUtils.Cross(velocityConstraintPoint3.rA, tSVector19) + MathUtils.Cross(velocityConstraintPoint4.rA, tSVector20)); tSVector2 += invMassB * (tSVector19 + tSVector20); fP2 += invIB * (MathUtils.Cross(velocityConstraintPoint3.rB, tSVector19) + MathUtils.Cross(velocityConstraintPoint4.rB, tSVector20)); velocityConstraintPoint3.normalImpulse = tSVector8.x; velocityConstraintPoint4.normalImpulse = tSVector8.y; } } } } } this._velocities[indexA].v = tSVector; this._velocities[indexA].w = fP; this._velocities[indexB].v = tSVector2; this._velocities[indexB].w = fP2; } }
public void InitializeVelocityConstraints() { for (int i = 0; i < this._count; i++) { ContactVelocityConstraint contactVelocityConstraint = this._velocityConstraints[i]; ContactPositionConstraint contactPositionConstraint = this._positionConstraints[i]; FP radiusA = contactPositionConstraint.radiusA; FP radiusB = contactPositionConstraint.radiusB; Manifold manifold = this._contacts[contactVelocityConstraint.contactIndex].Manifold; int indexA = contactVelocityConstraint.indexA; int indexB = contactVelocityConstraint.indexB; FP invMassA = contactVelocityConstraint.invMassA; FP invMassB = contactVelocityConstraint.invMassB; FP invIA = contactVelocityConstraint.invIA; FP invIB = contactVelocityConstraint.invIB; TSVector2 localCenterA = contactPositionConstraint.localCenterA; TSVector2 localCenterB = contactPositionConstraint.localCenterB; TSVector2 c = this._positions[indexA].c; FP a = this._positions[indexA].a; TSVector2 v = this._velocities[indexA].v; FP w = this._velocities[indexA].w; TSVector2 c2 = this._positions[indexB].c; FP a2 = this._positions[indexB].a; TSVector2 v2 = this._velocities[indexB].v; FP w2 = this._velocities[indexB].w; Debug.Assert(manifold.PointCount > 0); Transform transform = default(Transform); Transform transform2 = default(Transform); transform.q.Set(a); transform2.q.Set(a2); transform.p = c - MathUtils.Mul(transform.q, localCenterA); transform2.p = c2 - MathUtils.Mul(transform2.q, localCenterB); TSVector2 normal; FixedArray2 <TSVector2> fixedArray; ContactSolver.WorldManifold.Initialize(ref manifold, ref transform, radiusA, ref transform2, radiusB, out normal, out fixedArray); contactVelocityConstraint.normal = normal; int pointCount = contactVelocityConstraint.pointCount; for (int j = 0; j < pointCount; j++) { VelocityConstraintPoint velocityConstraintPoint = contactVelocityConstraint.points[j]; velocityConstraintPoint.rA = fixedArray[j] - c; velocityConstraintPoint.rB = fixedArray[j] - c2; FP y = MathUtils.Cross(velocityConstraintPoint.rA, contactVelocityConstraint.normal); FP y2 = MathUtils.Cross(velocityConstraintPoint.rB, contactVelocityConstraint.normal); FP fP = invMassA + invMassB + invIA * y * y + invIB * y2 * y2; velocityConstraintPoint.normalMass = ((fP > 0f) ? (1f / fP) : 0f); TSVector2 b = MathUtils.Cross(contactVelocityConstraint.normal, 1f); FP y3 = MathUtils.Cross(velocityConstraintPoint.rA, b); FP y4 = MathUtils.Cross(velocityConstraintPoint.rB, b); FP fP2 = invMassA + invMassB + invIA * y3 * y3 + invIB * y4 * y4; velocityConstraintPoint.tangentMass = ((fP2 > 0f) ? (1f / fP2) : 0f); velocityConstraintPoint.velocityBias = 0f; FP fP3 = TSVector2.Dot(contactVelocityConstraint.normal, v2 + MathUtils.Cross(w2, velocityConstraintPoint.rB) - v - MathUtils.Cross(w, velocityConstraintPoint.rA)); bool flag = fP3 < -Settings.VelocityThreshold; if (flag) { velocityConstraintPoint.velocityBias = -contactVelocityConstraint.restitution * fP3; } } bool flag2 = contactVelocityConstraint.pointCount == 2; if (flag2) { VelocityConstraintPoint velocityConstraintPoint2 = contactVelocityConstraint.points[0]; VelocityConstraintPoint velocityConstraintPoint3 = contactVelocityConstraint.points[1]; FP y5 = MathUtils.Cross(velocityConstraintPoint2.rA, contactVelocityConstraint.normal); FP y6 = MathUtils.Cross(velocityConstraintPoint2.rB, contactVelocityConstraint.normal); FP y7 = MathUtils.Cross(velocityConstraintPoint3.rA, contactVelocityConstraint.normal); FP y8 = MathUtils.Cross(velocityConstraintPoint3.rB, contactVelocityConstraint.normal); FP fP4 = invMassA + invMassB + invIA * y5 * y5 + invIB * y6 * y6; FP y9 = invMassA + invMassB + invIA * y7 * y7 + invIB * y8 * y8; FP fP5 = invMassA + invMassB + invIA * y5 * y7 + invIB * y6 * y8; FP x = 1000f; bool flag3 = fP4 * fP4 < x * (fP4 * y9 - fP5 * fP5); if (flag3) { contactVelocityConstraint.K.ex = new TSVector2(fP4, fP5); contactVelocityConstraint.K.ey = new TSVector2(fP5, y9); contactVelocityConstraint.normalMass = contactVelocityConstraint.K.Inverse; } else { contactVelocityConstraint.pointCount = 1; } } } }
public void Reset(TimeStep step, int count, Contact[] contacts, Position[] positions, Velocity[] velocities, bool warmstarting = true) { this._step = step; this._count = count; this._positions = positions; this._velocities = velocities; this._contacts = contacts; bool flag = this._velocityConstraints == null || this._velocityConstraints.Length < count; if (flag) { this._velocityConstraints = new ContactVelocityConstraint[count * 2]; this._positionConstraints = new ContactPositionConstraint[count * 2]; for (int i = 0; i < this._velocityConstraints.Length; i++) { this._velocityConstraints[i] = new ContactVelocityConstraint(); } for (int j = 0; j < this._positionConstraints.Length; j++) { this._positionConstraints[j] = new ContactPositionConstraint(); } } for (int k = 0; k < this._count; k++) { Contact contact = contacts[k]; Fixture fixtureA = contact.FixtureA; Fixture fixtureB = contact.FixtureB; Shape shape = fixtureA.Shape; Shape shape2 = fixtureB.Shape; FP radius = shape.Radius; FP radius2 = shape2.Radius; Body body = fixtureA.Body; Body body2 = fixtureB.Body; Manifold manifold = contact.Manifold; int pointCount = manifold.PointCount; Debug.Assert(pointCount > 0); ContactVelocityConstraint contactVelocityConstraint = this._velocityConstraints[k]; contactVelocityConstraint.friction = contact.Friction; contactVelocityConstraint.restitution = contact.Restitution; contactVelocityConstraint.tangentSpeed = contact.TangentSpeed; contactVelocityConstraint.indexA = body.IslandIndex; contactVelocityConstraint.indexB = body2.IslandIndex; contactVelocityConstraint.invMassA = body._invMass; contactVelocityConstraint.invMassB = body2._invMass; contactVelocityConstraint.invIA = body._invI; contactVelocityConstraint.invIB = body2._invI; contactVelocityConstraint.contactIndex = k; contactVelocityConstraint.pointCount = pointCount; contactVelocityConstraint.K.SetZero(); contactVelocityConstraint.normalMass.SetZero(); ContactPositionConstraint contactPositionConstraint = this._positionConstraints[k]; contactPositionConstraint.indexA = body.IslandIndex; contactPositionConstraint.indexB = body2.IslandIndex; contactPositionConstraint.invMassA = body._invMass; contactPositionConstraint.invMassB = body2._invMass; contactPositionConstraint.localCenterA = body._sweep.LocalCenter; contactPositionConstraint.localCenterB = body2._sweep.LocalCenter; contactPositionConstraint.invIA = body._invI; contactPositionConstraint.invIB = body2._invI; contactPositionConstraint.localNormal = manifold.LocalNormal; contactPositionConstraint.localPoint = manifold.LocalPoint; contactPositionConstraint.pointCount = pointCount; contactPositionConstraint.radiusA = radius; contactPositionConstraint.radiusB = radius2; contactPositionConstraint.type = manifold.Type; for (int l = 0; l < pointCount; l++) { ManifoldPoint manifoldPoint = manifold.Points[l]; VelocityConstraintPoint velocityConstraintPoint = contactVelocityConstraint.points[l]; velocityConstraintPoint.normalImpulse = this._step.dtRatio * manifoldPoint.NormalImpulse; velocityConstraintPoint.tangentImpulse = this._step.dtRatio * manifoldPoint.TangentImpulse; velocityConstraintPoint.rA = TSVector2.zero; velocityConstraintPoint.rB = TSVector2.zero; velocityConstraintPoint.normalMass = 0f; velocityConstraintPoint.tangentMass = 0f; velocityConstraintPoint.velocityBias = 0f; contactPositionConstraint.localPoints[l] = manifoldPoint.LocalPoint; } } }
public void Reset(TimeStep step, int count, Contact[] contacts, Position[] positions, Velocity[] velocities, bool warmstarting = Settings.EnableWarmstarting) { _step = step; _count = count; _positions = positions; _velocities = velocities; _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 < _count; ++i) { Contact contact = contacts[i]; Fixture fixtureA = contact.FixtureA; Fixture fixtureB = contact.FixtureB; Shape shapeA = fixtureA.Shape; Shape shapeB = fixtureB.Shape; FP radiusA = shapeA.Radius; FP 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 = 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 (Settings.EnableWarmstarting) { vcp.normalImpulse = _step.dtRatio * cp.NormalImpulse; vcp.tangentImpulse = _step.dtRatio * cp.TangentImpulse; } else { vcp.normalImpulse = 0.0f; vcp.tangentImpulse = 0.0f; } vcp.rA = TSVector2.zero; vcp.rB = TSVector2.zero; vcp.normalMass = 0.0f; vcp.tangentMass = 0.0f; vcp.velocityBias = 0.0f; pc.localPoints[j] = cp.LocalPoint; } } }
public void SolveVelocityConstraints() { for (int i = 0; i < _count; ++i) { ContactVelocityConstraint vc = _velocityConstraints[i]; int indexA = vc.indexA; int indexB = vc.indexB; FP mA = vc.invMassA; FP iA = vc.invIA; FP mB = vc.invMassB; FP iB = vc.invIB; int pointCount = vc.pointCount; TSVector2 vA = _velocities[indexA].v; FP wA = _velocities[indexA].w; TSVector2 vB = _velocities[indexB].v; FP wB = _velocities[indexB].w; TSVector2 normal = vc.normal; TSVector2 tangent = MathUtils.Cross(normal, 1.0f); FP friction = vc.friction; Debug.Assert(pointCount == 1 || pointCount == 2); // Solve tangent constraints first because non-penetration is more important // than friction. for (int j = 0; j < pointCount; ++j) { VelocityConstraintPoint vcp = vc.points[j]; // Relative velocity at contact TSVector2 dv = vB + MathUtils.Cross(wB, vcp.rB) - vA - MathUtils.Cross(wA, vcp.rA); // Compute tangent force FP vt = TSVector2.Dot(dv, tangent) - vc.tangentSpeed; FP lambda = vcp.tangentMass * (-vt); // b2Clamp the accumulated force FP maxFriction = friction * vcp.normalImpulse; FP newImpulse = MathUtils.Clamp(vcp.tangentImpulse + lambda, -maxFriction, maxFriction); lambda = newImpulse - vcp.tangentImpulse; vcp.tangentImpulse = newImpulse; // Apply contact impulse TSVector2 P = lambda * tangent; vA -= mA * P; wA -= iA * MathUtils.Cross(vcp.rA, P); vB += mB * P; wB += iB * MathUtils.Cross(vcp.rB, P); } // Solve normal constraints if (vc.pointCount == 1) { VelocityConstraintPoint vcp = vc.points[0]; // Relative velocity at contact TSVector2 dv = vB + MathUtils.Cross(wB, vcp.rB) - vA - MathUtils.Cross(wA, vcp.rA); // Compute normal impulse FP vn = TSVector2.Dot(dv, normal); FP lambda = -vcp.normalMass * (vn - vcp.velocityBias); // b2Clamp the accumulated impulse FP newImpulse = TrueSync.TSMath.Max(vcp.normalImpulse + lambda, 0.0f); lambda = newImpulse - vcp.normalImpulse; vcp.normalImpulse = newImpulse; // Apply contact impulse TSVector2 P = lambda * normal; vA -= mA * P; wA -= iA * MathUtils.Cross(vcp.rA, P); vB += mB * P; wB += iB * MathUtils.Cross(vcp.rB, 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 = vc.points[0]; VelocityConstraintPoint cp2 = vc.points[1]; TSVector2 a = new TSVector2(cp1.normalImpulse, cp2.normalImpulse); Debug.Assert(a.x >= 0.0f && a.y >= 0.0f); // Relative velocity at contact TSVector2 dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA); TSVector2 dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA); // Compute normal velocity FP vn1 = TSVector2.Dot(dv1, normal); FP vn2 = TSVector2.Dot(dv2, normal); TSVector2 b = new TSVector2(); b.x = vn1 - cp1.velocityBias; b.y = vn2 - cp2.velocityBias; // Compute b' b -= MathUtils.Mul(ref vc.K, a); //FP 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' // TSVector2 x = -MathUtils.Mul(ref vc.normalMass, b); if (x.x >= 0.0f && x.y >= 0.0f) { // Get the incremental impulse TSVector2 d = x - a; // Apply incremental impulse TSVector2 P1 = d.x * normal; TSVector2 P2 = d.y * normal; vA -= mA * (P1 + P2); wA -= iA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2)); vB += mB * (P1 + P2); wB += iB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2)); // Accumulate cp1.normalImpulse = x.x; cp2.normalImpulse = x.y; #if B2_DEBUG_SOLVER // 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); b2Assert(b2Abs(vn1 - cp1.velocityBias) < k_errorTol); b2Assert(b2Abs(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' // 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) { // Get the incremental impulse TSVector2 d = x - a; // Apply incremental impulse TSVector2 P1 = d.x * normal; TSVector2 P2 = d.y * normal; vA -= mA * (P1 + P2); wA -= iA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2)); vB += mB * (P1 + P2); wB += iB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2)); // Accumulate cp1.normalImpulse = x.x; cp2.normalImpulse = x.y; #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); b2Assert(b2Abs(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' // 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) { // Resubstitute for the incremental impulse TSVector2 d = x - a; // Apply incremental impulse TSVector2 P1 = d.x * normal; TSVector2 P2 = d.y * normal; vA -= mA * (P1 + P2); wA -= iA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2)); vB += mB * (P1 + P2); wB += iB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2)); // Accumulate cp1.normalImpulse = x.x; cp2.normalImpulse = x.y; #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); b2Assert(b2Abs(vn2 - cp2.velocityBias) < k_errorTol); #endif 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 TSVector2 d = x - a; // Apply incremental impulse TSVector2 P1 = d.x * normal; TSVector2 P2 = d.y * normal; vA -= mA * (P1 + P2); wA -= iA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2)); vB += mB * (P1 + P2); wB += iB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, 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; } } _velocities[indexA].v = vA; _velocities[indexA].w = wA; _velocities[indexB].v = vB; _velocities[indexB].w = wB; } }
public void InitializeVelocityConstraints() { for (int i = 0; i < _count; ++i) { ContactVelocityConstraint vc = _velocityConstraints[i]; ContactPositionConstraint pc = _positionConstraints[i]; FP radiusA = pc.radiusA; FP radiusB = pc.radiusB; Manifold manifold = _contacts[vc.contactIndex].Manifold; int indexA = vc.indexA; int indexB = vc.indexB; FP mA = vc.invMassA; FP mB = vc.invMassB; FP iA = vc.invIA; FP iB = vc.invIB; TSVector2 localCenterA = pc.localCenterA; TSVector2 localCenterB = pc.localCenterB; TSVector2 cA = _positions[indexA].c; FP aA = _positions[indexA].a; TSVector2 vA = _velocities[indexA].v; FP wA = _velocities[indexA].w; TSVector2 cB = _positions[indexB].c; FP aB = _positions[indexB].a; TSVector2 vB = _velocities[indexB].v; FP 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); TSVector2 normal; FixedArray2 <TSVector2> points; WorldManifold.Initialize(ref manifold, ref xfA, radiusA, ref xfB, radiusB, out normal, out points); 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; FP rnA = MathUtils.Cross(vcp.rA, vc.normal); FP rnB = MathUtils.Cross(vcp.rB, vc.normal); FP kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB; vcp.normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; TSVector2 tangent = MathUtils.Cross(vc.normal, 1.0f); FP rtA = MathUtils.Cross(vcp.rA, tangent); FP rtB = MathUtils.Cross(vcp.rB, tangent); FP 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; FP vRel = TSVector2.Dot(vc.normal, vB + MathUtils.Cross(wB, vcp.rB) - vA - MathUtils.Cross(wA, vcp.rA)); if (vRel < -Settings.VelocityThreshold) { vcp.velocityBias = -vc.restitution * vRel; } } // If we have two points, then prepare the block solver. if (vc.pointCount == 2) { VelocityConstraintPoint vcp1 = vc.points[0]; VelocityConstraintPoint vcp2 = vc.points[1]; FP rn1A = MathUtils.Cross(vcp1.rA, vc.normal); FP rn1B = MathUtils.Cross(vcp1.rB, vc.normal); FP rn2A = MathUtils.Cross(vcp2.rA, vc.normal); FP rn2B = MathUtils.Cross(vcp2.rB, vc.normal); FP k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B; FP k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B; FP k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B; // Ensure a reasonable condition number. FP k_maxConditionNumber = 1000.0f; if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) { // K is safe to invert. vc.K.ex = new TSVector2(k11, k12); vc.K.ey = new TSVector2(k12, k22); vc.normalMass = vc.K.Inverse; } else { // The constraints are redundant, just use one. // TODO_ERIN use deepest? vc.pointCount = 1; } } } }