internal void PreStep(float dt)
{
vehicleEntity = wheel.Vehicle.Body;
supportEntity = wheel.SupportingEntity;
supportIsDynamic = supportEntity != null && supportEntity.isDynamic;
Vector3.Cross(ref wheel.worldForwardDirection, ref wheel.normal, out slidingFrictionAxis);
float axisLength = slidingFrictionAxis.LengthSquared();
//Safety against bad cross product
if (axisLength < Toolbox.BigEpsilon)
{
Vector3.Cross(ref wheel.worldForwardDirection, ref Toolbox.UpVector, out slidingFrictionAxis);
axisLength = slidingFrictionAxis.LengthSquared();
if (axisLength < Toolbox.BigEpsilon)
{
Vector3.Cross(ref wheel.worldForwardDirection, ref Toolbox.RightVector, out slidingFrictionAxis);
}
}
slidingFrictionAxis.Normalize();
linearAX = slidingFrictionAxis.X;
linearAY = slidingFrictionAxis.Y;
linearAZ = slidingFrictionAxis.Z;
//angular A = Ra x N
angularAX = (wheel.ra.Y * linearAZ) - (wheel.ra.Z * linearAY);
angularAY = (wheel.ra.Z * linearAX) - (wheel.ra.X * linearAZ);
angularAZ = (wheel.ra.X * linearAY) - (wheel.ra.Y * linearAX);
//Angular B = N x Rb
angularBX = (linearAY * wheel.rb.Z) - (linearAZ * wheel.rb.Y);
angularBY = (linearAZ * wheel.rb.X) - (linearAX * wheel.rb.Z);
angularBZ = (linearAX * wheel.rb.Y) - (linearAY * wheel.rb.X);
//Compute inverse effective mass matrix
float entryA, entryB;
//these are the transformed coordinates
float tX, tY, tZ;
if (vehicleEntity.isDynamic)
{
tX = angularAX * vehicleEntity.inertiaTensorInverse.M11 + angularAY * vehicleEntity.inertiaTensorInverse.M21 + angularAZ * vehicleEntity.inertiaTensorInverse.M31;
tY = angularAX * vehicleEntity.inertiaTensorInverse.M12 + angularAY * vehicleEntity.inertiaTensorInverse.M22 + angularAZ * vehicleEntity.inertiaTensorInverse.M32;
tZ = angularAX * vehicleEntity.inertiaTensorInverse.M13 + angularAY * vehicleEntity.inertiaTensorInverse.M23 + angularAZ * vehicleEntity.inertiaTensorInverse.M33;
entryA = tX * angularAX + tY * angularAY + tZ * angularAZ + vehicleEntity.inverseMass;
}
else
entryA = 0;
if (supportIsDynamic)
{
tX = angularBX * supportEntity.inertiaTensorInverse.M11 + angularBY * supportEntity.inertiaTensorInverse.M21 + angularBZ * supportEntity.inertiaTensorInverse.M31;
tY = angularBX * supportEntity.inertiaTensorInverse.M12 + angularBY * supportEntity.inertiaTensorInverse.M22 + angularBZ * supportEntity.inertiaTensorInverse.M32;
tZ = angularBX * supportEntity.inertiaTensorInverse.M13 + angularBY * supportEntity.inertiaTensorInverse.M23 + angularBZ * supportEntity.inertiaTensorInverse.M33;
entryB = tX * angularBX + tY * angularBY + tZ * angularBZ + supportEntity.inverseMass;
}
else
entryB = 0;
velocityToImpulse = -1 / (entryA + entryB); //Softness?
//Compute friction.
//Which coefficient? Check velocity.
if (Math.Abs(RelativeVelocity) < staticFrictionVelocityThreshold)
blendedCoefficient = frictionBlender(staticCoefficient, wheel.supportMaterial.staticFriction, false, wheel);
else
blendedCoefficient = frictionBlender(kineticCoefficient, wheel.supportMaterial.kineticFriction, true, wheel);
}