private void IntegrateCallback(object obj)
{
RigidBody body = obj as RigidBody;
TSVector temp;
TSVector.Multiply(ref body.linearVelocity, timestep, out temp);
TSVector.Add(ref temp, ref body.position, out body.position);
if (!(body.isParticle))
{
//exponential map
TSVector axis;
FP angle = body.angularVelocity.magnitude;
FP halfTimeStep = FP.Half * timestep;
FP halfTimeStepAngle = halfTimeStep * angle;
if (angle < FP.EN3)
{
// use Taylor's expansions of sync function
// axis = body.angularVelocity * (FP.Half * timestep - (timestep * timestep * timestep) * (0.020833333333f) * angle * angle);
//TSVector.Multiply(ref body.angularVelocity, (halfTimeStep - (timestep * timestep * timestep) * (2082 * FP.EN6) * angle * angle), out axis);
TSVector.Multiply(ref body.angularVelocity, halfTimeStep, out axis);
}
else
{
// sync(fAngle) = sin(c*fAngle)/t
TSVector.Multiply(ref body.angularVelocity, (FP.Sin(halfTimeStepAngle) / angle), out axis);
}
TSQuaternion dorn = new TSQuaternion(axis.x, axis.y, axis.z, FP.Cos(halfTimeStepAngle));
TSQuaternion ornA;
TSQuaternion.CreateFromMatrix(ref body.orientation, out ornA);
TSQuaternion.Multiply(ref dorn, ref ornA, out dorn);
dorn.Normalize();
TSMatrix.CreateFromQuaternion(ref dorn, out body.orientation);
}
body.linearVelocity /= (1 + timestep * body.linearDrag);
body.angularVelocity /= (1 + timestep * body.angularDrag);
/*if ((body.Damping & RigidBody.DampingType.Linear) != 0)
* TSVector.Multiply(ref body.linearVelocity, currentLinearDampFactor, out body.linearVelocity);
*
* if ((body.Damping & RigidBody.DampingType.Angular) != 0)
* TSVector.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();
}
}
