public void Initialize() { for (int i = 0; i < ContactCount; ++i) { Vector2 ra = Contacts[i] - (Vector2)A.transform.position; Vector2 rb = Contacts[i] - (Vector2)B.transform.position; Vector2 rv = B.Velocity + Vector2Helper.Cross(B.AngularVelocity, rb) - A.Velocity - Vector2Helper.Cross(A.AngularVelocity, ra); if (rv.sqrMagnitude - (Core.PhysicsEngine.DeltaTime * Core.PhysicsEngine.Gravity).sqrMagnitude < Mathf.Epsilon) { AverageRestitution = 0.0f; } } }
public void ApplyImpulse(Vector2 impulse, Vector2 contact) { Velocity += (InverseMass * impulse); AngularVelocity += InverseInertia * Vector2Helper.Cross(contact, impulse); }
public void ApplyImpulse() { if (Math.Abs(A.InverseMass) < Mathf.Epsilon && Math.Abs(+B.InverseMass) < Mathf.Epsilon) { InfiniteMassCorrection(); return; } for (int i = 0; i < ContactCount; ++i) { Vector2 ra = Contacts[i] - (Vector2)A.transform.position; Vector2 rb = Contacts[i] - (Vector2)B.transform.position; Vector2 rv = B.Velocity + B.AngularVelocity * rb - A.Velocity - A.AngularVelocity * ra; float contactVel = Vector2.Dot(rv, Normal); if (contactVel > 0) { return; } float raCrossN = Vector2Helper.Cross(ra, Normal); float rbCrossN = Vector2Helper.Cross(rb, Normal); float invMassSum = A.InverseMass + B.InverseMass + (raCrossN * raCrossN) * A.InverseInertia + (rbCrossN * rbCrossN) * B.InverseInertia; float j = -(1.0f + AverageRestitution) * contactVel; j /= invMassSum; j /= ContactCount; Vector2 impulse = Normal * j; A.ApplyImpulse(-1f * impulse, ra); B.ApplyImpulse(impulse, rb); rv = B.Velocity + Vector2Helper.Cross(B.AngularVelocity, rb) - A.Velocity - Vector2Helper.Cross(A.AngularVelocity, ra); Vector2 t = rv - (Normal * Vector2.Dot(rv, Normal)); t = t.normalized; float jt = -Vector2.Dot(rv, t); jt /= invMassSum; jt /= ContactCount; if (Math.Abs(jt) < Mathf.Epsilon) { return; } Vector2 tangentImpulse; if (Mathf.Abs(jt) < j * StaticFriction) { tangentImpulse = t * jt; } else { tangentImpulse = t * -j * DynamicFriction; } A.ApplyImpulse(-tangentImpulse, ra); B.ApplyImpulse(tangentImpulse, rb); } }