// Integrate - Integrates particle forward in time by specified amount.
void Particle::Integrate(marb duration)
{
assert(duration > 0.0);
position.AddScaledVector(velocity, duration); // Updates linear position.
// Work out the acceleration from the force.
Vector3 resultingAcc = acceleration;
resultingAcc.AddScaledVector(forceAccum, inverseMass);
velocity.AddScaledVector(resultingAcc, duration); // Update linear velocity based on the calculated acceleration.
velocity *= marb_pow(damping, duration); // Inflicts drag on the object by applying damping
ClearAccumulator(); // Clear the forces acting on the particle
}
void Particle::Integrate(real duration)
{
if(inverseMass <= 0.0f) return;
assert(duration > 0.0);
//update linear position
position.AddScaledVector(velocity, duration);
//Work out the acceleration from the force
Vector3 resultingAcceleration = acceleration;
resultingAcceleration.AddScaledVector(forceAccum, inverseMass);
//Update linear velocity from the accerlation
velocity.AddScaledVector(resultingAcceleration, duration);
//Impose Drag
velocity *= real_pow(damping, duration);
//clear the forces
ClearAccumlator();
}
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();
}
}