void VehicleBody::_update_friction(PhysicsDirectBodyState *s) {
//calculate the impulse, so that the wheels don't move sidewards
int numWheel = wheels.size();
if (!numWheel)
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<wheels.size();i++)
{
VehicleWheel& wheelInfo = *wheels[i];
if (wheelInfo.m_raycastInfo.m_isInContact)
numWheelsOnGround++;
m_sideImpulse[i] = real_t(0.);
m_forwardImpulse[i] = real_t(0.);
}
{
for (int i=0;i<wheels.size();i++)
{
VehicleWheel& wheelInfo = *wheels[i];
if (wheelInfo.m_raycastInfo.m_isInContact)
{
//const btTransform& wheelTrans = getWheelTransformWS( i );
Matrix3 wheelBasis0 = wheelInfo.m_worldTransform.basis;//get_global_transform().basis;
m_axle[i] = wheelBasis0.get_axis(Vector3::AXIS_X);
//m_axle[i] = wheelInfo.m_raycastInfo.m_wheelAxleWS;
const Vector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
real_t proj = m_axle[i].dot(surfNormalWS);
m_axle[i] -= surfNormalWS * proj;
m_axle[i] = m_axle[i].normalized();
m_forwardWS[i] = surfNormalWS.cross(m_axle[i]);
m_forwardWS[i].normalize();
_resolve_single_bilateral(s, wheelInfo.m_raycastInfo.m_contactPointWS,
wheelInfo.m_raycastInfo.m_groundObject, wheelInfo.m_raycastInfo.m_contactPointWS,
m_axle[i],m_sideImpulse[i]);
m_sideImpulse[i] *= sideFrictionStiffness2;
}
}
}
real_t sideFactor = real_t(1.);
real_t fwdFactor = 0.5;
bool sliding = false;
{
for (int wheel =0;wheel <wheels.size();wheel++)
{
VehicleWheel& wheelInfo = *wheels[wheel];
//class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
real_t rollingFriction = 0.f;
if (wheelInfo.m_raycastInfo.m_isInContact)
{
if (engine_force != 0.f)
{
rollingFriction = -engine_force* s->get_step();
} else
{
real_t defaultRollingFrictionImpulse = 0.f;
float cbrake = MAX(wheelInfo.m_brake,brake);
real_t maxImpulse = cbrake ? cbrake : defaultRollingFrictionImpulse;
btVehicleWheelContactPoint contactPt(s,wheelInfo.m_raycastInfo.m_groundObject,wheelInfo.m_raycastInfo.m_contactPointWS,m_forwardWS[wheel],maxImpulse);
rollingFriction = _calc_rolling_friction(contactPt);
}
}
//switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break)
m_forwardImpulse[wheel] = real_t(0.);
wheelInfo.m_skidInfo= real_t(1.);
if (wheelInfo.m_raycastInfo.m_isInContact)
{
wheelInfo.m_skidInfo= real_t(1.);
real_t maximp = wheelInfo.m_wheelsSuspensionForce * s->get_step() * wheelInfo.m_frictionSlip;
real_t maximpSide = maximp;
real_t maximpSquared = maximp * maximpSide;
m_forwardImpulse[wheel] = rollingFriction;//wheelInfo.m_engineForce* timeStep;
real_t x = (m_forwardImpulse[wheel] ) * fwdFactor;
real_t y = (m_sideImpulse[wheel] ) * sideFactor;
real_t impulseSquared = (x*x + y*y);
if (impulseSquared > maximpSquared)
{
sliding = true;
real_t factor = maximp / Math::sqrt(impulseSquared);
wheelInfo.m_skidInfo *= factor;
}
}
}
}
if (sliding)
{
for (int wheel = 0;wheel < wheels.size(); wheel++)
{
if (m_sideImpulse[wheel] != real_t(0.))
{
if (wheels[wheel]->m_skidInfo< real_t(1.))
{
m_forwardImpulse[wheel] *= wheels[wheel]->m_skidInfo;
m_sideImpulse[wheel] *= wheels[wheel]->m_skidInfo;
}
}
}
}
// apply the impulses
{
for (int wheel = 0;wheel<wheels.size(); wheel++)
{
VehicleWheel& wheelInfo = *wheels[wheel];
Vector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS -
s->get_transform().origin;
if (m_forwardImpulse[wheel] != real_t(0.))
{
s->apply_impulse(rel_pos,m_forwardWS[wheel]*(m_forwardImpulse[wheel]));
}
if (m_sideImpulse[wheel] != real_t(0.))
{
PhysicsBody* groundObject = wheelInfo.m_raycastInfo.m_groundObject;
Vector3 rel_pos2;
if (groundObject) {
rel_pos2=wheelInfo.m_raycastInfo.m_contactPointWS - groundObject->get_global_transform().origin;
}
Vector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel];
#if defined ROLLING_INFLUENCE_FIX // fix. It only worked if car's up was along Y - VT.
Vector3 vChassisWorldUp = s->get_transform().basis.transposed()[1];//getRigidBody()->getCenterOfMassTransform().getBasis().getColumn(m_indexUpAxis);
rel_pos -= vChassisWorldUp * (vChassisWorldUp.dot(rel_pos) * (1.f-wheelInfo.m_rollInfluence));
#else
rel_pos[1] *= wheelInfo.m_rollInfluence; //?
#endif
s->apply_impulse(rel_pos,sideImp);
//apply friction impulse on the ground
//todo
//groundObject->applyImpulse(-sideImp,rel_pos2);
}
}
}
}
