/// <summary> // TODO teting testing testing ... /// Adds the forces die to this wheel to the parent. Return value indicates if it's /// on the ground. /// </summary> /// <param name="dt"></param> public bool AddForcesToCar(float dt) { Vector3 force = Vector3.Zero; lastDisplacement = displacement; displacement = 0.0f; Body carBody = car.Chassis.Body; Vector3 worldPos = carBody.Position + Vector3.Transform(pos, carBody.Orientation); // *mPos; Vector3 worldAxis = Vector3.Transform(axisUp, carBody.Orientation); // *mAxisUp; //Vector3 wheelFwd = RotationMatrix(mSteerAngle, worldAxis) * carBody.Orientation.GetCol(0); // OpenGl has differnet row/column order for matrixes than XNA has .. Vector3 wheelFwd = Vector3.Transform(carBody.Orientation.Right, JiggleMath.RotationMatrix(steerAngle, worldAxis)); //Vector3 wheelFwd = RotationMatrix(mSteerAngle, worldAxis) * carBody.GetOrientation().GetCol(0); Vector3 wheelUp = worldAxis; Vector3 wheelLeft = Vector3.Cross(wheelUp, wheelFwd); wheelLeft.Normalize(); wheelUp = Vector3.Cross(wheelFwd, wheelLeft); // start of ray float rayLen = 2.0f * radius + travel; Vector3 wheelRayEnd = worldPos - radius * worldAxis; Segment wheelRay = new Segment(wheelRayEnd + rayLen * worldAxis, -rayLen * worldAxis); //Assert(PhysicsSystem.CurrentPhysicsSystem); CollisionSystem collSystem = PhysicsSystem.CurrentPhysicsSystem.CollisionSystem; ///Assert(collSystem); int numRaysUse = System.Math.Min(numRays, maxNumRays); // adjust the start position of the ray - divide the wheel into numRays+2 // rays, but don't use the first/last. float deltaFwd = (2.0f * radius) / (numRaysUse + 1); float deltaFwdStart = deltaFwd; lastOnFloor = false; int bestIRay = 0; int iRay; for (iRay = 0; iRay < numRaysUse; ++iRay) { fracs[iRay] = float.MaxValue; //SCALAR_HUGE; // work out the offset relative to the middle ray float distFwd = (deltaFwdStart + iRay * deltaFwd) - radius; //float zOffset = mRadius * (1.0f - CosDeg(90.0f * (distFwd / mRadius))); float zOffset = radius * (1.0f - (float)System.Math.Cos(MathHelper.ToRadians(90.0f * (distFwd / radius)))); segments[iRay] = wheelRay; segments[iRay].Origin += distFwd * wheelFwd + zOffset * wheelUp; if (collSystem.SegmentIntersect(out fracs[iRay], out otherSkins[iRay], out groundPositions[iRay], out groundNormals[iRay], segments[iRay], pred)) { lastOnFloor = true; if (fracs[iRay] < fracs[bestIRay]) { bestIRay = iRay; } } } if (!lastOnFloor) { return(false); } //Assert(bestIRay < numRays); // use the best one Vector3 groundPos = groundPositions[bestIRay]; float frac = fracs[bestIRay]; CollisionSkin otherSkin = otherSkins[bestIRay]; // const Vector3 groundNormal = (worldPos - segments[bestIRay].GetEnd()).NormaliseSafe(); // const Vector3 groundNormal = groundNormals[bestIRay]; Vector3 groundNormal = worldAxis; if (numRaysUse > 1) { for (iRay = 0; iRay < numRaysUse; ++iRay) { if (fracs[iRay] <= 1.0f) { groundNormal += (1.0f - fracs[iRay]) * (worldPos - segments[iRay].GetEnd()); } } JiggleMath.NormalizeSafe(ref groundNormal); } else { groundNormal = groundNormals[bestIRay]; } //Assert(otherSkin); Body worldBody = otherSkin.Owner; displacement = rayLen * (1.0f - frac); displacement = MathHelper.Clamp(displacement, 0, travel); float displacementForceMag = displacement * spring; // reduce force when suspension is par to ground displacementForceMag *= Vector3.Dot(groundNormals[bestIRay], worldAxis); // apply damping float dampingForceMag = upSpeed * damping; float totalForceMag = displacementForceMag + dampingForceMag; if (totalForceMag < 0.0f) { totalForceMag = 0.0f; } Vector3 extraForce = totalForceMag * worldAxis; force += extraForce; // side-slip friction and drive force. Work out wheel- and floor-relative coordinate frame Vector3 groundUp = groundNormal; Vector3 groundLeft = Vector3.Cross(groundNormal, wheelFwd); JiggleMath.NormalizeSafe(ref groundLeft); Vector3 groundFwd = Vector3.Cross(groundLeft, groundUp); Vector3 wheelPointVel = carBody.Velocity + Vector3.Cross(carBody.AngularVelocity, Vector3.Transform(pos, carBody.Orientation));// * mPos); Vector3 rimVel = angVel * Vector3.Cross(wheelLeft, groundPos - worldPos); wheelPointVel += rimVel; // if sitting on another body then adjust for its velocity. if (worldBody != null) { Vector3 worldVel = worldBody.Velocity + Vector3.Cross(worldBody.AngularVelocity, groundPos - worldBody.Position); wheelPointVel -= worldVel; } // sideways forces float noslipVel = 0.2f; float slipVel = 0.4f; float slipFactor = 0.7f; float smallVel = 3; float friction = sideFriction; float sideVel = Vector3.Dot(wheelPointVel, groundLeft); if ((sideVel > slipVel) || (sideVel < -slipVel)) { friction *= slipFactor; } else if ((sideVel > noslipVel) || (sideVel < -noslipVel)) { friction *= 1.0f - (1.0f - slipFactor) * (System.Math.Abs(sideVel) - noslipVel) / (slipVel - noslipVel); } if (sideVel < 0.0f) { friction *= -1.0f; } if (System.Math.Abs(sideVel) < smallVel) { friction *= System.Math.Abs(sideVel) / smallVel; } float sideForce = -friction * totalForceMag; extraForce = sideForce * groundLeft; force += extraForce; // fwd/back forces friction = fwdFriction; float fwdVel = Vector3.Dot(wheelPointVel, groundFwd); if ((fwdVel > slipVel) || (fwdVel < -slipVel)) { friction *= slipFactor; } else if ((fwdVel > noslipVel) || (fwdVel < -noslipVel)) { friction *= 1.0f - (1.0f - slipFactor) * (System.Math.Abs(fwdVel) - noslipVel) / (slipVel - noslipVel); } if (fwdVel < 0.0f) { friction *= -1.0f; } if (System.Math.Abs(fwdVel) < smallVel) { friction *= System.Math.Abs(fwdVel) / smallVel; } float fwdForce = -friction * totalForceMag; extraForce = fwdForce * groundFwd; force += extraForce; //if (!force.IsSensible()) //{ // TRACE_FILE_IF(ONCE_1) // TRACE("Bad force in car wheel\n"); // return true; //} // fwd force also spins the wheel Vector3 wheelCentreVel = carBody.Velocity + Vector3.Cross(carBody.AngularVelocity, Vector3.Transform(pos, carBody.Orientation));// * mPos); angVelForGrip = Vector3.Dot(wheelCentreVel, groundFwd) / radius; torque += -fwdForce * radius; // add force to car carBody.AddWorldForce(force, groundPos); //if (float.IsNaN(force.X)) // while(true){} //System.Diagnostics.Debug.WriteLine(force.ToString()); // add force to the world if (worldBody != null && !worldBody.Immovable) { // todo get the position in the right place... // also limit the velocity that this force can produce by looking at the // mass/inertia of the other object float maxOtherBodyAcc = 500.0f; float maxOtherBodyForce = maxOtherBodyAcc * worldBody.Mass; if (force.LengthSquared() > (maxOtherBodyForce * maxOtherBodyForce)) { force *= maxOtherBodyForce / force.Length(); } worldBody.AddWorldForce(-force, groundPos); } return(true); }
public bool AddForcesToCar(float dt) { var force = Vector3.Zero; _lastDisplacement = Displacement; Displacement = 0.0f; var carBody = _car.Chassis.Body; var worldPos = carBody.Position + Vector3.TransformNormal(Pos, carBody.Orientation); var worldAxis = Vector3.TransformNormal(LocalAxisUp, carBody.Orientation); var wheelFwd = Vector3.TransformNormal(carBody.Orientation.Right, JiggleMath.RotationMatrix(SteerAngle, worldAxis)); var wheelUp = worldAxis; var wheelLeft = Vector3.Cross(wheelUp, wheelFwd); wheelLeft.Normalize(); wheelUp = Vector3.Cross(wheelFwd, wheelLeft); var rayLen = 2.0f * Radius + _travel; var wheelRayEnd = worldPos - Radius * worldAxis; var wheelRay = new Segment(wheelRayEnd + rayLen * worldAxis, -rayLen * worldAxis); var collSystem = PhysicsSystem.CurrentPhysicsSystem.CollisionSystem; var numRaysUse = System.Math.Min(_numRays, MaxNumRays); var deltaFwd = 2.0f * Radius / (numRaysUse + 1); var deltaFwdStart = deltaFwd; OnFloor = false; var bestIRay = 0; int iRay; for (iRay = 0; iRay < numRaysUse; ++iRay) { _fracs[iRay] = float.MaxValue; var distFwd = deltaFwdStart + iRay * deltaFwd - Radius; var zOffset = Radius * (1.0f - (float)System.Math.Cos(MathHelper.ToRadians(90.0f * (distFwd / Radius)))); _segments[iRay] = wheelRay; _segments[iRay].Origin += distFwd * wheelFwd + zOffset * wheelUp; if (collSystem.SegmentIntersect(out _fracs[iRay], out _otherSkins[iRay], out _groundPositions[iRay], out _groundNormals[iRay], _segments[iRay], _pred)) { OnFloor = true; if (_fracs[iRay] < _fracs[bestIRay]) { bestIRay = iRay; } } } if (!OnFloor) { return(false); } var groundPos = _groundPositions[bestIRay]; var frac = _fracs[bestIRay]; var otherSkin = _otherSkins[bestIRay]; var groundNormal = worldAxis; if (numRaysUse > 1) { for (iRay = 0; iRay < numRaysUse; ++iRay) { if (_fracs[iRay] <= 1.0f) { groundNormal += (1.0f - _fracs[iRay]) * (worldPos - _segments[iRay].GetEnd()); } } JiggleMath.NormalizeSafe(ref groundNormal); } else { groundNormal = _groundNormals[bestIRay]; } var worldBody = otherSkin.Owner; Displacement = rayLen * (1.0f - frac); Displacement = MathHelper.Clamp(Displacement, 0, _travel); var displacementForceMag = Displacement * _spring; displacementForceMag *= Vector3.Dot(_groundNormals[bestIRay], worldAxis); var dampingForceMag = _upSpeed * _damping; var totalForceMag = displacementForceMag + dampingForceMag; if (totalForceMag < 0.0f) { totalForceMag = 0.0f; } var extraForce = totalForceMag * worldAxis; force += extraForce; var groundUp = groundNormal; var groundLeft = Vector3.Cross(groundNormal, wheelFwd); JiggleMath.NormalizeSafe(ref groundLeft); var groundFwd = Vector3.Cross(groundLeft, groundUp); var wheelPointVel = carBody.Velocity + Vector3.Cross(carBody.AngularVelocity, Vector3.TransformNormal(Pos, carBody.Orientation)); var rimVel = _angVel * Vector3.Cross(wheelLeft, groundPos - worldPos); wheelPointVel += rimVel; if (worldBody != null) { var worldVel = worldBody.Velocity + Vector3.Cross(worldBody.AngularVelocity, groundPos - worldBody.Position); wheelPointVel -= worldVel; } var noslipVel = 0.2f; var slipVel = 0.4f; var slipFactor = 0.7f; float smallVel = 3; var friction = _sideFriction; var sideVel = Vector3.Dot(wheelPointVel, groundLeft); if (sideVel > slipVel || sideVel < -slipVel) { friction *= slipFactor; } else if (sideVel > noslipVel || sideVel < -noslipVel) { friction *= 1.0f - (1.0f - slipFactor) * (System.Math.Abs(sideVel) - noslipVel) / (slipVel - noslipVel); } if (sideVel < 0.0f) { friction *= -1.0f; } if (System.Math.Abs(sideVel) < smallVel) { friction *= System.Math.Abs(sideVel) / smallVel; } var sideForce = -friction * totalForceMag; extraForce = sideForce * groundLeft; force += extraForce; friction = _fwdFriction; var fwdVel = Vector3.Dot(wheelPointVel, groundFwd); if (fwdVel > slipVel || fwdVel < -slipVel) { friction *= slipFactor; } else if (fwdVel > noslipVel || fwdVel < -noslipVel) { friction *= 1.0f - (1.0f - slipFactor) * (System.Math.Abs(fwdVel) - noslipVel) / (slipVel - noslipVel); } if (fwdVel < 0.0f) { friction *= -1.0f; } if (System.Math.Abs(fwdVel) < smallVel) { friction *= System.Math.Abs(fwdVel) / smallVel; } var fwdForce = -friction * totalForceMag; extraForce = fwdForce * groundFwd; force += extraForce; var wheelCentreVel = carBody.Velocity + Vector3.Cross(carBody.AngularVelocity, Vector3.TransformNormal(Pos, carBody.Orientation)); _angVelForGrip = Vector3.Dot(wheelCentreVel, groundFwd) / Radius; _torque += -fwdForce * Radius; carBody.AddWorldForce(force, groundPos); if (worldBody != null && !worldBody.Immovable) { var maxOtherBodyAcc = 500.0f; var maxOtherBodyForce = maxOtherBodyAcc * worldBody.Mass; if (force.LengthSquared() > maxOtherBodyForce * maxOtherBodyForce) { force *= maxOtherBodyForce / force.Length(); } worldBody.AddWorldForce(-force, groundPos); } return(true); }