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);
}
}