internal void PreStep(float dt)
{
vehicleEntity = wheel.vehicle.Body;
supportEntity = wheel.supportingEntity;
supportIsDynamic = supportEntity != null && supportEntity.isDynamic;
//The next line is commented out because the world direction is computed by the wheelshape. Weird, but necessary.
//Vector3.TransformNormal(ref myLocalDirection, ref parentA.myInternalOrientationMatrix, out myWorldDirection);
//Set up the jacobians.
linearAX = -wheel.normal.X; //myWorldDirection.X;
linearAY = -wheel.normal.Y; //myWorldDirection.Y;
linearAZ = -wheel.normal.Z; // myWorldDirection.Z;
//linearBX = -linearAX;
//linearBY = -linearAY;
//linearBZ = -linearAZ;
//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;
//Convert spring constant and damping constant into ERP and CFM.
float biasFactor;
springSettings.ComputeErrorReductionAndSoftness(dt, out biasFactor, out softness);
velocityToImpulse = -1 / (entryA + entryB + softness);
//Correction velocity
bias = MathHelper.Min(MathHelper.Max(0, (restLength - currentLength) - allowedCompression) * biasFactor, maximumSpringCorrectionSpeed);
}
internal void PreStep(float dt)
{
Matrix.CreateFromAxisAngle(ref suspension.localDirection, shape.steeringAngle, out shape.steeringTransform);
//Matrix.TransformNormal(ref localForwardDirection, ref shape.steeringTransform, out worldForwardDirection);
Vector3.Transform(ref localForwardDirection, ref shape.steeringTransform, out worldForwardDirection);
Matrix3x3.Transform(ref worldForwardDirection, ref Vehicle.Body.orientationMatrix, out worldForwardDirection);
if (HasSupport)
{
Vector3.Subtract(ref supportLocation, ref Vehicle.Body.position, out ra);
if (supportingEntity != null)
{
Vector3.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;
}
}
