void IPositionUpdateable.PreUpdatePosition(float dt)
{
System.Numerics.Vector3 increment;
Vector3Ex.Multiply(ref angularVelocity, dt * .5f, out increment);
var multiplier = new System.Numerics.Quaternion(increment.X, increment.Y, increment.Z, 0);
QuaternionEx.Multiply(ref multiplier, ref orientation, out multiplier);
QuaternionEx.Add(ref orientation, ref multiplier, out orientation);
orientation = System.Numerics.Quaternion.Normalize(orientation);
Matrix3x3.CreateFromQuaternion(ref orientation, out orientationMatrix);
//Only do the linear motion if this object doesn't obey CCD.
if (PositionUpdateMode == PositionUpdateMode.Discrete)
{
Vector3Ex.Multiply(ref linearVelocity, dt, out increment);
Vector3Ex.Add(ref position, ref increment, out position);
collisionInformation.UpdateWorldTransform(ref position, ref orientation);
//The position update is complete if this is a discretely updated object.
if (PositionUpdated != null)
{
PositionUpdated(this);
}
}
MathChecker.Validate(linearVelocity);
MathChecker.Validate(angularVelocity);
MathChecker.Validate(position);
MathChecker.Validate(orientation);
#if CONSERVE
MathChecker.Validate(angularMomentum);
#endif
}
/// <summary>
/// Integrates the position and orientation of the bone forward based upon the current linear and angular velocity.
/// </summary>
internal void UpdatePosition()
{
//Update the position based on the linear velocity.
Vector3Ex.Add(ref Position, ref linearVelocity, out Position);
//Update the orientation based on the angular velocity.
System.Numerics.Vector3 increment;
Vector3Ex.Multiply(ref angularVelocity, .5f, out increment);
var multiplier = new System.Numerics.Quaternion(increment.X, increment.Y, increment.Z, 0);
QuaternionEx.Multiply(ref multiplier, ref Orientation, out multiplier);
QuaternionEx.Add(ref Orientation, ref multiplier, out Orientation);
Orientation = System.Numerics.Quaternion.Normalize(Orientation);
//Eliminate any latent velocity in the bone to prevent unwanted simulation feedback.
//This is the only thing conceptually separating this "IK" solver from the regular dynamics loop in BEPUphysics.
//(Well, that and the whole lack of collision detection...)
linearVelocity = new System.Numerics.Vector3();
angularVelocity = new System.Numerics.Vector3();
//Note: Unlike a regular dynamics simulation, we do not include any 'dt' parameter in the above integration.
//Setting the velocity to 0 every update means that no more than a single iteration's worth of velocity accumulates.
//Since the softness of constraints already varies with the time step and bones never accelerate for more than one frame,
//scaling the velocity for position integration actually turns out generally worse.
//This is not a rigorously justifiable approach, but this isn't a regular dynamic simulation anyway.
}