/// <summary>
/// Restore from the stored state into our current state.
/// </summary>
public void RestoreState()
{
transform = storedTransform;
transformRate = storedTransformRate;
#region REFERENCE: invOrientation = Matrix.Transpose(transform.Orientation);
Matrix.Transpose(ref transform.Orientation, out invOrientation);
#endregion
//worldInvInertia = transform.Orientation * bodyInvInertia * invOrientation;
#region REFERENCE: worldInvInertia = invOrientation *bodyInvInertia* transform.Orientation;
Matrix.Multiply(ref invOrientation, ref bodyInvInertia, out worldInvInertia);
Matrix.Multiply(ref worldInvInertia, ref transform.Orientation, out worldInvInertia);
#endregion
//worldInertia = transform.Orientation * bodyInertia * invOrientation;
#region REFERENCE: worldInertia = invOrientation *bodyInertia * transform.Orientation;
Matrix.Multiply(ref invOrientation, ref bodyInertia, out worldInertia);
Matrix.Multiply(ref worldInertia, ref transform.Orientation, out worldInertia);
#endregion
}
/// <summary>
/// Updates the position with the auxilary velocities, and zeros them.
/// </summary>
/// <param name="dt"></param>
public void UpdatePositionWithAux(float dt)
{
if (immovable || !IsActive)
{
transformRateAux = TransformRate.Zero;
return;
}
PhysicsSystem physics = PhysicsSystem.CurrentPhysicsSystem;
int ga = physics.MainGravityAxis;
if (ga != -1)
{
int ga2 = (ga + 1) % 3;
if (ga2 == 0)
{
transformRateAux.Velocity.X *= 0.1f; transformRateAux.Velocity.Y *= 0.1f;
}
else if (ga2 == 1)
{
transformRateAux.Velocity.Y *= 0.1f; transformRateAux.Velocity.Z *= 0.1f;
}
else if (ga2 == 2)
{
transformRateAux.Velocity.Z *= 0.1f; transformRateAux.Velocity.X *= 0.1f;
}
}
#region REFERENCE: Vector3 angMomBefore = Vector3.Transform(transformRate.AngularVelocity,worldInertia;
Vector3 angMomBefore;
Vector3.Transform(ref transformRate.AngularVelocity, ref worldInertia, out angMomBefore);
#endregion
#region REFERENCE: TransformRate rate = transformRate + tranformRateAux;
TransformRate rate;
TransformRate.Add(ref transformRate, ref transformRateAux, out rate);
#endregion
transform.ApplyTransformRate(ref rate, dt);
#region INLINE: tranformRateAux = TransformRate.Zero;
transformRateAux.AngularVelocity.X = 0.0f;
transformRateAux.AngularVelocity.Y = 0.0f;
transformRateAux.AngularVelocity.Z = 0.0f;
transformRateAux.Velocity.X = 0.0f;
transformRateAux.Velocity.Y = 0.0f;
transformRateAux.Velocity.Z = 0.0f;
#endregion
#region REFERENCE: invOrientation = Matrix.Transpose(transform.Orientation);
Matrix.Transpose(ref transform.Orientation, out invOrientation);
#endregion
// recalculate the world inertia
//worldInvInertia = transform.Orientation * bodyInvInertia * invOrientation;
#region REFERENCE: worldInvInertia = invOrientation * bodyInvInertia* transform.Orientation;
Matrix.Multiply(ref invOrientation, ref bodyInvInertia, out worldInvInertia);
Matrix.Multiply(ref worldInvInertia, ref transform.Orientation, out worldInvInertia);
#endregion
//worldInertia = transform.Orientation * bodyInertia * invOrientation;
#region REFERENCE: worldInertia = invOrientation * bodyInertia* transform.Orientation;
Matrix.Multiply(ref invOrientation, ref bodyInertia, out worldInertia);
Matrix.Multiply(ref worldInertia, ref transform.Orientation, out worldInertia);
#endregion
// conservation of momentum
#region transformRate.AngularVelocity = Vector3.Transform(angMomBefore, worldInvInertia);
Vector3.Transform(ref angMomBefore, ref worldInvInertia, out transformRate.AngularVelocity);
#endregion
if (this.CollisionSkin != null)
{
this.CollisionSkin.SetTransform(ref oldTransform, ref transform);
}
}
/// <summary>
/// Copy our current state (position, velocity etc) into the stored state
/// </summary>
public void StoreState()
{
storedTransform = transform;
storedTransformRate = transformRate;
}
/// <summary>
/// copies the current position etc to old - normally called only
/// by tPhysicsSystem.
/// </summary>
public void CopyCurrentStateToOld()
{
oldTransform = transform;
oldTransformRate = transformRate;
}
