public static float CalcRollingFriction(WheelContactPoint contactPoint)
{
float j1=0f;
Vector3 contactPosWorld = contactPoint.m_frictionPositionWorld;
Vector3 rel_pos1 = contactPosWorld - contactPoint.m_body0.GetCenterOfMassPosition();
Vector3 rel_pos2 = contactPosWorld - contactPoint.m_body1.GetCenterOfMassPosition();
float maxImpulse = contactPoint.m_maxImpulse;
Vector3 vel1 = contactPoint.m_body0.GetVelocityInLocalPoint(ref rel_pos1);
Vector3 vel2 = contactPoint.m_body1.GetVelocityInLocalPoint(ref rel_pos2);
Vector3 vel = vel1 - vel2;
float vrel = Vector3.Dot(contactPoint.m_frictionDirectionWorld,vel);
// calculate j that moves us to zero relative velocity
j1 = -vrel * contactPoint.m_jacDiagABInv;
j1 = System.Math.Min(j1,maxImpulse);
j1 = System.Math.Max(j1, -maxImpulse);
return j1;
}
public virtual void UpdateFriction(float timeStep)
{
//calculate the impulse, so that the wheels don't move sidewards
int numWheel = GetNumWheels();
if (numWheel == 0)
return;
//m_forwardWS.resize(numWheel);
//m_axle.resize(numWheel);
//m_forwardImpulse.resize(numWheel);
//m_sideImpulse.resize(numWheel);
int numWheelsOnGround = 0;
//collapse all those loops into one!
for (int i=0;i<numWheel;i++)
{
WheelInfo wheelInfo = m_wheelInfo[i];
RigidBody groundObject = (RigidBody) wheelInfo.m_raycastInfo.m_groundObject;
if (groundObject != null)
{
numWheelsOnGround++;
}
m_sideImpulse[i] = 0f;
m_forwardImpulse[i] = 0f;
}
if (numWheelsOnGround != 4)
{
int ibreak = 0;
}
{
//foreach(WheelInfo wheelInfo in m_wheelInfo)
for(int i=0;i<numWheel;++i)
{
WheelInfo wheelInfo = m_wheelInfo[i];
RigidBody groundObject = (RigidBody) wheelInfo.m_raycastInfo.m_groundObject;
if (groundObject != null)
{
Matrix wheelTrans = GetWheelTransformWS( i );
Matrix wheelBasis0 = wheelTrans;
m_axle[i] = MathUtil.MatrixColumn(ref wheelBasis0,m_indexRightAxis);
Vector3 surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
float proj = Vector3.Dot(m_axle[i],surfNormalWS);
m_axle[i] -= surfNormalWS * proj;
m_axle[i].Normalize();
m_forwardWS[i] = Vector3.Cross(surfNormalWS,m_axle[i]);
m_forwardWS[i].Normalize();
Vector3 tempAxle = m_axle[i];
float tempImpulse = m_sideImpulse[i];
ContactConstraint.ResolveSingleBilateral(m_chassisBody, ref wheelInfo.m_raycastInfo.m_contactPointWS,
groundObject, ref wheelInfo.m_raycastInfo.m_contactPointWS,
0f, ref tempAxle, ref tempImpulse, timeStep);
m_sideImpulse[i] = (tempImpulse * sideFrictionStiffness2);
}
}
}
float sideFactor = 1f;
float fwdFactor = 0.5f;
bool sliding = false;
{
for (int wheel =0;wheel <numWheel;wheel++)
{
WheelInfo wheelInfo = m_wheelInfo[wheel];
RigidBody groundObject = (RigidBody) wheelInfo.m_raycastInfo.m_groundObject;
float rollingFriction = 0f;
if(groundObject != null)
{
if(wheelInfo.m_engineForce != 0.0f)
{
rollingFriction = wheelInfo.m_engineForce* timeStep;
}
else
{
float defaultRollingFrictionImpulse = 0f;
float maxImpulse = (wheelInfo.m_brake != 0f) ? wheelInfo.m_brake : defaultRollingFrictionImpulse;
Vector3 tempWheel = m_forwardWS[wheel];
WheelContactPoint contactPt = new WheelContactPoint(m_chassisBody,groundObject,ref wheelInfo.m_raycastInfo.m_contactPointWS,ref tempWheel,maxImpulse);
m_forwardWS[wheel] = tempWheel;
rollingFriction = CalcRollingFriction(contactPt);
}
}
//switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break)
m_forwardImpulse[wheel] = 0f;
m_wheelInfo[wheel].m_skidInfo= 1f;
if (groundObject != null)
{
m_wheelInfo[wheel].m_skidInfo= 1f;
float maximp = wheelInfo.m_wheelsSuspensionForce * timeStep * wheelInfo.m_frictionSlip;
float maximpSide = maximp;
float maximpSquared = maximp * maximpSide;
m_forwardImpulse[wheel] = rollingFriction;//wheelInfo.m_engineForce* timeStep;
float x = (m_forwardImpulse[wheel] ) * fwdFactor;
float y = (m_sideImpulse[wheel] ) * sideFactor;
float impulseSquared = (x*x + y*y);
if (impulseSquared > maximpSquared)
{
sliding = true;
float factor = (float)(maximp / System.Math.Sqrt(impulseSquared));
m_wheelInfo[wheel].m_skidInfo *= factor;
}
}
}
}
if (sliding)
{
for (int wheel = 0; wheel < numWheel; wheel++)
{
if (m_sideImpulse[wheel] != 0f)
{
if (m_wheelInfo[wheel].m_skidInfo < 1f)
{
m_forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
}
}
}
}
// apply the impulses
{
for (int wheel = 0;wheel<numWheel ; wheel++)
{
WheelInfo wheelInfo = m_wheelInfo[wheel];
Vector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS -
m_chassisBody.GetCenterOfMassPosition();
if (m_forwardImpulse[wheel] > 5f || m_sideImpulse[wheel] > 5f)
{
int ibreak = 0;
}
if (m_forwardImpulse[wheel] != 0f)
{
m_chassisBody.ApplyImpulse(m_forwardWS[wheel] * (m_forwardImpulse[wheel]), rel_pos);
}
if (m_sideImpulse[wheel] != 0f)
{
RigidBody groundObject = (RigidBody)m_wheelInfo[wheel].m_raycastInfo.m_groundObject;
Vector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS -
groundObject.GetCenterOfMassPosition();
Vector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel];
MathUtil.VectorComponentMultiplyAssign(ref rel_pos, m_indexUpAxis, wheelInfo.m_rollInfluence);
if (sideImp.LengthSquared() > 1f)
{
int ibreak = 0;
}
m_chassisBody.ApplyImpulse(ref sideImp, ref rel_pos);
//apply friction impulse on the ground
Vector3 temp = -sideImp;
groundObject.ApplyImpulse(ref temp, ref rel_pos2);
}
}
}
}
