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