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 )
{
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 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
void Vehicle::initialize(btDiscreteDynamicsWorld &dynamicsWorld_in)
{
// SceneObject *nullObj;
if (!cmap_obj)
{
cmap_obj = new Mesh;
cmap_obj->cloneStructure(*obj);
}
#ifndef ARCH_PSP
#ifndef OPENGL_ES
#ifndef ARCH_DC
cmap_obj->triangulate();
#endif
#endif
#endif
cmap = new CollisionMap;
cmap->addMesh(*cmap_obj);
colShape = cmap->makeCollisionShape(mass);
// if (wheelObj)
// {
// // front left
// wheelRef[0].bind(*wheelObj);
// wheel[0].bindChild(wheelRef[0]);
// wheel[0].setPosition(XYZ(0.486f,0.141f,-0.9125f));
// wheel[0].setMatrixLock(true);
//
// // front right
// wheelRef[1].bind(*wheelObj);
// wheelRef[1].setRotation(XYZ(0,180,0));
// wheel[1].bindChild(wheelRef[1]);
// wheel[1].setPosition(XYZ(-0.486f,0.141f,-0.9125f));
// wheel[1].setMatrixLock(true);
//
// // front left
// wheelRef[2].bind(*wheelObj);
// wheelRef[2].setRotation(XYZ(0,180,0));
// wheel[2].bindChild(wheelRef[2]);
// wheel[2].setPosition(XYZ(0.486f,0.141f,0.843f));
// wheel[2].setMatrixLock(true);
//
// // front right
// wheelRef[3].bind(*wheelObj);
// wheel[3].bindChild(wheelRef[3]);
// wheel[3].setPosition(XYZ(-0.486f,0.141f,0.843f));
// wheel[3].setMatrixLock(true);
// }
btVector3 wheelAxleCSR,wheelAxleCSL;
wheelDirectionCS0 = btVector3(0,-1,0);
wheelAxleCS = btVector3(-1,0,0);
RigidSceneObject::initialize(dynamicsWorld_in);
btTransform tr;
tr.setIdentity();
setPivot(XYZ(0,0,0));
btCompoundShape* compound = new btCompoundShape();
btTransform localTrans;
localTrans.setIdentity();
localTrans.setOrigin(btVector3(0,0,0));
compound->addChildShape(localTrans,colShape);
tr.setOrigin(btVector3(0,0.f,0));
setMass(mass);
mRigidBody = &createRigidBody(*compound);
// reset scene
gVehicleSteering = 0.f;
mRigidBody->setCenterOfMassTransform(btTransform::getIdentity());
mRigidBody->setLinearVelocity(btVector3(0,0,0));
mRigidBody->setAngularVelocity(btVector3(0,0,0));
// dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(m_carChassis->getBroadphaseHandle(),dynamicsWorld->getDispatcher());
/// create vehicle
m_vehicleRayCaster = new btDefaultVehicleRaycaster(dynamicsWorld);
m_vehicle = new btRaycastVehicle(m_tuning,mRigidBody,m_vehicleRayCaster);
///never deactivate the vehicle
mRigidBody->setActivationState(DISABLE_DEACTIVATION);
dynamicsWorld->addVehicle(m_vehicle);
//choose coordinate system
m_vehicle->setCoordinateSystem(rightIndex,upIndex,forwardIndex);
// btVector3 connectionPointCS0(axelWidth/2.0f,connectionHeight,frontAxelZ);
for (unsigned int i = 0; i < wheels.size(); i++)
{
btVector3 wpos = wheels[i]->getWheelPosition().cast();
// printf("Wheel Radius: %f\n",wheels[i]->getWheelRadius());
// printf("Wheel Width: %f\n",wheels[i]->getWheelWidth());
m_vehicle->addWheel(wpos,wheelDirectionCS0,wheelAxleCS,wheels[i]->getSuspensionRest(),wheels[i]->getWheelRadius(),m_tuning,wheels[i]->getSteering());
}
// connectionPointCS0 = btVector3(axelWidth/2.0f,connectionHeight,rearAxelZ);
// m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel);
updateSuspension();
/// end car
//worldspaceChildren = true;
}
