void btRaycastVehicle::updateWheelTransformsWS(btWheelInfo& wheel , bool interpolatedTransform)
{
wheel.m_raycastInfo.m_isInContact = false;
btTransform chassisTrans = getChassisWorldTransform();
if (interpolatedTransform && (getRigidBody()->getMotionState()))
{
getRigidBody()->getMotionState()->getWorldTransform(chassisTrans);
}
wheel.m_raycastInfo.m_hardPointWS = chassisTrans( wheel.m_chassisConnectionPointCS );
wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans.getBasis() * wheel.m_wheelDirectionCS ;
wheel.m_raycastInfo.m_wheelAxleWS = chassisTrans.getBasis() * wheel.m_wheelAxleCS;
}
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::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::updateVehicle( btScalar step )
{
updateAllWheelPositions();
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++)
{
rayCast( i);
if(m_wheelInfo[i].m_raycastInfo.m_isInContact)
m_num_wheels_on_ground++;
}
// Test if the kart is falling so fast
// that the chassis might hit the track
// ------------------------------------
bool needs_cushioning_test = false;
for(int i=0; i<m_wheelInfo.size(); i++)
{
btWheelInfo &wheel = m_wheelInfo[i];
if(!wheel.m_was_on_ground && wheel.m_raycastInfo.m_isInContact)
{
needs_cushioning_test = true;
break;
}
}
if(needs_cushioning_test)
{
const btVector3 &v = m_chassisBody->getLinearVelocity();
btVector3 down(0, 1, 0);
btVector3 v_down = (v * down) * down;
// Estimate what kind of downward speed can be compensated by the
// suspension. Atm the parameters are set that the suspension is
// actually capped at max suspension force, so the maximum
// speed that can be caught by the suspension without the chassis
// hitting the ground can be based on that. Note that there are
// 4 suspensions, all adding together.
btScalar max_compensate_speed = m_wheelInfo[0].m_maxSuspensionForce
* m_chassisBody->getInvMass()
* step * 4;
// If the downward speed is too fast to be caught by the suspension,
// slow down the falling speed by applying an appropriately impulse:
if(-v_down.getY() > max_compensate_speed)
{
btVector3 impulse = down * (-v_down.getY() - max_compensate_speed)
/ m_chassisBody->getInvMass()*0.5f;
//float v_old = m_chassisBody->getLinearVelocity().getY();
//float x = m_wheelInfo[0].m_raycastInfo.m_isInContact ? m_wheelInfo[0].m_raycastInfo.m_contactPointWS.getY() : -100;
m_chassisBody->applyCentralImpulse(impulse);
//Log::verbose("physics", "Cushioning %f from %f m/s to %f m/s wheel %f kart %f", impulse.getY(),
// v_old, m_chassisBody->getLinearVelocity().getY(), x,
// m_chassisBody->getWorldTransform().getOrigin().getY()
// );
}
}
for(int i=0; i<m_wheelInfo.size(); i++)
m_wheelInfo[i].m_was_on_ground = m_wheelInfo[i].m_raycastInfo.m_isInContact;
// 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 =
m_kart->getMaterial() && m_kart->getMaterial()->hasGravity() ?
m_kart->getNormal() : Vec3(0, 1, 0);
// Length of axis depends on the angle - i.e. the further awat
// the kart is from being upright, the larger the applied impulse
// will be, resulting in fast changes when the kart is on its
// side, but not overcompensating (and therefore shaking) when
// the kart is not much away from being upright.
btVector3 axis = kart_up.cross(terrain_up);
// To avoid the kart going backwards/forwards (or rolling sideways),
// set the pitch/roll to 0 before applying the 'straightening' impulse.
// TODO: make this works if gravity is changed.
btVector3 av = m_chassisBody->getAngularVelocity();
av.setX(0);
av.setZ(0);
m_chassisBody->setAngularVelocity(av);
// Give a nicely balanced feeling for rebalancing the kart
m_chassisBody->applyTorqueImpulse(axis * m_kart->getKartProperties()->getStabilitySmoothFlyingImpulse());
}
// 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 and then the kart
// rotates very abruptly.
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
// Apply suspension forcen (i.e. upwards force)
// --------------------------------------------
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);
}
// Update friction (i.e. forward force)
// ------------------------------------
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;
}
}
// If configured, add a force to keep karts on the track
// -----------------------------------------------------
float dif = m_kart->getKartProperties()->getStabilityDownwardImpulseFactor();
if(dif!=0 && m_num_wheels_on_ground==4)
{
float f = -fabsf(m_kart->getSpeed()) * dif;
btVector3 downwards_impulse = m_chassisBody->getWorldTransform().getBasis()
* btVector3(0, f, 0);
m_chassisBody->applyCentralImpulse(downwards_impulse);
}
// Apply additional impulse set by supertuxkart
// --------------------------------------------
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;
}
// Apply additional rotation set by supertuxkart
// ---------------------------------------------
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
btScalar btKart::rayCast(unsigned int index)
{
btWheelInfo &wheel = m_wheelInfo[index];
// Work around a bullet problem: when using a convex hull the raycast
// would sometimes hit the chassis (which does not happen when using a
// box shape). Therefore set the collision mask in the chassis body so
// that it is not hit anymore.
short int old_group=0;
if(m_chassisBody->getBroadphaseHandle())
{
old_group = m_chassisBody->getBroadphaseHandle()
->m_collisionFilterGroup;
m_chassisBody->getBroadphaseHandle()->m_collisionFilterGroup = 0;
}
updateWheelTransformsWS( wheel,false);
btScalar max_susp_len = wheel.getSuspensionRestLength()
+ wheel.m_maxSuspensionTravel;
// Do a slightly longer raycast to see if the kart might soon hit the
// ground and some 'cushioning' is needed to avoid that the chassis
// hits the ground.
btScalar raylen = max_susp_len + 0.5f;
btVector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen);
const btVector3& source = wheel.m_raycastInfo.m_hardPointWS;
wheel.m_raycastInfo.m_contactPointWS = source + rayvector;
const btVector3& target = wheel.m_raycastInfo.m_contactPointWS;
btVehicleRaycaster::btVehicleRaycasterResult rayResults;
btAssert(m_vehicleRaycaster);
void* object = m_vehicleRaycaster->castRay(source,target,rayResults);
wheel.m_raycastInfo.m_groundObject = 0;
btScalar depth = raylen * rayResults.m_distFraction;
if (object && depth < max_susp_len)
{
wheel.m_raycastInfo.m_contactNormalWS = rayResults.m_hitNormalInWorld;
wheel.m_raycastInfo.m_contactNormalWS.normalize();
wheel.m_raycastInfo.m_isInContact = true;
///@todo for driving on dynamic/movable objects!;
wheel.m_raycastInfo.m_triangle_index = rayResults.m_triangle_index;;
wheel.m_raycastInfo.m_groundObject = &getFixedBody();
wheel.m_raycastInfo.m_suspensionLength = depth;
//clamp on max suspension travel
btScalar minSuspensionLength = wheel.getSuspensionRestLength()
- wheel.m_maxSuspensionTravel;
btScalar maxSuspensionLength = wheel.getSuspensionRestLength()
+ wheel.m_maxSuspensionTravel;
if (wheel.m_raycastInfo.m_suspensionLength < minSuspensionLength)
{
wheel.m_raycastInfo.m_suspensionLength = minSuspensionLength;
}
if (wheel.m_raycastInfo.m_suspensionLength > maxSuspensionLength)
{
wheel.m_raycastInfo.m_suspensionLength = maxSuspensionLength;
}
wheel.m_raycastInfo.m_contactPointWS = rayResults.m_hitPointInWorld;
btScalar denominator = wheel.m_raycastInfo.m_contactNormalWS.dot(
wheel.m_raycastInfo.m_wheelDirectionWS );
btVector3 chassis_velocity_at_contactPoint;
btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS
- getRigidBody()->getCenterOfMassPosition();
chassis_velocity_at_contactPoint =
getRigidBody()->getVelocityInLocalPoint(relpos);
btScalar projVel = wheel.m_raycastInfo.m_contactNormalWS.dot(
chassis_velocity_at_contactPoint );
if ( denominator >= btScalar(-0.1))
{
wheel.m_suspensionRelativeVelocity = btScalar(0.0);
wheel.m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1);
}
else
{
btScalar inv = btScalar(-1.) / denominator;
wheel.m_suspensionRelativeVelocity = projVel * inv;
wheel.m_clippedInvContactDotSuspension = inv;
}
} else
{
depth = btScalar(-1.0);
//put wheel info as in rest position
wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength();
wheel.m_suspensionRelativeVelocity = btScalar(0.0);
wheel.m_raycastInfo.m_contactNormalWS =
- wheel.m_raycastInfo.m_wheelDirectionWS;
wheel.m_clippedInvContactDotSuspension = btScalar(1.0);
}
#define USE_VISUAL
#ifndef USE_VISUAL
m_visual_contact_point[index] = rayResults.m_hitPointInWorld;
#else
if(index==2 || index==3)
{
btTransform chassisTrans = getChassisWorldTransform();
if (getRigidBody()->getMotionState())
{
getRigidBody()->getMotionState()->getWorldTransform(chassisTrans);
}
btQuaternion q(m_visual_rotation, 0, 0);
btQuaternion rot_new = chassisTrans.getRotation() * q;
chassisTrans.setRotation(rot_new);
btVector3 pos = m_kart->getKartModel()->getWheelGraphicsPosition(index);
pos.setZ(pos.getZ()*0.9f);
btVector3 source = chassisTrans( pos );
btVector3 target = source + rayvector;
btVehicleRaycaster::btVehicleRaycasterResult rayResults;
void* object = m_vehicleRaycaster->castRay(source,target,rayResults);
m_visual_contact_point[index] = rayResults.m_hitPointInWorld;
m_visual_contact_point[index-2] = source;
m_visual_wheels_touch_ground &= (object!=NULL);
}
#endif
if(m_chassisBody->getBroadphaseHandle())
{
m_chassisBody->getBroadphaseHandle()->m_collisionFilterGroup
= old_group;
}
return depth;
} // rayCast
void RaycastCar::update_engine(btScalar step)
{
const btTransform & chassis_t = getChassisWorldTransform();
btVector3 fwd(chassis_t.getBasis()[0][m_indexForwardAxis],
chassis_t.getBasis()[1][m_indexForwardAxis],
chassis_t.getBasis()[2][m_indexForwardAxis]);
WheelInfo & wheelInfo0 = m_wheelInfo[0];
wheelInfo0.m_last_linear_velocity = wheelInfo0.m_linear_velocity;
btVector3 relpos0 = wheelInfo0.m_raycastInfo.m_hardPointWS - getRigidBody()->getCenterOfMassPosition();
btVector3 vel0 = getRigidBody()->getVelocityInLocalPoint(relpos0);
btScalar proj0 = fwd.dot(wheelInfo0.m_raycastInfo.m_contactNormalWS);
btVector3 fwd_w0 = fwd - wheelInfo0.m_raycastInfo.m_contactNormalWS * proj0;
btScalar lin_vel0 = fwd_w0.dot(vel0);
btScalar w0_rpm = fabs(rads2rpm(lin_vel0/wheelInfo0.m_wheelsRadius));
btScalar delta0 = (lin_vel0/wheelInfo0.m_wheelsRadius)*step;
wheelInfo0.m_linear_velocity = lin_vel0;
wheelInfo0.m_angular_velocity = lin_vel0/wheelInfo0.m_wheelsRadius;
WheelInfo & wheelInfo1 = m_wheelInfo[1];
wheelInfo1.m_last_linear_velocity = wheelInfo1.m_linear_velocity;
btVector3 relpos1 = wheelInfo1.m_raycastInfo.m_hardPointWS - getRigidBody()->getCenterOfMassPosition();
btVector3 vel1 = getRigidBody()->getVelocityInLocalPoint(relpos1);
btScalar proj1 = fwd.dot(wheelInfo1.m_raycastInfo.m_contactNormalWS);
btVector3 fwd_w1 = fwd - wheelInfo1.m_raycastInfo.m_contactNormalWS * proj1;
btScalar lin_vel1 = fwd_w1.dot(vel1);
btScalar w1_rpm = fabs(rads2rpm(lin_vel1/wheelInfo1.m_wheelsRadius));
btScalar delta1 = (lin_vel1/wheelInfo1.m_wheelsRadius)*step;
wheelInfo1.m_linear_velocity = lin_vel1;
wheelInfo1.m_angular_velocity = lin_vel1/wheelInfo1.m_wheelsRadius;
WheelInfo & wheelInfo2 = m_wheelInfo[2];
wheelInfo2.m_last_linear_velocity = wheelInfo2.m_linear_velocity;
btVector3 relpos2 = wheelInfo2.m_raycastInfo.m_hardPointWS - getRigidBody()->getCenterOfMassPosition();
btVector3 vel2 = getRigidBody()->getVelocityInLocalPoint(relpos2);
btScalar proj2 = fwd.dot(wheelInfo2.m_raycastInfo.m_contactNormalWS);
btVector3 fwd_w2 = fwd - wheelInfo2.m_raycastInfo.m_contactNormalWS * proj2;
btScalar lin_vel2 = fwd_w2.dot(vel2);
//btScalar w2_rpm = fabs(rads2rpm(lin_vel2/wheelInfo2.m_wheelsRadius));
btScalar delta2 = (lin_vel2/wheelInfo2.m_wheelsRadius)*step;
wheelInfo2.m_linear_velocity = lin_vel2;
wheelInfo2.m_angular_velocity = lin_vel2/wheelInfo2.m_wheelsRadius;
WheelInfo & wheelInfo3 = m_wheelInfo[3];
wheelInfo3.m_last_linear_velocity = wheelInfo3.m_linear_velocity;
btVector3 relpos3 = wheelInfo3.m_raycastInfo.m_hardPointWS - getRigidBody()->getCenterOfMassPosition();
btVector3 vel3 = getRigidBody()->getVelocityInLocalPoint(relpos3);
btScalar proj3 = fwd.dot(wheelInfo3.m_raycastInfo.m_contactNormalWS);
btVector3 fwd_w3 = fwd - wheelInfo3.m_raycastInfo.m_contactNormalWS * proj3;
btScalar lin_vel3 = fwd_w3.dot(vel3);
//btScalar w3_rpm = fabs(rads2rpm(lin_vel3/wheelInfo3.m_wheelsRadius));
btScalar delta3 = (lin_vel3/wheelInfo3.m_wheelsRadius)*step;
wheelInfo3.m_linear_velocity = lin_vel3;
wheelInfo3.m_angular_velocity = lin_vel3/wheelInfo3.m_wheelsRadius;
/////////////////////////////
//
// Update engine
//
//
m_last_engine_rpm = m_engine_rpm;
m_last_throttle = m_throttle;
btScalar to_engine_rpm = btScalar(0.0f);
if (m_differential_type == 0)
{
to_engine_rpm = btMax(wheelInfo2.m_rpm,wheelInfo3.m_rpm);
}
if (m_gear == 1) // idle
{
m_engine_rpm = btScalar(m_rpm_data[m_graphs_size-1]*m_throttle);
}
else
{
m_engine_rpm = to_engine_rpm*m_gears_coefficients[m_gear]*m_final_drive*m_gears_eff;
}
if (m_engine_rpm > btScalar(m_rpm_data[m_graphs_size-1]*m_throttle))
{
m_engine_rpm = m_rpm_data[m_graphs_size-1]*m_throttle;
}
if (m_engine_rpm < btScalar(m_rpm_data[0])) {
m_engine_rpm = btScalar(m_rpm_data[0]);
}
btScalar torque = lookup_value(m_engine_rpm, m_rpm_data,m_torque_data,m_graphs_size)*m_throttle;
#ifdef SHOW_MOTOR_INFO
m_info_engine_torque = torque;
m_info_engine_power = (torque*2.0f*SIMD_PI*m_engine_rpm)/60.0f;
#endif
m_info_engine_rpm = m_engine_rpm + (rand() % 10 + 1);
// drive force to apply to chassis at both rear wheel's
// contact points in local space
btScalar Fdrive;
if (m_gear == 0)
{
Fdrive = (m_gears_coefficients[m_gear]*m_gears_eff*torque)/wheelInfo2.m_wheelsRadius;
Fdrive *= 0.5f; // per wheel
Fdrive *= -1.0f;
}
//if (m_gear == 1)
//{
//Fdrive = btScalar(0.0f);
//}
else
{
Fdrive = (m_gears_coefficients[m_gear]*m_final_drive*m_gears_eff*torque)/wheelInfo2.m_wheelsRadius;
Fdrive *= 0.5f; // per wheel
}
btScalar to_rear_wheel_rpm;
// acceleration (depends on m_inertia_coef) is essential to update engine RPM
btScalar acceleration = (torque*m_gears_coefficients[m_gear]*m_final_drive*m_gears_eff)/m_inertia_coef;
btScalar engine_rpm = m_engine_rpm + rads2rpm(acceleration)*step;
if (m_gear == 0)
{
to_rear_wheel_rpm = engine_rpm/(m_gears_coefficients[m_gear]*m_gears_eff);
to_rear_wheel_rpm *= 0.2f; //?
wheelInfo2.m_engine_force = wheelInfo3.m_engine_force = Fdrive;
wheelInfo2.m_rpm = wheelInfo3.m_rpm = to_rear_wheel_rpm;
if (to_rear_wheel_rpm
>
(m_rpm_data[m_graphs_size-1]*m_throttle)/(m_gears_coefficients[m_gear]*m_final_drive*m_gears_eff))
{
to_rear_wheel_rpm = (m_rpm_data[m_graphs_size-1]*m_throttle)/(m_gears_coefficients[m_gear]*m_final_drive*m_gears_eff);
}
}
else if (m_gear == 1)
{
to_rear_wheel_rpm = 0.0f;
wheelInfo2.m_engine_force = wheelInfo3.m_engine_force = btScalar(0.0f);
wheelInfo2.m_rpm = wheelInfo3.m_rpm = to_rear_wheel_rpm = 0.0f;
}
else
{
to_rear_wheel_rpm = engine_rpm/(m_gears_coefficients[m_gear]*m_final_drive*m_gears_eff);
wheelInfo2.m_engine_force = wheelInfo3.m_engine_force = Fdrive;
wheelInfo2.m_rpm = wheelInfo3.m_rpm = to_rear_wheel_rpm;
if (to_rear_wheel_rpm > (m_rpm_data[m_graphs_size-1]*m_throttle)/(m_gears_coefficients[m_gear]*m_final_drive*m_gears_eff))
{
to_rear_wheel_rpm = (m_rpm_data[m_graphs_size-1]*m_throttle)/(m_gears_coefficients[m_gear]*m_final_drive*m_gears_eff);
}
}
btScalar rear_wheel_delta = rpm2rads(to_rear_wheel_rpm)*step;
//
//
//
/////////////////////////////
/////////////////////////////
//
// Update wheel rotation
//
if (wheelInfo0.m_brake > btScalar(0.0f)) delta0 = btScalar(0.0f);
if (wheelInfo1.m_brake > btScalar(0.0f)) delta1 = btScalar(0.0f);
if (wheelInfo2.m_brake > btScalar(0.0f))
{
delta2 = btScalar(0.0f);
rear_wheel_delta = btScalar(0.0f);
}
if (wheelInfo3.m_brake > btScalar(0.0f))
{
delta3 = btScalar(0.0f);
rear_wheel_delta = btScalar(0.0f);
}
// 0
if (wheelInfo0.m_raycastInfo.m_isInContact)
{
wheelInfo0.m_rotation += delta0;
wheelInfo0.m_rpm = w0_rpm;
}
else
{
wheelInfo0.m_rotation *= 0.99f;
}
// 1
if (wheelInfo1.m_raycastInfo.m_isInContact)
{
wheelInfo1.m_rotation += delta1;
wheelInfo1.m_rpm = w1_rpm;
}
else
{
wheelInfo1.m_rotation *= 0.99f;
}
// 2
if ( wheelInfo2.m_raycastInfo.m_isInContact
&&
( fabs(rear_wheel_delta)
<
fabs(delta2) ) )
{
wheelInfo2.m_rotation += delta2;
}
else
{
if ((m_throttle > btScalar(0.0f)) && (m_gear != 1))
{
if (m_gear == 0)
{
wheelInfo2.m_rotation -= rear_wheel_delta;
}
else
{
wheelInfo2.m_rotation += rear_wheel_delta;
}
}
}
// 3
if ( wheelInfo3.m_raycastInfo.m_isInContact
&&
( fabs(rear_wheel_delta)
<
fabs(delta3) ) )
{
wheelInfo3.m_rotation += delta3;
}
else
{
if ((m_throttle > btScalar(0.0f)) && (m_gear != 1))
{
if (m_gear == 0)
{
wheelInfo3.m_rotation -= rear_wheel_delta;
}
else
{
wheelInfo3.m_rotation += rear_wheel_delta;
}
}
}
//
//
/////////////////////////////
}
// ----------------------------------------------------------------------------
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
