void Aero::UpdateForceFromTensor(RigidBody *body, marb duration,
const Matrix3 &tensor) {
// Calculate total velocity (windspeed and body's velocity).
Vector3 velocity = body->GetVelocity();
velocity += *windspeed;
// Calculate the velocity in body coordinates
Vector3 bodyVel = body->GetTransform().TransformInverseDirection(velocity);
// Calculate the force in body coordinates
Vector3 bodyForce = tensor.Transform(bodyVel);
Vector3 force = body->GetTransform().TransformDirection(bodyForce);
// Apply the force
body->AddForceAtBodyPoint(force, position);
}
inline Vector3 Contact::CalculateFrictionImpulse(Matrix3 * inverseInertiaTensor) {
Vector3 impulseContact;
marb inverseMass = body[0]->GetInverseMass();
// The equivalent of a cross product in matrices is multiplication
// by a skew symmetric matrix - we build the matrix for converting
// between linear and angular quantities.
Matrix3 impulseToTorque;
impulseToTorque.SetSkewSymmetric(relativeContactPosition[0]);
// Build the matrix to convert contact impulse to change in velocity
// in world coordinates.
Matrix3 deltaVelWorld = impulseToTorque;
deltaVelWorld *= inverseInertiaTensor[0];
deltaVelWorld *= impulseToTorque;
deltaVelWorld *= -1;
// Check if we need to add body two's data
if (body[1]) {
// Set the cross product matrix
impulseToTorque.SetSkewSymmetric(relativeContactPosition[1]);
// Calculate the velocity change matrix
Matrix3 deltaVelWorld2 = impulseToTorque;
deltaVelWorld2 *= inverseInertiaTensor[1];
deltaVelWorld2 *= impulseToTorque;
deltaVelWorld2 *= -1;
// Add to the total delta velocity.
deltaVelWorld += deltaVelWorld2;
// Add to the inverse mass
inverseMass += body[1]->GetInverseMass();
}
// Do a change of basis to convert into contact coordinates.
Matrix3 deltaVelocity = contactToWorld.Transpose();
deltaVelocity *= deltaVelWorld;
deltaVelocity *= contactToWorld;
// Add in the linear velocity change
deltaVelocity.data[0] += inverseMass;
deltaVelocity.data[4] += inverseMass;
deltaVelocity.data[8] += inverseMass;
// Invert to get the impulse needed per unit velocity
Matrix3 impulseMatrix = deltaVelocity.Inverse();
// Find the target velocities to kill
Vector3 velKill(desiredDeltaVelocity,
-contactVelocity.y,
-contactVelocity.z);
// Find the impulse to kill target velocities
impulseContact = impulseMatrix.Transform(velKill);
// Check for exceeding friction
marb planarImpulse = marb_sqrt(impulseContact.y*impulseContact.y + impulseContact.z*impulseContact.z);
if (planarImpulse > impulseContact.x * friction) {
// We need to use dynamic friction
impulseContact.y /= planarImpulse;
impulseContact.z /= planarImpulse;
impulseContact.x = deltaVelocity.data[0] + deltaVelocity.data[1]*friction*impulseContact.y
+ deltaVelocity.data[2]*friction*impulseContact.z;
impulseContact.x = desiredDeltaVelocity / impulseContact.x;
impulseContact.y *= friction * impulseContact.x;
impulseContact.z *= friction * impulseContact.x;
}
return impulseContact;
}
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();
}
}
