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);
}
}
internal void PostStep()
{
if (_freezeConstraints > 0)
{
bool freezePosX = (_freezeConstraints & FPRigidBodyConstraints.FreezePositionX) == FPRigidBodyConstraints.FreezePositionX;
bool freezePosY = (_freezeConstraints & FPRigidBodyConstraints.FreezePositionY) == FPRigidBodyConstraints.FreezePositionY;
bool freezePosZ = (_freezeConstraints & FPRigidBodyConstraints.FreezePositionZ) == FPRigidBodyConstraints.FreezePositionZ;
if (freezePosX)
{
position.x = _freezePosition.x;
}
if (freezePosY)
{
position.y = _freezePosition.y;
}
if (freezePosZ)
{
position.z = _freezePosition.z;
}
bool freezeRotX = (_freezeConstraints & FPRigidBodyConstraints.FreezeRotationX) == FPRigidBodyConstraints.FreezeRotationX;
bool freezeRotY = (_freezeConstraints & FPRigidBodyConstraints.FreezeRotationY) == FPRigidBodyConstraints.FreezeRotationY;
bool freezeRotZ = (_freezeConstraints & FPRigidBodyConstraints.FreezeRotationZ) == FPRigidBodyConstraints.FreezeRotationZ;
if (freezeRotX || freezeRotY || freezeRotZ)
{
FPQuaternion q = FPQuaternion.CreateFromMatrix(Orientation);
if (freezeRotX)
{
q.x = _freezeRotationQuat.x;
}
if (freezeRotY)
{
q.y = _freezeRotationQuat.y;
}
if (freezeRotZ)
{
q.z = _freezeRotationQuat.z;
}
q.Normalize();
Orientation = FPMatrix.CreateFromQuaternion(q);
}
}
}
public override void PostStep()
{
FPVector pos = Body1.Position;
pos.z = 0;
Body1.Position = pos;
FPQuaternion q = FPQuaternion.CreateFromMatrix(Body1.Orientation);
q.Normalize();
q.x = 0;
q.y = 0;
if (freezeZAxis)
{
q.z = 0;
}
Body1.Orientation = FPMatrix.CreateFromQuaternion(q);
if (Body1.isStatic)
{
return;
}
FPVector vel = Body1.LinearVelocity;
vel.z = 0;
Body1.LinearVelocity = vel;
FPVector av = Body1.AngularVelocity;
av.x = 0;
av.y = 0;
if (freezeZAxis)
{
av.z = 0;
}
Body1.AngularVelocity = av;
}
public static Quaternion ToQuaternion(this FPMatrix jMatrix)
{
return(FPQuaternion.CreateFromMatrix(jMatrix).ToQuaternion());
}