//
// 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;
}
void btRaycastVehicle::updateWheelTransform( int wheelIndex , bool interpolatedTransform)
{
btWheelInfo& wheel = m_wheelInfo[ wheelIndex ];
updateWheelTransformsWS(wheel,interpolatedTransform);
btVector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS;
const btVector3& right = wheel.m_raycastInfo.m_wheelAxleWS;
btVector3 fwd = up.cross(right);
fwd = fwd.normalize();
// up = right.cross(fwd);
// up.normalize();
//rotate around steering over de wheelAxleWS
btScalar steering = wheel.m_steering;
btQuaternion steeringOrn(up,steering);//wheel.m_steering);
btMatrix3x3 steeringMat(steeringOrn);
btQuaternion rotatingOrn(right,-wheel.m_rotation);
btMatrix3x3 rotatingMat(rotatingOrn);
btMatrix3x3 basis2(
right[0],fwd[0],up[0],
right[1],fwd[1],up[1],
right[2],fwd[2],up[2]
);
wheel.m_worldTransform.setBasis(steeringMat * rotatingMat * basis2);
wheel.m_worldTransform.setOrigin(
wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength
);
}
void RaycastCar::addWheel(VehicleConfig * c,
int wheel)
{
WheelInfo i;
i.m_wheelDirectionCS = c->wheel_direction_CS0;
i.m_wheelAxleCS = c->wheel_axle_CS;
i.m_suspensionRestLength = c->suspension_rest_length;
i.m_suspensionStiffness = c->suspension_stiffness;
i.m_wheelsDampingCompression = c->damping_compression;
i.m_wheelsDampingRelaxation = c->damping_relaxation;
i.m_maxSuspensionTravelCm = c->max_suspension_travel;
i.m_maxSuspensionForce = c->max_suspension_force;
i.m_rollInfluence = c->roll_influence;
if (wheel == 0)
{
i.m_wheelsRadius = c->fwheel_radius;
i.m_chassisConnectionPointCS = c->wheel_fr_v;
i.m_tire_pressure = c->fwheel_tire_pressure;
}
else if (wheel == 1)
{
i.m_wheelsRadius = c->fwheel_radius;
i.m_chassisConnectionPointCS = c->wheel_fl_v;
i.m_tire_pressure = c->fwheel_tire_pressure;
}
else if (wheel == 2)
{
i.m_wheelsRadius = c->rwheel_radius;
i.m_chassisConnectionPointCS = c->wheel_br_v;
i.m_tire_pressure = c->rwheel_tire_pressure;
}
else if (wheel == 3)
{
i.m_wheelsRadius = c->rwheel_radius;
i.m_chassisConnectionPointCS = c->wheel_bl_v;
i.m_tire_pressure = c->rwheel_tire_pressure;
}
else
{
;;
}
i.m_worldTransform.setIdentity();
i.m_wheel_direction_WS = btVector3(0,0,1);
i.m_wheel_axle_WS = btVector3(1,0,0);
m_wheelInfo.push_back(i);
updateWheelTransformsWS(i, false);
}
btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
{
updateWheelTransformsWS( wheel,false);
btScalar depth = -1;
btScalar raylen = wheel.getSuspensionRestLength()+wheel.m_wheelsRadius;
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;
btScalar param = btScalar(0.);
btVehicleRaycaster::btVehicleRaycasterResult rayResults;
btAssert(m_vehicleRaycaster);
void* object = m_vehicleRaycaster->castRay(source,target,rayResults);
wheel.m_raycastInfo.m_groundObject = 0;
if (object)
{
param = rayResults.m_distFraction;
depth = raylen * rayResults.m_distFraction;
wheel.m_raycastInfo.m_contactNormalWS = rayResults.m_hitNormalInWorld;
wheel.m_raycastInfo.m_isInContact = true;
wheel.m_raycastInfo.m_groundObject = &getFixedBody();///@todo for driving on dynamic/movable objects!;
//wheel.m_raycastInfo.m_groundObject = object;
btScalar hitDistance = param*raylen;
wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelsRadius;
//clamp on max suspension travel
btScalar minSuspensionLength = wheel.getSuspensionRestLength() - wheel.m_maxSuspensionTravelCm*btScalar(0.01);
btScalar maxSuspensionLength = wheel.getSuspensionRestLength()+ wheel.m_maxSuspensionTravelCm*btScalar(0.01);
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
{
//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);
}
return depth;
}
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