public void UpdateSuspension(float step)
{
float chassisMass = 1.0f / chassisBody.InvMass;
for (int w_it = 0; w_it < NumWheels; w_it++)
{
WheelInfo wheel_info = wheelInfo[w_it];
if (wheel_info.RaycastInfo.IsInContact)
{
float force;
// Spring
{
float susp_length = wheel_info.SuspensionRestLength;
float current_length = wheel_info.RaycastInfo.SuspensionLength;
float length_diff = (susp_length - current_length);
force = wheel_info.SuspensionStiffness
* length_diff * wheel_info.ClippedInvContactDotSuspension;
}
// Damper
{
float projected_rel_vel = wheel_info.SuspensionRelativeVelocity;
{
float susp_damping;
if (projected_rel_vel < 0.0f)
{
susp_damping = wheel_info.WheelsDampingCompression;
}
else
{
susp_damping = wheel_info.WheelsDampingRelaxation;
}
force -= susp_damping * projected_rel_vel;
}
}
// RESULT
wheel_info.WheelsSuspensionForce = force * chassisMass;
if (wheel_info.WheelsSuspensionForce < 0)
{
wheel_info.WheelsSuspensionForce = 0;
}
}
else
{
wheel_info.WheelsSuspensionForce = 0;
}
}
}
void ResetSuspension()
{
for (int i = 0; i < NumWheels; i++)
{
WheelInfo wheel = GetWheelInfo(i);
wheel.RaycastInfo.SuspensionLength = wheel.SuspensionRestLength;
wheel.SuspensionRelativeVelocity = 0;
wheel.RaycastInfo.ContactNormalWS = -wheel.RaycastInfo.WheelDirectionWS;
//wheel.ContactFriction = 0;
wheel.ClippedInvContactDotSuspension = 1;
}
}
void UpdateWheelTransformsWS(WheelInfo wheel, bool interpolatedTransform)
{
wheel.RaycastInfo.IsInContact = false;
Matrix chassisTrans = ChassisWorldTransform;
if (interpolatedTransform && RigidBody.MotionState != null)
{
chassisTrans = RigidBody.MotionState.WorldTransform;
}
wheel.RaycastInfo.HardPointWS = Vector3.TransformCoordinate(wheel.ChassisConnectionPointCS, chassisTrans);
Matrix chassisTransBasis = chassisTrans.Basis;
wheel.RaycastInfo.WheelDirectionWS = Vector3.TransformCoordinate(wheel.WheelDirectionCS, chassisTransBasis);
wheel.RaycastInfo.WheelAxleWS = Vector3.TransformCoordinate(wheel.WheelAxleCS, chassisTransBasis);
}
void UpdateWheelTransformsWS(WheelInfo wheel, bool interpolatedTransform)
{
wheel.RaycastInfo.IsInContact = false;
Matrix chassisTrans = ChassisWorldTransform;
if (interpolatedTransform && RigidBody.MotionState != null)
{
chassisTrans = RigidBody.MotionState.WorldTransform;
}
wheel.RaycastInfo.HardPointWS = Vector3.TransformCoordinate(wheel.ChassisConnectionPointCS, chassisTrans);
Matrix chassisTransBasis = chassisTrans;
chassisTransBasis.Origin = Vector3.Zero;
wheel.RaycastInfo.WheelDirectionWS = Vector3.TransformCoordinate(wheel.WheelDirectionCS, chassisTransBasis);
wheel.RaycastInfo.WheelAxleWS = Vector3.TransformCoordinate(wheel.WheelAxleCS, chassisTransBasis);
}
private float RayCast(WheelInfo wheel)
{
UpdateWheelTransformsWS(wheel, false);
float depth = -1;
float raylen = wheel.SuspensionRestLength + wheel.WheelsRadius;
Vector3 rayvector = wheel.RaycastInfo.WheelDirectionWS * raylen;
Vector3 source = wheel.RaycastInfo.HardPointWS;
wheel.RaycastInfo.ContactPointWS = source + rayvector;
Vector3 target = wheel.RaycastInfo.ContactPointWS;
float param = 0;
VehicleRaycasterResult rayResults = new VehicleRaycasterResult();
Debug.Assert(vehicleRaycaster != null);
object obj = vehicleRaycaster.CastRay(source, target, rayResults);
wheel.RaycastInfo.GroundObject = null;
if (obj != null)
{
param = rayResults.DistFraction;
depth = raylen * rayResults.DistFraction;
wheel.RaycastInfo.ContactNormalWS = rayResults.HitNormalInWorld;
wheel.RaycastInfo.IsInContact = true;
wheel.RaycastInfo.GroundObject = fixedBody;///@todo for driving on dynamic/movable objects!;
/////wheel.RaycastInfo.GroundObject = object;
float hitDistance = param * raylen;
wheel.RaycastInfo.SuspensionLength = hitDistance - wheel.WheelsRadius;
//clamp on max suspension travel
float minSuspensionLength = wheel.SuspensionRestLength - wheel.MaxSuspensionTravelCm * 0.01f;
float maxSuspensionLength = wheel.SuspensionRestLength + wheel.MaxSuspensionTravelCm * 0.01f;
if (wheel.RaycastInfo.SuspensionLength < minSuspensionLength)
{
wheel.RaycastInfo.SuspensionLength = minSuspensionLength;
}
if (wheel.RaycastInfo.SuspensionLength > maxSuspensionLength)
{
wheel.RaycastInfo.SuspensionLength = maxSuspensionLength;
}
wheel.RaycastInfo.ContactPointWS = rayResults.HitPointInWorld;
float denominator = Vector3.Dot(wheel.RaycastInfo.ContactNormalWS, wheel.RaycastInfo.WheelDirectionWS);
Vector3 chassis_velocity_at_contactPoint;
Vector3 relpos = wheel.RaycastInfo.ContactPointWS - RigidBody.CenterOfMassPosition;
chassis_velocity_at_contactPoint = RigidBody.GetVelocityInLocalPoint(relpos);
float projVel = Vector3.Dot(wheel.RaycastInfo.ContactNormalWS, chassis_velocity_at_contactPoint);
if (denominator >= -0.1f)
{
wheel.SuspensionRelativeVelocity = 0;
wheel.ClippedInvContactDotSuspension = 1.0f / 0.1f;
}
else
{
float inv = -1.0f / denominator;
wheel.SuspensionRelativeVelocity = projVel * inv;
wheel.ClippedInvContactDotSuspension = inv;
}
}
else
{
//put wheel info as in rest position
wheel.RaycastInfo.SuspensionLength = wheel.SuspensionRestLength;
wheel.SuspensionRelativeVelocity = 0.0f;
wheel.RaycastInfo.ContactNormalWS = -wheel.RaycastInfo.WheelDirectionWS;
wheel.ClippedInvContactDotSuspension = 1.0f;
}
rayResults.Dispose();
return depth;
}
public void UpdateVehicle(float step)
{
for (int i = 0; i < wheelInfo.Length; i++)
{
UpdateWheelTransform(i, false);
}
currentVehicleSpeedKmHour = 3.6f * RigidBody.LinearVelocity.Length;
Matrix chassisTrans = ChassisWorldTransform;
Vector3 forwardW = new Vector3(
chassisTrans[0, indexForwardAxis],
chassisTrans[1, indexForwardAxis],
chassisTrans[2, indexForwardAxis]);
if (Vector3.Dot(forwardW, RigidBody.LinearVelocity) < 0)
{
currentVehicleSpeedKmHour *= -1.0f;
}
// Simulate suspension
for (int i = 0; i < wheelInfo.Length; i++)
{
//float depth =
RayCast(wheelInfo[i]);
}
UpdateSuspension(step);
for (int i = 0; i < wheelInfo.Length; i++)
{
//apply suspension force
WheelInfo wheel = wheelInfo[i];
float suspensionForce = wheel.WheelsSuspensionForce;
if (suspensionForce > wheel.MaxSuspensionForce)
{
suspensionForce = wheel.MaxSuspensionForce;
}
Vector3 impulse = wheel.RaycastInfo.ContactNormalWS * suspensionForce * step;
Vector3 relpos = wheel.RaycastInfo.ContactPointWS - RigidBody.CenterOfMassPosition;
RigidBody.ApplyImpulse(impulse, relpos);
}
UpdateFriction(step);
for (int i = 0; i < wheelInfo.Length; i++)
{
WheelInfo wheel = wheelInfo[i];
Vector3 relpos = wheel.RaycastInfo.HardPointWS - RigidBody.CenterOfMassPosition;
Vector3 vel = RigidBody.GetVelocityInLocalPoint(relpos);
if (wheel.RaycastInfo.IsInContact)
{
Matrix chassisWorldTransform = ChassisWorldTransform;
Vector3 fwd = new Vector3(
chassisWorldTransform[0, indexForwardAxis],
chassisWorldTransform[1, indexForwardAxis],
chassisWorldTransform[2, indexForwardAxis]);
float proj = Vector3.Dot(fwd, wheel.RaycastInfo.ContactNormalWS);
fwd -= wheel.RaycastInfo.ContactNormalWS * proj;
float proj2;
Vector3.Dot(ref fwd, ref vel, out proj2);
wheel.DeltaRotation = (proj2 * step) / (wheel.WheelsRadius);
wheel.Rotation += wheel.DeltaRotation;
}
else
{
wheel.Rotation += wheel.DeltaRotation;
}
wheel.DeltaRotation *= 0.99f;//damping of rotation when not in contact
}
}
public void UpdateFriction(float timeStep)
{
//calculate the impulse, so that the wheels don't move sidewards
int numWheel = NumWheels;
if (numWheel == 0)
{
return;
}
Array.Resize(ref forwardWS, numWheel);
Array.Resize(ref axle, numWheel);
Array.Resize(ref forwardImpulse, numWheel);
Array.Resize(ref sideImpulse, numWheel);
int numWheelsOnGround = 0;
//collapse all those loops into one!
for (int i = 0; i < NumWheels; i++)
{
RigidBody groundObject = wheelInfo[i].RaycastInfo.GroundObject as RigidBody;
if (groundObject != null)
{
numWheelsOnGround++;
}
sideImpulse[i] = 0;
forwardImpulse[i] = 0;
}
for (int i = 0; i < NumWheels; i++)
{
WheelInfo wheel = wheelInfo[i];
RigidBody groundObject = wheel.RaycastInfo.GroundObject as RigidBody;
if (groundObject != null)
{
Matrix wheelTrans = GetWheelTransformWS(i);
axle[i] = new Vector3(
wheelTrans[0, indexRightAxis],
wheelTrans[1, indexRightAxis],
wheelTrans[2, indexRightAxis]);
Vector3 surfNormalWS = wheel.RaycastInfo.ContactNormalWS;
float proj;
Vector3.Dot(ref axle[i], ref surfNormalWS, out proj);
axle[i] -= surfNormalWS * proj;
axle[i].Normalize();
Vector3.Cross(ref surfNormalWS, ref axle[i], out forwardWS[i]);
forwardWS[i].Normalize();
ResolveSingleBilateral(chassisBody, wheel.RaycastInfo.ContactPointWS,
groundObject, wheel.RaycastInfo.ContactPointWS,
0, axle[i], ref sideImpulse[i], timeStep);
sideImpulse[i] *= sideFrictionStiffness2;
}
}
const float sideFactor = 1.0f;
const float fwdFactor = 0.5f;
bool sliding = false;
for (int i = 0; i < NumWheels; i++)
{
WheelInfo wheel = wheelInfo[i];
RigidBody groundObject = wheel.RaycastInfo.GroundObject as RigidBody;
float rollingFriction = 0.0f;
if (groundObject != null)
{
if (wheel.EngineForce != 0.0f)
{
rollingFriction = wheel.EngineForce * timeStep;
}
else
{
float defaultRollingFrictionImpulse = 0.0f;
float maxImpulse = (wheel.Brake != 0) ? wheel.Brake : defaultRollingFrictionImpulse;
rollingFriction = CalcRollingFriction(chassisBody, groundObject, wheel.RaycastInfo.ContactPointWS, forwardWS[i], maxImpulse);
}
}
//switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break)
forwardImpulse[i] = 0;
wheelInfo[i].SkidInfo = 1.0f;
if (groundObject != null)
{
wheelInfo[i].SkidInfo = 1.0f;
float maximp = wheel.WheelsSuspensionForce * timeStep * wheel.FrictionSlip;
float maximpSide = maximp;
float maximpSquared = maximp * maximpSide;
forwardImpulse[i] = rollingFriction;//wheel.EngineForce* timeStep;
float x = forwardImpulse[i] * fwdFactor;
float y = sideImpulse[i] * sideFactor;
float impulseSquared = (x * x + y * y);
if (impulseSquared > maximpSquared)
{
sliding = true;
float factor = maximp / (float)System.Math.Sqrt(impulseSquared);
wheelInfo[i].SkidInfo *= factor;
}
}
}
if (sliding)
{
for (int wheel = 0; wheel < NumWheels; wheel++)
{
if (sideImpulse[wheel] != 0)
{
if (wheelInfo[wheel].SkidInfo < 1.0f)
{
forwardImpulse[wheel] *= wheelInfo[wheel].SkidInfo;
sideImpulse[wheel] *= wheelInfo[wheel].SkidInfo;
}
}
}
}
// apply the impulses
for (int i = 0; i < NumWheels; i++)
{
WheelInfo wheel = wheelInfo[i];
Vector3 rel_pos = wheel.RaycastInfo.ContactPointWS -
chassisBody.CenterOfMassPosition;
if (forwardImpulse[i] != 0)
{
chassisBody.ApplyImpulse(forwardWS[i] * forwardImpulse[i], rel_pos);
}
if (sideImpulse[i] != 0)
{
RigidBody groundObject = wheel.RaycastInfo.GroundObject as RigidBody;
Vector3 rel_pos2 = wheel.RaycastInfo.ContactPointWS -
groundObject.CenterOfMassPosition;
Vector3 sideImp = axle[i] * sideImpulse[i];
#if ROLLING_INFLUENCE_FIX // fix. It only worked if car's up was along Y - VT.
//Vector4 vChassisWorldUp = RigidBody.CenterOfMassTransform.get_Columns(indexUpAxis);
Vector3 vChassisWorldUp = new Vector3(
RigidBody.CenterOfMassTransform.Row1[indexUpAxis],
RigidBody.CenterOfMassTransform.Row2[indexUpAxis],
RigidBody.CenterOfMassTransform.Row3[indexUpAxis]);
float dot;
Vector3.Dot(ref vChassisWorldUp, ref rel_pos, out dot);
rel_pos -= vChassisWorldUp * (dot * (1.0f - wheel.RollInfluence));
#else
rel_pos[indexUpAxis] *= wheel.RollInfluence;
#endif
chassisBody.ApplyImpulse(sideImp, rel_pos);
//apply friction impulse on the ground
groundObject.ApplyImpulse(-sideImp, rel_pos2);
}
}
}
public WheelInfo AddWheel(Vector3 connectionPointCS, Vector3 wheelDirectionCS0, Vector3 wheelAxleCS, float suspensionRestLength, float wheelRadius, VehicleTuning tuning, bool isFrontWheel)
{
WheelInfoConstructionInfo ci = new WheelInfoConstructionInfo();
ci.ChassisConnectionCS = connectionPointCS;
ci.WheelDirectionCS = wheelDirectionCS0;
ci.WheelAxleCS = wheelAxleCS;
ci.SuspensionRestLength = suspensionRestLength;
ci.WheelRadius = wheelRadius;
ci.SuspensionStiffness = tuning.SuspensionStiffness;
ci.WheelsDampingCompression = tuning.SuspensionCompression;
ci.WheelsDampingRelaxation = tuning.SuspensionDamping;
ci.FrictionSlip = tuning.FrictionSlip;
ci.IsFrontWheel = isFrontWheel;
ci.MaxSuspensionTravelCm = tuning.MaxSuspensionTravelCm;
ci.MaxSuspensionForce = tuning.MaxSuspensionForce;
Array.Resize<WheelInfo>(ref wheelInfo, wheelInfo.Length + 1);
WheelInfo wheel = new WheelInfo(ci);
wheelInfo[wheelInfo.Length - 1] = wheel;
UpdateWheelTransformsWS(wheel, false);
UpdateWheelTransform(NumWheels - 1, false);
return wheel;
}
private float RayCast(WheelInfo wheel)
{
UpdateWheelTransformsWS(wheel, false);
float depth = -1;
float raylen = wheel.SuspensionRestLength + wheel.WheelsRadius;
Vector3 rayvector = wheel.RaycastInfo.WheelDirectionWS * raylen;
Vector3 source = wheel.RaycastInfo.HardPointWS;
wheel.RaycastInfo.ContactPointWS = source + rayvector;
Vector3 target = wheel.RaycastInfo.ContactPointWS;
float param = 0;
VehicleRaycasterResult rayResults = new VehicleRaycasterResult();
Debug.Assert(vehicleRaycaster != null);
object obj = vehicleRaycaster.CastRay(ref source, ref target, rayResults);
wheel.RaycastInfo.GroundObject = null;
if (obj != null)
{
param = rayResults.DistFraction;
depth = raylen * rayResults.DistFraction;
wheel.RaycastInfo.ContactNormalWS = rayResults.HitNormalInWorld;
wheel.RaycastInfo.IsInContact = true;
wheel.RaycastInfo.GroundObject = fixedBody;///@todo for driving on dynamic/movable objects!;
/////wheel.RaycastInfo.GroundObject = object;
float hitDistance = param * raylen;
wheel.RaycastInfo.SuspensionLength = hitDistance - wheel.WheelsRadius;
//clamp on max suspension travel
float minSuspensionLength = wheel.SuspensionRestLength - wheel.MaxSuspensionTravelCm * 0.01f;
float maxSuspensionLength = wheel.SuspensionRestLength + wheel.MaxSuspensionTravelCm * 0.01f;
if (wheel.RaycastInfo.SuspensionLength < minSuspensionLength)
{
wheel.RaycastInfo.SuspensionLength = minSuspensionLength;
}
if (wheel.RaycastInfo.SuspensionLength > maxSuspensionLength)
{
wheel.RaycastInfo.SuspensionLength = maxSuspensionLength;
}
wheel.RaycastInfo.ContactPointWS = rayResults.HitPointInWorld;
float denominator = Vector3.Dot(wheel.RaycastInfo.ContactNormalWS, wheel.RaycastInfo.WheelDirectionWS);
Vector3 chassis_velocity_at_contactPoint;
Vector3 relpos = wheel.RaycastInfo.ContactPointWS - RigidBody.CenterOfMassPosition;
chassis_velocity_at_contactPoint = RigidBody.GetVelocityInLocalPoint(relpos);
float projVel = Vector3.Dot(wheel.RaycastInfo.ContactNormalWS, chassis_velocity_at_contactPoint);
if (denominator >= -0.1f)
{
wheel.SuspensionRelativeVelocity = 0;
wheel.ClippedInvContactDotSuspension = 1.0f / 0.1f;
}
else
{
float inv = -1.0f / denominator;
wheel.SuspensionRelativeVelocity = projVel * inv;
wheel.ClippedInvContactDotSuspension = inv;
}
}
else
{
//put wheel info as in rest position
wheel.RaycastInfo.SuspensionLength = wheel.SuspensionRestLength;
wheel.SuspensionRelativeVelocity = 0.0f;
wheel.RaycastInfo.ContactNormalWS = -wheel.RaycastInfo.WheelDirectionWS;
wheel.ClippedInvContactDotSuspension = 1.0f;
}
return(depth);
}
public static void SetWheelDirectionCS(this WheelInfo obj, OpenTK.Vector3 value)
{
SetWheelDirectionCS(obj, ref value);
}
public static void SetWorldTransform(this WheelInfo obj, OpenTK.Matrix4 value)
{
SetWorldTransform(obj, ref value);
}
public static void SetChassisConnectionPointCS(this WheelInfo obj, OpenTK.Vector3 value)
{
SetChassisConnectionPointCS(obj, ref value);
}
public static OpenTK.Matrix4 GetWorldTransform(this WheelInfo obj)
{
OpenTK.Matrix4 value;
GetWorldTransform(obj, out value);
return(value);
}
public static OpenTK.Vector3 GetWheelDirectionCS(this WheelInfo obj)
{
OpenTK.Vector3 value;
GetWheelDirectionCS(obj, out value);
return(value);
}
public static OpenTK.Vector3 GetChassisConnectionPointCS(this WheelInfo obj)
{
OpenTK.Vector3 value;
GetChassisConnectionPointCS(obj, out value);
return(value);
}
/// <summary>
/// Changes wheel friction depending on our current speed
/// </summary>
protected void ChangeFriction(WheelInfo info, float currentSpeed)
{
// really strong friction so we don't roll down hills when we're stopped
if (AccelerateMultiplier == 0f && currentSpeed > -2f && currentSpeed < 2f)
info.FrictionSlip = 10000f;
// if we're going slower than the slow speed, keep friction at maximum
else if (currentSpeed < DefaultSlowSpeed)
info.FrictionSlip = DefaultFrictionSlip;
// otherwise, change friction based on speed to a minimum
else {
float newFric = this.Friction * (1 - ((currentSpeed - DefaultSlowSpeed) / ((kart.MaxSpeed * 3.6f) - DefaultSlowSpeed)));
info.FrictionSlip = System.Math.Max(newFric, MINIMUM_FRICTION);
}
}
