private void IntegrateCallback(object obj)
{
RigidBody body = obj as RigidBody;
FPVector temp;
FPVector.Multiply(ref body.linearVelocity, timestep, out temp);
FPVector.Add(ref temp, ref body.position, out body.position);
if (!(body.isParticle))
{
//exponential map
FPVector axis;
FP angle = body.angularVelocity.magnitude;
if (angle < FP.EN3)
{
// use Taylor's expansions of sync function
// axis = body.angularVelocity * (FP.Half * timestep - (timestep * timestep * timestep) * (0.020833333333f) * angle * angle);
FPVector.Multiply(ref body.angularVelocity, (FP.Half * timestep - (timestep * timestep * timestep) * (2082 * FP.EN6) * angle * angle), out axis);
}
else
{
// sync(fAngle) = sin(c*fAngle)/t
FPVector.Multiply(ref body.angularVelocity, (FP.Sin(FP.Half * angle * timestep) / angle), out axis);
}
FPQuaternion dorn = new FPQuaternion(axis.x, axis.y, axis.z, FP.Cos(angle * timestep * FP.Half));
FPQuaternion ornA; FPQuaternion.CreateFromMatrix(ref body.orientation, out ornA);
FPQuaternion.Multiply(ref dorn, ref ornA, out dorn);
dorn.Normalize(); FPMatrix.CreateFromQuaternion(ref dorn, out body.orientation);
}
body.linearVelocity *= 1 / (1 + timestep * body.linearDrag);
body.angularVelocity *= 1 / (1 + timestep * body.angularDrag);
/*if ((body.Damping & RigidBody.DampingType.Linear) != 0)
* FPVector.Multiply(ref body.linearVelocity, currentLinearDampFactor, out body.linearVelocity);
*
* if ((body.Damping & RigidBody.DampingType.Angular) != 0)
* FPVector.Multiply(ref body.angularVelocity, currentAngularDampFactor, out body.angularVelocity);*/
body.Update();
if (CollisionSystem.EnableSpeculativeContacts || body.EnableSpeculativeContacts)
{
body.SweptExpandBoundingBox(timestep);
}
}
/// <summary>
/// Constructor.
/// </summary>
/// <param name="body1">The first body which should get constrained. Can be null.</param>
/// <param name="body2">The second body which should get constrained. Can be null.</param>
public Constraint(RigidBody body1, RigidBody body2)
{
this.body1 = body1;
this.body2 = body2;
instanceCount++;
instance = instanceCount;
// calling body.update does not hurt
// if the user set orientations all
// inverse orientations etc. get also
// recalculated.
if (body1 != null)
{
body1.Update();
}
if (body2 != null)
{
body2.Update();
}
}