void Contact::ApplyPositionChange(Vector3 linearChange[2], Vector3 angularChange[2], marb penetration) {
const marb angularLimit = (marb)0.2f;
marb angularMove[2];
marb linearMove[2];
marb totalInertia = 0;
marb linearInertia[2];
marb angularInertia[2];
// We need to work out the inertia of each object in the direction
// of the contact normal, due to angular inertia only.
for (unsigned i = 0; i < 2; i++) if (body[i]) {
Matrix3 inverseInertiaTensor;
body[i]->GetInverseInertiaTensorWorld(&inverseInertiaTensor);
// Use the same procedure as for calculating frictionless
// velocity change to work out the angular inertia.
Vector3 angularInertiaWorld =
relativeContactPosition[i] % contactNormal;
angularInertiaWorld =
inverseInertiaTensor.Transform(angularInertiaWorld);
angularInertiaWorld =
angularInertiaWorld % relativeContactPosition[i];
angularInertia[i] =
angularInertiaWorld * contactNormal;
// The linear component is simply the inverse mass
linearInertia[i] = body[i]->GetInverseMass();
// Keep track of the total inertia from all components
totalInertia += linearInertia[i] + angularInertia[i];
// We break the loop here so that the totalInertia value is
// completely calculated (by both iterations) before
// continuing.
}
// Loop through again calculating and applying the changes
for (unsigned i = 0; i < 2; i++) if (body[i]) {
// The linear and angular movements required are in proportion to
// the two inverse inertias.
marb sign = (i == 0)?1:-1;
angularMove[i] =
sign * penetration * (angularInertia[i] / totalInertia);
linearMove[i] =
sign * penetration * (linearInertia[i] / totalInertia);
// To avoid angular projections that are too great (when mass is large
// but inertia tensor is small) limit the angular move.
Vector3 projection = relativeContactPosition[i];
projection.AddScaledVector(
contactNormal,
-relativeContactPosition[i].ScalarProduct(contactNormal)
);
// Use the small angle approximation for the sine of the angle (i.e.
// the magnitude would be sine(angularLimit) * projection.magnitude
// but we approximate sine(angularLimit) to angularLimit).
marb maxMagnitude = angularLimit * projection.Magnitude();
if (angularMove[i] < -maxMagnitude) {
marb totalMove = angularMove[i] + linearMove[i];
angularMove[i] = -maxMagnitude;
linearMove[i] = totalMove - angularMove[i];
} else if (angularMove[i] > maxMagnitude) {
marb totalMove = angularMove[i] + linearMove[i];
angularMove[i] = maxMagnitude;
linearMove[i] = totalMove - angularMove[i];
}
// We have the linear amount of movement required by turning
// the rigid body (in angularMove[i]). We now need to
// calculate the desired rotation to achieve that.
if (angularMove[i] == 0) {
// Easy case - no angular movement means no rotation.
angularChange[i].Clear();
} else {
// Work out the direction we'd like to rotate in.
Vector3 targetAngularDirection =
relativeContactPosition[i].VectorProduct(contactNormal);
Matrix3 inverseInertiaTensor;
body[i]->GetInverseInertiaTensorWorld(&inverseInertiaTensor);
// Work out the direction we'd need to rotate to achieve that
angularChange[i] =
inverseInertiaTensor.Transform(targetAngularDirection) *
(angularMove[i] / angularInertia[i]);
}
// Velocity change is easier - it is just the linear movement
// along the contact normal.
linearChange[i] = contactNormal * linearMove[i];
// Now we can start to apply the values we've calculated.
// Apply the linear movement
Vector3 pos;
body[i]->GetPosition(&pos);
pos.AddScaledVector(contactNormal, linearMove[i]);
body[i]->SetPosition(pos);
// And the change in orientation
Quaternion q;
body[i]->GetOrientation(&q);
q.AddScaledVector(angularChange[i], ((marb)1.0));
body[i]->SetOrientation(q);
// We need to calculate the derived data for any body that is
// asleep, so that the changes are reflected in the object's
// data. Otherwise the resolution will not change the position
// of the object, and the next collision detection round will
// have the same penetration.
if (!body[i]->GetAwake()) body[i]->CalculateDerivedData();
}
}
void RigidBodyContact::ApplyPositionChange(Vector3* linearChange, Vector3* angularChange, float penetration)
{
float angularLimit = 5.0f;
float angularMove[2];
float linearMove[2];
float linearInertia[2];
float angularInteria[2];
float totalInertia = 0.0f;
// Calculate the angular inertia using the same process as for calculating frictionless velocity change
for (int i = 0; i < 2; i++)
{
if (Body[i] != NULL)
{
Matrix3x3 inverseInertiaTensor = Body[i]->GetInverseIntertiaTensorWorld();
Vector3 angularInertiaWorld = m_relativeContactPosition[i].Cross(ContactNormal);
angularInertiaWorld = angularInertiaWorld*inverseInertiaTensor; // TODO physics: is order correct here?
angularInertiaWorld = angularInertiaWorld.Cross(m_relativeContactPosition[i]);
angularInteria[i] = angularInertiaWorld.Dot(ContactNormal);
// The linear inertia component is just the inverse mass
linearInertia[i] = Body[i]->GetInverseMass();
// Combine angular and linear to calculate total
totalInertia += linearInertia[i] + angularInteria[i];
}
}
// Calculate and apply changes
for (int i = 0; i < 2; i++)
{
if (Body[i] != NULL)
{
// The linear and angular movements required are in proportion to the two inverse inertias
float sign = (i == 0) ? 1.f : -1.f;
angularMove[i] = sign * penetration * (angularInteria[i] / totalInertia);
linearMove[i] = sign * penetration * (linearInertia[i] / totalInertia);
// To avoid angular projections that are too great (when mass is large but inertia
// tensor is small), limit the angular move
Vector3 projection = m_relativeContactPosition[i];
projection += ContactNormal * -m_relativeContactPosition[i].Dot(ContactNormal);
// Use the small angle approximation for the sine of the angle (i.e. the magnitude would be
// sin(angularLimit) * projection.magnitude, but we approximate sin(angularLimit) to angularLimit.
float maxMagnitude = angularLimit * projection.Magnitude();
if (angularMove[i] < -maxMagnitude)
{
float totalMove = angularMove[i] + linearMove[i];
angularMove[i] = -maxMagnitude;
linearMove[i] = totalMove - angularMove[i];
}
else if (angularMove[i] > maxMagnitude)
{
float totalMove = angularMove[i] + linearMove[i];
angularMove[i] = maxMagnitude;
linearMove[i] = totalMove - angularMove[i];
}
// We have the linear amount of movement required by turning the rigid body (angularMove[i]).
// We now need to calculate the desired rotation to achieve that.
if (Approximately(angularMove[i], 0.f))
{
// Easy case - no angular movement means no rotation.
angularChange[i] = Vector3::Zero;
}
else
{
// Work out the direction we'd like to rotate in
Vector3 targetAngularDirection = m_relativeContactPosition[i].Cross(ContactNormal); // TODO physics: should this get normalized?
// Work out the direction we'd need to rotate to achieve that
Matrix3x3 inverseInertiaTensor = Body[i]->GetInverseIntertiaTensorWorld();
angularChange[i] = (targetAngularDirection*inverseInertiaTensor)* (angularMove[i] / angularInteria[i]); // TODO physics: is order correct here?
}
// Velocity change is just the linear movement along the contact normal
linearChange[i] = ContactNormal * linearMove[i];
// Apply the linear movement
Vector3 pos = Body[i]->GetPosition();
pos += linearMove[i] * ContactNormal;
Body[i]->SetPosition(pos);
// Apply the change in orientation
Quaternion rot = Body[i]->GetRotation();
rot.AddScaledVector(angularChange[i], 1.0);
Body[i]->SetRotation(rot);
// TODO calculate derived data for bodies that are not awake
}
}
}