//
// basically most of the code is general for 2 or 4 wheel vehicles, but some of it needs to be reviewed
//
btWheelInfo& btRaycastVehicle::addWheel( const btVector3& connectionPointCS, const btVector3& wheelDirectionCS0,const btVector3& wheelAxleCS, btScalar suspensionRestLength, btScalar wheelRadius,const btVehicleTuning& tuning, bool isFrontWheel)
{
btWheelInfoConstructionInfo ci;
ci.m_chassisConnectionCS = connectionPointCS;
ci.m_wheelDirectionCS = wheelDirectionCS0;
ci.m_wheelAxleCS = wheelAxleCS;
ci.m_suspensionRestLength = suspensionRestLength;
ci.m_wheelRadius = wheelRadius;
ci.m_suspensionStiffness = tuning.m_suspensionStiffness;
ci.m_wheelsDampingCompression = tuning.m_suspensionCompression;
ci.m_wheelsDampingRelaxation = tuning.m_suspensionDamping;
ci.m_frictionSlip = tuning.m_frictionSlip;
ci.m_bIsFrontWheel = isFrontWheel;
ci.m_maxSuspensionTravelCm = tuning.m_maxSuspensionTravelCm;
ci.m_maxSuspensionForce = tuning.m_maxSuspensionForce;
m_wheelInfo.push_back( btWheelInfo(ci));
btWheelInfo& wheel = m_wheelInfo[getNumWheels()-1];
updateWheelTransformsWS( wheel , false );
updateWheelTransform(getNumWheels()-1,false);
return wheel;
}
/** Resets the kart before a (re)start, to make sure all physics variable
* are properly defined. This is especially important for physics replay.
*/
void btKart::reset()
{
for(int i=0; i<getNumWheels(); i++)
{
btWheelInfo &wheel = m_wheelInfo[i];
wheel.m_raycastInfo.m_suspensionLength = 0;
wheel.m_rotation = 0;
updateWheelTransform(i, true);
}
m_visual_wheels_touch_ground = false;
m_zipper_active = false;
m_zipper_velocity = btScalar(0);
m_skid_angular_velocity = 0;
m_is_skidding = false;
m_allow_sliding = false;
m_num_wheels_on_ground = 0;
m_additional_impulse = btVector3(0,0,0);
m_time_additional_impulse = 0;
m_additional_rotation = btVector3(0,0,0);
m_time_additional_rotation = 0;
m_visual_rotation = 0;
// Set the brakes so that karts don't slide downhill
setAllBrakes(5.0f);
} // reset
void btRaycastVehicle::setSteeringValue(btScalar steering,int wheel)
{
btAssert(wheel>=0 && wheel < getNumWheels());
btWheelInfo& wheelInfo = getWheelInfo(wheel);
wheelInfo.m_steering = steering;
}
const btTransform& btRaycastVehicle::getWheelTransformWS( int wheelIndex ) const
{
btAssert(wheelIndex < getNumWheels());
const btWheelInfo& wheel = m_wheelInfo[wheelIndex];
return wheel.m_worldTransform;
}
// ----------------------------------------------------------------------------
void btKart::updateAllWheelPositions()
{
for (int i=0;i<getNumWheels();i++)
{
updateWheelTransform(i,false);
}
} // updateAllWheelPositions
// ----------------------------------------------------------------------------
void btKart::updateSuspension(btScalar deltaTime)
{
(void)deltaTime;
btScalar chassisMass = btScalar(1.) / m_chassisBody->getInvMass();
for (int w_it=0; w_it<getNumWheels(); w_it++)
{
btWheelInfo &wheel_info = m_wheelInfo[w_it];
if ( !wheel_info.m_raycastInfo.m_isInContact )
{
// A very unphysical thing to handle slopes that are a bit too
// steep or uneven (resulting in only one wheel on the ground)
// If only the front or only the rear wheels are on the ground, add
// a force pulling the axis down (towards the ground). Note that it
// is already guaranteed that either both or no wheels on one axis
// are on the ground, so we have to test only one of the wheels
wheel_info.m_wheelsSuspensionForce =
-m_kart->getKartProperties()->getStabilityTrackConnectionAccel()
* chassisMass;
continue;
}
btScalar force;
// Spring
btScalar susp_length = wheel_info.getSuspensionRestLength();
btScalar current_length = wheel_info.m_raycastInfo.m_suspensionLength;
btScalar length_diff = (susp_length - current_length);
if(m_kart->getKartProperties()->getSuspensionExpSpringResponse())
length_diff *= fabsf(length_diff)/susp_length;
float f = (1.0f + fabsf(length_diff) / susp_length);
// Scale the length diff. This results that in uphill sections, when
// the suspension is more compressed (i.e. length is bigger), more
// force is used, which makes it much less likely for the kart to hit
// the terrain, while when driving on flat terrain (length small),
// there is hardly any difference
length_diff *= f*f;
force = wheel_info.m_suspensionStiffness * length_diff
* wheel_info.m_clippedInvContactDotSuspension;
// Damper
btScalar projected_rel_vel = wheel_info.m_suspensionRelativeVelocity;
btScalar susp_damping = projected_rel_vel < btScalar(0.0)
? wheel_info.m_wheelsDampingCompression
: wheel_info.m_wheelsDampingRelaxation;
force -= susp_damping * projected_rel_vel;
// RESULT
wheel_info.m_wheelsSuspensionForce = force * chassisMass;
if (wheel_info.m_wheelsSuspensionForce < btScalar(0.))
{
wheel_info.m_wheelsSuspensionForce = btScalar(0.);
}
} // for (int w_it=0; w_it<getNumWheels(); w_it++)
} // updateSuspension
void btRaycastVehicle::updateSuspension(btScalar deltaTime)
{
(void)deltaTime;
btScalar chassisMass = btScalar(1.) / m_chassisBody->getInvMass();
for (int w_it=0; w_it<getNumWheels(); w_it++)
{
btWheelInfo &wheel_info = m_wheelInfo[w_it];
if ( wheel_info.m_raycastInfo.m_isInContact )
{
btScalar force;
// Spring
{
btScalar susp_length = wheel_info.getSuspensionRestLength();
btScalar current_length = wheel_info.m_raycastInfo.m_suspensionLength;
btScalar length_diff = (susp_length - current_length);
force = wheel_info.m_suspensionStiffness
* length_diff * wheel_info.m_clippedInvContactDotSuspension;
}
// Damper
{
btScalar projected_rel_vel = wheel_info.m_suspensionRelativeVelocity;
{
btScalar susp_damping;
if ( projected_rel_vel < btScalar(0.0) )
{
susp_damping = wheel_info.m_wheelsDampingCompression;
}
else
{
susp_damping = wheel_info.m_wheelsDampingRelaxation;
}
force -= susp_damping * projected_rel_vel;
}
}
// RESULT
wheel_info.m_wheelsSuspensionForce = force * chassisMass;
if (wheel_info.m_wheelsSuspensionForce < btScalar(0.))
{
wheel_info.m_wheelsSuspensionForce = btScalar(0.);
}
}
else
{
wheel_info.m_wheelsSuspensionForce = btScalar(0.0);
}
}
}
void RaycastCar::resetSuspension()
{
for (int i = 0; i < getNumWheels(); i++)
{
WheelInfo& wheel = m_wheelInfo[i];
wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength();
wheel.m_suspensionRelativeVelocity = btScalar(0.0f);
wheel.m_raycastInfo.m_contactNormalWS = -(wheel.m_raycastInfo.m_wheelDirectionWS);
wheel.m_clippedInvContactDotSuspension = btScalar(1.0f);
}
}
// ----------------------------------------------------------------------------
void btKart::updateSuspension(btScalar deltaTime)
{
(void)deltaTime;
btScalar chassisMass = btScalar(1.) / m_chassisBody->getInvMass();
for (int w_it=0; w_it<getNumWheels(); w_it++)
{
btWheelInfo &wheel_info = m_wheelInfo[w_it];
if ( !wheel_info.m_raycastInfo.m_isInContact )
{
// A very unphysical thing to handle slopes that are a bit too
// steep or uneven (resulting in only one wheel on the ground)
// If only the front or only the rear wheels are on the ground, add
// a force pulling the axis down (towards the ground). Note that it
// is already guaranteed that either both or no wheels on one axis
// are on the ground, so we have to test only one of the wheels
wheel_info.m_wheelsSuspensionForce =
-m_kart->getKartProperties()->getTrackConnectionAccel()
* chassisMass;
continue;
}
btScalar force;
// Spring
btScalar susp_length = wheel_info.getSuspensionRestLength();
btScalar current_length = wheel_info.m_raycastInfo.m_suspensionLength;
btScalar length_diff = (susp_length - current_length);
if(m_kart->getKartProperties()->getExpSpringResponse())
length_diff *= fabsf(length_diff)/susp_length;
force = wheel_info.m_suspensionStiffness * length_diff
* wheel_info.m_clippedInvContactDotSuspension;
// Damper
btScalar projected_rel_vel = wheel_info.m_suspensionRelativeVelocity;
btScalar susp_damping = projected_rel_vel < btScalar(0.0)
? wheel_info.m_wheelsDampingCompression
: wheel_info.m_wheelsDampingRelaxation;
force -= susp_damping * projected_rel_vel;
// RESULT
wheel_info.m_wheelsSuspensionForce = force * chassisMass;
if (wheel_info.m_wheelsSuspensionForce < btScalar(0.))
{
wheel_info.m_wheelsSuspensionForce = btScalar(0.);
}
} // for (int w_it=0; w_it<getNumWheels(); w_it++)
} // updateSuspension
void RaycastCar::update_suspension(btScalar step)
{
(void)step;
btScalar chassisMass = btScalar(1.0f) / m_chassisBody->getInvMass();
for (int w_it = 0; w_it < getNumWheels(); w_it++)
{
WheelInfo & wheel_info = m_wheelInfo[w_it];
if (wheel_info.m_raycastInfo.m_isInContact)
{
btScalar force;
// spring
{
btScalar susp_length = wheel_info.getSuspensionRestLength();
btScalar current_length = wheel_info.m_raycastInfo.m_suspensionLength;
btScalar length_diff = (susp_length - current_length);
force = wheel_info.m_suspensionStiffness*length_diff*wheel_info.m_clippedInvContactDotSuspension;
}
// damper
{
btScalar projected_rel_vel = wheel_info.m_suspensionRelativeVelocity;
{
btScalar susp_damping;
if (projected_rel_vel < btScalar(0.0f))
{
susp_damping = wheel_info.m_wheelsDampingCompression;
}
else
{
susp_damping = wheel_info.m_wheelsDampingRelaxation;
}
force -= susp_damping * projected_rel_vel;
}
}
// result
wheel_info.m_suspension_force = force * chassisMass;
if (wheel_info.m_suspension_force < btScalar(0.0f))
wheel_info.m_suspension_force = btScalar(0.0f);
}
else
{
wheel_info.m_suspension_force = btScalar(0.0f);
}
}
}
void RaycastCar::debugDraw(btIDebugDraw * debugDrawer)
{
for (int v = 0; v < getNumWheels(); v++)
{
btVector3 wheelColor(0.0f,1.0f,1.0f);
if (getWheelInfo(v).m_raycastInfo.m_isInContact)
{
wheelColor.setValue(0.0f,0.0f,1.0f);
}
else
{
wheelColor.setValue(1.0f,0.0f,1.0f);
}
btVector3 wheelPosWS = getWheelInfo(v).m_worldTransform.getOrigin();
btVector3 axle = btVector3(
getWheelInfo(v).m_worldTransform.getBasis()[0][getRightAxis()],
getWheelInfo(v).m_worldTransform.getBasis()[1][getRightAxis()],
getWheelInfo(v).m_worldTransform.getBasis()[2][getRightAxis()]);
debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor);
debugDrawer->drawLine(wheelPosWS,getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor);
}
}
void RaycastCar::updateVehicle( btScalar step )
{
m_currentVehicleSpeedKmHour = btScalar(3.6f) * getRigidBody()->getLinearVelocity().length();
const btTransform & chassisTrans = getChassisWorldTransform();
btVector3 forwardW(chassisTrans.getBasis()[0][m_indexForwardAxis],
chassisTrans.getBasis()[1][m_indexForwardAxis],
chassisTrans.getBasis()[2][m_indexForwardAxis]);
if (forwardW.dot(getRigidBody()->getLinearVelocity()) < btScalar(0.0f))
m_currentVehicleSpeedKmHour *= btScalar(-1.0f);
for (int i = 0; i < getNumWheels(); i++)
{
updateWheelTransform(i, false);
btScalar depth;
depth = rayCast(m_wheelInfo[i]);
}
update_suspension(step);
update_engine(step);
update_forces(step);
apply_impulses(step);
}
void btRaycastVehicle::setBrake(btScalar brake,int wheelIndex)
{
btAssert((wheelIndex >= 0) && (wheelIndex < getNumWheels()));
getWheelInfo(wheelIndex).m_brake = brake;
}
btWheelInfo& btRaycastVehicle::getWheelInfo(int index)
{
btAssert((index >= 0) && (index < getNumWheels()));
return m_wheelInfo[index];
}
void btRaycastVehicle::applyEngineForce(btScalar force, int wheel)
{
btAssert(wheel>=0 && wheel < getNumWheels());
btWheelInfo& wheelInfo = getWheelInfo(wheel);
wheelInfo.m_engineForce = force;
}
// ----------------------------------------------------------------------------
void btKart::updateVehicle( btScalar step )
{
for (int i=0;i<getNumWheels();i++)
{
updateWheelTransform(i,false);
}
const btTransform& chassisTrans = getChassisWorldTransform();
btVector3 forwardW(chassisTrans.getBasis()[0][m_indexForwardAxis],
chassisTrans.getBasis()[1][m_indexForwardAxis],
chassisTrans.getBasis()[2][m_indexForwardAxis]);
// Simulate suspension
// -------------------
m_num_wheels_on_ground = 0;
m_visual_wheels_touch_ground = true;
for (int i=0;i<m_wheelInfo.size();i++)
{
btScalar depth;
depth = rayCast( i);
if(m_wheelInfo[i].m_raycastInfo.m_isInContact)
m_num_wheels_on_ground++;
}
// If the kart is flying, try to keep it parallel to the ground.
if(m_num_wheels_on_ground==0)
{
btVector3 kart_up = getChassisWorldTransform().getBasis().getColumn(1);
btVector3 terrain_up(0,1,0);
btVector3 axis = kart_up.cross(terrain_up);
// Give a nicely balanced feeling for rebalancing the kart
m_chassisBody->applyTorqueImpulse(axis * m_kart->getKartProperties()->getSmoothFlyingImpulse());
}
// Work around: make sure that either both wheels on one axis
// are on ground, or none of them. This avoids the problem of
// the kart suddenly getting additional angular velocity because
// e.g. only one rear wheel is on the ground.
for(int i=0; i<m_wheelInfo.size(); i+=2)
{
if( m_wheelInfo[i ].m_raycastInfo.m_isInContact !=
m_wheelInfo[i+1].m_raycastInfo.m_isInContact)
{
int wheel_air_index = i;
int wheel_ground_index = i+1;
if (m_wheelInfo[i].m_raycastInfo.m_isInContact)
{
wheel_air_index = i+1;
wheel_ground_index = i;
}
btWheelInfo& wheel_air = m_wheelInfo[wheel_air_index];
btWheelInfo& wheel_ground = m_wheelInfo[wheel_ground_index];
wheel_air.m_raycastInfo = wheel_ground.m_raycastInfo;
}
} // for i=0; i<m_wheelInfo.size(); i+=2
updateSuspension(step);
for (int i=0;i<m_wheelInfo.size();i++)
{
//apply suspension force
btWheelInfo& wheel = m_wheelInfo[i];
btScalar suspensionForce = wheel.m_wheelsSuspensionForce;
if (suspensionForce > wheel.m_maxSuspensionForce)
{
suspensionForce = wheel.m_maxSuspensionForce;
}
btVector3 impulse = wheel.m_raycastInfo.m_contactNormalWS
* suspensionForce * step;
btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS
- getRigidBody()->getCenterOfMassPosition();
getRigidBody()->applyImpulse(impulse, relpos);
}
updateFriction( step);
for (int i=0;i<m_wheelInfo.size();i++)
{
btWheelInfo& wheel = m_wheelInfo[i];
btVector3 relpos = wheel.m_raycastInfo.m_hardPointWS
- getRigidBody()->getCenterOfMassPosition();
btVector3 vel = getRigidBody()->getVelocityInLocalPoint(relpos);
if (wheel.m_raycastInfo.m_isInContact)
{
const btTransform& chassisWorldTransform =
getChassisWorldTransform();
btVector3 fwd (
chassisWorldTransform.getBasis()[0][m_indexForwardAxis],
chassisWorldTransform.getBasis()[1][m_indexForwardAxis],
chassisWorldTransform.getBasis()[2][m_indexForwardAxis]);
btScalar proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS);
fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj;
btScalar proj2 = fwd.dot(vel);
wheel.m_deltaRotation = (proj2 * step) / (wheel.m_wheelsRadius);
wheel.m_rotation += wheel.m_deltaRotation;
} else
{
wheel.m_rotation += wheel.m_deltaRotation;
}
//damping of rotation when not in contact
wheel.m_deltaRotation *= btScalar(0.99);
}
float f = -m_kart->getSpeed()
* m_kart->getKartProperties()->getDownwardImpulseFactor();
btVector3 downwards_impulse = m_chassisBody->getWorldTransform().getBasis()
* btVector3(0, f, 0);
m_chassisBody->applyCentralImpulse(downwards_impulse);
if(m_time_additional_impulse>0)
{
float dt = step > m_time_additional_impulse
? m_time_additional_impulse
: step;
m_chassisBody->applyCentralImpulse(m_additional_impulse*dt);
m_time_additional_impulse -= dt;
}
if(m_time_additional_rotation>0)
{
btTransform &t = m_chassisBody->getWorldTransform();
float dt = step > m_time_additional_rotation
? m_time_additional_rotation
: step;
btQuaternion add_rot(m_additional_rotation.getY()*dt,
m_additional_rotation.getX()*dt,
m_additional_rotation.getZ()*dt);
t.setRotation(t.getRotation()*add_rot);
m_chassisBody->setWorldTransform(t);
// Also apply the rotation to the interpolated world transform.
// This is important (at least if the rotation is only applied
// in one frame) since STK will actually use the interpolated
// transform, which would otherwise only be updated one frame
// later, resulting in a one-frame incorrect rotation of the
// kart, or a strongly 'visual jolt' of the kart
btTransform &iwt=m_chassisBody->getInterpolationWorldTransform();
iwt.setRotation(iwt.getRotation()*add_rot);
m_time_additional_rotation -= dt;
}
} // updateVehicle
void btRaycastVehicle::updateVehicle( btScalar step )
{
{
for (int i=0; i<getNumWheels(); i++)
{
updateWheelTransform(i,false);
}
}
m_currentVehicleSpeedKmHour = btScalar(3.6) * getRigidBody()->getLinearVelocity().length();
const btTransform& chassisTrans = getChassisWorldTransform();
btVector3 forwardW (
chassisTrans.getBasis()[0][m_indexForwardAxis],
chassisTrans.getBasis()[1][m_indexForwardAxis],
chassisTrans.getBasis()[2][m_indexForwardAxis]);
if (forwardW.dot(getRigidBody()->getLinearVelocity()) < btScalar(0.))
{
m_currentVehicleSpeedKmHour *= btScalar(-1.);
}
//
// simulate suspension
//
int i=0;
for (i=0; i<m_wheelInfo.size(); i++)
{
btScalar depth;
depth = rayCast( m_wheelInfo[i]);
}
updateSuspension(step);
for (i=0; i<m_wheelInfo.size(); i++)
{
//apply suspension force
btWheelInfo& wheel = m_wheelInfo[i];
btScalar suspensionForce = wheel.m_wheelsSuspensionForce;
if (suspensionForce > wheel.m_maxSuspensionForce)
{
suspensionForce = wheel.m_maxSuspensionForce;
}
btVector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step;
btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - getRigidBody()->getCenterOfMassPosition();
getRigidBody()->applyImpulse(impulse, relpos);
}
updateFriction( step);
for (i=0; i<m_wheelInfo.size(); i++)
{
btWheelInfo& wheel = m_wheelInfo[i];
btVector3 relpos = wheel.m_raycastInfo.m_hardPointWS - getRigidBody()->getCenterOfMassPosition();
btVector3 vel = getRigidBody()->getVelocityInLocalPoint( relpos );
if (wheel.m_raycastInfo.m_isInContact)
{
const btTransform& chassisWorldTransform = getChassisWorldTransform();
btVector3 fwd (
chassisWorldTransform.getBasis()[0][m_indexForwardAxis],
chassisWorldTransform.getBasis()[1][m_indexForwardAxis],
chassisWorldTransform.getBasis()[2][m_indexForwardAxis]);
btScalar proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS);
fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj;
btScalar proj2 = fwd.dot(vel);
wheel.m_deltaRotation = (proj2 * step) / (wheel.m_wheelsRadius);
wheel.m_rotation += wheel.m_deltaRotation;
} else
{
wheel.m_rotation += wheel.m_deltaRotation;
}
wheel.m_deltaRotation *= btScalar(0.99);//damping of rotation when not in contact
}
}
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 RaycastCar::apply_impulses(btScalar step)
{
for (int wheel = 0; wheel < getNumWheels(); wheel++)
{
WheelInfo & wheel_info = m_wheelInfo[wheel];
if (!wheel_info.m_raycastInfo.m_isInContact) continue;
btVector3 rel_pos = wheel_info.m_raycastInfo.m_contactPointWS - getRigidBody()->getCenterOfMassPosition();
#ifndef USE_TMP_FIXED_OBJECT
btRigidBody * ground_body = 0;
btVector3 rel_pos_ground(0.0f,0.0f,0.0f);
if (wheel_info.m_raycastInfo.m_isInContact)
{
btCollisionObject * ground_object = 0;
ground_object = static_cast<btCollisionObject*>(wheel_info.m_raycastInfo.m_groundObject);
if (ground_object) ground_body = static_cast<btRigidBody*>(ground_object);
if (ground_body)
{
rel_pos_ground = wheel_info.m_raycastInfo.m_contactPointWS
-
ground_body->getCenterOfMassPosition();
}
}
#endif
////////////////////////////////////////
//
// suspension
btScalar suspension_force = wheel_info.m_suspension_force;
if (suspension_force > wheel_info.m_maxSuspensionForce)
suspension_force = wheel_info.m_maxSuspensionForce;
btVector3 suspension_impulse = wheel_info.m_raycastInfo.m_contactNormalWS * suspension_force * step;
m_chassisBody->applyImpulse(suspension_impulse, rel_pos);
#ifndef USE_TMP_FIXED_OBJECT
if (wheel_info.m_raycastInfo.m_isInContact
&&
ground_body
&&
!ground_body->isStaticOrKinematicObject())
{
ground_body->applyImpulse(-suspension_impulse, rel_pos_ground);
}
#endif
//
//
////////////////////////////////////////
////////////////////////////////////////
//
// forward
//
btVector3 forward_impulse(0,0,0);
wheel_info.m_forward_impulse = wheel_info.m_engine_force*step +
wheel_info.m_rolling_resistance_impulse +
wheel_info.m_engine_braking_impulse +
wheel_info.m_brakes_braking_impulse;
forward_impulse = wheel_info.m_wheel_direction_WS * wheel_info.m_forward_impulse;
if (!forward_impulse.fuzzyZero())
m_chassisBody->applyImpulse(forward_impulse,rel_pos);
#ifndef USE_TMP_FIXED_OBJECT
if (wheel_info.m_raycastInfo.m_isInContact
&&
ground_body
&&
!ground_body->isStaticOrKinematicObject())
{
ground_body->applyImpulse(-forward_impulse, rel_pos_ground);
}
#endif
//
//
////////////////////////////////////////
////////////////////////////////////////
//
// side
//
btVector3 side_impulse = wheel_info.m_wheel_axle_WS * wheel_info.m_side_impulse;
if (!side_impulse.fuzzyZero())
{
btVector3 vChassisWorldUp = getRigidBody()->getCenterOfMassTransform().getBasis().getColumn(m_indexUpAxis);
rel_pos -= vChassisWorldUp * (vChassisWorldUp.dot(rel_pos) * (1.0f-wheel_info.m_rollInfluence));
m_chassisBody->applyImpulse(side_impulse,rel_pos);
#ifndef USE_TMP_FIXED_OBJECT
if (wheel_info.m_raycastInfo.m_isInContact
&&
ground_body
&&
!ground_body->isStaticOrKinematicObject())
{
ground_body->applyImpulse(-side_impulse, rel_pos_ground);
}
#endif
}
//
//
////////////////////////////////////////
}
///////////////////////////
//
// air resistance force
//
// http://en.wikipedia.org/wiki/Drag_%28physics%29
// http://www.mayfco.com/lambor.htm
//
//
btScalar Cdrag = btScalar(0.5f) * m_drag_coef * m_area * m_air_dens;
btVector3 linear_velocity = getRigidBody()->getLinearVelocity();
if (!linear_velocity.fuzzyZero())
{
btScalar drag = Cdrag*linear_velocity.length2();
btVector3 Fdrag = -1.0f*drag*linear_velocity.normalized();
#ifdef SHOW_MOTOR_INFO
m_info_air_resist = drag;
#endif
m_chassisBody->applyCentralImpulse(Fdrag*step);
}
#ifdef SHOW_MOTOR_INFO
else
{
m_info_air_resist = btScalar(0.0f);
}
#endif
//
//
///////////////////////////
}
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
}
}
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);
}
}
}
}
// ----------------------------------------------------------------------------
btWheelInfo& btKart::getWheelInfo(int index)
{
btAssert((index >= 0) && (index < getNumWheels()));
return m_wheelInfo[index];
}
// ----------------------------------------------------------------------------
void btKart::setAllBrakes(btScalar brake)
{
for(int i=0; i<getNumWheels(); i++)
getWheelInfo(i).m_brake = brake;
} // setAllBrakes
void btRaycastVehicle::updateSuspension(btScalar deltaTime)
{
(void)deltaTime;
btScalar chassisMass = btScalar(1.) / m_chassisBody->getInvMass();
for (int w_it=0; w_it<getNumWheels(); w_it++)
{
btWheelInfo &wheel_info = m_wheelInfo[w_it];
if ( wheel_info.m_raycastInfo.m_isInContact )
{
btScalar force;
// Spring
{
btScalar susp_length = wheel_info.getSuspensionRestLength();
btScalar current_length = wheel_info.m_raycastInfo.m_suspensionLength;
btScalar length_diff = (susp_length - current_length);
force = wheel_info.m_suspensionStiffness
* length_diff * wheel_info.m_clippedInvContactDotSuspension;
}
// Damper
{
btScalar projected_rel_vel = wheel_info.m_suspensionRelativeVelocity;
{
btScalar susp_damping;
if ( projected_rel_vel < btScalar(0.0) )
{
susp_damping = wheel_info.m_wheelsDampingCompression;
}
else
{
susp_damping = wheel_info.m_wheelsDampingRelaxation;
}
force -= susp_damping * projected_rel_vel;
}
}
// RESULT
wheel_info.m_wheelsSuspensionForce = force * chassisMass;
if (wheel_info.m_wheelsSuspensionForce < btScalar(0.))
{
wheel_info.m_wheelsSuspensionForce = btScalar(0.);
}
}
else
{
wheel_info.m_wheelsSuspensionForce = btScalar(0.0);
}
}
// Connect suspension between front & back
float wheelsSuspensionForce[8];
int asNumWheels = getNumWheels();
btScalar sideBalance = 0.0f;
btScalar forwardBalance = 0.0f;
btScalar sideForwardBalance = 0.0f;
if(asNumWheels == 4)
{
//wheel[0]
sideBalance = m_wheelInfo[1].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[0].m_sideBalanceFactor : 0.0f;
forwardBalance = m_wheelInfo[2].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[0].m_forwardBalanceFactor : 0.0f;
sideForwardBalance = m_wheelInfo[3].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[0].m_sideBalanceFactor * m_wheelInfo[0].m_forwardBalanceFactor : 0.0f;
wheelsSuspensionForce[0] = 0.0f;
if (m_wheelInfo[0].m_wheelsSuspensionForce > 0.0f)
{
wheelsSuspensionForce[0] =
+ sideBalance * m_wheelInfo[1].m_wheelsSuspensionForce
+ forwardBalance * m_wheelInfo[2].m_wheelsSuspensionForce
+ sideForwardBalance * m_wheelInfo[3].m_wheelsSuspensionForce
+ (1.0f - sideForwardBalance - sideBalance - forwardBalance) * m_wheelInfo[0].m_wheelsSuspensionForce;
}
//wheel[1]
sideBalance = m_wheelInfo[0].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[1].m_sideBalanceFactor : 0.0f;
forwardBalance = m_wheelInfo[3].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[1].m_forwardBalanceFactor : 0.0f;
sideForwardBalance = m_wheelInfo[2].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[1].m_sideBalanceFactor * m_wheelInfo[1].m_forwardBalanceFactor : 0.0f;
wheelsSuspensionForce[1] = 0.0f;
if (m_wheelInfo[1].m_wheelsSuspensionForce > 0.0f)
{
wheelsSuspensionForce[1] =
+ sideBalance * m_wheelInfo[0].m_wheelsSuspensionForce
+ forwardBalance * m_wheelInfo[3].m_wheelsSuspensionForce
+ sideForwardBalance * m_wheelInfo[2].m_wheelsSuspensionForce
+ (1.0f - sideForwardBalance - sideBalance - forwardBalance) * m_wheelInfo[1].m_wheelsSuspensionForce;
}
//wheel[2]
sideBalance = m_wheelInfo[3].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[2].m_sideBalanceFactor : 0.0f;
forwardBalance = m_wheelInfo[0].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[2].m_forwardBalanceFactor : 0.0f;
sideForwardBalance = m_wheelInfo[1].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[2].m_sideBalanceFactor * m_wheelInfo[2].m_forwardBalanceFactor : 0.0f;
wheelsSuspensionForce[2] = 0.0f;
if (m_wheelInfo[2].m_wheelsSuspensionForce > 0.0f)
{
wheelsSuspensionForce[2] =
+ sideBalance * m_wheelInfo[3].m_wheelsSuspensionForce
+ forwardBalance * m_wheelInfo[0].m_wheelsSuspensionForce
+ sideForwardBalance * m_wheelInfo[1].m_wheelsSuspensionForce
+ (1.0f - sideForwardBalance - sideBalance - forwardBalance) * m_wheelInfo[2].m_wheelsSuspensionForce;
}
//wheel[3]
sideBalance = m_wheelInfo[2].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[3].m_sideBalanceFactor : 0.0f;
forwardBalance = m_wheelInfo[1].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[3].m_forwardBalanceFactor : 0.0f;
sideForwardBalance = m_wheelInfo[0].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[3].m_sideBalanceFactor * m_wheelInfo[3].m_forwardBalanceFactor : 0.0f;
wheelsSuspensionForce[3] = 0.0f;
if (m_wheelInfo[3].m_wheelsSuspensionForce > 0.0f)
{
wheelsSuspensionForce[3] =
+ sideBalance * m_wheelInfo[2].m_wheelsSuspensionForce
+ forwardBalance * m_wheelInfo[1].m_wheelsSuspensionForce
+ sideForwardBalance * m_wheelInfo[0].m_wheelsSuspensionForce
+ (1.0f - sideForwardBalance - sideBalance - forwardBalance) * m_wheelInfo[3].m_wheelsSuspensionForce;
}
}
if(asNumWheels == 6)
{
//wheel[0]
sideBalance = m_wheelInfo[1].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[0].m_sideBalanceFactor : 0.0f;
forwardBalance = m_wheelInfo[2].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[0].m_forwardBalanceFactor : 0.0f;
sideForwardBalance = m_wheelInfo[3].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[0].m_sideBalanceFactor * m_wheelInfo[0].m_forwardBalanceFactor : 0.0f;
wheelsSuspensionForce[0] = 0.0f;
if (m_wheelInfo[0].m_wheelsSuspensionForce > 0.0f)
{
wheelsSuspensionForce[0] =
+ sideBalance * m_wheelInfo[1].m_wheelsSuspensionForce
+ forwardBalance * m_wheelInfo[2].m_wheelsSuspensionForce
+ sideForwardBalance * m_wheelInfo[3].m_wheelsSuspensionForce
+ (1.0f - sideForwardBalance - sideBalance - forwardBalance) * m_wheelInfo[0].m_wheelsSuspensionForce;
}
//wheel[1]
sideBalance = m_wheelInfo[0].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[1].m_sideBalanceFactor : 0.0f;
forwardBalance = m_wheelInfo[3].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[1].m_forwardBalanceFactor : 0.0f;
sideForwardBalance = m_wheelInfo[2].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[1].m_sideBalanceFactor * m_wheelInfo[1].m_forwardBalanceFactor : 0.0f;
wheelsSuspensionForce[1] = 0.0f;
if (m_wheelInfo[1].m_wheelsSuspensionForce > 0.0f)
{
wheelsSuspensionForce[1] =
+ sideBalance * m_wheelInfo[0].m_wheelsSuspensionForce
+ forwardBalance * m_wheelInfo[3].m_wheelsSuspensionForce
+ sideForwardBalance * m_wheelInfo[2].m_wheelsSuspensionForce
+ (1.0f - sideForwardBalance - sideBalance - forwardBalance) * m_wheelInfo[1].m_wheelsSuspensionForce;
}
//wheel[2]
sideBalance = m_wheelInfo[3].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[2].m_sideBalanceFactor : 0.0f;
forwardBalance = m_wheelInfo[0].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[2].m_forwardBalanceFactor : 0.0f;
sideForwardBalance = m_wheelInfo[1].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[2].m_sideBalanceFactor * m_wheelInfo[2].m_forwardBalanceFactor : 0.0f;
wheelsSuspensionForce[2] = 0.0f;
if (m_wheelInfo[2].m_wheelsSuspensionForce > 0.0f)
{
wheelsSuspensionForce[2] =
+ sideBalance * m_wheelInfo[3].m_wheelsSuspensionForce
+ forwardBalance * m_wheelInfo[0].m_wheelsSuspensionForce
+ sideForwardBalance * m_wheelInfo[1].m_wheelsSuspensionForce
+ (1.0f - sideForwardBalance - sideBalance - forwardBalance) * m_wheelInfo[2].m_wheelsSuspensionForce;
}
//wheel[3]
sideBalance = m_wheelInfo[2].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[3].m_sideBalanceFactor : 0.0f;
forwardBalance = m_wheelInfo[1].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[3].m_forwardBalanceFactor : 0.0f;
sideForwardBalance = m_wheelInfo[0].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[3].m_sideBalanceFactor * m_wheelInfo[3].m_forwardBalanceFactor : 0.0f;
wheelsSuspensionForce[3] = 0.0f;
if (m_wheelInfo[3].m_wheelsSuspensionForce > 0.0f)
{
wheelsSuspensionForce[3] =
+ sideBalance * m_wheelInfo[2].m_wheelsSuspensionForce
+ forwardBalance * m_wheelInfo[1].m_wheelsSuspensionForce
+ sideForwardBalance * m_wheelInfo[0].m_wheelsSuspensionForce
+ (1.0f - sideForwardBalance - sideBalance - forwardBalance) * m_wheelInfo[3].m_wheelsSuspensionForce;
}
//wheel[4]
sideBalance = m_wheelInfo[5].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[4].m_sideBalanceFactor : 0.0f;
forwardBalance = m_wheelInfo[2].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[4].m_forwardBalanceFactor : 0.0f;
sideForwardBalance = m_wheelInfo[3].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[4].m_sideBalanceFactor * m_wheelInfo[4].m_forwardBalanceFactor : 0.0f;
wheelsSuspensionForce[4] = 0.0f;
if (m_wheelInfo[4].m_wheelsSuspensionForce > 0.0f)
{
wheelsSuspensionForce[4] =
+ sideBalance * m_wheelInfo[5].m_wheelsSuspensionForce
+ forwardBalance * m_wheelInfo[2].m_wheelsSuspensionForce
+ sideForwardBalance * m_wheelInfo[3].m_wheelsSuspensionForce
+ (1.0f - sideForwardBalance - sideBalance - forwardBalance) * m_wheelInfo[4].m_wheelsSuspensionForce;
}
//wheel[5]
sideBalance = m_wheelInfo[4].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[5].m_sideBalanceFactor : 0.0f;
forwardBalance = m_wheelInfo[3].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[5].m_forwardBalanceFactor : 0.0f;
sideForwardBalance = m_wheelInfo[2].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[5].m_sideBalanceFactor * m_wheelInfo[5].m_forwardBalanceFactor : 0.0f;
wheelsSuspensionForce[5] = 0.0f;
if (m_wheelInfo[5].m_wheelsSuspensionForce > 0.0f)
{
wheelsSuspensionForce[5] =
+ sideBalance * m_wheelInfo[4].m_wheelsSuspensionForce
+ forwardBalance * m_wheelInfo[3].m_wheelsSuspensionForce
+ sideForwardBalance * m_wheelInfo[2].m_wheelsSuspensionForce
+ (1.0f - sideForwardBalance - sideBalance - forwardBalance) * m_wheelInfo[5].m_wheelsSuspensionForce;
}
}
if(asNumWheels == 8)
{
//wheel[0]
sideBalance = m_wheelInfo[1].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[0].m_sideBalanceFactor : 0.0f;
forwardBalance = m_wheelInfo[2].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[0].m_forwardBalanceFactor : 0.0f;
sideForwardBalance = m_wheelInfo[3].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[0].m_sideBalanceFactor * m_wheelInfo[0].m_forwardBalanceFactor : 0.0f;
wheelsSuspensionForce[0] = 0.0f;
if (m_wheelInfo[0].m_wheelsSuspensionForce > 0.0f)
{
wheelsSuspensionForce[0] =
+ sideBalance * m_wheelInfo[1].m_wheelsSuspensionForce
+ forwardBalance * m_wheelInfo[2].m_wheelsSuspensionForce
+ sideForwardBalance * m_wheelInfo[3].m_wheelsSuspensionForce
+ (1.0f - sideForwardBalance - sideBalance - forwardBalance) * m_wheelInfo[0].m_wheelsSuspensionForce;
}
//wheel[1]
sideBalance = m_wheelInfo[0].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[1].m_sideBalanceFactor : 0.0f;
forwardBalance = m_wheelInfo[3].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[1].m_forwardBalanceFactor : 0.0f;
sideForwardBalance = m_wheelInfo[2].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[1].m_sideBalanceFactor * m_wheelInfo[1].m_forwardBalanceFactor : 0.0f;
wheelsSuspensionForce[1] = 0.0f;
if (m_wheelInfo[1].m_wheelsSuspensionForce > 0.0f)
{
wheelsSuspensionForce[1] =
+ sideBalance * m_wheelInfo[0].m_wheelsSuspensionForce
+ forwardBalance * m_wheelInfo[3].m_wheelsSuspensionForce
+ sideForwardBalance * m_wheelInfo[2].m_wheelsSuspensionForce
+ (1.0f - sideForwardBalance - sideBalance - forwardBalance) * m_wheelInfo[1].m_wheelsSuspensionForce;
}
//wheel[2]
sideBalance = m_wheelInfo[3].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[2].m_sideBalanceFactor : 0.0f;
forwardBalance = m_wheelInfo[0].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[2].m_forwardBalanceFactor : 0.0f;
sideForwardBalance = m_wheelInfo[1].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[2].m_sideBalanceFactor * m_wheelInfo[2].m_forwardBalanceFactor : 0.0f;
wheelsSuspensionForce[2] = 0.0f;
if (m_wheelInfo[2].m_wheelsSuspensionForce > 0.0f)
{
wheelsSuspensionForce[2] =
+ sideBalance * m_wheelInfo[3].m_wheelsSuspensionForce
+ forwardBalance * m_wheelInfo[0].m_wheelsSuspensionForce
+ sideForwardBalance * m_wheelInfo[1].m_wheelsSuspensionForce
+ (1.0f - sideForwardBalance - sideBalance - forwardBalance) * m_wheelInfo[2].m_wheelsSuspensionForce;
}
//wheel[3]
sideBalance = m_wheelInfo[2].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[3].m_sideBalanceFactor : 0.0f;
forwardBalance = m_wheelInfo[1].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[3].m_forwardBalanceFactor : 0.0f;
sideForwardBalance = m_wheelInfo[0].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[3].m_sideBalanceFactor * m_wheelInfo[3].m_forwardBalanceFactor : 0.0f;
wheelsSuspensionForce[3] = 0.0f;
if (m_wheelInfo[3].m_wheelsSuspensionForce > 0.0f)
{
wheelsSuspensionForce[3] =
+ sideBalance * m_wheelInfo[2].m_wheelsSuspensionForce
+ forwardBalance * m_wheelInfo[1].m_wheelsSuspensionForce
+ sideForwardBalance * m_wheelInfo[0].m_wheelsSuspensionForce
+ (1.0f - sideForwardBalance - sideBalance - forwardBalance) * m_wheelInfo[3].m_wheelsSuspensionForce;
}
//wheel[4]
sideBalance = m_wheelInfo[5].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[4].m_sideBalanceFactor : 0.0f;
forwardBalance = m_wheelInfo[2].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[4].m_forwardBalanceFactor : 0.0f;
sideForwardBalance = m_wheelInfo[3].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[4].m_sideBalanceFactor * m_wheelInfo[4].m_forwardBalanceFactor : 0.0f;
wheelsSuspensionForce[4] = 0.0f;
if (m_wheelInfo[4].m_wheelsSuspensionForce > 0.0f)
{
wheelsSuspensionForce[4] =
+ sideBalance * m_wheelInfo[5].m_wheelsSuspensionForce
+ forwardBalance * m_wheelInfo[2].m_wheelsSuspensionForce
+ sideForwardBalance * m_wheelInfo[3].m_wheelsSuspensionForce
+ (1.0f - sideForwardBalance - sideBalance - forwardBalance) * m_wheelInfo[4].m_wheelsSuspensionForce;
}
//wheel[5]
sideBalance = m_wheelInfo[4].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[5].m_sideBalanceFactor : 0.0f;
forwardBalance = m_wheelInfo[3].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[5].m_forwardBalanceFactor : 0.0f;
sideForwardBalance = m_wheelInfo[2].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[5].m_sideBalanceFactor * m_wheelInfo[5].m_forwardBalanceFactor : 0.0f;
wheelsSuspensionForce[5] = 0.0f;
if (m_wheelInfo[5].m_wheelsSuspensionForce > 0.0f)
{
wheelsSuspensionForce[5] =
+ sideBalance * m_wheelInfo[4].m_wheelsSuspensionForce
+ forwardBalance * m_wheelInfo[3].m_wheelsSuspensionForce
+ sideForwardBalance * m_wheelInfo[2].m_wheelsSuspensionForce
+ (1.0f - sideForwardBalance - sideBalance - forwardBalance) * m_wheelInfo[5].m_wheelsSuspensionForce;
}
//wheel[6]
sideBalance = m_wheelInfo[7].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[6].m_sideBalanceFactor : 0.0f;
forwardBalance = m_wheelInfo[4].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[6].m_forwardBalanceFactor : 0.0f;
sideForwardBalance = m_wheelInfo[5].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[6].m_sideBalanceFactor * m_wheelInfo[6].m_forwardBalanceFactor : 0.0f;
wheelsSuspensionForce[6] = 0.0f;
if (m_wheelInfo[6].m_wheelsSuspensionForce > 0.0f)
{
wheelsSuspensionForce[6] =
+ sideBalance * m_wheelInfo[7].m_wheelsSuspensionForce
+ forwardBalance * m_wheelInfo[4].m_wheelsSuspensionForce
+ sideForwardBalance * m_wheelInfo[5].m_wheelsSuspensionForce
+ (1.0f - sideForwardBalance - sideBalance - forwardBalance) * m_wheelInfo[6].m_wheelsSuspensionForce;
}
//wheel[7]
sideBalance = m_wheelInfo[6].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[7].m_sideBalanceFactor : 0.0f;
forwardBalance = m_wheelInfo[5].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[7].m_forwardBalanceFactor : 0.0f;
sideForwardBalance = m_wheelInfo[4].m_wheelsSuspensionForce > 0.0f ? m_wheelInfo[7].m_sideBalanceFactor * m_wheelInfo[7].m_forwardBalanceFactor : 0.0f;
wheelsSuspensionForce[7] = 0.0f;
if (m_wheelInfo[7].m_wheelsSuspensionForce > 0.0f)
{
wheelsSuspensionForce[7] =
+ sideBalance * m_wheelInfo[6].m_wheelsSuspensionForce
+ forwardBalance * m_wheelInfo[5].m_wheelsSuspensionForce
+ sideForwardBalance * m_wheelInfo[4].m_wheelsSuspensionForce
+ (1.0f - sideForwardBalance - sideBalance - forwardBalance) * m_wheelInfo[7].m_wheelsSuspensionForce;
}
}
for (int wheel = 0; wheel < getNumWheels(); ++wheel)
{
m_wheelInfo[wheel].m_wheelsSuspensionForce = wheelsSuspensionForce[wheel];
}
}