Page MenuHome
Differential D8762 Diff 28333 extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp

Changeset View

Standalone View

View Options

extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp

Show All 18 Lines
#include "btVehicleRaycaster.h" #include "btVehicleRaycaster.h"
#include "btWheelInfo.h" #include "btWheelInfo.h"
#include "LinearMath/btMinMax.h" #include "LinearMath/btMinMax.h"
#include "LinearMath/btIDebugDraw.h" #include "LinearMath/btIDebugDraw.h"
#include "BulletDynamics/ConstraintSolver/btContactConstraint.h" #include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
#define ROLLING_INFLUENCE_FIX #define ROLLING_INFLUENCE_FIX
btRigidBody& btActionInterface::getFixedBody() btRigidBody& btActionInterface::getFixedBody()
{ {
static btRigidBody s_fixed(0, 0,0); static btRigidBody s_fixed(0, 0, 0);
s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); s_fixed.setMassProps(btScalar(0.), btVector3(btScalar(0.), btScalar(0.), btScalar(0.)));
return s_fixed; return s_fixed;
} }
btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis, btVehicleRaycaster* raycaster ) btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning, btRigidBody* chassis, btVehicleRaycaster* raycaster)
:m_vehicleRaycaster(raycaster), : m_vehicleRaycaster(raycaster),
m_pitchControl(btScalar(0.)) m_pitchControl(btScalar(0.))
{ {
m_chassisBody = chassis; m_chassisBody = chassis;
m_indexRightAxis = 0; m_indexRightAxis = 0;
m_indexUpAxis = 2; m_indexUpAxis = 2;
m_indexForwardAxis = 1; m_indexForwardAxis = 1;
defaultInit(tuning); defaultInit(tuning);
} }
void btRaycastVehicle::defaultInit(const btVehicleTuning& tuning) void btRaycastVehicle::defaultInit(const btVehicleTuning& tuning)
{ {
(void)tuning; (void)tuning;
m_currentVehicleSpeedKmHour = btScalar(0.); m_currentVehicleSpeedKmHour = btScalar(0.);
m_steeringValue = btScalar(0.); m_steeringValue = btScalar(0.);
} }
btRaycastVehicle::~btRaycastVehicle() btRaycastVehicle::~btRaycastVehicle()
{ {
} }
// //
// basically most of the code is general for 2 or 4 wheel vehicles, but some of it needs to be reviewed // 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) btWheelInfo& btRaycastVehicle::addWheel(const btVector3& connectionPointCS, const btVector3& wheelDirectionCS0, const btVector3& wheelAxleCS, btScalar suspensionRestLength, btScalar wheelRadius, const btVehicleTuning& tuning, bool isFrontWheel)
{ {
btWheelInfoConstructionInfo ci; btWheelInfoConstructionInfo ci;
ci.m_chassisConnectionCS = connectionPointCS; ci.m_chassisConnectionCS = connectionPointCS;
ci.m_wheelDirectionCS = wheelDirectionCS0; ci.m_wheelDirectionCS = wheelDirectionCS0;
ci.m_wheelAxleCS = wheelAxleCS; ci.m_wheelAxleCS = wheelAxleCS;
ci.m_suspensionRestLength = suspensionRestLength; ci.m_suspensionRestLength = suspensionRestLength;
ci.m_wheelRadius = wheelRadius; ci.m_wheelRadius = wheelRadius;
ci.m_suspensionStiffness = tuning.m_suspensionStiffness; ci.m_suspensionStiffness = tuning.m_suspensionStiffness;
ci.m_wheelsDampingCompression = tuning.m_suspensionCompression; ci.m_wheelsDampingCompression = tuning.m_suspensionCompression;
ci.m_wheelsDampingRelaxation = tuning.m_suspensionDamping; ci.m_wheelsDampingRelaxation = tuning.m_suspensionDamping;
ci.m_frictionSlip = tuning.m_frictionSlip; ci.m_frictionSlip = tuning.m_frictionSlip;
ci.m_bIsFrontWheel = isFrontWheel; ci.m_bIsFrontWheel = isFrontWheel;
ci.m_maxSuspensionTravelCm = tuning.m_maxSuspensionTravelCm; ci.m_maxSuspensionTravelCm = tuning.m_maxSuspensionTravelCm;
ci.m_maxSuspensionForce = tuning.m_maxSuspensionForce; ci.m_maxSuspensionForce = tuning.m_maxSuspensionForce;
m_wheelInfo.push_back( btWheelInfo(ci)); m_wheelInfo.push_back(btWheelInfo(ci));
btWheelInfo& wheel = m_wheelInfo[getNumWheels()-1]; btWheelInfo& wheel = m_wheelInfo[getNumWheels() - 1];
updateWheelTransformsWS( wheel , false ); updateWheelTransformsWS(wheel, false);
updateWheelTransform(getNumWheels()-1,false); updateWheelTransform(getNumWheels() - 1, false);
return wheel; return wheel;
} }
const btTransform& btRaycastVehicle::getWheelTransformWS( int wheelIndex ) const const btTransform& btRaycastVehicle::getWheelTransformWS(int wheelIndex) const
{ {
btAssert(wheelIndex < getNumWheels()); btAssert(wheelIndex < getNumWheels());
const btWheelInfo& wheel = m_wheelInfo[wheelIndex]; const btWheelInfo& wheel = m_wheelInfo[wheelIndex];
return wheel.m_worldTransform; return wheel.m_worldTransform;
} }
void btRaycastVehicle::updateWheelTransform( int wheelIndex , bool interpolatedTransform) void btRaycastVehicle::updateWheelTransform(int wheelIndex, bool interpolatedTransform)
{ {
btWheelInfo& wheel = m_wheelInfo[ wheelIndex ]; btWheelInfo& wheel = m_wheelInfo[wheelIndex];
updateWheelTransformsWS(wheel,interpolatedTransform); updateWheelTransformsWS(wheel, interpolatedTransform);
btVector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS; btVector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS;
const btVector3& right = wheel.m_raycastInfo.m_wheelAxleWS; const btVector3& right = wheel.m_raycastInfo.m_wheelAxleWS;
btVector3 fwd = up.cross(right); btVector3 fwd = up.cross(right);
fwd = fwd.normalize(); fwd = fwd.normalize();
// up = right.cross(fwd); // up = right.cross(fwd);
// up.normalize(); // up.normalize();
//rotate around steering over de wheelAxleWS //rotate around steering over de wheelAxleWS
btScalar steering = wheel.m_steering; btScalar steering = wheel.m_steering;
btQuaternion steeringOrn(up,steering);//wheel.m_steering); btQuaternion steeringOrn(up, steering); //wheel.m_steering);
btMatrix3x3 steeringMat(steeringOrn); btMatrix3x3 steeringMat(steeringOrn);
btQuaternion rotatingOrn(right,-wheel.m_rotation); btQuaternion rotatingOrn(right, -wheel.m_rotation);
btMatrix3x3 rotatingMat(rotatingOrn); btMatrix3x3 rotatingMat(rotatingOrn);
btMatrix3x3 basis2( btMatrix3x3 basis2;
right[0],fwd[0],up[0], basis2[0][m_indexRightAxis] = -right[0];
right[1],fwd[1],up[1], basis2[1][m_indexRightAxis] = -right[1];
right[2],fwd[2],up[2] basis2[2][m_indexRightAxis] = -right[2];
);
basis2[0][m_indexUpAxis] = up[0];
basis2[1][m_indexUpAxis] = up[1];
basis2[2][m_indexUpAxis] = up[2];
basis2[0][m_indexForwardAxis] = fwd[0];
basis2[1][m_indexForwardAxis] = fwd[1];
basis2[2][m_indexForwardAxis] = fwd[2];
wheel.m_worldTransform.setBasis(steeringMat * rotatingMat * basis2); wheel.m_worldTransform.setBasis(steeringMat * rotatingMat * basis2);
wheel.m_worldTransform.setOrigin( wheel.m_worldTransform.setOrigin(
wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength);
);
} }
void btRaycastVehicle::resetSuspension() void btRaycastVehicle::resetSuspension()
{ {
int i; int i;
for (i=0;i<m_wheelInfo.size(); i++) for (i = 0; i < m_wheelInfo.size(); i++)
{ {
btWheelInfo& wheel = m_wheelInfo[i]; btWheelInfo& wheel = m_wheelInfo[i];
wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength(); wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength();
wheel.m_suspensionRelativeVelocity = btScalar(0.0); wheel.m_suspensionRelativeVelocity = btScalar(0.0);
wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS; wheel.m_raycastInfo.m_contactNormalWS = -wheel.m_raycastInfo.m_wheelDirectionWS;
//wheel_info.setContactFriction(btScalar(0.0)); //wheel_info.setContactFriction(btScalar(0.0));
wheel.m_clippedInvContactDotSuspension = btScalar(1.0); wheel.m_clippedInvContactDotSuspension = btScalar(1.0);
} }
} }
void btRaycastVehicle::updateWheelTransformsWS(btWheelInfo& wheel , bool interpolatedTransform) void btRaycastVehicle::updateWheelTransformsWS(btWheelInfo& wheel, bool interpolatedTransform)
{ {
wheel.m_raycastInfo.m_isInContact = false; wheel.m_raycastInfo.m_isInContact = false;
btTransform chassisTrans = getChassisWorldTransform(); btTransform chassisTrans = getChassisWorldTransform();
if (interpolatedTransform && (getRigidBody()->getMotionState())) if (interpolatedTransform && (getRigidBody()->getMotionState()))
{ {
getRigidBody()->getMotionState()->getWorldTransform(chassisTrans); getRigidBody()->getMotionState()->getWorldTransform(chassisTrans);
} }
wheel.m_raycastInfo.m_hardPointWS = chassisTrans( wheel.m_chassisConnectionPointCS ); wheel.m_raycastInfo.m_hardPointWS = chassisTrans(wheel.m_chassisConnectionPointCS);
wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans.getBasis() * wheel.m_wheelDirectionCS ; wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans.getBasis() * wheel.m_wheelDirectionCS;
wheel.m_raycastInfo.m_wheelAxleWS = chassisTrans.getBasis() * wheel.m_wheelAxleCS; wheel.m_raycastInfo.m_wheelAxleWS = chassisTrans.getBasis() * wheel.m_wheelAxleCS;
} }
btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel) btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
{ {
updateWheelTransformsWS( wheel,false); updateWheelTransformsWS(wheel, false);
btScalar depth = -1; btScalar depth = -1;
btScalar raylen = wheel.getSuspensionRestLength()+wheel.m_wheelsRadius; btScalar raylen = wheel.getSuspensionRestLength() + wheel.m_wheelsRadius;
btVector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen); btVector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen);
const btVector3& source = wheel.m_raycastInfo.m_hardPointWS; const btVector3& source = wheel.m_raycastInfo.m_hardPointWS;
wheel.m_raycastInfo.m_contactPointWS = source + rayvector; wheel.m_raycastInfo.m_contactPointWS = source + rayvector;
const btVector3& target = wheel.m_raycastInfo.m_contactPointWS; const btVector3& target = wheel.m_raycastInfo.m_contactPointWS;
btScalar param = btScalar(0.); btScalar param = btScalar(0.);
btVehicleRaycaster::btVehicleRaycasterResult rayResults; btVehicleRaycaster::btVehicleRaycasterResult rayResults;
btAssert(m_vehicleRaycaster); btAssert(m_vehicleRaycaster);
void* object = m_vehicleRaycaster->castRay(source,target,rayResults); void* object = m_vehicleRaycaster->castRay(source, target, rayResults);
wheel.m_raycastInfo.m_groundObject = 0; wheel.m_raycastInfo.m_groundObject = 0;
if (object) if (object)
{ {
param = rayResults.m_distFraction; param = rayResults.m_distFraction;
depth = raylen * rayResults.m_distFraction; depth = raylen * rayResults.m_distFraction;
wheel.m_raycastInfo.m_contactNormalWS = rayResults.m_hitNormalInWorld; wheel.m_raycastInfo.m_contactNormalWS = rayResults.m_hitNormalInWorld;
wheel.m_raycastInfo.m_isInContact = true; wheel.m_raycastInfo.m_isInContact = true;
wheel.m_raycastInfo.m_groundObject = &getFixedBody();///@todo for driving on dynamic/movable objects!; wheel.m_raycastInfo.m_groundObject = &getFixedBody(); ///@todo for driving on dynamic/movable objects!;
//wheel.m_raycastInfo.m_groundObject = object; //wheel.m_raycastInfo.m_groundObject = object;
btScalar hitDistance = param*raylen; btScalar hitDistance = param * raylen;
wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelsRadius; wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelsRadius;
//clamp on max suspension travel //clamp on max suspension travel
btScalar minSuspensionLength = wheel.getSuspensionRestLength() - wheel.m_maxSuspensionTravelCm*btScalar(0.01); btScalar minSuspensionLength = wheel.getSuspensionRestLength() - wheel.m_maxSuspensionTravelCm * btScalar(0.01);
btScalar maxSuspensionLength = 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) if (wheel.m_raycastInfo.m_suspensionLength < minSuspensionLength)
{ {
wheel.m_raycastInfo.m_suspensionLength = minSuspensionLength; wheel.m_raycastInfo.m_suspensionLength = minSuspensionLength;
} }
if (wheel.m_raycastInfo.m_suspensionLength > maxSuspensionLength) if (wheel.m_raycastInfo.m_suspensionLength > maxSuspensionLength)
{ {
wheel.m_raycastInfo.m_suspensionLength = maxSuspensionLength; wheel.m_raycastInfo.m_suspensionLength = maxSuspensionLength;
} }
wheel.m_raycastInfo.m_contactPointWS = rayResults.m_hitPointInWorld; wheel.m_raycastInfo.m_contactPointWS = rayResults.m_hitPointInWorld;
btScalar denominator= wheel.m_raycastInfo.m_contactNormalWS.dot( wheel.m_raycastInfo.m_wheelDirectionWS ); btScalar denominator = wheel.m_raycastInfo.m_contactNormalWS.dot(wheel.m_raycastInfo.m_wheelDirectionWS);
btVector3 chassis_velocity_at_contactPoint; btVector3 chassis_velocity_at_contactPoint;
btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS-getRigidBody()->getCenterOfMassPosition(); btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - getRigidBody()->getCenterOfMassPosition();
chassis_velocity_at_contactPoint = getRigidBody()->getVelocityInLocalPoint(relpos); chassis_velocity_at_contactPoint = getRigidBody()->getVelocityInLocalPoint(relpos);
btScalar projVel = wheel.m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint ); btScalar projVel = wheel.m_raycastInfo.m_contactNormalWS.dot(chassis_velocity_at_contactPoint);
if ( denominator >= btScalar(-0.1)) if (denominator >= btScalar(-0.1))
{ {
wheel.m_suspensionRelativeVelocity = btScalar(0.0); wheel.m_suspensionRelativeVelocity = btScalar(0.0);
wheel.m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1); wheel.m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1);
} }
else else
{ {
btScalar inv = btScalar(-1.) / denominator; btScalar inv = btScalar(-1.) / denominator;
wheel.m_suspensionRelativeVelocity = projVel * inv; wheel.m_suspensionRelativeVelocity = projVel * inv;
wheel.m_clippedInvContactDotSuspension = inv; wheel.m_clippedInvContactDotSuspension = inv;
} }
}
} else else
{ {
//put wheel info as in rest position //put wheel info as in rest position
wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength(); wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength();
wheel.m_suspensionRelativeVelocity = btScalar(0.0); wheel.m_suspensionRelativeVelocity = btScalar(0.0);
wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS; wheel.m_raycastInfo.m_contactNormalWS = -wheel.m_raycastInfo.m_wheelDirectionWS;
wheel.m_clippedInvContactDotSuspension = btScalar(1.0); wheel.m_clippedInvContactDotSuspension = btScalar(1.0);
} }
return depth; return depth;
} }
const btTransform& btRaycastVehicle::getChassisWorldTransform() const const btTransform& btRaycastVehicle::getChassisWorldTransform() const
{ {
/*if (getRigidBody()->getMotionState()) /*if (getRigidBody()->getMotionState())
{ {
btTransform chassisWorldTrans; btTransform chassisWorldTrans;
getRigidBody()->getMotionState()->getWorldTransform(chassisWorldTrans); getRigidBody()->getMotionState()->getWorldTransform(chassisWorldTrans);
return chassisWorldTrans; return chassisWorldTrans;
} }
*/ */
return getRigidBody()->getCenterOfMassTransform(); return getRigidBody()->getCenterOfMassTransform();
} }
void btRaycastVehicle::updateVehicle( btScalar step ) void btRaycastVehicle::updateVehicle(btScalar step)
{ {
{ {
for (int i=0;i<getNumWheels();i++) for (int i = 0; i < getNumWheels(); i++)
{ {
updateWheelTransform(i,false); updateWheelTransform(i, false);
} }
} }
m_currentVehicleSpeedKmHour = btScalar(3.6) * getRigidBody()->getLinearVelocity().length(); m_currentVehicleSpeedKmHour = btScalar(3.6) * getRigidBody()->getLinearVelocity().length();
const btTransform& chassisTrans = getChassisWorldTransform(); const btTransform& chassisTrans = getChassisWorldTransform();
btVector3 forwardW ( btVector3 forwardW(
chassisTrans.getBasis()[0][m_indexForwardAxis], chassisTrans.getBasis()[0][m_indexForwardAxis],
chassisTrans.getBasis()[1][m_indexForwardAxis], chassisTrans.getBasis()[1][m_indexForwardAxis],
chassisTrans.getBasis()[2][m_indexForwardAxis]); chassisTrans.getBasis()[2][m_indexForwardAxis]);
if (forwardW.dot(getRigidBody()->getLinearVelocity()) < btScalar(0.)) if (forwardW.dot(getRigidBody()->getLinearVelocity()) < btScalar(0.))
{ {
m_currentVehicleSpeedKmHour *= btScalar(-1.); m_currentVehicleSpeedKmHour *= btScalar(-1.);
} }
// //
// simulate suspension // simulate suspension
// //
int i=0; int i = 0;
for (i=0;i<m_wheelInfo.size();i++) for (i = 0; i < m_wheelInfo.size(); i++)
{ {
//btScalar depth; //btScalar depth;
//depth = //depth =
rayCast( m_wheelInfo[i]); rayCast(m_wheelInfo[i]);
} }
updateSuspension(step); updateSuspension(step);
for (i=0;i<m_wheelInfo.size();i++) for (i = 0; i < m_wheelInfo.size(); i++)
{ {
//apply suspension force //apply suspension force
btWheelInfo& wheel = m_wheelInfo[i]; btWheelInfo& wheel = m_wheelInfo[i];
btScalar suspensionForce = wheel.m_wheelsSuspensionForce; btScalar suspensionForce = wheel.m_wheelsSuspensionForce;
if (suspensionForce > wheel.m_maxSuspensionForce) if (suspensionForce > wheel.m_maxSuspensionForce)
{ {
suspensionForce = wheel.m_maxSuspensionForce; suspensionForce = wheel.m_maxSuspensionForce;
} }
btVector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step; btVector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step;
btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - getRigidBody()->getCenterOfMassPosition(); btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - getRigidBody()->getCenterOfMassPosition();
getRigidBody()->applyImpulse(impulse, relpos); getRigidBody()->applyImpulse(impulse, relpos);
} }
updateFriction( step); updateFriction(step);
for (i=0;i<m_wheelInfo.size();i++) for (i = 0; i < m_wheelInfo.size(); i++)
{ {
btWheelInfo& wheel = m_wheelInfo[i]; btWheelInfo& wheel = m_wheelInfo[i];
btVector3 relpos = wheel.m_raycastInfo.m_hardPointWS - getRigidBody()->getCenterOfMassPosition(); btVector3 relpos = wheel.m_raycastInfo.m_hardPointWS - getRigidBody()->getCenterOfMassPosition();
btVector3 vel = getRigidBody()->getVelocityInLocalPoint( relpos ); btVector3 vel = getRigidBody()->getVelocityInLocalPoint(relpos);
if (wheel.m_raycastInfo.m_isInContact) if (wheel.m_raycastInfo.m_isInContact)
{ {
const btTransform& chassisWorldTransform = getChassisWorldTransform(); const btTransform& chassisWorldTransform = getChassisWorldTransform();
btVector3 fwd ( btVector3 fwd(
chassisWorldTransform.getBasis()[0][m_indexForwardAxis], chassisWorldTransform.getBasis()[0][m_indexForwardAxis],
chassisWorldTransform.getBasis()[1][m_indexForwardAxis], chassisWorldTransform.getBasis()[1][m_indexForwardAxis],
chassisWorldTransform.getBasis()[2][m_indexForwardAxis]); chassisWorldTransform.getBasis()[2][m_indexForwardAxis]);
btScalar proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS); btScalar proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS);
fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj; fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj;
btScalar proj2 = fwd.dot(vel); btScalar proj2 = fwd.dot(vel);
wheel.m_deltaRotation = (proj2 * step) / (wheel.m_wheelsRadius); wheel.m_deltaRotation = (proj2 * step) / (wheel.m_wheelsRadius);
wheel.m_rotation += wheel.m_deltaRotation; wheel.m_rotation += wheel.m_deltaRotation;
}
} else else
{ {
wheel.m_rotation += wheel.m_deltaRotation; wheel.m_rotation += wheel.m_deltaRotation;
} }
wheel.m_deltaRotation *= btScalar(0.99);//damping of rotation when not in contact wheel.m_deltaRotation *= btScalar(0.99); //damping of rotation when not in contact
} }
} }
void btRaycastVehicle::setSteeringValue(btScalar steering,int wheel) void btRaycastVehicle::setSteeringValue(btScalar steering, int wheel)
{ {
btAssert(wheel>=0 && wheel < getNumWheels()); btAssert(wheel >= 0 && wheel < getNumWheels());
btWheelInfo& wheelInfo = getWheelInfo(wheel); btWheelInfo& wheelInfo = getWheelInfo(wheel);
wheelInfo.m_steering = steering; wheelInfo.m_steering = steering;
} }
btScalar btRaycastVehicle::getSteeringValue(int wheel) const btScalar btRaycastVehicle::getSteeringValue(int wheel) const
{ {
return getWheelInfo(wheel).m_steering; return getWheelInfo(wheel).m_steering;
} }
void btRaycastVehicle::applyEngineForce(btScalar force, int wheel) void btRaycastVehicle::applyEngineForce(btScalar force, int wheel)
{ {
btAssert(wheel>=0 && wheel < getNumWheels()); btAssert(wheel >= 0 && wheel < getNumWheels());
btWheelInfo& wheelInfo = getWheelInfo(wheel); btWheelInfo& wheelInfo = getWheelInfo(wheel);
wheelInfo.m_engineForce = force; wheelInfo.m_engineForce = force;
} }
const btWheelInfo& btRaycastVehicle::getWheelInfo(int index) const const btWheelInfo& btRaycastVehicle::getWheelInfo(int index) const
{ {
btAssert((index >= 0) && (index < getNumWheels())); btAssert((index >= 0) && (index < getNumWheels()));
return m_wheelInfo[index]; return m_wheelInfo[index];
} }
btWheelInfo& btRaycastVehicle::getWheelInfo(int index) btWheelInfo& btRaycastVehicle::getWheelInfo(int index)
{ {
btAssert((index >= 0) && (index < getNumWheels())); btAssert((index >= 0) && (index < getNumWheels()));
return m_wheelInfo[index]; return m_wheelInfo[index];
} }
void btRaycastVehicle::setBrake(btScalar brake,int wheelIndex) void btRaycastVehicle::setBrake(btScalar brake, int wheelIndex)
{ {
btAssert((wheelIndex >= 0) && (wheelIndex < getNumWheels())); btAssert((wheelIndex >= 0) && (wheelIndex < getNumWheels()));
getWheelInfo(wheelIndex).m_brake = brake; getWheelInfo(wheelIndex).m_brake = brake;
} }
void btRaycastVehicle::updateSuspension(btScalar deltaTime) void btRaycastVehicle::updateSuspension(btScalar deltaTime)
{ {
(void)deltaTime; (void)deltaTime;
btScalar chassisMass = btScalar(1.) / m_chassisBody->getInvMass(); btScalar chassisMass = btScalar(1.) / m_chassisBody->getInvMass();
for (int w_it=0; w_it<getNumWheels(); w_it++) for (int w_it = 0; w_it < getNumWheels(); w_it++)
{ {
btWheelInfo &wheel_info = m_wheelInfo[w_it]; btWheelInfo& wheel_info = m_wheelInfo[w_it];
if ( wheel_info.m_raycastInfo.m_isInContact ) if (wheel_info.m_raycastInfo.m_isInContact)
{ {
btScalar force; btScalar force;
// Spring // Spring
{ {
btScalar susp_length = wheel_info.getSuspensionRestLength(); btScalar susp_length = wheel_info.getSuspensionRestLength();
btScalar current_length = wheel_info.m_raycastInfo.m_suspensionLength; btScalar current_length = wheel_info.m_raycastInfo.m_suspensionLength;
btScalar length_diff = (susp_length - current_length); btScalar length_diff = (susp_length - current_length);
force = wheel_info.m_suspensionStiffness force = wheel_info.m_suspensionStiffness * length_diff * wheel_info.m_clippedInvContactDotSuspension;
* length_diff * wheel_info.m_clippedInvContactDotSuspension;
} }
// Damper // Damper
{ {
btScalar projected_rel_vel = wheel_info.m_suspensionRelativeVelocity; btScalar projected_rel_vel = wheel_info.m_suspensionRelativeVelocity;
{ {
btScalar susp_damping; btScalar susp_damping;
if ( projected_rel_vel < btScalar(0.0) ) if (projected_rel_vel < btScalar(0.0))
{ {
susp_damping = wheel_info.m_wheelsDampingCompression; susp_damping = wheel_info.m_wheelsDampingCompression;
} }
else else
{ {
susp_damping = wheel_info.m_wheelsDampingRelaxation; susp_damping = wheel_info.m_wheelsDampingRelaxation;
} }
force -= susp_damping * projected_rel_vel; force -= susp_damping * projected_rel_vel;
} }
} }
// RESULT // RESULT
wheel_info.m_wheelsSuspensionForce = force * chassisMass; wheel_info.m_wheelsSuspensionForce = force * chassisMass;
if (wheel_info.m_wheelsSuspensionForce < btScalar(0.)) if (wheel_info.m_wheelsSuspensionForce < btScalar(0.))
{ {
wheel_info.m_wheelsSuspensionForce = btScalar(0.); wheel_info.m_wheelsSuspensionForce = btScalar(0.);
} }
} }
else else
{ {
wheel_info.m_wheelsSuspensionForce = btScalar(0.0); wheel_info.m_wheelsSuspensionForce = btScalar(0.0);
} }
} }
} }
struct btWheelContactPoint struct btWheelContactPoint
{ {
btRigidBody* m_body0; btRigidBody* m_body0;
btRigidBody* m_body1; btRigidBody* m_body1;
btVector3 m_frictionPositionWorld; btVector3 m_frictionPositionWorld;
btVector3 m_frictionDirectionWorld; btVector3 m_frictionDirectionWorld;
btScalar m_jacDiagABInv; btScalar m_jacDiagABInv;
btScalar m_maxImpulse; btScalar m_maxImpulse;
btWheelContactPoint(btRigidBody* body0,btRigidBody* body1,const btVector3& frictionPosWorld,const btVector3& frictionDirectionWorld, btScalar maxImpulse) btWheelContactPoint(btRigidBody* body0, btRigidBody* body1, const btVector3& frictionPosWorld, const btVector3& frictionDirectionWorld, btScalar maxImpulse)
:m_body0(body0), : m_body0(body0),
m_body1(body1), m_body1(body1),
m_frictionPositionWorld(frictionPosWorld), m_frictionPositionWorld(frictionPosWorld),
m_frictionDirectionWorld(frictionDirectionWorld), m_frictionDirectionWorld(frictionDirectionWorld),
m_maxImpulse(maxImpulse) m_maxImpulse(maxImpulse)
{ {
btScalar denom0 = body0->computeImpulseDenominator(frictionPosWorld,frictionDirectionWorld); btScalar denom0 = body0->computeImpulseDenominator(frictionPosWorld, frictionDirectionWorld);
btScalar denom1 = body1->computeImpulseDenominator(frictionPosWorld,frictionDirectionWorld); btScalar denom1 = body1->computeImpulseDenominator(frictionPosWorld, frictionDirectionWorld);
btScalar relaxation = 1.f; btScalar relaxation = 1.f;
m_jacDiagABInv = relaxation/(denom0+denom1); m_jacDiagABInv = relaxation / (denom0 + denom1);
} }
}; };
btScalar calcRollingFriction(btWheelContactPoint& contactPoint); btScalar calcRollingFriction(btWheelContactPoint& contactPoint, int numWheelsOnGround);
btScalar calcRollingFriction(btWheelContactPoint& contactPoint) btScalar calcRollingFriction(btWheelContactPoint& contactPoint, int numWheelsOnGround)
{ {
btScalar j1=0.f; btScalar j1 = 0.f;
const btVector3& contactPosWorld = contactPoint.m_frictionPositionWorld; const btVector3& contactPosWorld = contactPoint.m_frictionPositionWorld;
btVector3 rel_pos1 = contactPosWorld - contactPoint.m_body0->getCenterOfMassPosition(); btVector3 rel_pos1 = contactPosWorld - contactPoint.m_body0->getCenterOfMassPosition();
btVector3 rel_pos2 = contactPosWorld - contactPoint.m_body1->getCenterOfMassPosition(); btVector3 rel_pos2 = contactPosWorld - contactPoint.m_body1->getCenterOfMassPosition();
btScalar maxImpulse = contactPoint.m_maxImpulse; btScalar maxImpulse = contactPoint.m_maxImpulse;
btVector3 vel1 = contactPoint.m_body0->getVelocityInLocalPoint(rel_pos1); btVector3 vel1 = contactPoint.m_body0->getVelocityInLocalPoint(rel_pos1);
btVector3 vel2 = contactPoint.m_body1->getVelocityInLocalPoint(rel_pos2); btVector3 vel2 = contactPoint.m_body1->getVelocityInLocalPoint(rel_pos2);
btVector3 vel = vel1 - vel2; btVector3 vel = vel1 - vel2;
btScalar vrel = contactPoint.m_frictionDirectionWorld.dot(vel); btScalar vrel = contactPoint.m_frictionDirectionWorld.dot(vel);
// calculate j that moves us to zero relative velocity // calculate j that moves us to zero relative velocity
j1 = -vrel * contactPoint.m_jacDiagABInv; j1 = -vrel * contactPoint.m_jacDiagABInv / btScalar(numWheelsOnGround);
btSetMin(j1, maxImpulse); btSetMin(j1, maxImpulse);
btSetMax(j1, -maxImpulse); btSetMax(j1, -maxImpulse);
return j1; return j1;
} }
btScalar sideFrictionStiffness2 = btScalar(1.0); btScalar sideFrictionStiffness2 = btScalar(1.0);
void btRaycastVehicle::updateFriction(btScalar timeStep) void btRaycastVehicle::updateFriction(btScalar timeStep)
{ {
//calculate the impulse, so that the wheels don't move sidewards //calculate the impulse, so that the wheels don't move sidewards
int numWheel = getNumWheels(); int numWheel = getNumWheels();
if (!numWheel) if (!numWheel)
return; return;
m_forwardWS.resize(numWheel); m_forwardWS.resize(numWheel);
m_axle.resize(numWheel); m_axle.resize(numWheel);
m_forwardImpulse.resize(numWheel); m_forwardImpulse.resize(numWheel);
m_sideImpulse.resize(numWheel); m_sideImpulse.resize(numWheel);
int numWheelsOnGround = 0; int numWheelsOnGround = 0;
//collapse all those loops into one! //collapse all those loops into one!
for (int i=0;i<getNumWheels();i++) for (int i = 0; i < getNumWheels(); i++)
{ {
btWheelInfo& wheelInfo = m_wheelInfo[i]; btWheelInfo& wheelInfo = m_wheelInfo[i];
class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject; class btRigidBody* groundObject = (class btRigidBody*)wheelInfo.m_raycastInfo.m_groundObject;
if (groundObject) if (groundObject)
numWheelsOnGround++; numWheelsOnGround++;
m_sideImpulse[i] = btScalar(0.); m_sideImpulse[i] = btScalar(0.);
m_forwardImpulse[i] = btScalar(0.); m_forwardImpulse[i] = btScalar(0.);
} }
{ {
for (int i=0;i<getNumWheels();i++) for (int i = 0; i < getNumWheels(); i++)
{ {
btWheelInfo& wheelInfo = m_wheelInfo[i]; btWheelInfo& wheelInfo = m_wheelInfo[i];
class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject; class btRigidBody* groundObject = (class btRigidBody*)wheelInfo.m_raycastInfo.m_groundObject;
if (groundObject) if (groundObject)
{ {
const btTransform& wheelTrans = getWheelTransformWS( i ); const btTransform& wheelTrans = getWheelTransformWS(i);
btMatrix3x3 wheelBasis0 = wheelTrans.getBasis(); btMatrix3x3 wheelBasis0 = wheelTrans.getBasis();
m_axle[i] = btVector3( m_axle[i] = -btVector3(
wheelBasis0[0][m_indexRightAxis], wheelBasis0[0][m_indexRightAxis],
wheelBasis0[1][m_indexRightAxis], wheelBasis0[1][m_indexRightAxis],
wheelBasis0[2][m_indexRightAxis]); wheelBasis0[2][m_indexRightAxis]);
const btVector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS; const btVector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
btScalar proj = m_axle[i].dot(surfNormalWS); btScalar proj = m_axle[i].dot(surfNormalWS);
m_axle[i] -= surfNormalWS * proj; m_axle[i] -= surfNormalWS * proj;
m_axle[i] = m_axle[i].normalize(); m_axle[i] = m_axle[i].normalize();
m_forwardWS[i] = surfNormalWS.cross(m_axle[i]); m_forwardWS[i] = surfNormalWS.cross(m_axle[i]);
m_forwardWS[i].normalize(); m_forwardWS[i].normalize();
resolveSingleBilateral(*m_chassisBody, wheelInfo.m_raycastInfo.m_contactPointWS, resolveSingleBilateral(*m_chassisBody, wheelInfo.m_raycastInfo.m_contactPointWS,
*groundObject, wheelInfo.m_raycastInfo.m_contactPointWS, *groundObject, wheelInfo.m_raycastInfo.m_contactPointWS,
btScalar(0.), m_axle[i],m_sideImpulse[i],timeStep); btScalar(0.), m_axle[i], m_sideImpulse[i], timeStep);
m_sideImpulse[i] *= sideFrictionStiffness2; m_sideImpulse[i] *= sideFrictionStiffness2;
} }
} }
} }
btScalar sideFactor = btScalar(1.); btScalar sideFactor = btScalar(1.);
btScalar fwdFactor = 0.5; btScalar fwdFactor = 0.5;
bool sliding = false; bool sliding = false;
{ {
for (int wheel =0;wheel <getNumWheels();wheel++) for (int wheel = 0; wheel < getNumWheels(); wheel++)
{ {
btWheelInfo& wheelInfo = m_wheelInfo[wheel]; btWheelInfo& wheelInfo = m_wheelInfo[wheel];
class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject; class btRigidBody* groundObject = (class btRigidBody*)wheelInfo.m_raycastInfo.m_groundObject;
btScalar rollingFriction = 0.f; btScalar rollingFriction = 0.f;
if (groundObject) if (groundObject)
{ {
if (wheelInfo.m_engineForce != 0.f) if (wheelInfo.m_engineForce != 0.f)
{ {
rollingFriction = wheelInfo.m_engineForce* timeStep; rollingFriction = wheelInfo.m_engineForce * timeStep;
} else }
else
{ {
btScalar defaultRollingFrictionImpulse = 0.f; btScalar defaultRollingFrictionImpulse = 0.f;
btScalar maxImpulse = wheelInfo.m_brake ? wheelInfo.m_brake : defaultRollingFrictionImpulse; btScalar maxImpulse = wheelInfo.m_brake ? wheelInfo.m_brake : defaultRollingFrictionImpulse;
btWheelContactPoint contactPt(m_chassisBody,groundObject,wheelInfo.m_raycastInfo.m_contactPointWS,m_forwardWS[wheel],maxImpulse); btWheelContactPoint contactPt(m_chassisBody, groundObject, wheelInfo.m_raycastInfo.m_contactPointWS, m_forwardWS[wheel], maxImpulse);
rollingFriction = calcRollingFriction(contactPt); btAssert(numWheelsOnGround > 0);
rollingFriction = calcRollingFriction(contactPt, numWheelsOnGround);
} }
} }
//switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break) //switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break)
m_forwardImpulse[wheel] = btScalar(0.); m_forwardImpulse[wheel] = btScalar(0.);
m_wheelInfo[wheel].m_skidInfo= btScalar(1.); m_wheelInfo[wheel].m_skidInfo = btScalar(1.);
if (groundObject) if (groundObject)
{ {
m_wheelInfo[wheel].m_skidInfo= btScalar(1.); m_wheelInfo[wheel].m_skidInfo = btScalar(1.);
btScalar maximp = wheelInfo.m_wheelsSuspensionForce * timeStep * wheelInfo.m_frictionSlip; btScalar maximp = wheelInfo.m_wheelsSuspensionForce * timeStep * wheelInfo.m_frictionSlip;
btScalar maximpSide = maximp; btScalar maximpSide = maximp;
btScalar maximpSquared = maximp * maximpSide; btScalar maximpSquared = maximp * maximpSide;
m_forwardImpulse[wheel] = rollingFriction;//wheelInfo.m_engineForce* timeStep; m_forwardImpulse[wheel] = rollingFriction; //wheelInfo.m_engineForce* timeStep;
btScalar x = (m_forwardImpulse[wheel] ) * fwdFactor; btScalar x = (m_forwardImpulse[wheel]) * fwdFactor;
btScalar y = (m_sideImpulse[wheel] ) * sideFactor; btScalar y = (m_sideImpulse[wheel]) * sideFactor;
btScalar impulseSquared = (x*x + y*y); btScalar impulseSquared = (x * x + y * y);
if (impulseSquared > maximpSquared) if (impulseSquared > maximpSquared)
{ {
sliding = true; sliding = true;
btScalar factor = maximp / btSqrt(impulseSquared); btScalar factor = maximp / btSqrt(impulseSquared);
m_wheelInfo[wheel].m_skidInfo *= factor; m_wheelInfo[wheel].m_skidInfo *= factor;
} }
} }
} }
} }
if (sliding) if (sliding)
{ {
for (int wheel = 0;wheel < getNumWheels(); wheel++) for (int wheel = 0; wheel < getNumWheels(); wheel++)
{ {
if (m_sideImpulse[wheel] != btScalar(0.)) if (m_sideImpulse[wheel] != btScalar(0.))
{ {
if (m_wheelInfo[wheel].m_skidInfo< btScalar(1.)) if (m_wheelInfo[wheel].m_skidInfo < btScalar(1.))
{ {
m_forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo; m_forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo; m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
} }
} }
} }
} }
// apply the impulses // apply the impulses
{ {
for (int wheel = 0;wheel<getNumWheels() ; wheel++) for (int wheel = 0; wheel < getNumWheels(); wheel++)
{ {
btWheelInfo& wheelInfo = m_wheelInfo[wheel]; btWheelInfo& wheelInfo = m_wheelInfo[wheel];
btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS - btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS -
m_chassisBody->getCenterOfMassPosition(); m_chassisBody->getCenterOfMassPosition();
if (m_forwardImpulse[wheel] != btScalar(0.)) if (m_forwardImpulse[wheel] != btScalar(0.))
{ {
m_chassisBody->applyImpulse(m_forwardWS[wheel]*(m_forwardImpulse[wheel]),rel_pos); m_chassisBody->applyImpulse(m_forwardWS[wheel] * (m_forwardImpulse[wheel]), rel_pos);
} }
if (m_sideImpulse[wheel] != btScalar(0.)) if (m_sideImpulse[wheel] != btScalar(0.))
{ {
class btRigidBody* groundObject = (class btRigidBody*) m_wheelInfo[wheel].m_raycastInfo.m_groundObject; class btRigidBody* groundObject = (class btRigidBody*)m_wheelInfo[wheel].m_raycastInfo.m_groundObject;
btVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS - btVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS -
groundObject->getCenterOfMassPosition(); groundObject->getCenterOfMassPosition();
btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel]; 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. #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); btVector3 vChassisWorldUp = getRigidBody()->getCenterOfMassTransform().getBasis().getColumn(m_indexUpAxis);
rel_pos -= vChassisWorldUp * (vChassisWorldUp.dot(rel_pos) * (1.f-wheelInfo.m_rollInfluence)); rel_pos -= vChassisWorldUp * (vChassisWorldUp.dot(rel_pos) * (1.f - wheelInfo.m_rollInfluence));
#else #else
rel_pos[m_indexUpAxis] *= wheelInfo.m_rollInfluence; rel_pos[m_indexUpAxis] *= wheelInfo.m_rollInfluence;
#endif #endif
m_chassisBody->applyImpulse(sideImp,rel_pos); m_chassisBody->applyImpulse(sideImp, rel_pos);
//apply friction impulse on the ground //apply friction impulse on the ground
groundObject->applyImpulse(-sideImp,rel_pos2); groundObject->applyImpulse(-sideImp, rel_pos2);
} }
} }
} }
} }
void btRaycastVehicle::debugDraw(btIDebugDraw* debugDrawer) void btRaycastVehicle::debugDraw(btIDebugDraw* debugDrawer)
{ {
for (int v=0;v<this->getNumWheels();v++) for (int v = 0; v < this->getNumWheels(); v++)
{ {
btVector3 wheelColor(0,1,1); btVector3 wheelColor(0, 1, 1);
if (getWheelInfo(v).m_raycastInfo.m_isInContact) if (getWheelInfo(v).m_raycastInfo.m_isInContact)
{ {
wheelColor.setValue(0,0,1); wheelColor.setValue(0, 0, 1);
} else }
else
{ {
wheelColor.setValue(1,0,1); wheelColor.setValue(1, 0, 1);
} }
btVector3 wheelPosWS = getWheelInfo(v).m_worldTransform.getOrigin(); btVector3 wheelPosWS = getWheelInfo(v).m_worldTransform.getOrigin();
btVector3 axle = btVector3( btVector3 axle = btVector3(
getWheelInfo(v).m_worldTransform.getBasis()[0][getRightAxis()], getWheelInfo(v).m_worldTransform.getBasis()[0][getRightAxis()],
getWheelInfo(v).m_worldTransform.getBasis()[1][getRightAxis()], getWheelInfo(v).m_worldTransform.getBasis()[1][getRightAxis()],
getWheelInfo(v).m_worldTransform.getBasis()[2][getRightAxis()]); getWheelInfo(v).m_worldTransform.getBasis()[2][getRightAxis()]);
//debug wheels (cylinders) //debug wheels (cylinders)
debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor); debugDrawer->drawLine(wheelPosWS, wheelPosWS + axle, wheelColor);
debugDrawer->drawLine(wheelPosWS,getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor); debugDrawer->drawLine(wheelPosWS, getWheelInfo(v).m_raycastInfo.m_contactPointWS, wheelColor);
} }
} }
void* btDefaultVehicleRaycaster::castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result) void* btDefaultVehicleRaycaster::castRay(const btVector3& from, const btVector3& to, btVehicleRaycasterResult& result)
{ {
// RayResultCallback& resultCallback; // RayResultCallback& resultCallback;
btCollisionWorld::ClosestRayResultCallback rayCallback(from,to); btCollisionWorld::ClosestRayResultCallback rayCallback(from, to);
m_dynamicsWorld->rayTest(from, to, rayCallback); m_dynamicsWorld->rayTest(from, to, rayCallback);
if (rayCallback.hasHit()) if (rayCallback.hasHit())
{ {
const btRigidBody* body = btRigidBody::upcast(rayCallback.m_collisionObject); const btRigidBody* body = btRigidBody::upcast(rayCallback.m_collisionObject);
if (body && body->hasContactResponse()) if (body && body->hasContactResponse())
{ {
result.m_hitPointInWorld = rayCallback.m_hitPointWorld; result.m_hitPointInWorld = rayCallback.m_hitPointWorld;
result.m_hitNormalInWorld = rayCallback.m_hitNormalWorld; result.m_hitNormalInWorld = rayCallback.m_hitNormalWorld;
result.m_hitNormalInWorld.normalize(); result.m_hitNormalInWorld.normalize();
result.m_distFraction = rayCallback.m_closestHitFraction; result.m_distFraction = rayCallback.m_closestHitFraction;
return (void*)body; return (void*)body;
} }
} }
return 0; return 0;
} }