public void ApplyTransformRate(ref TransformRate rate, float dt)
{
//Position += dt * rate.Velocity;
Vector3 pos;
Vector3.Multiply(ref rate.Velocity, dt, out pos);
Vector3.Add(ref Position, ref pos, out Position);
Vector3 dir = rate.AngularVelocity;
float ang = dir.Length();
if (ang > 0.0f)
{
Vector3.Divide(ref dir, ang, out dir); // dir /= ang;
ang *= dt;
Matrix rot;
Matrix.RotationAxis(ref dir, ang, out rot);
Matrix.Multiply(ref Orientation, ref rot, out Orientation);
}
//JiggleMath.Orthonormalise(ref this.Orientation);
}
/// <summary>
/// copies the current position etc to old - normally called only
/// by tPhysicsSystem.
/// </summary>
public void CopyCurrentStateToOld()
{
oldTransform = transform;
oldTransformRate = transformRate;
}
/// <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.TransformCoordinate(transformRate.AngularVelocity,worldInertia;
Vector3 angMomBefore;
Vector3.TransformCoordinate(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.TransformCoordinate(angMomBefore, worldInvInertia);
Vector3.TransformCoordinate(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>
/// 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
}
public static void Add(ref TransformRate rate1, ref TransformRate rate2 ,out TransformRate result)
{
Vector3.Add(ref rate1.Velocity, ref rate2.Velocity, out result.Velocity);
Vector3.Add(ref rate1.AngularVelocity, ref rate2.AngularVelocity, out result.AngularVelocity);
}
public static TransformRate Add(TransformRate rate1, TransformRate rate2)
{
TransformRate result = new TransformRate();
Vector3.Add(ref rate1.Velocity, ref rate2.Velocity, out result.Velocity);
Vector3.Add(ref rate1.AngularVelocity, ref rate2.AngularVelocity, out result.AngularVelocity);
return result;
}