internal void PreStep(float dt)
{
Matrix4x4Ex.CreateFromAxisAngle(ref suspension.localDirection, shape.steeringAngle, out shape.steeringTransform);
Matrix4x4Ex.TransformNormal(ref localForwardDirection, ref shape.steeringTransform, out worldForwardDirection);
Matrix3x3.Transform(ref worldForwardDirection, ref Vehicle.Body.orientationMatrix, out worldForwardDirection);
if (HasSupport)
{
Vector3Ex.Subtract(ref supportLocation, ref Vehicle.Body.position, out ra);
if (supportingEntity != null)
{
Vector3Ex.Subtract(ref supportLocation, ref SupportingEntity.position, out rb);
}
//Mind the order of updating! sliding friction must come before driving force or rolling friction
//because it computes the sliding direction.
suspension.isActive = true;
suspension.numIterationsAtZeroImpulse = 0;
suspension.solverSettings.currentIterations = 0;
slidingFriction.isActive = true;
slidingFriction.numIterationsAtZeroImpulse = 0;
slidingFriction.solverSettings.currentIterations = 0;
drivingMotor.isActive = true;
drivingMotor.numIterationsAtZeroImpulse = 0;
drivingMotor.solverSettings.currentIterations = 0;
brake.isActive = true;
brake.numIterationsAtZeroImpulse = 0;
brake.solverSettings.currentIterations = 0;
suspension.PreStep(dt);
slidingFriction.PreStep(dt);
drivingMotor.PreStep(dt);
brake.PreStep(dt);
}
else
{
//No support, don't need any solving.
suspension.isActive = false;
slidingFriction.isActive = false;
drivingMotor.isActive = false;
brake.isActive = false;
suspension.accumulatedImpulse = 0;
slidingFriction.accumulatedImpulse = 0;
drivingMotor.accumulatedImpulse = 0;
brake.accumulatedImpulse = 0;
}
}
/// <summary>
/// Updates the wheel's world transform for graphics.
/// Called automatically by the owning wheel at the end of each frame.
/// If the engine is updating asynchronously, you can call this inside of a space read buffer lock
/// and update the wheel transforms safely.
/// </summary>
public override void UpdateWorldTransform()
{
#if !WINDOWS
System.Numerics.Vector3 newPosition = new System.Numerics.Vector3();
#else
System.Numerics.Vector3 newPosition;
#endif
System.Numerics.Vector3 worldAttachmentPoint;
System.Numerics.Vector3 localAttach;
Vector3Ex.Add(ref wheel.suspension.localAttachmentPoint, ref wheel.vehicle.Body.CollisionInformation.localPosition, out localAttach);
worldTransform = Matrix3x3.ToMatrix4X4(wheel.vehicle.Body.BufferedStates.InterpolatedStates.OrientationMatrix);
Matrix4x4Ex.TransformNormal(ref localAttach, ref worldTransform, out worldAttachmentPoint);
worldAttachmentPoint += wheel.vehicle.Body.BufferedStates.InterpolatedStates.Position;
System.Numerics.Vector3 worldDirection;
Matrix4x4Ex.Transform(ref wheel.suspension.localDirection, ref worldTransform, out worldDirection);
float length = wheel.suspension.currentLength - graphicalRadius;
newPosition.X = worldAttachmentPoint.X + worldDirection.X * length;
newPosition.Y = worldAttachmentPoint.Y + worldDirection.Y * length;
newPosition.Z = worldAttachmentPoint.Z + worldDirection.Z * length;
System.Numerics.Matrix4x4 spinTransform;
System.Numerics.Vector3 localSpinAxis;
Vector3Ex.Cross(ref wheel.localForwardDirection, ref wheel.suspension.localDirection, out localSpinAxis);
Matrix4x4Ex.CreateFromAxisAngle(ref localSpinAxis, spinAngle, out spinTransform);
System.Numerics.Matrix4x4 localTurnTransform;
Matrix4x4Ex.Multiply(ref localGraphicTransform, ref spinTransform, out localTurnTransform);
Matrix4x4Ex.Multiply(ref localTurnTransform, ref steeringTransform, out localTurnTransform);
//System.Numerics.Matrix4x4.Multiply(ref localTurnTransform, ref spinTransform, out localTurnTransform);
Matrix4x4Ex.Multiply(ref localTurnTransform, ref worldTransform, out worldTransform);
worldTransform.Translation += newPosition;
}