/// <summary> /// Get the height at a particular index, indices are clamped /// </summary> /// <param name="i"></param> /// <param name="j"></param> /// <returns>float</returns> public float GetHeight(int i, int j) { i = (int)OpenTKHelper.Clamp(i, 0, mHeights.Nx - 1); j = (int)OpenTKHelper.Clamp(j, 0, mHeights.Nz - 1); return(mHeights[i, j]); }
/// <summary> /// Interpolate /// </summary> /// <param name="fi"></param> /// <param name="fj"></param> /// <returns>float</returns> public float Interpolate(float fi, float fj) { fi = OpenTKHelper.Clamp(fi, 0.0f, (this.nx - 1.0f)); fj = OpenTKHelper.Clamp(fj, 0.0f, (this.nz - 1.0f)); int i0 = (int)(fi); int j0 = (int)(fj); int i1 = i0 + 1; int j1 = j0 + 1; if (i1 >= this.nx) { i1 = this.nx - 1; } if (j1 >= this.nz) { j1 = this.nz - 1; } float iFrac = fi - i0; float jFrac = fj - j0; float result = jFrac * (iFrac * this[i1, j1] + (1.0f - iFrac) * this[i0, j1]) + (1.0f - jFrac) * (iFrac * this[i1, j0] + (1.0f - iFrac) * this[i0, j0]); return(result); }
/// <summary> /// SegmentIntersect /// </summary> /// <param name="fracOut"></param> /// <param name="skinOut"></param> /// <param name="posOut"></param> /// <param name="normalOut"></param> /// <param name="seg"></param> /// <param name="collisionPredicate"></param> /// <returns>bool</returns> public override bool SegmentIntersect(out float fracOut, out CollisionSkin skinOut, out Vector3 posOut, out Vector3 normalOut, Segment seg, CollisionSkinPredicate1 collisionPredicate) { int numSkins = skins.Count; BoundingBox segBox = BoundingBoxHelper.InitialBox; BoundingBoxHelper.AddSegment(seg, ref segBox); //initialise the outputs fracOut = float.MaxValue; skinOut = null; posOut = normalOut = Vector3.Zero; // working vars float frac; Vector3 pos; Vector3 normal; for (int iskin = 0; iskin < numSkins; ++iskin) { CollisionSkin skin = skins[iskin]; if ((collisionPredicate == null) || collisionPredicate.ConsiderSkin(skin)) { // basic bbox test if (BoundingBoxHelper.OverlapTest(ref skin.WorldBoundingBox, ref segBox)) { if (skin.SegmentIntersect(out frac, out pos, out normal, seg)) { if (frac < fracOut) { posOut = pos; normalOut = normal; skinOut = skin; fracOut = frac; } } } } } if (fracOut > 1.0f) { return(false); } fracOut = OpenTKHelper.Clamp(fracOut, 0.0f, 1.0f); return(true); }
/// <summary> /// Get the normal /// </summary> /// <param name="i"></param> /// <param name="j"></param> /// <returns>Vector3</returns> public Vector3 GetNormal(int i, int j) { int i0 = i - 1; int i1 = i + 1; int j0 = j - 1; int j1 = j + 1; i0 = (int)OpenTKHelper.Clamp(i0, 0, (int)mHeights.Nx - 1); j0 = (int)OpenTKHelper.Clamp(j0, 0, (int)mHeights.Nz - 1); i1 = (int)OpenTKHelper.Clamp(i1, 0, (int)mHeights.Nx - 1); j1 = (int)OpenTKHelper.Clamp(j1, 0, (int)mHeights.Nz - 1); float dx = (i1 - i0) * this.dx; float dz = (j1 - j0) * this.dz; if (i0 == i1) { dx = 1.0f; } if (j0 == j1) { dz = 1.0f; } if (i0 == i1 && j0 == j1) { return(Vector3Extensions.Up); } float hFwd = mHeights[i1, j]; float hBack = mHeights[i0, j]; float hLeft = mHeights[i, j1]; float hRight = mHeights[i, j0]; Vector3 v1 = new Vector3(dx, hFwd - hBack, 0.0f); Vector3 v2 = new Vector3(0.0f, hLeft - hRight, dz); #region REFERENCE: Vector3 normal = Vector3.Cross(v1,v2); Vector3 normal; Vector3.Cross(ref v1, ref v2, out normal); #endregion normal.Normalize(); return(normal); }
/// <summary> /// Updates the rotational state etc /// </summary> /// <param name="dt"></param> public void Update(float dt) { if (dt <= 0.0f) { return; } float origAngVel = angVel; upSpeed = (displacement - lastDisplacement) / System.Math.Max(dt, JiggleMath.Epsilon); if (locked) { angVel = 0; torque = 0; } else { angVel += torque * dt / inertia; torque = 0; // prevent friction from reversing dir - todo do this better // by limiting the torque if (((origAngVel > angVelForGrip) && (angVel < angVelForGrip)) || ((origAngVel < angVelForGrip) && (angVel > angVelForGrip))) { angVel = angVelForGrip; } angVel += driveTorque * dt / inertia; driveTorque = 0; float maxAngVel = 200; angVel = OpenTKHelper.Clamp(angVel, -maxAngVel, maxAngVel); axisAngle += OpenTKHelper.ToDegrees(dt * angVel); } }
/// <summary> /// SegmentIntersect /// </summary> /// <param name="fracOut"></param> /// <param name="posOut"></param> /// <param name="normalOut"></param> /// <param name="seg"></param> /// <returns>bool</returns> public override bool SegmentIntersect(out float fracOut, out Vector3 posOut, out Vector3 normalOut, Segment seg) { fracOut = float.MaxValue; posOut = normalOut = Vector3.Zero; // algo taken from p674 of realting rendering // needs debugging float min = float.MinValue; float max = float.MaxValue; // BEN-OPTIMISATION: Faster code. Vector3 centre = GetCentre(); Vector3 p; Vector3.Subtract(ref centre, ref seg.Origin, out p); Vector3 h; h.X = sideLengths.X * 0.5f; h.Y = sideLengths.Y * 0.5f; h.Z = sideLengths.Z * 0.5f; int dirMax = 0; int dirMin = 0; int dir = 0; // BEN-OPTIMISATIOIN: Ugly inlining and variable reuse for marginal speed increase. #region "Original Code" /* Vector3[] matrixVec = new Vector3[3]; matrixVec[0] = transform.Orientation.Right; matrixVec[1] = transform.Orientation.Up; matrixVec[2] = transform.Orientation.Backward; float[] vectorFloat = new float[3]; vectorFloat[0] = h.X; vectorFloat[1] = h.Y; vectorFloat[2] = h.Z; for (dir = 0; dir < 3; dir++) { float e = Vector3.Dot(matrixVec[dir], p); float f = Vector3.Dot(matrixVec[dir], seg.Delta); if (System.Math.Abs(f) > JiggleMath.Epsilon) { float t1 = (e + vectorFloat[dir]) / f; float t2 = (e - vectorFloat[dir]) / f; if (t1 > t2) { float tmp = t1; t1 = t2; t2 = tmp; } if (t1 > min) { min = t1; dirMin = dir; } if (t2 < max) { max = t2; dirMax = dir; } if (min > max) return false; if (max < 0.0f) return false; } else if ((-e - vectorFloat[dir] > 0.0f) || (-e + vectorFloat[dir] < 0.0f)) { return false; } } */ #endregion #region "Faster code albeit scarier code!" float e = Vector3.Dot(transform.Orientation.Right(), p); float f = Vector3.Dot(transform.Orientation.Right(), seg.Delta); if (System.Math.Abs(f) > JiggleMath.Epsilon) { float t1 = (e + h.X) / f; float t2 = (e - h.X) / f; if (t1 > t2) { float tmp = t1; t1 = t2; t2 = tmp; } if (t1 > min) { min = t1; dirMin = 0; } if (t2 < max) { max = t2; dirMax = 0; } if (min > max) return false; if (max < 0.0f) return false; } else if ((-e - h.X > 0.0f) || (-e + h.X < 0.0f)) { return false; } e = Vector3.Dot(transform.Orientation.Up(), p); f = Vector3.Dot(transform.Orientation.Up(), seg.Delta); if (System.Math.Abs(f) > JiggleMath.Epsilon) { float t1 = (e + h.Y) / f; float t2 = (e - h.Y) / f; if (t1 > t2) { float tmp = t1; t1 = t2; t2 = tmp; } if (t1 > min) { min = t1; dirMin = 1; } if (t2 < max) { max = t2; dirMax = 1; } if (min > max) return false; if (max < 0.0f) return false; } else if ((-e - h.Y > 0.0f) || (-e + h.Y < 0.0f)) { return false; } e = Vector3.Dot(transform.Orientation.Backward(), p); f = Vector3.Dot(transform.Orientation.Backward(), seg.Delta); if (System.Math.Abs(f) > JiggleMath.Epsilon) { float t1 = (e + h.Z) / f; float t2 = (e - h.Z) / f; if (t1 > t2) { float tmp = t1; t1 = t2; t2 = tmp; } if (t1 > min) { min = t1; dirMin = 2; } if (t2 < max) { max = t2; dirMax = 2; } if (min > max) return false; if (max < 0.0f) return false; } else if ((-e - h.Z > 0.0f) || (-e + h.Z < 0.0f)) { return false; } #endregion if (min > 0.0f) { dir = dirMin; fracOut = min; } else { dir = dirMax; fracOut = max; } if (dir == 0) { fracOut = OpenTKHelper.Clamp(fracOut, 0.0f, 1.0f); posOut = seg.GetPoint(fracOut); if (Vector3.Dot(transform.Orientation.Right(), seg.Delta) > 0.0f) normalOut = -transform.Orientation.Right(); else normalOut = transform.Orientation.Right(); } else if (dir == 1) { fracOut = OpenTKHelper.Clamp(fracOut, 0.0f, 1.0f); posOut = seg.GetPoint(fracOut); if (Vector3.Dot(transform.Orientation.Up(), seg.Delta) > 0.0f) normalOut = -transform.Orientation.Up(); else normalOut = transform.Orientation.Up(); } else { fracOut = OpenTKHelper.Clamp(fracOut, 0.0f, 1.0f); posOut = seg.GetPoint(fracOut); if (Vector3.Dot(transform.Orientation.Backward(), seg.Delta) > 0.0f) normalOut = -transform.Orientation.Backward(); else normalOut = transform.Orientation.Backward(); } return true; }
/// <summary> /// GetHeightAndNormal /// </summary> /// <param name="h"></param> /// <param name="normal"></param> /// <param name="point"></param> public void GetHeightAndNormal(out float h, out Vector3 normal, Vector3 point) { float x = point.X; float z = point.Z; x = OpenTKHelper.Clamp(x, xMin, xMax); z = OpenTKHelper.Clamp(z, zMin, zMax); int i0 = (int)((x - xMin) / dx); int j0 = (int)((point.Z - zMin) / dz); i0 = (int)OpenTKHelper.Clamp((int)i0, 0, mHeights.Nx - 1); j0 = (int)OpenTKHelper.Clamp((int)j0, 0, mHeights.Nz - 1); int i1 = i0 + 1; int j1 = j0 + 1; if (i1 >= (int)mHeights.Nx) { i1 = mHeights.Nx - 1; } if (j1 >= (int)mHeights.Nz) { j1 = mHeights.Nz - 1; } float iFrac = (x - (i0 * dx + xMin)) / dx; float jFrac = (z - (j0 * dz + zMin)) / dz; iFrac = OpenTKHelper.Clamp(iFrac, 0.0f, 1.0f); jFrac = OpenTKHelper.Clamp(jFrac, 0.0f, 1.0f); float h00 = mHeights[i0, j0]; float h01 = mHeights[i0, j1]; float h10 = mHeights[i1, j0]; float h11 = mHeights[i1, j1]; // All the triangles are orientated the same way. // work out the normal, then z is in the plane of this normal if ((i0 == i1) && (j0 == j1)) { normal = Vector3Extensions.Up; } else if (i0 == i1) { Vector3 right = Vector3Extensions.Right; normal = Vector3.Cross(new Vector3(0.0f, h01 - h00, dz), right); normal.Normalize(); } if (j0 == j1) { Vector3 backw = Vector3Extensions.Backward; normal = Vector3.Cross(backw, new Vector3(dx, h10 - h00, 0.0f)); normal.Normalize(); } else if (iFrac > jFrac) { normal = Vector3.Cross(new Vector3(dx, h11 - h00, dz), new Vector3(dx, h10 - h00, 0.0f)); normal.Normalize(); } else { normal = Vector3.Cross(new Vector3(0.0f, h01 - h00, dz), new Vector3(dx, h11 - h00, dz)); normal.Normalize(); } // get the plane equation // h00 is in all the triangles JiggleMath.NormalizeSafe(ref normal); Vector3 pos = new Vector3((i0 * dx + xMin), h00, (j0 * dz + zMin)); float d; Vector3.Dot(ref normal, ref pos, out d); d = -d; h = Distance.PointPlaneDistance(ref point, ref normal, d); }
/// <summary> /// Initialise /// </summary> /// <param name="body0"></param> /// <param name="body1"></param> /// <param name="hingeAxis"></param> /// <param name="hingePosRel0"></param> /// <param name="hingeHalfWidth"></param> /// <param name="hingeFwdAngle"></param> /// <param name="hingeBckAngle"></param> /// <param name="sidewaysSlack"></param> /// <param name="damping"></param> public void Initialise(Body body0, Body body1, Vector3 hingeAxis, Vector3 hingePosRel0, float hingeHalfWidth, float hingeFwdAngle, float hingeBckAngle, float sidewaysSlack, float damping) { this.body0 = body0; this.body1 = body1; this.hingeAxis = hingeAxis; this.hingePosRel0 = hingePosRel0; this.usingLimit = false; this.damping = damping; // tScalar allowedDistance = 0.005f; this.hingeAxis.Normalize(); Vector3 hingePosRel1 = body0.Position + hingePosRel0 - body1.Position; // generate the two positions relative to each body Vector3 relPos0a = hingePosRel0 + hingeHalfWidth * hingeAxis; Vector3 relPos0b = hingePosRel0 - hingeHalfWidth * hingeAxis; Vector3 relPos1a = hingePosRel1 + hingeHalfWidth * hingeAxis; Vector3 relPos1b = hingePosRel1 - hingeHalfWidth * hingeAxis; float timescale = 1.0f / 20.0f; float allowedDistanceMid = 0.005f; float allowedDistanceSide = sidewaysSlack * hingeHalfWidth; mSidePointConstraints = new ConstraintMaxDistance[2]; mSidePointConstraints[0] = new ConstraintMaxDistance(); mSidePointConstraints[1] = new ConstraintMaxDistance(); mSidePointConstraints[0].Initialise(body0, relPos0a, body1, relPos1a, allowedDistanceSide); mSidePointConstraints[1].Initialise(body0, relPos0b, body1, relPos1b, allowedDistanceSide); mMidPointConstraint = new ConstraintPoint(); mMidPointConstraint.Initialise(body0, hingePosRel0, body1, hingePosRel1, allowedDistanceMid, timescale); if (hingeFwdAngle <= 150) // MAX_HINGE_ANGLE_LIMIT { // choose a direction that is perpendicular to the hinge Vector3 perpDir = Vector3Extensions.Up; if (Vector3.Dot(perpDir, hingeAxis) > 0.1f) { perpDir = Vector3Extensions.Right; } // now make it perpendicular to the hinge Vector3 sideAxis = Vector3.Cross(hingeAxis, perpDir); perpDir = Vector3.Cross(sideAxis, hingeAxis); perpDir.Normalize(); // the length of the "arm" TODO take this as a parameter? what's // the effect of changing it? float len = 10.0f * hingeHalfWidth; // Choose a position using that dir. this will be the anchor point // for body 0. relative to hinge Vector3 hingeRelAnchorPos0 = perpDir * len; // anchor point for body 2 is chosen to be in the middle of the // angle range. relative to hinge float angleToMiddle = 0.5f * (hingeFwdAngle - hingeBckAngle); Vector3 hingeRelAnchorPos1 = Vector3Extensions.TransformNormal(hingeRelAnchorPos0, Matrix4.CreateFromAxisAngle(hingeAxis, OpenTKHelper.ToRadians(-angleToMiddle))); // work out the "string" length float hingeHalfAngle = 0.5f * (hingeFwdAngle + hingeBckAngle); float allowedDistance = len * 2.0f * (float)System.Math.Sin(OpenTKHelper.ToRadians(hingeHalfAngle * 0.5f)); Vector3 hingePos = body1.Position + hingePosRel0; Vector3 relPos0c = hingePos + hingeRelAnchorPos0 - body0.Position; Vector3 relPos1c = hingePos + hingeRelAnchorPos1 - body1.Position; mMaxDistanceConstraint = new ConstraintMaxDistance(); mMaxDistanceConstraint.Initialise(body0, relPos0c, body1, relPos1c, allowedDistance); usingLimit = true; } if (damping <= 0.0f) { damping = -1.0f; // just make sure that a value of 0.0 doesn't mess up... } else { damping = OpenTKHelper.Clamp(damping, 0, 1); } }
/// <summary> /// Update stuff at the end of physics /// </summary> /// <param name="dt"></param> public void PostPhysics(float dt) { for (int i = 0; i < wheels.Count; i++) { wheels[i].Update(dt); } // control inputs float deltaAccelerate = dt * 4.0f; float deltaSteering = dt * steerRate; // update the actual values float dAccelerate = destAccelerate - accelerate; dAccelerate = OpenTKHelper.Clamp(dAccelerate, -deltaAccelerate, deltaAccelerate); accelerate += dAccelerate; float dSteering = destSteering - steering; dSteering = OpenTKHelper.Clamp(dSteering, -deltaSteering, deltaSteering); steering += dSteering; // apply these inputs float maxTorque = driveTorque; if (fWDrive && bWDrive) { maxTorque *= 0.5f; } if (fWDrive) { wheels[(int)WheelId.WheelFL].AddTorque(maxTorque * accelerate); wheels[(int)WheelId.WheelFR].AddTorque(maxTorque * accelerate); } if (bWDrive) { wheels[(int)WheelId.WheelBL].AddTorque(maxTorque * accelerate); wheels[(int)WheelId.WheelBR].AddTorque(maxTorque * accelerate); } wheels[(int)WheelId.WheelBL].Lock = (hBrake > 0.5f); wheels[(int)WheelId.WheelBR].Lock = (hBrake > 0.5f); // steering angle applies to the inner wheel. The outer one needs to match it int inner, outer; if (steering > 0.0f) { inner = (int)WheelId.WheelFL; outer = (int)WheelId.WheelFR; } else { inner = (int)WheelId.WheelFR; outer = (int)WheelId.WheelFL; } float alpha = System.Math.Abs(maxSteerAngle * steering); float angleSgn = steering > 0.0f ? 1.0f : -1.0f; wheels[inner].SteerAngle = (angleSgn * alpha); float beta; if (alpha == 0.0f) { beta = alpha; } else { float dx = (wheels[(int)WheelId.WheelFR].Pos.X - wheels[(int)WheelId.WheelBR].Pos.X); float dy = (wheels[(int)WheelId.WheelFL].Pos.Z - wheels[(int)WheelId.WheelFR].Pos.Z); //beta = ATan2Deg(dy, dx + (dy / TanDeg(alpha))); beta = (float)System.Math.Atan2(OpenTKHelper.ToRadians(dy), OpenTKHelper.ToRadians(dx + (dy / (float)System.Math.Tan(OpenTKHelper.ToRadians(alpha))))); beta = OpenTKHelper.ToDegrees(beta); } wheels[outer].SteerAngle = (angleSgn * beta); }
/// <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 + Vector3Extensions.TransformNormal(pos, carBody.Orientation); // *mPos; Vector3 worldAxis = Vector3Extensions.TransformNormal(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 = Vector3Extensions.TransformNormal(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(OpenTKHelper.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 = OpenTKHelper.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, Vector3Extensions.TransformNormal(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, Vector3Extensions.TransformNormal(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); }