protected override void Apply(RigidBody obj, int slice)
{
Vector3D pos = this.FPosition[slice];
Vector3D force = this.FImpulse[slice];
obj.ApplyImpulse(new Vector3((float)force.x, (float)force.y, (float)force.z),
new Vector3((float)pos.x, (float)pos.y, (float)pos.z));
}
public unsafe static void ApplyImpulse(this RigidBody obj, ref OpenTK.Vector3 impulse, ref OpenTK.Vector3 rel_pos)
{
fixed(OpenTK.Vector3 *impulsePtr = &impulse)
{
fixed(OpenTK.Vector3 *rel_posPtr = &rel_pos)
{
obj.ApplyImpulse(ref *(BulletSharp.Math.Vector3 *)impulsePtr, ref *(BulletSharp.Math.Vector3 *)rel_posPtr);
}
}
}
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);
}
}
}