public void UpdateWheelTransformsWS(WheelInfo wheel, bool interpolatedTransform)
{
wheel.m_raycastInfo.m_isInContact = false;
Matrix chassisTrans = GetChassisWorldTransform();
if (interpolatedTransform && (GetRigidBody().GetMotionState() != null))
{
GetRigidBody().GetMotionState().GetWorldTransform(ref chassisTrans);
}
wheel.m_raycastInfo.m_hardPointWS = Vector3.Transform(wheel.m_chassisConnectionPointCS,chassisTrans);
wheel.m_raycastInfo.m_wheelDirectionWS = Vector3.TransformNormal(wheel.m_wheelDirectionCS,chassisTrans);
wheel.m_raycastInfo.m_wheelAxleWS = Vector3.TransformNormal(wheel.m_wheelAxleCS,chassisTrans);
}
public float RayCast(WheelInfo wheel)
{
UpdateWheelTransformsWS(wheel, false);
float depth = -1;
float raylen = wheel.GetSuspensionRestLength() + wheel.m_wheelsRadius;
Vector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen);
Vector3 source = wheel.m_raycastInfo.m_hardPointWS;
wheel.m_raycastInfo.m_contactPointWS = source + rayvector;
Vector3 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 && ((RigidBody)object1).m_debugBodyId != 1)
{
int ibreak = 0;
}
//{
// Vector3 from1 = new Vector3(0.7957098f, -9.13606f, 1.794605f);
// Vector3 to1 = new Vector3(0.886791f, -10.23207f, 1.815941f);
// VehicleRaycasterResult results1 = new VehicleRaycasterResult();
// Object o2 = m_vehicleRaycaster.castRay(ref from1, ref to1, ref results1);
// Vector3 from2 = new Vector3(0.7957281f, -9.136093f, 1.794625f);
// Vector3 to2 = new Vector3(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 (System.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 = Vector3.Dot(wheel.m_raycastInfo.m_contactNormalWS, wheel.m_raycastInfo.m_wheelDirectionWS);
Vector3 chassis_velocity_at_contactPoint;
Vector3 relpos = wheel.m_raycastInfo.m_contactPointWS - GetRigidBody().GetCenterOfMassPosition();
chassis_velocity_at_contactPoint = GetRigidBody().GetVelocityInLocalPoint(ref relpos);
float projVel = Vector3.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 WheelInfo AddWheel(ref Vector3 connectionPointCS0, ref Vector3 wheelDirectionCS0, ref Vector3 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;
}