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