private void CorrectLinearMotion(Rigidbody rigidbodyA, Rigidbody rigidbodyB,
PhysicsMaterial.Result surfaceA, PhysicsMaterial.Result surfaceB, Vector3 normal)
{
// Cache the required movement data of each object.
var massA = rigidbodyA.mass;
var massB = rigidbodyB.mass;
var momentumA = rigidbodyA.momentum;
var momentumB = rigidbodyB.momentum;
var velocityA = rigidbodyA.velocity;
var velocityB = rigidbodyB.velocity;
// Conservation of momentum formula:
// V1 = e(I1 + 2 * I2 - m2 * u1) / (m1 + m2).
// V2 = e(u1 - u2) + V1.
var v1 = surfaceA.restitution * (momentumA + 2f * momentumB - massB * velocityA) / (massA + massB);
var v2 = surfaceB.restitution * (velocityA - velocityB) + v1;
// We need the direction that both objects were moving to reflect the velocity correctly.
var v1Direction = Vector3.Reflect(velocityA, -normal).normalized;
var v2Direction = Vector3.Reflect(velocityB, normal).normalized;
// Finally set the velocity of each object.
rigidbodyA.velocity = v1Direction * v1.magnitude;
rigidbodyB.velocity = v2Direction * v2.magnitude;
}
private void CorrectAngularMotion(Rigidbody rigidbodyA, Rigidbody rigidbodyB,
PhysicsMaterial.Result surfaceA, PhysicsMaterial.Result surfaceB, Vector3 normal)
{
// Now do the same for rotation.
var inertiaA = rigidbodyA.inertiaTensor;
var inertiaB = rigidbodyB.inertiaTensor;
var momentumA = rigidbodyA.angularMomentum;
var momentumB = rigidbodyB.angularMomentum;
var velocityA = rigidbodyA.angularVelocity;
var velocityB = rigidbodyB.angularVelocity;
// Conservation of momentum formula:
// V1 = e(I1 + 2 * I2 - m2 * u1) / (m1 + m2).
// V2 = e(u1 - u2) + V1.
var v1 = Maths.Divide(surfaceA.restitution * (momentumA + 2f * momentumB - Maths.Multiply(inertiaB, velocityA)), inertiaA + inertiaB);
var v2 = surfaceB.restitution * (velocityA - velocityB) + v1;
// We need the direction that both objects were moving to reflect the velocity correctly.
var v1Direction = Vector3.Reflect(-momentumA, normal).normalized;
var v2Direction = Vector3.Reflect(-momentumB, normal).normalized;
// Finally set the velocity of each object.
rigidbodyA.angularVelocity = v1Direction * v1.magnitude;
rigidbodyB.angularVelocity = v2Direction * v2.magnitude;
}
private void ImpulseResponse(Collider a, Collider b,
Vector3 direction, float intersection, Vector3 contactPoint, bool initialResponse)
{
// Cache the rigidbodies since we'll use them a lot.
var rigidbodyA = a.attachedRigidbody;
var rigidbodyB = b.attachedRigidbody;
// Determine the surface properties that should be applied to both objects.
var surfaceA = new PhysicsMaterial.Result();
var surfaceB = new PhysicsMaterial.Result();
PhysicsMaterial.Combine(a.material, b.material, out surfaceA, out surfaceB);
// Correct the linear and rotational motion of the object.
if (initialResponse)
{
// Adjust the position.
//DynamicPositionCorrection (a.attachedRigidbody, b.attachedRigidbody, direction, intersection);
CorrectLinearMotion(a.attachedRigidbody, b.attachedRigidbody, surfaceA, surfaceB, direction);
CorrectAngularMotion(a.attachedRigidbody, b.attachedRigidbody, surfaceA, surfaceB, direction);
}
// Apply friction.
ApplyFriction(rigidbodyA, surfaceA.staticFriction, surfaceA.kineticFriction, contactPoint);
ApplyFriction(rigidbodyB, surfaceB.staticFriction, surfaceB.kineticFriction, contactPoint);
}