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 StaticResponse(Collider dynamic, Collider stationary,
Vector3 direction, float intersection, Vector3 contactPoint, bool initialResponse)
{
// Cache the rigidbody as we'll be using it a lot.
var rigidbody = dynamic.attachedRigidbody;
// Get the surface properties.
var staticFriction = PhysicsMaterial.CalculateStaticFriction(dynamic.material, stationary.material);
var kineticFriction = PhysicsMaterial.CalculateKineticFriction(dynamic.material, stationary.material);
var restitution = PhysicsMaterial.CalculateRestitution(dynamic.material, stationary.material);
if (initialResponse)
{
// Ensure the object is moved away.
//StaticPositionCorrection (rigidbody, direction, intersection);
// We assume the static object has a mass so great it can't be moved and just scale by restitution.
var momentumDirection = Vector3.Reflect(rigidbody.momentum, direction);
rigidbody.momentum = momentumDirection * restitution;
// Now do the same for angular momentum.
var angularDirection = Vector3.Reflect(rigidbody.angularMomentum, direction).normalized;
rigidbody.angularMomentum = -angularDirection * restitution;
}
// Apply friction.
ApplyFriction(rigidbody, staticFriction, kineticFriction, contactPoint);
}
