public void UpdateSuspension(float deltaTime)
{
float chassisMass = 1f / m_chassisBody.GetInvMass();
for (int w_it = 0; w_it < GetNumWheels(); w_it++)
{
WheelInfo wheel_info = m_wheelInfo[w_it];
if (wheel_info.m_raycastInfo.m_isInContact)
{
float force = 0f;
// Spring
{
float susp_length = wheel_info.GetSuspensionRestLength();
float current_length = wheel_info.m_raycastInfo.m_suspensionLength;
float length_diff = (susp_length - current_length);
//length_diff = current_length;
force = wheel_info.m_suspensionStiffness
* length_diff * wheel_info.m_clippedInvContactDotSuspension;
}
// Damper
{
float projected_rel_vel = wheel_info.m_suspensionRelativeVelocity;
{
float susp_damping;
if (projected_rel_vel < 0f)
{
susp_damping = wheel_info.m_wheelsDampingCompression;
}
else
{
susp_damping = wheel_info.m_wheelsDampingRelaxation;
}
force -= susp_damping * projected_rel_vel;
}
}
// RESULT
wheel_info.m_wheelsSuspensionForce = force * chassisMass;
if (wheel_info.m_wheelsSuspensionForce < 0f)
{
wheel_info.m_wheelsSuspensionForce = 0f;
}
}
else
{
wheel_info.m_wheelsSuspensionForce = 0f;
}
}
}
public void UpdateWheelTransformsWS(WheelInfo wheel, bool interpolatedTransform)
{
wheel.m_raycastInfo.m_isInContact = false;
IndexedMatrix chassisTrans = GetChassisWorldTransform();
if (interpolatedTransform && (GetRigidBody().GetMotionState() != null))
{
GetRigidBody().GetMotionState().GetWorldTransform(out chassisTrans);
}
wheel.m_raycastInfo.m_hardPointWS = chassisTrans * wheel.m_chassisConnectionPointCS;
wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans._basis * wheel.m_wheelDirectionCS;
wheel.m_raycastInfo.m_wheelAxleWS = chassisTrans._basis * wheel.m_wheelAxleCS;
}
public float RayCast(WheelInfo wheel)
{
UpdateWheelTransformsWS(wheel, false);
float depth = -1;
float raylen = wheel.GetSuspensionRestLength() + wheel.m_wheelsRadius;
IndexedVector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen);
IndexedVector3 source = wheel.m_raycastInfo.m_hardPointWS;
wheel.m_raycastInfo.m_contactPointWS = source + rayvector;
IndexedVector3 target = wheel.m_raycastInfo.m_contactPointWS;
float param = 0f;
VehicleRaycasterResult rayResults = new VehicleRaycasterResult();
Debug.Assert(m_vehicleRaycaster != null);
Object object1 = m_vehicleRaycaster.CastRay(ref source, ref target, ref rayResults);
if (object1 != null && (object1 as RigidBody).m_debugBodyId != 1)
{
int ibreak = 0;
}
//{
// IndexedVector3 from1 = new IndexedVector3(0.7957098f, -9.13606f, 1.794605f);
// IndexedVector3 to1 = new IndexedVector3(0.886791f, -10.23207f, 1.815941f);
// VehicleRaycasterResult results1 = new VehicleRaycasterResult();
// Object o2 = m_vehicleRaycaster.castRay(ref from1, ref to1, ref results1);
// IndexedVector3 from2 = new IndexedVector3(0.7957281f, -9.136093f, 1.794625f);
// IndexedVector3 to2 = new IndexedVector3(0.8867911f, -10.23211f, 1.815956f);
// VehicleRaycasterResult results2 = new VehicleRaycasterResult();
// Object o3 = m_vehicleRaycaster.castRay(ref from2, ref to2, ref results2);
// if (Math.Abs(results1.m_distFraction - results2.m_distFraction) > 0.1f)
// {
// int ibreak = 0;
// }
//}
wheel.m_raycastInfo.m_groundObject = null;
if (object1 != null)
{
param = rayResults.m_distFraction;
depth = raylen * rayResults.m_distFraction;
wheel.m_raycastInfo.m_contactNormalWS = rayResults.m_hitNormalInWorld;
wheel.m_raycastInfo.m_isInContact = true;
wheel.m_raycastInfo.m_groundObject = s_fixedObject;///@todo for driving on dynamic/movable objects!;
//wheel.m_raycastInfo.m_groundObject = object;
float hitDistance = param * raylen;
wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelsRadius;
//clamp on max suspension travel
float minSuspensionLength = wheel.GetSuspensionRestLength() - wheel.m_maxSuspensionTravelCm * 0.01f;
float maxSuspensionLength = wheel.GetSuspensionRestLength() + wheel.m_maxSuspensionTravelCm * 0.01f;
if (wheel.m_raycastInfo.m_suspensionLength < minSuspensionLength)
{
wheel.m_raycastInfo.m_suspensionLength = minSuspensionLength;
}
if (wheel.m_raycastInfo.m_suspensionLength > maxSuspensionLength)
{
wheel.m_raycastInfo.m_suspensionLength = maxSuspensionLength;
}
if (Math.Abs(wheel.m_raycastInfo.m_suspensionLength - wheel.m_raycastInfo.m_suspensionLengthBak) > 0.1f)
{
int ibreak = 0;
}
wheel.m_raycastInfo.m_suspensionLengthBak = wheel.m_raycastInfo.m_suspensionLength;
wheel.m_raycastInfo.m_contactPointWS = rayResults.m_hitPointInWorld;
float denominator = IndexedVector3.Dot(wheel.m_raycastInfo.m_contactNormalWS, wheel.m_raycastInfo.m_wheelDirectionWS);
IndexedVector3 chassis_velocity_at_contactPoint;
IndexedVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - GetRigidBody().GetCenterOfMassPosition();
chassis_velocity_at_contactPoint = GetRigidBody().GetVelocityInLocalPoint(ref relpos);
float projVel = IndexedVector3.Dot(wheel.m_raycastInfo.m_contactNormalWS, chassis_velocity_at_contactPoint);
if (projVel > 1f)
{
int ibreak = 0;
}
if (denominator >= -0.1f)
{
wheel.m_suspensionRelativeVelocity = 0f;
wheel.m_clippedInvContactDotSuspension = 1.0f / 0.1f;
}
else
{
float inv = -1f / denominator;
wheel.m_suspensionRelativeVelocity = projVel * inv;
wheel.m_clippedInvContactDotSuspension = inv;
}
}
else
{
//put wheel info as in rest position
wheel.m_raycastInfo.m_suspensionLength = wheel.GetSuspensionRestLength();
wheel.m_suspensionRelativeVelocity = 0.0f;
wheel.m_raycastInfo.m_contactNormalWS = -wheel.m_raycastInfo.m_wheelDirectionWS;
wheel.m_clippedInvContactDotSuspension = 1.0f;
}
return(depth);
}
public virtual void UpdateFriction(float timeStep)
{
//calculate the impulse, so that the wheels don't move sidewards
int numWheel = GetNumWheels();
if (numWheel == 0)
{
return;
}
//m_forwardWS.resize(numWheel);
//m_axle.resize(numWheel);
//m_forwardImpulse.resize(numWheel);
//m_sideImpulse.resize(numWheel);
int numWheelsOnGround = 0;
//collapse all those loops into one!
for (int i = 0; i < numWheel; i++)
{
WheelInfo wheelInfo = m_wheelInfo[i];
RigidBody groundObject = wheelInfo.m_raycastInfo.m_groundObject as RigidBody;
if (groundObject != null)
{
numWheelsOnGround++;
}
m_sideImpulse[i] = 0f;
m_forwardImpulse[i] = 0f;
}
if (numWheelsOnGround != 4)
{
int ibreak = 0;
}
{
//foreach(WheelInfo wheelInfo in m_wheelInfo)
for (int i = 0; i < numWheel; ++i)
{
WheelInfo wheelInfo = m_wheelInfo[i];
RigidBody groundObject = wheelInfo.m_raycastInfo.m_groundObject as RigidBody;
if (groundObject != null)
{
IndexedMatrix wheelTrans = GetWheelTransformWS(i);
IndexedBasisMatrix wheelBasis0 = wheelTrans._basis;
m_axle[i] = new IndexedVector3(
wheelBasis0._el0[m_indexRightAxis],
wheelBasis0._el1[m_indexRightAxis],
wheelBasis0._el2[m_indexRightAxis]);
IndexedVector3 surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
float proj = IndexedVector3.Dot(m_axle[i], surfNormalWS);
m_axle[i] -= surfNormalWS * proj;
m_axle[i].Normalize();
m_forwardWS[i] = IndexedVector3.Cross(surfNormalWS, m_axle[i]);
m_forwardWS[i].Normalize();
IndexedVector3 tempAxle = m_axle[i];
float tempImpulse = m_sideImpulse[i];
ContactConstraint.ResolveSingleBilateral(m_chassisBody, ref wheelInfo.m_raycastInfo.m_contactPointWS,
groundObject, ref wheelInfo.m_raycastInfo.m_contactPointWS,
0f, ref tempAxle, ref tempImpulse, timeStep);
m_sideImpulse[i] = (tempImpulse * sideFrictionStiffness2);
}
}
}
float sideFactor = 1f;
float fwdFactor = 0.5f;
bool sliding = false;
{
for (int wheel = 0; wheel < numWheel; wheel++)
{
WheelInfo wheelInfo = m_wheelInfo[wheel];
RigidBody groundObject = wheelInfo.m_raycastInfo.m_groundObject as RigidBody;
float rollingFriction = 0f;
if (groundObject != null)
{
if (wheelInfo.m_engineForce != 0.0f)
{
rollingFriction = wheelInfo.m_engineForce * timeStep;
}
else
{
float defaultRollingFrictionImpulse = 0f;
float maxImpulse = (wheelInfo.m_brake != 0f) ? wheelInfo.m_brake : defaultRollingFrictionImpulse;
IndexedVector3 tempWheel = m_forwardWS[wheel];
WheelContactPoint contactPt = new WheelContactPoint(m_chassisBody, groundObject, ref wheelInfo.m_raycastInfo.m_contactPointWS, ref tempWheel, maxImpulse);
m_forwardWS[wheel] = tempWheel;
rollingFriction = CalcRollingFriction(contactPt);
}
}
//switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break)
m_forwardImpulse[wheel] = 0f;
m_wheelInfo[wheel].m_skidInfo = 1f;
if (groundObject != null)
{
m_wheelInfo[wheel].m_skidInfo = 1f;
float maximp = wheelInfo.m_wheelsSuspensionForce * timeStep * wheelInfo.m_frictionSlip;
float maximpSide = maximp;
float maximpSquared = maximp * maximpSide;
m_forwardImpulse[wheel] = rollingFriction; //wheelInfo.m_engineForce* timeStep;
float x = (m_forwardImpulse[wheel]) * fwdFactor;
float y = (m_sideImpulse[wheel]) * sideFactor;
float impulseSquared = (x * x + y * y);
if (impulseSquared > maximpSquared)
{
sliding = true;
float factor = (float)(maximp / Math.Sqrt(impulseSquared));
m_wheelInfo[wheel].m_skidInfo *= factor;
}
}
}
}
if (sliding)
{
for (int wheel = 0; wheel < numWheel; wheel++)
{
if (m_sideImpulse[wheel] != 0f)
{
if (m_wheelInfo[wheel].m_skidInfo < 1f)
{
m_forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
}
}
}
}
// apply the impulses
{
for (int wheel = 0; wheel < numWheel; wheel++)
{
WheelInfo wheelInfo = m_wheelInfo[wheel];
IndexedVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS -
m_chassisBody.GetCenterOfMassPosition();
if (m_forwardImpulse[wheel] > 5f || m_sideImpulse[wheel] > 5f)
{
int ibreak = 0;
}
if (m_forwardImpulse[wheel] != 0f)
{
m_chassisBody.ApplyImpulse(m_forwardWS[wheel] * (m_forwardImpulse[wheel]), rel_pos);
}
if (m_sideImpulse[wheel] != 0f)
{
RigidBody groundObject = m_wheelInfo[wheel].m_raycastInfo.m_groundObject as RigidBody;
IndexedVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS -
groundObject.GetCenterOfMassPosition();
IndexedVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel];
#if ROLLING_INFLUENCE_FIX // fix. It only worked if car's up was along Y - VT.
IndexedVector3 vChassisWorldUp = GetRigidBody().GetCenterOfMassTransform()._basis.GetColumn(m_indexUpAxis);
rel_pos -= vChassisWorldUp * (IndexedVector3.Dot(vChassisWorldUp, rel_pos) * (1.0f - wheelInfo.m_rollInfluence));
#else
rel_pos[m_indexUpAxis] *= wheelInfo.m_rollInfluence;
#endif
m_chassisBody.ApplyImpulse(ref sideImp, ref rel_pos);
//apply friction impulse on the ground
IndexedVector3 temp = -sideImp;
groundObject.ApplyImpulse(ref temp, ref rel_pos2);
}
}
}
}
public virtual void UpdateVehicle(float step)
{
int numWheels = GetNumWheels();
{
for (int i = 0; i < numWheels; i++)
{
UpdateWheelTransform(i, false);
}
}
m_currentVehicleSpeedKmHour = 3.6f * GetRigidBody().GetLinearVelocity().Length();
IndexedMatrix chassisTrans = GetChassisWorldTransform();
IndexedVector3 forwardW = new IndexedVector3(
chassisTrans._basis[0, m_indexForwardAxis],
chassisTrans._basis[1, m_indexForwardAxis],
chassisTrans._basis[2, m_indexForwardAxis]);
if (IndexedVector3.Dot(forwardW, GetRigidBody().GetLinearVelocity()) < 0f)
{
m_currentVehicleSpeedKmHour *= -1f;
}
//
// simulate suspension
//
float[] depthData = new float[numWheels];
float depth = 0f;
for (int i = 0; i < numWheels; i++)
{
depth = RayCast(m_wheelInfo[i]);
depthData[i] = depth;
if (m_wheelInfo[i].m_raycastInfo.m_isInContact == false)
{
int ibreak = 0;
depth = RayCast(m_wheelInfo[i]);
depthData[i] = depth;
}
}
UpdateSuspension(step);
for (int i = 0; i < numWheels; ++i)
{
WheelInfo wheel = m_wheelInfo[i];
//apply suspension force
float suspensionForce = wheel.m_wheelsSuspensionForce;
if (suspensionForce > wheel.m_maxSuspensionForce)
{
suspensionForce = wheel.m_maxSuspensionForce;
}
IndexedVector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step;
IndexedVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - GetRigidBody().GetCenterOfMassPosition();
if (impulse.Y < 30 || impulse.Y > 40)
{
int ibreak = 0;
}
//impulse = new IndexedVector3(0f, 1f, 0f);
GetRigidBody().ApplyImpulse(ref impulse, ref relpos);
}
UpdateFriction(step);
for (int i = 0; i < numWheels; ++i)
{
WheelInfo wheel = m_wheelInfo[i];
IndexedVector3 relpos = wheel.m_raycastInfo.m_hardPointWS - GetRigidBody().GetCenterOfMassPosition();
IndexedVector3 vel = GetRigidBody().GetVelocityInLocalPoint(ref relpos);
if (wheel.m_raycastInfo.m_isInContact)
{
IndexedMatrix chassisWorldTransform = GetChassisWorldTransform();
IndexedVector3 fwd = new IndexedVector3(
chassisWorldTransform._basis[0, m_indexForwardAxis],
chassisWorldTransform._basis[1, m_indexForwardAxis],
chassisWorldTransform._basis[2, m_indexForwardAxis]);
float proj = IndexedVector3.Dot(fwd, wheel.m_raycastInfo.m_contactNormalWS);
fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj;
float proj2 = IndexedVector3.Dot(fwd, vel);
wheel.m_deltaRotation = (proj2 * step) / (wheel.m_wheelsRadius);
wheel.m_rotation += wheel.m_deltaRotation;
}
else
{
wheel.m_rotation += wheel.m_deltaRotation;
}
wheel.m_deltaRotation *= 0.99f; //damping of rotation when not in contact
}
}
public float RayCast(WheelInfo wheel)
{
UpdateWheelTransformsWS(wheel, false);
float depth = -1;
float raylen = wheel.GetSuspensionRestLength() + wheel.m_wheelsRadius;
IndexedVector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen);
IndexedVector3 source = wheel.m_raycastInfo.m_hardPointWS;
wheel.m_raycastInfo.m_contactPointWS = source + rayvector;
IndexedVector3 target = wheel.m_raycastInfo.m_contactPointWS;
float param = 0f;
VehicleRaycasterResult rayResults = new VehicleRaycasterResult();
Debug.Assert(m_vehicleRaycaster != null);
Object object1 = m_vehicleRaycaster.CastRay(ref source, ref target, ref rayResults);
if (object1 != null && (object1 as RigidBody).m_debugBodyId != 1)
{
int ibreak = 0;
}
//{
// IndexedVector3 from1 = new IndexedVector3(0.7957098f, -9.13606f, 1.794605f);
// IndexedVector3 to1 = new IndexedVector3(0.886791f, -10.23207f, 1.815941f);
// VehicleRaycasterResult results1 = new VehicleRaycasterResult();
// Object o2 = m_vehicleRaycaster.castRay(ref from1, ref to1, ref results1);
// IndexedVector3 from2 = new IndexedVector3(0.7957281f, -9.136093f, 1.794625f);
// IndexedVector3 to2 = new IndexedVector3(0.8867911f, -10.23211f, 1.815956f);
// VehicleRaycasterResult results2 = new VehicleRaycasterResult();
// Object o3 = m_vehicleRaycaster.castRay(ref from2, ref to2, ref results2);
// if (Math.Abs(results1.m_distFraction - results2.m_distFraction) > 0.1f)
// {
// int ibreak = 0;
// }
//}
wheel.m_raycastInfo.m_groundObject = null;
if (object1 != null)
{
param = rayResults.m_distFraction;
depth = raylen * rayResults.m_distFraction;
wheel.m_raycastInfo.m_contactNormalWS = rayResults.m_hitNormalInWorld;
wheel.m_raycastInfo.m_isInContact = true;
wheel.m_raycastInfo.m_groundObject = s_fixedObject;///@todo for driving on dynamic/movable objects!;
//wheel.m_raycastInfo.m_groundObject = object;
float hitDistance = param * raylen;
wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelsRadius;
//clamp on max suspension travel
float minSuspensionLength = wheel.GetSuspensionRestLength() - wheel.m_maxSuspensionTravelCm * 0.01f;
float maxSuspensionLength = wheel.GetSuspensionRestLength() + wheel.m_maxSuspensionTravelCm * 0.01f;
if (wheel.m_raycastInfo.m_suspensionLength < minSuspensionLength)
{
wheel.m_raycastInfo.m_suspensionLength = minSuspensionLength;
}
if (wheel.m_raycastInfo.m_suspensionLength > maxSuspensionLength)
{
wheel.m_raycastInfo.m_suspensionLength = maxSuspensionLength;
}
if (Math.Abs(wheel.m_raycastInfo.m_suspensionLength - wheel.m_raycastInfo.m_suspensionLengthBak) > 0.1f)
{
int ibreak = 0;
}
wheel.m_raycastInfo.m_suspensionLengthBak = wheel.m_raycastInfo.m_suspensionLength;
wheel.m_raycastInfo.m_contactPointWS = rayResults.m_hitPointInWorld;
float denominator = IndexedVector3.Dot(wheel.m_raycastInfo.m_contactNormalWS, wheel.m_raycastInfo.m_wheelDirectionWS);
IndexedVector3 chassis_velocity_at_contactPoint;
IndexedVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - GetRigidBody().GetCenterOfMassPosition();
chassis_velocity_at_contactPoint = GetRigidBody().GetVelocityInLocalPoint(ref relpos);
float projVel = IndexedVector3.Dot(wheel.m_raycastInfo.m_contactNormalWS, chassis_velocity_at_contactPoint);
if (projVel > 1f)
{
int ibreak = 0;
}
if (denominator >= -0.1f)
{
wheel.m_suspensionRelativeVelocity = 0f;
wheel.m_clippedInvContactDotSuspension = 1.0f / 0.1f;
}
else
{
float inv = -1f / denominator;
wheel.m_suspensionRelativeVelocity = projVel * inv;
wheel.m_clippedInvContactDotSuspension = inv;
}
}
else
{
//put wheel info as in rest position
wheel.m_raycastInfo.m_suspensionLength = wheel.GetSuspensionRestLength();
wheel.m_suspensionRelativeVelocity = 0.0f;
wheel.m_raycastInfo.m_contactNormalWS = -wheel.m_raycastInfo.m_wheelDirectionWS;
wheel.m_clippedInvContactDotSuspension = 1.0f;
}
return depth;
}
public void UpdateWheelTransformsWS(WheelInfo wheel, bool interpolatedTransform)
{
wheel.m_raycastInfo.m_isInContact = false;
IndexedMatrix chassisTrans = GetChassisWorldTransform();
if (interpolatedTransform && (GetRigidBody().GetMotionState() != null))
{
GetRigidBody().GetMotionState().GetWorldTransform(out chassisTrans);
}
wheel.m_raycastInfo.m_hardPointWS = chassisTrans * wheel.m_chassisConnectionPointCS;
wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans._basis * wheel.m_wheelDirectionCS;
wheel.m_raycastInfo.m_wheelAxleWS = chassisTrans._basis * wheel.m_wheelAxleCS;
}
public WheelInfo AddWheel(ref IndexedVector3 connectionPointCS0, ref IndexedVector3 wheelDirectionCS0, ref IndexedVector3 wheelAxleCS, float suspensionRestLength, float wheelRadius, VehicleTuning tuning, bool isFrontWheel)
{
WheelInfoConstructionInfo ci = new WheelInfoConstructionInfo();
ci.m_chassisConnectionCS = connectionPointCS0;
ci.m_wheelDirectionCS = wheelDirectionCS0;
ci.m_wheelAxleCS = wheelAxleCS;
ci.m_suspensionRestLength = suspensionRestLength;
ci.m_wheelRadius = wheelRadius;
ci.m_suspensionStiffness = tuning.m_suspensionStiffness;
ci.m_wheelsDampingCompression = tuning.m_suspensionCompression;
ci.m_wheelsDampingRelaxation = tuning.m_suspensionDamping;
ci.m_frictionSlip = tuning.m_frictionSlip;
ci.m_bIsFrontWheel = isFrontWheel;
ci.m_maxSuspensionTravelCm = tuning.m_maxSuspensionTravelCm;
ci.m_maxSuspensionForce = tuning.m_maxSuspensionForce;
WheelInfo wheel = new WheelInfo(ref ci);
m_wheelInfo.Add(wheel);
UpdateWheelTransformsWS(wheel, false);
UpdateWheelTransform(GetNumWheels() - 1, false);
return wheel;
}
