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