/// <summary>
/// By calling this method the shape inertia and mass is used.
/// </summary>
public void SetMassProperties()
{
this.inertia = Shape.inertia;
JMatrix.Inverse(ref inertia, out invInertia);
this.inverseMass = GMath.ToFixed(1.0f / Shape.mass);
useShapeMassProperties = true;
}
private void IntegrateCallback(object obj)
{
RigidBody body = obj as RigidBody;
JVector temp;
if (!body.isParticle)
{
body.linearVelocity = GMath.ToFixed(body.linearVelocity);
}
JVector.Multiply(ref body.linearVelocity, timestep, out temp);
if (body.isParticle)
{
body.linearVelocity.Y = 0;
}
JVector.Add(ref temp, ref body.position, out body.position);
if (!(body.isParticle))
{
//exponential map
JVector axis;
float angle = body.angularVelocity.Length();
if (angle < 0.001f)
{
// use Taylor's expansions of sync function
// axis = body.angularVelocity * (0.5f * timestep - (timestep * timestep * timestep) * (0.020833333333f) * angle * angle);
JVector.Multiply(ref body.angularVelocity, (0.5f * timestep - (timestep * timestep * timestep) * (0.020833333333f) * angle * angle), out axis);
}
else
{
// sync(fAngle) = sin(c*fAngle)/t
JVector.Multiply(ref body.angularVelocity, ((float)Math.Sin(0.5f * angle * timestep) / angle), out axis);
}
JQuaternion dorn = new JQuaternion(axis.X, axis.Y, axis.Z, (float)Math.Cos(angle * timestep * 0.5f));
JQuaternion ornA; JQuaternion.CreateFromMatrix(ref body.orientation, out ornA);
JQuaternion.Multiply(ref dorn, ref ornA, out dorn);
dorn.Normalize(); JMatrix.CreateFromQuaternion(ref dorn, out body.orientation);
}
//if ((body.Damping & RigidBody.DampingType.Linear) != 0)
//JVector.Multiply(ref body.linearVelocity, currentLinearDampFactor, out body.linearVelocity);
if ((body.Damping & RigidBody.DampingType.Angular) != 0)
{
JVector.Multiply(ref body.angularVelocity, currentAngularDampFactor, out body.angularVelocity);
}
body.Update();
if (CollisionSystem.EnableSpeculativeContacts || body.EnableSpeculativeContacts)
{
body.SweptExpandBoundingBox(timestep);
}
}
