///<summary>
/// Integrates the rigid body forward in time by the given amount.
/// This function uses a Newton-Euler integration method, which is a
/// linear approximation to the correct integral. For this reason it
/// may be inaccurate in some cases.
///</summary>
public void Integrate(double dt)
{
if (!m_isAwake)
{
return;
}
// Calculate linear acceleration from force inputs.
LastFrameAcceleration = m_acceleration;
LastFrameAcceleration += m_forceAccum * InverseMass;
// Calculate angular acceleration from torque inputs.
Vector3d angularAcceleration = InverseInertiaTensorWorld * m_torqueAccum;
// Adjust velocities
// Update linear velocity from both acceleration and impulse.
Velocity += LastFrameAcceleration * dt;
// Update angular velocity from both acceleration and impulse.
Rotation += angularAcceleration * dt;
// Impose drag.
Velocity *= Math.Pow(LinearDamping, dt);
Rotation *= Math.Pow(AngularDamping, dt);
// Adjust positions
// Update linear position.
Position += Velocity * dt;
// Update angular position.
Orientation.AddScaledVector(Rotation, dt);
// Normalise the orientation, and update the matrices with the new
// position and orientation
CalculateDerivedData();
// Clear accumulators.
ClearAccumulators();
// Update the kinetic energy store, and possibly put the body to
// sleep.
if (m_canSleep)
{
double currentMotion = Vector3d.Dot(Velocity, Velocity) + Vector3d.Dot(Rotation, Rotation);
double bias = Math.Pow(0.5, dt);
m_motion = bias * m_motion + (1 - bias) * currentMotion;
if (m_motion < SleepEpsilon)
{
SetAwake(false);
}
else if (m_motion > 10 * SleepEpsilon)
{
m_motion = 10 * SleepEpsilon;
}
}
}
///<summary>
/// Performs an inertia weighted penetration resolution of this
/// contact alone.
///</summary>
public void ApplyPositionChange(Vector3d[] linearChange, Vector3d[] angularChange, double penetration)
{
double angularLimit = 0.2;
double[] angularMove = new double[2];
double[] linearMove = new double[2];
double totalInertia = 0;
double[] linearInertia = new double[2];
double[] angularInertia = new double[2];
// We need to work out the inertia of each object in the direction
// of the contact normal, due to angular inertia only.
for (int i = 0; i < 2; i++)
{
if (Body[i] == null)
{
continue;
}
Matrix3 inverseInertiaTensor = Body[i].InverseInertiaTensorWorld;
// Use the same procedure as for calculating frictionless
// velocity change to work out the angular inertia.
Vector3d angularInertiaWorld = Vector3d.Cross(RelativeContactPosition[i], ContactNormal);
angularInertiaWorld = inverseInertiaTensor.Transform(angularInertiaWorld);
angularInertiaWorld = Vector3d.Cross(angularInertiaWorld, RelativeContactPosition[i]);
angularInertia[i] = Vector3d.Dot(angularInertiaWorld, ContactNormal);
// The linear component is simply the inverse mass
linearInertia[i] = Body[i].InverseMass;
// 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 (int i = 0; i < 2; i++)
{
if (Body[i] == null)
{
continue;
}
// The linear and angular movements required are in proportion to
// the two inverse inertias.
double 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.
Vector3d projection = RelativeContactPosition[i];
projection += ContactNormal * Vector3d.Dot(-RelativeContactPosition[i], 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).
double maxMagnitude = angularLimit * projection.Magnitude;
if (angularMove[i] < -maxMagnitude)
{
double totalMove = angularMove[i] + linearMove[i];
angularMove[i] = -maxMagnitude;
linearMove[i] = totalMove - angularMove[i];
}
else if (angularMove[i] > maxMagnitude)
{
double 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] = Vector3d.Zero;
}
else
{
// Work out the direction we'd like to rotate in.
Vector3d targetAngularDirection = Vector3d.Cross(RelativeContactPosition[i], ContactNormal);
Matrix3 inverseInertiaTensor = Body[i].InverseInertiaTensorWorld;
// 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
Vector3d pos = Body[i].Position;
pos += ContactNormal * linearMove[i];
Body[i].Position = pos;
// And the change in orientation
Quaternion q = Body[i].Orientation;
q.AddScaledVector(angularChange[i], 1.0);
q.Normalise();
Body[i].Orientation = 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();
}
}
}
