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