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 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 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;
}
