public void UpdateSuspension(float step) { float chassisMass = 1.0f / chassisBody.InvMass; for (int w_it = 0; w_it < NumWheels; w_it++) { WheelInfo wheel_info = wheelInfo[w_it]; if (wheel_info.RaycastInfo.IsInContact) { float force; // Spring { float susp_length = wheel_info.SuspensionRestLength; float current_length = wheel_info.RaycastInfo.SuspensionLength; float length_diff = (susp_length - current_length); force = wheel_info.SuspensionStiffness * length_diff * wheel_info.ClippedInvContactDotSuspension; } // Damper { float projected_rel_vel = wheel_info.SuspensionRelativeVelocity; { float susp_damping; if (projected_rel_vel < 0.0f) { susp_damping = wheel_info.WheelsDampingCompression; } else { susp_damping = wheel_info.WheelsDampingRelaxation; } force -= susp_damping * projected_rel_vel; } } // RESULT wheel_info.WheelsSuspensionForce = force * chassisMass; if (wheel_info.WheelsSuspensionForce < 0) { wheel_info.WheelsSuspensionForce = 0; } } else { wheel_info.WheelsSuspensionForce = 0; } } }
void ResetSuspension() { for (int i = 0; i < NumWheels; i++) { WheelInfo wheel = GetWheelInfo(i); wheel.RaycastInfo.SuspensionLength = wheel.SuspensionRestLength; wheel.SuspensionRelativeVelocity = 0; wheel.RaycastInfo.ContactNormalWS = -wheel.RaycastInfo.WheelDirectionWS; //wheel.ContactFriction = 0; wheel.ClippedInvContactDotSuspension = 1; } }
void UpdateWheelTransformsWS(WheelInfo wheel, bool interpolatedTransform) { wheel.RaycastInfo.IsInContact = false; Matrix chassisTrans = ChassisWorldTransform; if (interpolatedTransform && RigidBody.MotionState != null) { chassisTrans = RigidBody.MotionState.WorldTransform; } wheel.RaycastInfo.HardPointWS = Vector3.TransformCoordinate(wheel.ChassisConnectionPointCS, chassisTrans); Matrix chassisTransBasis = chassisTrans.Basis; wheel.RaycastInfo.WheelDirectionWS = Vector3.TransformCoordinate(wheel.WheelDirectionCS, chassisTransBasis); wheel.RaycastInfo.WheelAxleWS = Vector3.TransformCoordinate(wheel.WheelAxleCS, chassisTransBasis); }
void UpdateWheelTransformsWS(WheelInfo wheel, bool interpolatedTransform) { wheel.RaycastInfo.IsInContact = false; Matrix chassisTrans = ChassisWorldTransform; if (interpolatedTransform && RigidBody.MotionState != null) { chassisTrans = RigidBody.MotionState.WorldTransform; } wheel.RaycastInfo.HardPointWS = Vector3.TransformCoordinate(wheel.ChassisConnectionPointCS, chassisTrans); Matrix chassisTransBasis = chassisTrans; chassisTransBasis.Origin = Vector3.Zero; wheel.RaycastInfo.WheelDirectionWS = Vector3.TransformCoordinate(wheel.WheelDirectionCS, chassisTransBasis); wheel.RaycastInfo.WheelAxleWS = Vector3.TransformCoordinate(wheel.WheelAxleCS, chassisTransBasis); }
private float RayCast(WheelInfo wheel) { UpdateWheelTransformsWS(wheel, false); float depth = -1; float raylen = wheel.SuspensionRestLength + wheel.WheelsRadius; Vector3 rayvector = wheel.RaycastInfo.WheelDirectionWS * raylen; Vector3 source = wheel.RaycastInfo.HardPointWS; wheel.RaycastInfo.ContactPointWS = source + rayvector; Vector3 target = wheel.RaycastInfo.ContactPointWS; float param = 0; VehicleRaycasterResult rayResults = new VehicleRaycasterResult(); Debug.Assert(vehicleRaycaster != null); object obj = vehicleRaycaster.CastRay(source, target, rayResults); wheel.RaycastInfo.GroundObject = null; if (obj != null) { param = rayResults.DistFraction; depth = raylen * rayResults.DistFraction; wheel.RaycastInfo.ContactNormalWS = rayResults.HitNormalInWorld; wheel.RaycastInfo.IsInContact = true; wheel.RaycastInfo.GroundObject = fixedBody;///@todo for driving on dynamic/movable objects!; /////wheel.RaycastInfo.GroundObject = object; float hitDistance = param * raylen; wheel.RaycastInfo.SuspensionLength = hitDistance - wheel.WheelsRadius; //clamp on max suspension travel float minSuspensionLength = wheel.SuspensionRestLength - wheel.MaxSuspensionTravelCm * 0.01f; float maxSuspensionLength = wheel.SuspensionRestLength + wheel.MaxSuspensionTravelCm * 0.01f; if (wheel.RaycastInfo.SuspensionLength < minSuspensionLength) { wheel.RaycastInfo.SuspensionLength = minSuspensionLength; } if (wheel.RaycastInfo.SuspensionLength > maxSuspensionLength) { wheel.RaycastInfo.SuspensionLength = maxSuspensionLength; } wheel.RaycastInfo.ContactPointWS = rayResults.HitPointInWorld; float denominator = Vector3.Dot(wheel.RaycastInfo.ContactNormalWS, wheel.RaycastInfo.WheelDirectionWS); Vector3 chassis_velocity_at_contactPoint; Vector3 relpos = wheel.RaycastInfo.ContactPointWS - RigidBody.CenterOfMassPosition; chassis_velocity_at_contactPoint = RigidBody.GetVelocityInLocalPoint(relpos); float projVel = Vector3.Dot(wheel.RaycastInfo.ContactNormalWS, chassis_velocity_at_contactPoint); if (denominator >= -0.1f) { wheel.SuspensionRelativeVelocity = 0; wheel.ClippedInvContactDotSuspension = 1.0f / 0.1f; } else { float inv = -1.0f / denominator; wheel.SuspensionRelativeVelocity = projVel * inv; wheel.ClippedInvContactDotSuspension = inv; } } else { //put wheel info as in rest position wheel.RaycastInfo.SuspensionLength = wheel.SuspensionRestLength; wheel.SuspensionRelativeVelocity = 0.0f; wheel.RaycastInfo.ContactNormalWS = -wheel.RaycastInfo.WheelDirectionWS; wheel.ClippedInvContactDotSuspension = 1.0f; } rayResults.Dispose(); return depth; }
public void UpdateVehicle(float step) { for (int i = 0; i < wheelInfo.Length; i++) { UpdateWheelTransform(i, false); } currentVehicleSpeedKmHour = 3.6f * RigidBody.LinearVelocity.Length; Matrix chassisTrans = ChassisWorldTransform; Vector3 forwardW = new Vector3( chassisTrans[0, indexForwardAxis], chassisTrans[1, indexForwardAxis], chassisTrans[2, indexForwardAxis]); if (Vector3.Dot(forwardW, RigidBody.LinearVelocity) < 0) { currentVehicleSpeedKmHour *= -1.0f; } // Simulate suspension for (int i = 0; i < wheelInfo.Length; i++) { //float depth = RayCast(wheelInfo[i]); } UpdateSuspension(step); for (int i = 0; i < wheelInfo.Length; i++) { //apply suspension force WheelInfo wheel = wheelInfo[i]; float suspensionForce = wheel.WheelsSuspensionForce; if (suspensionForce > wheel.MaxSuspensionForce) { suspensionForce = wheel.MaxSuspensionForce; } Vector3 impulse = wheel.RaycastInfo.ContactNormalWS * suspensionForce * step; Vector3 relpos = wheel.RaycastInfo.ContactPointWS - RigidBody.CenterOfMassPosition; RigidBody.ApplyImpulse(impulse, relpos); } UpdateFriction(step); for (int i = 0; i < wheelInfo.Length; i++) { WheelInfo wheel = wheelInfo[i]; Vector3 relpos = wheel.RaycastInfo.HardPointWS - RigidBody.CenterOfMassPosition; Vector3 vel = RigidBody.GetVelocityInLocalPoint(relpos); if (wheel.RaycastInfo.IsInContact) { Matrix chassisWorldTransform = ChassisWorldTransform; Vector3 fwd = new Vector3( chassisWorldTransform[0, indexForwardAxis], chassisWorldTransform[1, indexForwardAxis], chassisWorldTransform[2, indexForwardAxis]); float proj = Vector3.Dot(fwd, wheel.RaycastInfo.ContactNormalWS); fwd -= wheel.RaycastInfo.ContactNormalWS * proj; float proj2; Vector3.Dot(ref fwd, ref vel, out proj2); wheel.DeltaRotation = (proj2 * step) / (wheel.WheelsRadius); wheel.Rotation += wheel.DeltaRotation; } else { wheel.Rotation += wheel.DeltaRotation; } wheel.DeltaRotation *= 0.99f;//damping of rotation when not in contact } }
public void UpdateFriction(float timeStep) { //calculate the impulse, so that the wheels don't move sidewards int numWheel = NumWheels; if (numWheel == 0) { return; } Array.Resize(ref forwardWS, numWheel); Array.Resize(ref axle, numWheel); Array.Resize(ref forwardImpulse, numWheel); Array.Resize(ref sideImpulse, numWheel); int numWheelsOnGround = 0; //collapse all those loops into one! for (int i = 0; i < NumWheels; i++) { RigidBody groundObject = wheelInfo[i].RaycastInfo.GroundObject as RigidBody; if (groundObject != null) { numWheelsOnGround++; } sideImpulse[i] = 0; forwardImpulse[i] = 0; } for (int i = 0; i < NumWheels; i++) { WheelInfo wheel = wheelInfo[i]; RigidBody groundObject = wheel.RaycastInfo.GroundObject as RigidBody; if (groundObject != null) { Matrix wheelTrans = GetWheelTransformWS(i); axle[i] = new Vector3( wheelTrans[0, indexRightAxis], wheelTrans[1, indexRightAxis], wheelTrans[2, indexRightAxis]); Vector3 surfNormalWS = wheel.RaycastInfo.ContactNormalWS; float proj; Vector3.Dot(ref axle[i], ref surfNormalWS, out proj); axle[i] -= surfNormalWS * proj; axle[i].Normalize(); Vector3.Cross(ref surfNormalWS, ref axle[i], out forwardWS[i]); forwardWS[i].Normalize(); ResolveSingleBilateral(chassisBody, wheel.RaycastInfo.ContactPointWS, groundObject, wheel.RaycastInfo.ContactPointWS, 0, axle[i], ref sideImpulse[i], timeStep); sideImpulse[i] *= sideFrictionStiffness2; } } const float sideFactor = 1.0f; const float fwdFactor = 0.5f; bool sliding = false; for (int i = 0; i < NumWheels; i++) { WheelInfo wheel = wheelInfo[i]; RigidBody groundObject = wheel.RaycastInfo.GroundObject as RigidBody; float rollingFriction = 0.0f; if (groundObject != null) { if (wheel.EngineForce != 0.0f) { rollingFriction = wheel.EngineForce * timeStep; } else { float defaultRollingFrictionImpulse = 0.0f; float maxImpulse = (wheel.Brake != 0) ? wheel.Brake : defaultRollingFrictionImpulse; rollingFriction = CalcRollingFriction(chassisBody, groundObject, wheel.RaycastInfo.ContactPointWS, forwardWS[i], maxImpulse); } } //switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break) forwardImpulse[i] = 0; wheelInfo[i].SkidInfo = 1.0f; if (groundObject != null) { wheelInfo[i].SkidInfo = 1.0f; float maximp = wheel.WheelsSuspensionForce * timeStep * wheel.FrictionSlip; float maximpSide = maximp; float maximpSquared = maximp * maximpSide; forwardImpulse[i] = rollingFriction;//wheel.EngineForce* timeStep; float x = forwardImpulse[i] * fwdFactor; float y = sideImpulse[i] * sideFactor; float impulseSquared = (x * x + y * y); if (impulseSquared > maximpSquared) { sliding = true; float factor = maximp / (float)System.Math.Sqrt(impulseSquared); wheelInfo[i].SkidInfo *= factor; } } } if (sliding) { for (int wheel = 0; wheel < NumWheels; wheel++) { if (sideImpulse[wheel] != 0) { if (wheelInfo[wheel].SkidInfo < 1.0f) { forwardImpulse[wheel] *= wheelInfo[wheel].SkidInfo; sideImpulse[wheel] *= wheelInfo[wheel].SkidInfo; } } } } // apply the impulses for (int i = 0; i < NumWheels; i++) { WheelInfo wheel = wheelInfo[i]; Vector3 rel_pos = wheel.RaycastInfo.ContactPointWS - chassisBody.CenterOfMassPosition; if (forwardImpulse[i] != 0) { chassisBody.ApplyImpulse(forwardWS[i] * forwardImpulse[i], rel_pos); } if (sideImpulse[i] != 0) { RigidBody groundObject = wheel.RaycastInfo.GroundObject as RigidBody; Vector3 rel_pos2 = wheel.RaycastInfo.ContactPointWS - groundObject.CenterOfMassPosition; Vector3 sideImp = axle[i] * sideImpulse[i]; #if ROLLING_INFLUENCE_FIX // fix. It only worked if car's up was along Y - VT. //Vector4 vChassisWorldUp = RigidBody.CenterOfMassTransform.get_Columns(indexUpAxis); Vector3 vChassisWorldUp = new Vector3( RigidBody.CenterOfMassTransform.Row1[indexUpAxis], RigidBody.CenterOfMassTransform.Row2[indexUpAxis], RigidBody.CenterOfMassTransform.Row3[indexUpAxis]); float dot; Vector3.Dot(ref vChassisWorldUp, ref rel_pos, out dot); rel_pos -= vChassisWorldUp * (dot * (1.0f - wheel.RollInfluence)); #else rel_pos[indexUpAxis] *= wheel.RollInfluence; #endif chassisBody.ApplyImpulse(sideImp, rel_pos); //apply friction impulse on the ground groundObject.ApplyImpulse(-sideImp, rel_pos2); } } }
public WheelInfo AddWheel(Vector3 connectionPointCS, Vector3 wheelDirectionCS0, Vector3 wheelAxleCS, float suspensionRestLength, float wheelRadius, VehicleTuning tuning, bool isFrontWheel) { WheelInfoConstructionInfo ci = new WheelInfoConstructionInfo(); ci.ChassisConnectionCS = connectionPointCS; ci.WheelDirectionCS = wheelDirectionCS0; ci.WheelAxleCS = wheelAxleCS; ci.SuspensionRestLength = suspensionRestLength; ci.WheelRadius = wheelRadius; ci.SuspensionStiffness = tuning.SuspensionStiffness; ci.WheelsDampingCompression = tuning.SuspensionCompression; ci.WheelsDampingRelaxation = tuning.SuspensionDamping; ci.FrictionSlip = tuning.FrictionSlip; ci.IsFrontWheel = isFrontWheel; ci.MaxSuspensionTravelCm = tuning.MaxSuspensionTravelCm; ci.MaxSuspensionForce = tuning.MaxSuspensionForce; Array.Resize<WheelInfo>(ref wheelInfo, wheelInfo.Length + 1); WheelInfo wheel = new WheelInfo(ci); wheelInfo[wheelInfo.Length - 1] = wheel; UpdateWheelTransformsWS(wheel, false); UpdateWheelTransform(NumWheels - 1, false); return wheel; }
private float RayCast(WheelInfo wheel) { UpdateWheelTransformsWS(wheel, false); float depth = -1; float raylen = wheel.SuspensionRestLength + wheel.WheelsRadius; Vector3 rayvector = wheel.RaycastInfo.WheelDirectionWS * raylen; Vector3 source = wheel.RaycastInfo.HardPointWS; wheel.RaycastInfo.ContactPointWS = source + rayvector; Vector3 target = wheel.RaycastInfo.ContactPointWS; float param = 0; VehicleRaycasterResult rayResults = new VehicleRaycasterResult(); Debug.Assert(vehicleRaycaster != null); object obj = vehicleRaycaster.CastRay(ref source, ref target, rayResults); wheel.RaycastInfo.GroundObject = null; if (obj != null) { param = rayResults.DistFraction; depth = raylen * rayResults.DistFraction; wheel.RaycastInfo.ContactNormalWS = rayResults.HitNormalInWorld; wheel.RaycastInfo.IsInContact = true; wheel.RaycastInfo.GroundObject = fixedBody;///@todo for driving on dynamic/movable objects!; /////wheel.RaycastInfo.GroundObject = object; float hitDistance = param * raylen; wheel.RaycastInfo.SuspensionLength = hitDistance - wheel.WheelsRadius; //clamp on max suspension travel float minSuspensionLength = wheel.SuspensionRestLength - wheel.MaxSuspensionTravelCm * 0.01f; float maxSuspensionLength = wheel.SuspensionRestLength + wheel.MaxSuspensionTravelCm * 0.01f; if (wheel.RaycastInfo.SuspensionLength < minSuspensionLength) { wheel.RaycastInfo.SuspensionLength = minSuspensionLength; } if (wheel.RaycastInfo.SuspensionLength > maxSuspensionLength) { wheel.RaycastInfo.SuspensionLength = maxSuspensionLength; } wheel.RaycastInfo.ContactPointWS = rayResults.HitPointInWorld; float denominator = Vector3.Dot(wheel.RaycastInfo.ContactNormalWS, wheel.RaycastInfo.WheelDirectionWS); Vector3 chassis_velocity_at_contactPoint; Vector3 relpos = wheel.RaycastInfo.ContactPointWS - RigidBody.CenterOfMassPosition; chassis_velocity_at_contactPoint = RigidBody.GetVelocityInLocalPoint(relpos); float projVel = Vector3.Dot(wheel.RaycastInfo.ContactNormalWS, chassis_velocity_at_contactPoint); if (denominator >= -0.1f) { wheel.SuspensionRelativeVelocity = 0; wheel.ClippedInvContactDotSuspension = 1.0f / 0.1f; } else { float inv = -1.0f / denominator; wheel.SuspensionRelativeVelocity = projVel * inv; wheel.ClippedInvContactDotSuspension = inv; } } else { //put wheel info as in rest position wheel.RaycastInfo.SuspensionLength = wheel.SuspensionRestLength; wheel.SuspensionRelativeVelocity = 0.0f; wheel.RaycastInfo.ContactNormalWS = -wheel.RaycastInfo.WheelDirectionWS; wheel.ClippedInvContactDotSuspension = 1.0f; } return(depth); }
public static void SetWheelDirectionCS(this WheelInfo obj, OpenTK.Vector3 value) { SetWheelDirectionCS(obj, ref value); }
public static void SetWorldTransform(this WheelInfo obj, OpenTK.Matrix4 value) { SetWorldTransform(obj, ref value); }
public static void SetChassisConnectionPointCS(this WheelInfo obj, OpenTK.Vector3 value) { SetChassisConnectionPointCS(obj, ref value); }
public static OpenTK.Matrix4 GetWorldTransform(this WheelInfo obj) { OpenTK.Matrix4 value; GetWorldTransform(obj, out value); return(value); }
public static OpenTK.Vector3 GetWheelDirectionCS(this WheelInfo obj) { OpenTK.Vector3 value; GetWheelDirectionCS(obj, out value); return(value); }
public static OpenTK.Vector3 GetChassisConnectionPointCS(this WheelInfo obj) { OpenTK.Vector3 value; GetChassisConnectionPointCS(obj, out value); return(value); }
/// <summary> /// Changes wheel friction depending on our current speed /// </summary> protected void ChangeFriction(WheelInfo info, float currentSpeed) { // really strong friction so we don't roll down hills when we're stopped if (AccelerateMultiplier == 0f && currentSpeed > -2f && currentSpeed < 2f) info.FrictionSlip = 10000f; // if we're going slower than the slow speed, keep friction at maximum else if (currentSpeed < DefaultSlowSpeed) info.FrictionSlip = DefaultFrictionSlip; // otherwise, change friction based on speed to a minimum else { float newFric = this.Friction * (1 - ((currentSpeed - DefaultSlowSpeed) / ((kart.MaxSpeed * 3.6f) - DefaultSlowSpeed))); info.FrictionSlip = System.Math.Max(newFric, MINIMUM_FRICTION); } }