void btKart::updateFriction(btScalar timeStep)
{
//calculate the impulse, so that the wheels don't move sidewards
for (int i=0;i<getNumWheels();i++)
{
m_sideImpulse[i] = btScalar(0.);
btWheelInfo& wheelInfo = m_wheelInfo[i];
btRigidBody* groundObject =
(btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
if(!groundObject) continue;
const btTransform& wheelTrans = getWheelTransformWS( i );
btMatrix3x3 wheelBasis0 = wheelTrans.getBasis();
m_axle[i] = btVector3(wheelBasis0[0][m_indexRightAxis],
wheelBasis0[1][m_indexRightAxis],
wheelBasis0[2][m_indexRightAxis] );
const btVector3& surfNormalWS =
wheelInfo.m_raycastInfo.m_contactNormalWS;
btScalar proj = m_axle[i].dot(surfNormalWS);
m_axle[i] -= surfNormalWS * proj;
m_axle[i] = m_axle[i].normalize();
m_forwardWS[i] = surfNormalWS.cross(m_axle[i]);
m_forwardWS[i].normalize();
resolveSingleBilateral(*m_chassisBody,
wheelInfo.m_raycastInfo.m_contactPointWS,
*groundObject,
wheelInfo.m_raycastInfo.m_contactPointWS,
btScalar(0.), m_axle[i],m_sideImpulse[i],
timeStep);
btScalar sideFrictionStiffness2 = btScalar(1.0);
m_sideImpulse[i] *= sideFrictionStiffness2;
}
btScalar sideFactor = btScalar(1.);
btScalar fwdFactor = 0.5;
bool sliding = false;
for (int wheel=0; wheel<getNumWheels(); wheel++)
{
btWheelInfo& wheelInfo = m_wheelInfo[wheel];
m_wheelInfo[wheel].m_skidInfo = btScalar(1.);
m_forwardImpulse[wheel] = btScalar(0.);
btRigidBody* groundObject =
(btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
if(!groundObject) continue;
if(m_zipper_speed > 0)
{
if (wheel==2 || wheel==3)
{
// The zipper velocity is the speed that should be
// reached. So compute the impulse to accelerate the
// kart up to that speed:
m_forwardImpulse[wheel] =
0.5f*(m_zipper_speed -
getRigidBody()->getLinearVelocity().length())
/ m_chassisBody->getInvMass();
}
}
else
{
btScalar rollingFriction = 0.f;
if (wheelInfo.m_engineForce != 0.f)
{
rollingFriction = wheelInfo.m_engineForce* timeStep;
}
else
{
btScalar defaultRollingFrictionImpulse = 0.f;
btScalar maxImpulse = wheelInfo.m_brake
? wheelInfo.m_brake
: defaultRollingFrictionImpulse;
btWheelContactPoint contactPt(m_chassisBody, groundObject,
wheelInfo.m_raycastInfo.m_contactPointWS,
m_forwardWS[wheel],maxImpulse);
rollingFriction = calcRollingFriction(contactPt);
// This is a work around for the problem that a kart shakes
// if it is braking: we get a minor impulse forward, which
// bullet then tries to offset by applying a backward
// impulse - which is a bit too big, causing a impulse
// backwards, ... till the kart is shaking backwards and
// forwards. By only applying half of the impulse in case
// of low friction this goes away.
if(wheelInfo.m_brake && fabsf(rollingFriction)<10)
rollingFriction*=0.5f;
}
m_forwardImpulse[wheel] = rollingFriction;
}
if(m_time_additional_impulse>0)
{
sliding = true;
m_wheelInfo[wheel].m_skidInfo = 0.0f;
}
else
{
btScalar maximp = wheelInfo.m_wheelsSuspensionForce
* timeStep * wheelInfo.m_frictionSlip;
btScalar maximpSide = maximp;
btScalar maximpSquared = maximp * maximpSide;
btScalar x = (m_forwardImpulse[wheel] ) * fwdFactor;
btScalar y = (m_sideImpulse[wheel] ) * sideFactor;
btScalar impulseSquared = (x*x + y*y);
if (impulseSquared > maximpSquared)
{
sliding = true;
btScalar factor = maximp / btSqrt(impulseSquared);
m_wheelInfo[wheel].m_skidInfo *= factor;
} // if impulseSquared > maximpSquared
} // else (!m_timed_impulse
} // for (int wheel=0; wheel<getNumWheels(); wheel++)
// Note: don't reset zipper speed, or the kart rewinder will
// get incorrect zipper information.
// The kart just stopped skidding. Adjust the velocity so that
// it points in the right direction.
// FIXME: this is not good enough, we need some smooth interpolation here.
if(m_is_skidding && m_skid_angular_velocity == 0)
{
btVector3 v = m_chassisBody->getLinearVelocity();
v.setZ(sqrt(v.getX()*v.getX()+v.getZ()*v.getZ()));
v.setX(0);
btVector3 v_new = m_chassisBody->getWorldTransform().getBasis()*v;
m_chassisBody->setLinearVelocity(v_new);
m_is_skidding = false;
}
if(m_skid_angular_velocity!=0)
{
m_is_skidding = true;
// Skidding is implemented by not having any forward impulse,
// but only add a side impulse
for(unsigned int i=0; i<4; i++)
{
m_forwardImpulse[i] = 0;
m_sideImpulse[i] = 0;
}
btVector3 av = m_chassisBody->getAngularVelocity();
av.setY(m_skid_angular_velocity);
m_chassisBody->setAngularVelocity(av);
}
else if (sliding && (m_allow_sliding || m_time_additional_impulse>0) )
{
for (int wheel = 0; wheel < getNumWheels(); wheel++)
{
if (m_sideImpulse[wheel] != btScalar(0.) &&
m_wheelInfo[wheel].m_skidInfo< btScalar(1.) )
{
m_forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
}
} // for wheel <getNumWheels
} // if sliding
// Apply the impulses
// ------------------
for (int wheel = 0;wheel<getNumWheels() ; wheel++)
{
btWheelInfo& wheelInfo = m_wheelInfo[wheel];
btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS
- m_chassisBody->getCenterOfMassPosition();
if (m_forwardImpulse[wheel] != btScalar(0.))
{
m_chassisBody->applyImpulse(
m_forwardWS[wheel]*(m_forwardImpulse[wheel]),
#define COMPATIBLE_0_7_3
#ifdef COMPATIBLE_0_7_3
// This was apparently done to help hexley
btVector3(0,0,0));
#else
rel_pos);
#endif
}
if (m_sideImpulse[wheel] != btScalar(0.))
{
btRigidBody* groundObject =
(btRigidBody*) m_wheelInfo[wheel].m_raycastInfo.m_groundObject;
btVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS
- groundObject->getCenterOfMassPosition();
//adjust relative position above ground so that force only
// acts sideways
btVector3 delta_vec = (wheelInfo.m_raycastInfo.m_hardPointWS
- wheelInfo.m_raycastInfo.m_contactPointWS);
if (delta_vec.length() != btScalar (0))
{
delta_vec = delta_vec.normalize();
rel_pos -= delta_vec * rel_pos.dot(delta_vec);
}
btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel];
#if defined ROLLING_INFLUENCE_FIX && !defined COMPATIBLE_0_7_3
// fix. It only worked if car's up was along Y - VT.
btVector3 vChassisWorldUp =
getRigidBody()->getCenterOfMassTransform()
.getBasis().getColumn(m_indexUpAxis);
rel_pos -= vChassisWorldUp * (vChassisWorldUp.dot(rel_pos) *
(1.f-wheelInfo.m_rollInfluence) );
#else
rel_pos[m_indexUpAxis] *= wheelInfo.m_rollInfluence;
#endif
m_chassisBody->applyImpulse(sideImp,rel_pos);
//apply friction impulse on the ground
groundObject->applyImpulse(-sideImp,rel_pos2);
} // if (m_sideImpulse[wheel] != btScalar(0.))
} // for wheel<getNumWheels()
void btRaycastVehicle::updateFriction(btScalar timeStep)
{
//calculate the impulse, so that the wheels don't move sidewards
int numWheel = getNumWheels();
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<getNumWheels(); i++)
{
btWheelInfo& wheelInfo = m_wheelInfo[i];
class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
if (groundObject)
numWheelsOnGround++;
m_sideImpulse[i] = btScalar(0.);
m_forwardImpulse[i] = btScalar(0.);
}
{
for (int i=0; i<getNumWheels(); i++)
{
btWheelInfo& wheelInfo = m_wheelInfo[i];
class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
if (groundObject)
{
const btTransform& wheelTrans = getWheelTransformWS( i );
btMatrix3x3 wheelBasis0 = wheelTrans.getBasis();
m_axle[i] = btVector3(
wheelBasis0[0][m_indexRightAxis],
wheelBasis0[1][m_indexRightAxis],
wheelBasis0[2][m_indexRightAxis]);
const btVector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
btScalar proj = m_axle[i].dot(surfNormalWS);
m_axle[i] -= surfNormalWS * proj;
m_axle[i] = m_axle[i].normalize();
m_forwardWS[i] = surfNormalWS.cross(m_axle[i]);
m_forwardWS[i].normalize();
resolveSingleBilateral(*m_chassisBody, wheelInfo.m_raycastInfo.m_contactPointWS,
*groundObject, wheelInfo.m_raycastInfo.m_contactPointWS,
btScalar(0.), m_axle[i],m_sideImpulse[i],timeStep);
m_sideImpulse[i] *= sideFrictionStiffness2;
}
}
}
btScalar sideFactor = btScalar(1.);
btScalar fwdFactor = 0.5;
bool sliding = false;
{
for (int wheel =0; wheel <getNumWheels(); wheel++)
{
btWheelInfo& wheelInfo = m_wheelInfo[wheel];
class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
btScalar rollingFriction = 0.f;
if (groundObject)
{
if (wheelInfo.m_engineForce != 0.f)
{
rollingFriction = wheelInfo.m_engineForce* timeStep;
} else
{
btScalar defaultRollingFrictionImpulse = 0.f;
btScalar maxImpulse = wheelInfo.m_brake ? wheelInfo.m_brake : defaultRollingFrictionImpulse;
btWheelContactPoint contactPt(m_chassisBody,groundObject,wheelInfo.m_raycastInfo.m_contactPointWS,m_forwardWS[wheel],maxImpulse);
rollingFriction = calcRollingFriction(contactPt);
}
}
//switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break)
m_forwardImpulse[wheel] = btScalar(0.);
m_wheelInfo[wheel].m_skidInfo= btScalar(1.);
if (groundObject)
{
m_wheelInfo[wheel].m_skidInfo= btScalar(1.);
btScalar maximp = wheelInfo.m_wheelsSuspensionForce * timeStep * wheelInfo.m_frictionSlip;
btScalar maximpSide = maximp;
btScalar maximpSquared = maximp * maximpSide;
m_forwardImpulse[wheel] = rollingFriction;//wheelInfo.m_engineForce* timeStep;
btScalar x = (m_forwardImpulse[wheel] ) * fwdFactor;
btScalar y = (m_sideImpulse[wheel] ) * sideFactor;
btScalar impulseSquared = (x*x + y*y);
if (impulseSquared > maximpSquared)
{
sliding = true;
btScalar factor = maximp / btSqrt(impulseSquared);
m_wheelInfo[wheel].m_skidInfo *= factor;
}
}
}
}
if (sliding)
{
for (int wheel = 0; wheel < getNumWheels(); wheel++)
{
if (m_sideImpulse[wheel] != btScalar(0.))
{
if (m_wheelInfo[wheel].m_skidInfo< btScalar(1.))
{
m_forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
}
}
}
}
// apply the impulses
{
for (int wheel = 0; wheel<getNumWheels() ; wheel++)
{
btWheelInfo& wheelInfo = m_wheelInfo[wheel];
btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS -
m_chassisBody->getCenterOfMassPosition();
if (m_forwardImpulse[wheel] != btScalar(0.))
{
m_chassisBody->applyImpulse(m_forwardWS[wheel]*(m_forwardImpulse[wheel]),rel_pos);
}
if (m_sideImpulse[wheel] != btScalar(0.))
{
class btRigidBody* groundObject = (class btRigidBody*) m_wheelInfo[wheel].m_raycastInfo.m_groundObject;
btVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS -
groundObject->getCenterOfMassPosition();
btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel];
#if defined ROLLING_INFLUENCE_FIX // fix. It only worked if car's up was along Y - VT.
btVector3 vChassisWorldUp = getRigidBody()->getCenterOfMassTransform().getBasis().getColumn(m_indexUpAxis);
rel_pos -= vChassisWorldUp * (vChassisWorldUp.dot(rel_pos) * (1.f-wheelInfo.m_rollInfluence));
#else
rel_pos[m_indexUpAxis] *= wheelInfo.m_rollInfluence;
#endif
m_chassisBody->applyImpulse(sideImp,rel_pos);
//apply friction impulse on the ground
groundObject->applyImpulse(-sideImp,rel_pos2);
}
}
}
}
void RaycastCar::update_forces(btScalar timeStep)
{
/////////////////////////////////////////
//
//
//
// forward impulse
//
//
//
float max_proj_lin_vel = std::max(m_wheelInfo[2].m_linear_velocity,m_wheelInfo[3].m_linear_velocity);
float max_to_wheel_rpm = std::max(m_wheelInfo[2].m_rpm,m_wheelInfo[3].m_rpm);
for (int wheel = getNumWheels()-1; wheel >= 0 ; wheel--)
{
WheelInfo & wheel_info = m_wheelInfo[wheel];
wheel_info.m_rolling_resistance_impulse = btScalar(0.0f);
wheel_info.m_brakes_braking_impulse = btScalar(0.0f);
wheel_info.m_engine_braking_impulse = btScalar(0.0f);
wheel_info.m_side_impulse = btScalar(0.0f);
wheel_info.m_forward_impulse = btScalar(0.0f);
btRigidBody * groundObject = 0;
groundObject = static_cast<btRigidBody*>(wheel_info.m_raycastInfo.m_groundObject);
const btTransform & wheelTrans = getWheelTransformWS(wheel);
btMatrix3x3 wheelBasis0 = wheelTrans.getBasis();
wheel_info.m_wheel_axle_WS = btVector3(wheelBasis0[0][m_indexRightAxis],
wheelBasis0[1][m_indexRightAxis],
wheelBasis0[2][m_indexRightAxis]);
const btVector3 & surfNormalWS = wheel_info.m_raycastInfo.m_contactNormalWS;
btScalar proj = wheel_info.m_wheel_axle_WS.dot(surfNormalWS);
wheel_info.m_wheel_axle_WS -= surfNormalWS * proj;
wheel_info.m_wheel_axle_WS.normalize();
wheel_info.m_wheel_direction_WS = surfNormalWS.cross(wheel_info.m_wheel_axle_WS);
wheel_info.m_wheel_direction_WS.normalize();
if (groundObject)
{
///////////////
//
// Rolling Resistance force
//
// http://www.engineeringtoolbox.com/rolling-friction-resistance-d_1303.html
if (fabs(m_wheelInfo[wheel].m_linear_velocity) > 0.01f)
{
double Crr = 0.005 +
(1.0/wheel_info.m_tire_pressure) * (0.01 + 0.0095*pow((m_currentVehicleSpeedKmHour/100.0),2));
WheelContactPoint contactPt0(m_chassisBody,
groundObject,
wheel_info.m_raycastInfo.m_contactPointWS,
wheel_info.m_wheel_direction_WS,
btScalar(Crr*wheel_info.m_suspension_force*timeStep));
wheel_info.m_rolling_resistance_impulse = calculateBrakingImpulse(contactPt0);
}
//
///////////////
///////////////
//
// Brakes braking force
//
if (wheel_info.m_brake > btScalar(0.0f))
{
WheelContactPoint contactPt1(m_chassisBody,
groundObject,
wheel_info.m_raycastInfo.m_contactPointWS,
wheel_info.m_wheel_direction_WS,
wheel_info.m_brake);
wheel_info.m_brakes_braking_impulse = calculateBrakingImpulse(contactPt1);
}
//
//
///////////////
///////////////
//
// Engine force TODO
//
//
if (m_gear != 1)
{
float diff_vel = 0.0f;
if (m_gear > 1)
{
diff_vel = max_proj_lin_vel
- rpm2rads(max_to_wheel_rpm)*wheel_info.m_wheelsRadius;
//
// tmp engine braking force (approximation!) dv = (F/m) dt = impulse/m
//
if (diff_vel >= 0.0f)
{
WheelContactPoint contactPt2(m_chassisBody,
groundObject,
wheel_info.m_raycastInfo.m_contactPointWS,
wheel_info.m_wheel_direction_WS,
(1.0f-m_throttle)*diff_vel*timeStep*m_mass*0.5f);//
btVector3 target_vel = wheel_info.m_wheel_direction_WS*rpm2rads(max_to_wheel_rpm)*wheel_info.m_wheelsRadius;
wheel_info.m_engine_braking_impulse = calculateEngineBrakingImpulse(contactPt2,target_vel);
wheel_info.m_engine_braking_impulse *= 0.5f; // per rear wheel
wheel_info.m_engine_force = btScalar(0.0f);
}
}
else if (m_gear == 0)
{
diff_vel = max_proj_lin_vel
- rpm2rads((-1.0f)*max_to_wheel_rpm)*wheel_info.m_wheelsRadius;
//
// engine braking force
//
if (diff_vel <= 0.0f)
{
WheelContactPoint contactPt2(m_chassisBody,
groundObject,
wheel_info.m_raycastInfo.m_contactPointWS,
wheel_info.m_wheel_direction_WS,
(1.0f-m_throttle)*diff_vel*timeStep*m_mass*0.5f);//dv = (F/m) dt = impulse/m
btVector3 target_vel = wheel_info.m_wheel_direction_WS*rpm2rads(-1.0f*max_to_wheel_rpm)*wheel_info.m_wheelsRadius;
wheel_info.m_engine_braking_impulse = calculateEngineBrakingImpulse(contactPt2,target_vel);
wheel_info.m_engine_braking_impulse *= 0.5f; // per rear wheel
wheel_info.m_engine_force = btScalar(0.0f);
}
}
}
else
{
;;
}
//
//
///////////////
//
//
//
/////////////////////////////////////////
/////////////////////////////////////////
//
// Side impulse TODO
//
//
this->resolveSingleBilateral(*m_chassisBody,
wheel_info.m_raycastInfo.m_contactPointWS,
*groundObject,
wheel_info.m_raycastInfo.m_contactPointWS,
wheel_info.m_wheel_axle_WS,
wheel_info.m_side_impulse);
wheel_info.m_side_impulse *= 0.25f;
//
//
/////////////////////////////////////////
}
#ifdef SHOW_MOTOR_INFO
wheel_info.m_info_brakes_braking_force = btScalar(wheel_info.m_brakes_braking_impulse/timeStep);
wheel_info.m_info_engine_braking_force = btScalar(wheel_info.m_engine_braking_impulse/timeStep);
wheel_info.m_info_rolling_resist_force = btScalar(wheel_info.m_rolling_resistance_impulse/timeStep);
wheel_info.m_info_side_force = btScalar(wheel_info.m_side_impulse/timeStep);
#endif
}
}
