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;
}
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
