Code-style consistency improvement:
Apply clang-format-all.sh using the _clang-format file through all the cpp/.h files. make sure not to apply it to certain serialization structures, since some parser expects the * as part of the name, instead of type. This commit contains no other changes aside from adding and applying clang-format-all.sh
This commit is contained in:
erwincoumans
@@ -24,17 +24,16 @@
|
||||
|
||||
#define ROLLING_INFLUENCE_FIX
|
||||
|
||||
|
||||
btRigidBody& btActionInterface::getFixedBody()
|
||||
{
|
||||
static btRigidBody s_fixed(0, 0,0);
|
||||
s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
|
||||
static btRigidBody s_fixed(0, 0, 0);
|
||||
s_fixed.setMassProps(btScalar(0.), btVector3(btScalar(0.), btScalar(0.), btScalar(0.)));
|
||||
return s_fixed;
|
||||
}
|
||||
|
||||
btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis, btVehicleRaycaster* raycaster )
|
||||
:m_vehicleRaycaster(raycaster),
|
||||
m_pitchControl(btScalar(0.))
|
||||
btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning, btRigidBody* chassis, btVehicleRaycaster* raycaster)
|
||||
: m_vehicleRaycaster(raycaster),
|
||||
m_pitchControl(btScalar(0.))
|
||||
{
|
||||
m_chassisBody = chassis;
|
||||
m_indexRightAxis = 0;
|
||||
@@ -43,28 +42,22 @@ m_pitchControl(btScalar(0.))
|
||||
defaultInit(tuning);
|
||||
}
|
||||
|
||||
|
||||
void btRaycastVehicle::defaultInit(const btVehicleTuning& tuning)
|
||||
{
|
||||
(void)tuning;
|
||||
m_currentVehicleSpeedKmHour = btScalar(0.);
|
||||
m_steeringValue = btScalar(0.);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
btRaycastVehicle::~btRaycastVehicle()
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
//
|
||||
// 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;
|
||||
|
||||
ci.m_chassisConnectionCS = connectionPointCS;
|
||||
@@ -80,83 +73,76 @@ btWheelInfo& btRaycastVehicle::addWheel( const btVector3& connectionPointCS, con
|
||||
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);
|
||||
m_wheelInfo.push_back(btWheelInfo(ci));
|
||||
|
||||
btWheelInfo& wheel = m_wheelInfo[getNumWheels() - 1];
|
||||
|
||||
updateWheelTransformsWS(wheel, false);
|
||||
updateWheelTransform(getNumWheels() - 1, false);
|
||||
return wheel;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
const btTransform& btRaycastVehicle::getWheelTransformWS( int wheelIndex ) const
|
||||
const btTransform& btRaycastVehicle::getWheelTransformWS(int wheelIndex) const
|
||||
{
|
||||
btAssert(wheelIndex < getNumWheels());
|
||||
const btWheelInfo& wheel = m_wheelInfo[wheelIndex];
|
||||
return wheel.m_worldTransform;
|
||||
|
||||
}
|
||||
|
||||
void btRaycastVehicle::updateWheelTransform( int wheelIndex , bool interpolatedTransform)
|
||||
void btRaycastVehicle::updateWheelTransform(int wheelIndex, bool interpolatedTransform)
|
||||
{
|
||||
|
||||
btWheelInfo& wheel = m_wheelInfo[ wheelIndex ];
|
||||
updateWheelTransformsWS(wheel,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();
|
||||
// up = right.cross(fwd);
|
||||
// up.normalize();
|
||||
|
||||
//rotate around steering over de wheelAxleWS
|
||||
btScalar steering = wheel.m_steering;
|
||||
|
||||
btQuaternion steeringOrn(up,steering);//wheel.m_steering);
|
||||
|
||||
btQuaternion steeringOrn(up, steering); //wheel.m_steering);
|
||||
btMatrix3x3 steeringMat(steeringOrn);
|
||||
|
||||
btQuaternion rotatingOrn(right,-wheel.m_rotation);
|
||||
btQuaternion rotatingOrn(right, -wheel.m_rotation);
|
||||
btMatrix3x3 rotatingMat(rotatingOrn);
|
||||
|
||||
btMatrix3x3 basis2;
|
||||
basis2[0][m_indexRightAxis] = -right[0];
|
||||
basis2[1][m_indexRightAxis] = -right[1];
|
||||
basis2[2][m_indexRightAxis] = -right[2];
|
||||
btMatrix3x3 basis2;
|
||||
basis2[0][m_indexRightAxis] = -right[0];
|
||||
basis2[1][m_indexRightAxis] = -right[1];
|
||||
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_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];
|
||||
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.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()
|
||||
{
|
||||
|
||||
int i;
|
||||
for (i=0;i<m_wheelInfo.size(); i++)
|
||||
for (i = 0; i < m_wheelInfo.size(); i++)
|
||||
{
|
||||
btWheelInfo& wheel = m_wheelInfo[i];
|
||||
wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength();
|
||||
wheel.m_suspensionRelativeVelocity = btScalar(0.0);
|
||||
|
||||
wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS;
|
||||
//wheel_info.setContactFriction(btScalar(0.0));
|
||||
wheel.m_clippedInvContactDotSuspension = btScalar(1.0);
|
||||
btWheelInfo& wheel = m_wheelInfo[i];
|
||||
wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength();
|
||||
wheel.m_suspensionRelativeVelocity = btScalar(0.0);
|
||||
|
||||
wheel.m_raycastInfo.m_contactNormalWS = -wheel.m_raycastInfo.m_wheelDirectionWS;
|
||||
//wheel_info.setContactFriction(btScalar(0.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;
|
||||
|
||||
@@ -166,19 +152,18 @@ void btRaycastVehicle::updateWheelTransformsWS(btWheelInfo& wheel , bool interpo
|
||||
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_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 btRaycastVehicle::rayCast(btWheelInfo& wheel)
|
||||
{
|
||||
updateWheelTransformsWS( wheel,false);
|
||||
updateWheelTransformsWS(wheel, false);
|
||||
|
||||
|
||||
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);
|
||||
const btVector3& source = wheel.m_raycastInfo.m_hardPointWS;
|
||||
@@ -186,12 +171,12 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
|
||||
const btVector3& target = wheel.m_raycastInfo.m_contactPointWS;
|
||||
|
||||
btScalar param = btScalar(0.);
|
||||
|
||||
btVehicleRaycaster::btVehicleRaycasterResult rayResults;
|
||||
|
||||
btVehicleRaycaster::btVehicleRaycasterResult rayResults;
|
||||
|
||||
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;
|
||||
|
||||
@@ -199,19 +184,18 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
|
||||
{
|
||||
param = 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_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;
|
||||
|
||||
|
||||
btScalar hitDistance = param*raylen;
|
||||
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);
|
||||
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;
|
||||
@@ -223,16 +207,16 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
|
||||
|
||||
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 relpos = wheel.m_raycastInfo.m_contactPointWS-getRigidBody()->getCenterOfMassPosition();
|
||||
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 );
|
||||
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_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1);
|
||||
@@ -243,20 +227,19 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
|
||||
wheel.m_suspensionRelativeVelocity = projVel * inv;
|
||||
wheel.m_clippedInvContactDotSuspension = inv;
|
||||
}
|
||||
|
||||
} else
|
||||
}
|
||||
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_raycastInfo.m_contactNormalWS = -wheel.m_raycastInfo.m_wheelDirectionWS;
|
||||
wheel.m_clippedInvContactDotSuspension = btScalar(1.0);
|
||||
}
|
||||
|
||||
return depth;
|
||||
}
|
||||
|
||||
|
||||
const btTransform& btRaycastVehicle::getChassisWorldTransform() const
|
||||
{
|
||||
/*if (getRigidBody()->getMotionState())
|
||||
@@ -267,26 +250,23 @@ const btTransform& btRaycastVehicle::getChassisWorldTransform() const
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
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();
|
||||
|
||||
|
||||
const btTransform& chassisTrans = getChassisWorldTransform();
|
||||
|
||||
btVector3 forwardW (
|
||||
btVector3 forwardW(
|
||||
chassisTrans.getBasis()[0][m_indexForwardAxis],
|
||||
chassisTrans.getBasis()[1][m_indexForwardAxis],
|
||||
chassisTrans.getBasis()[2][m_indexForwardAxis]);
|
||||
@@ -299,52 +279,47 @@ void btRaycastVehicle::updateVehicle( btScalar step )
|
||||
//
|
||||
// simulate suspension
|
||||
//
|
||||
|
||||
int i=0;
|
||||
for (i=0;i<m_wheelInfo.size();i++)
|
||||
|
||||
int i = 0;
|
||||
for (i = 0; i < m_wheelInfo.size(); i++)
|
||||
{
|
||||
//btScalar depth;
|
||||
//depth =
|
||||
rayCast( m_wheelInfo[i]);
|
||||
//btScalar depth;
|
||||
//depth =
|
||||
rayCast(m_wheelInfo[i]);
|
||||
}
|
||||
|
||||
updateSuspension(step);
|
||||
|
||||
|
||||
for (i=0;i<m_wheelInfo.size();i++)
|
||||
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);
|
||||
updateFriction(step);
|
||||
|
||||
|
||||
for (i=0;i<m_wheelInfo.size();i++)
|
||||
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 );
|
||||
btVector3 vel = getRigidBody()->getVelocityInLocalPoint(relpos);
|
||||
|
||||
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()[1][m_indexForwardAxis],
|
||||
chassisWorldTransform.getBasis()[2][m_indexForwardAxis]);
|
||||
@@ -353,99 +328,88 @@ void btRaycastVehicle::updateVehicle( btScalar step )
|
||||
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
|
||||
}
|
||||
else
|
||||
{
|
||||
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);
|
||||
wheelInfo.m_steering = steering;
|
||||
}
|
||||
|
||||
|
||||
|
||||
btScalar btRaycastVehicle::getSteeringValue(int wheel) const
|
||||
btScalar btRaycastVehicle::getSteeringValue(int wheel) const
|
||||
{
|
||||
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);
|
||||
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];
|
||||
}
|
||||
|
||||
btWheelInfo& btRaycastVehicle::getWheelInfo(int index)
|
||||
btWheelInfo& btRaycastVehicle::getWheelInfo(int index)
|
||||
{
|
||||
btAssert((index >= 0) && (index < getNumWheels()));
|
||||
|
||||
btAssert((index >= 0) && (index < getNumWheels()));
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
void btRaycastVehicle::updateSuspension(btScalar deltaTime)
|
||||
void btRaycastVehicle::updateSuspension(btScalar deltaTime)
|
||||
{
|
||||
(void)deltaTime;
|
||||
|
||||
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];
|
||||
|
||||
if ( wheel_info.m_raycastInfo.m_isInContact )
|
||||
btWheelInfo& wheel_info = m_wheelInfo[w_it];
|
||||
|
||||
if (wheel_info.m_raycastInfo.m_isInContact)
|
||||
{
|
||||
btScalar force;
|
||||
// Spring
|
||||
{
|
||||
btScalar susp_length = wheel_info.getSuspensionRestLength();
|
||||
btScalar current_length = wheel_info.m_raycastInfo.m_suspensionLength;
|
||||
btScalar susp_length = wheel_info.getSuspensionRestLength();
|
||||
btScalar current_length = wheel_info.m_raycastInfo.m_suspensionLength;
|
||||
|
||||
btScalar length_diff = (susp_length - current_length);
|
||||
|
||||
force = wheel_info.m_suspensionStiffness
|
||||
* length_diff * wheel_info.m_clippedInvContactDotSuspension;
|
||||
force = wheel_info.m_suspensionStiffness * length_diff * wheel_info.m_clippedInvContactDotSuspension;
|
||||
}
|
||||
|
||||
|
||||
// Damper
|
||||
{
|
||||
btScalar projected_rel_vel = wheel_info.m_suspensionRelativeVelocity;
|
||||
{
|
||||
btScalar susp_damping;
|
||||
if ( projected_rel_vel < btScalar(0.0) )
|
||||
btScalar susp_damping;
|
||||
if (projected_rel_vel < btScalar(0.0))
|
||||
{
|
||||
susp_damping = wheel_info.m_wheelsDampingCompression;
|
||||
}
|
||||
@@ -469,50 +433,43 @@ void btRaycastVehicle::updateSuspension(btScalar deltaTime)
|
||||
wheel_info.m_wheelsSuspensionForce = btScalar(0.0);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
struct btWheelContactPoint
|
||||
{
|
||||
btRigidBody* m_body0;
|
||||
btRigidBody* m_body1;
|
||||
btVector3 m_frictionPositionWorld;
|
||||
btVector3 m_frictionDirectionWorld;
|
||||
btScalar m_jacDiagABInv;
|
||||
btScalar m_maxImpulse;
|
||||
btVector3 m_frictionPositionWorld;
|
||||
btVector3 m_frictionDirectionWorld;
|
||||
btScalar m_jacDiagABInv;
|
||||
btScalar m_maxImpulse;
|
||||
|
||||
|
||||
btWheelContactPoint(btRigidBody* body0,btRigidBody* body1,const btVector3& frictionPosWorld,const btVector3& frictionDirectionWorld, btScalar maxImpulse)
|
||||
:m_body0(body0),
|
||||
m_body1(body1),
|
||||
m_frictionPositionWorld(frictionPosWorld),
|
||||
m_frictionDirectionWorld(frictionDirectionWorld),
|
||||
m_maxImpulse(maxImpulse)
|
||||
btWheelContactPoint(btRigidBody* body0, btRigidBody* body1, const btVector3& frictionPosWorld, const btVector3& frictionDirectionWorld, btScalar maxImpulse)
|
||||
: m_body0(body0),
|
||||
m_body1(body1),
|
||||
m_frictionPositionWorld(frictionPosWorld),
|
||||
m_frictionDirectionWorld(frictionDirectionWorld),
|
||||
m_maxImpulse(maxImpulse)
|
||||
{
|
||||
btScalar denom0 = body0->computeImpulseDenominator(frictionPosWorld,frictionDirectionWorld);
|
||||
btScalar denom1 = body1->computeImpulseDenominator(frictionPosWorld,frictionDirectionWorld);
|
||||
btScalar relaxation = 1.f;
|
||||
m_jacDiagABInv = relaxation/(denom0+denom1);
|
||||
btScalar denom0 = body0->computeImpulseDenominator(frictionPosWorld, frictionDirectionWorld);
|
||||
btScalar denom1 = body1->computeImpulseDenominator(frictionPosWorld, frictionDirectionWorld);
|
||||
btScalar relaxation = 1.f;
|
||||
m_jacDiagABInv = relaxation / (denom0 + denom1);
|
||||
}
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
btScalar calcRollingFriction(btWheelContactPoint& contactPoint, int numWheelsOnGround);
|
||||
btScalar calcRollingFriction(btWheelContactPoint& contactPoint, int numWheelsOnGround)
|
||||
{
|
||||
|
||||
btScalar j1=0.f;
|
||||
btScalar j1 = 0.f;
|
||||
|
||||
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();
|
||||
|
||||
btScalar maxImpulse = contactPoint.m_maxImpulse;
|
||||
|
||||
|
||||
btScalar maxImpulse = contactPoint.m_maxImpulse;
|
||||
|
||||
btVector3 vel1 = contactPoint.m_body0->getVelocityInLocalPoint(rel_pos1);
|
||||
btVector3 vel2 = contactPoint.m_body1->getVelocityInLocalPoint(rel_pos2);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
@@ -520,253 +477,225 @@ btScalar calcRollingFriction(btWheelContactPoint& contactPoint, int numWheelsOnG
|
||||
btScalar vrel = contactPoint.m_frictionDirectionWorld.dot(vel);
|
||||
|
||||
// calculate j that moves us to zero relative velocity
|
||||
j1 = -vrel * contactPoint.m_jacDiagABInv/btScalar(numWheelsOnGround);
|
||||
j1 = -vrel * contactPoint.m_jacDiagABInv / btScalar(numWheelsOnGround);
|
||||
btSetMin(j1, maxImpulse);
|
||||
btSetMax(j1, -maxImpulse);
|
||||
|
||||
return j1;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
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
|
||||
int numWheel = getNumWheels();
|
||||
if (!numWheel)
|
||||
return;
|
||||
|
||||
//calculate the impulse, so that the wheels don't move sidewards
|
||||
int numWheel = getNumWheels();
|
||||
if (!numWheel)
|
||||
return;
|
||||
m_forwardWS.resize(numWheel);
|
||||
m_axle.resize(numWheel);
|
||||
m_forwardImpulse.resize(numWheel);
|
||||
m_sideImpulse.resize(numWheel);
|
||||
|
||||
m_forwardWS.resize(numWheel);
|
||||
m_axle.resize(numWheel);
|
||||
m_forwardImpulse.resize(numWheel);
|
||||
m_sideImpulse.resize(numWheel);
|
||||
|
||||
int numWheelsOnGround = 0;
|
||||
|
||||
int numWheelsOnGround = 0;
|
||||
|
||||
//collapse all those loops into one!
|
||||
for (int i=0;i<getNumWheels();i++)
|
||||
//collapse all those loops into one!
|
||||
for (int i = 0; i < getNumWheels(); i++)
|
||||
{
|
||||
btWheelInfo& wheelInfo = m_wheelInfo[i];
|
||||
class btRigidBody* groundObject = (class btRigidBody*)wheelInfo.m_raycastInfo.m_groundObject;
|
||||
if (groundObject)
|
||||
numWheelsOnGround++;
|
||||
m_sideImpulse[i] = btScalar(0.);
|
||||
m_forwardImpulse[i] = btScalar(0.);
|
||||
}
|
||||
|
||||
{
|
||||
for (int i = 0; i < getNumWheels(); 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)
|
||||
numWheelsOnGround++;
|
||||
m_sideImpulse[i] = btScalar(0.);
|
||||
m_forwardImpulse[i] = btScalar(0.);
|
||||
|
||||
}
|
||||
|
||||
{
|
||||
|
||||
for (int i=0;i<getNumWheels();i++)
|
||||
{
|
||||
const btTransform& wheelTrans = getWheelTransformWS(i);
|
||||
|
||||
btWheelInfo& wheelInfo = m_wheelInfo[i];
|
||||
|
||||
class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
|
||||
btMatrix3x3 wheelBasis0 = wheelTrans.getBasis();
|
||||
m_axle[i] = -btVector3(
|
||||
wheelBasis0[0][m_indexRightAxis],
|
||||
wheelBasis0[1][m_indexRightAxis],
|
||||
wheelBasis0[2][m_indexRightAxis]);
|
||||
|
||||
if (groundObject)
|
||||
{
|
||||
const btVector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
|
||||
btScalar proj = m_axle[i].dot(surfNormalWS);
|
||||
m_axle[i] -= surfNormalWS * proj;
|
||||
m_axle[i] = m_axle[i].normalize();
|
||||
|
||||
const btTransform& wheelTrans = getWheelTransformWS( i );
|
||||
m_forwardWS[i] = surfNormalWS.cross(m_axle[i]);
|
||||
m_forwardWS[i].normalize();
|
||||
|
||||
btMatrix3x3 wheelBasis0 = wheelTrans.getBasis();
|
||||
m_axle[i] = -btVector3(
|
||||
wheelBasis0[0][m_indexRightAxis],
|
||||
wheelBasis0[1][m_indexRightAxis],
|
||||
wheelBasis0[2][m_indexRightAxis]);
|
||||
|
||||
const btVector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
|
||||
btScalar proj = m_axle[i].dot(surfNormalWS);
|
||||
m_axle[i] -= surfNormalWS * proj;
|
||||
m_axle[i] = m_axle[i].normalize();
|
||||
|
||||
m_forwardWS[i] = surfNormalWS.cross(m_axle[i]);
|
||||
m_forwardWS[i].normalize();
|
||||
|
||||
|
||||
resolveSingleBilateral(*m_chassisBody, wheelInfo.m_raycastInfo.m_contactPointWS,
|
||||
*groundObject, wheelInfo.m_raycastInfo.m_contactPointWS,
|
||||
btScalar(0.), m_axle[i],m_sideImpulse[i],timeStep);
|
||||
|
||||
m_sideImpulse[i] *= sideFrictionStiffness2;
|
||||
|
||||
}
|
||||
|
||||
resolveSingleBilateral(*m_chassisBody, wheelInfo.m_raycastInfo.m_contactPointWS,
|
||||
*groundObject, wheelInfo.m_raycastInfo.m_contactPointWS,
|
||||
btScalar(0.), m_axle[i], m_sideImpulse[i], timeStep);
|
||||
|
||||
m_sideImpulse[i] *= sideFrictionStiffness2;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
btScalar sideFactor = btScalar(1.);
|
||||
btScalar fwdFactor = 0.5;
|
||||
|
||||
bool sliding = false;
|
||||
{
|
||||
for (int wheel =0;wheel <getNumWheels();wheel++)
|
||||
for (int wheel = 0; wheel < getNumWheels(); 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 (wheelInfo.m_engineForce != 0.f)
|
||||
{
|
||||
rollingFriction = wheelInfo.m_engineForce* timeStep;
|
||||
} else
|
||||
rollingFriction = wheelInfo.m_engineForce * timeStep;
|
||||
}
|
||||
else
|
||||
{
|
||||
btScalar defaultRollingFrictionImpulse = 0.f;
|
||||
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);
|
||||
btAssert(numWheelsOnGround > 0);
|
||||
rollingFriction = calcRollingFriction(contactPt, numWheelsOnGround);
|
||||
}
|
||||
}
|
||||
|
||||
//switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break)
|
||||
|
||||
|
||||
|
||||
|
||||
m_forwardImpulse[wheel] = btScalar(0.);
|
||||
m_wheelInfo[wheel].m_skidInfo= btScalar(1.);
|
||||
m_wheelInfo[wheel].m_skidInfo = btScalar(1.);
|
||||
|
||||
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 maximpSide = maximp;
|
||||
|
||||
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 y = (m_sideImpulse[wheel] ) * sideFactor;
|
||||
|
||||
btScalar impulseSquared = (x*x + y*y);
|
||||
btScalar x = (m_forwardImpulse[wheel]) * fwdFactor;
|
||||
btScalar y = (m_sideImpulse[wheel]) * sideFactor;
|
||||
|
||||
btScalar impulseSquared = (x * x + y * y);
|
||||
|
||||
if (impulseSquared > maximpSquared)
|
||||
{
|
||||
sliding = true;
|
||||
|
||||
|
||||
btScalar factor = maximp / btSqrt(impulseSquared);
|
||||
|
||||
|
||||
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_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
|
||||
}
|
||||
m_forwardImpulse[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];
|
||||
|
||||
btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS -
|
||||
m_chassisBody->getCenterOfMassPosition();
|
||||
|
||||
if (m_forwardImpulse[wheel] != btScalar(0.))
|
||||
{
|
||||
btWheelInfo& wheelInfo = m_wheelInfo[wheel];
|
||||
m_chassisBody->applyImpulse(m_forwardWS[wheel] * (m_forwardImpulse[wheel]), rel_pos);
|
||||
}
|
||||
if (m_sideImpulse[wheel] != btScalar(0.))
|
||||
{
|
||||
class btRigidBody* groundObject = (class btRigidBody*)m_wheelInfo[wheel].m_raycastInfo.m_groundObject;
|
||||
|
||||
btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS -
|
||||
m_chassisBody->getCenterOfMassPosition();
|
||||
btVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS -
|
||||
groundObject->getCenterOfMassPosition();
|
||||
|
||||
if (m_forwardImpulse[wheel] != btScalar(0.))
|
||||
{
|
||||
m_chassisBody->applyImpulse(m_forwardWS[wheel]*(m_forwardImpulse[wheel]),rel_pos);
|
||||
}
|
||||
if (m_sideImpulse[wheel] != btScalar(0.))
|
||||
{
|
||||
class btRigidBody* groundObject = (class btRigidBody*) m_wheelInfo[wheel].m_raycastInfo.m_groundObject;
|
||||
btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel];
|
||||
|
||||
btVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS -
|
||||
groundObject->getCenterOfMassPosition();
|
||||
|
||||
|
||||
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.
|
||||
btVector3 vChassisWorldUp = getRigidBody()->getCenterOfMassTransform().getBasis().getColumn(m_indexUpAxis);
|
||||
rel_pos -= vChassisWorldUp * (vChassisWorldUp.dot(rel_pos) * (1.f-wheelInfo.m_rollInfluence));
|
||||
#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);
|
||||
rel_pos -= vChassisWorldUp * (vChassisWorldUp.dot(rel_pos) * (1.f - wheelInfo.m_rollInfluence));
|
||||
#else
|
||||
rel_pos[m_indexUpAxis] *= wheelInfo.m_rollInfluence;
|
||||
rel_pos[m_indexUpAxis] *= wheelInfo.m_rollInfluence;
|
||||
#endif
|
||||
m_chassisBody->applyImpulse(sideImp,rel_pos);
|
||||
m_chassisBody->applyImpulse(sideImp, rel_pos);
|
||||
|
||||
//apply friction impulse on the ground
|
||||
groundObject->applyImpulse(-sideImp,rel_pos2);
|
||||
}
|
||||
//apply friction impulse on the ground
|
||||
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)
|
||||
{
|
||||
wheelColor.setValue(0,0,1);
|
||||
} else
|
||||
wheelColor.setValue(0, 0, 1);
|
||||
}
|
||||
else
|
||||
{
|
||||
wheelColor.setValue(1,0,1);
|
||||
wheelColor.setValue(1, 0, 1);
|
||||
}
|
||||
|
||||
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()[1][getRightAxis()],
|
||||
getWheelInfo(v).m_worldTransform.getBasis()[2][getRightAxis()]);
|
||||
|
||||
//debug wheels (cylinders)
|
||||
debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor);
|
||||
debugDrawer->drawLine(wheelPosWS,getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor);
|
||||
|
||||
debugDrawer->drawLine(wheelPosWS, wheelPosWS + axle, 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);
|
||||
|
||||
if (rayCallback.hasHit())
|
||||
{
|
||||
|
||||
const btRigidBody* body = btRigidBody::upcast(rayCallback.m_collisionObject);
|
||||
if (body && body->hasContactResponse())
|
||||
if (body && body->hasContactResponse())
|
||||
{
|
||||
result.m_hitPointInWorld = rayCallback.m_hitPointWorld;
|
||||
result.m_hitNormalInWorld = rayCallback.m_hitNormalWorld;
|
||||
@@ -777,4 +706,3 @@ void* btDefaultVehicleRaycaster::castRay(const btVector3& from,const btVector3&
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
@@ -24,122 +24,111 @@ class btDynamicsWorld;
|
||||
///rayCast vehicle, very special constraint that turn a rigidbody into a vehicle.
|
||||
class btRaycastVehicle : public btActionInterface
|
||||
{
|
||||
btAlignedObjectArray<btVector3> m_forwardWS;
|
||||
btAlignedObjectArray<btVector3> m_axle;
|
||||
btAlignedObjectArray<btScalar> m_forwardImpulse;
|
||||
btAlignedObjectArray<btScalar> m_sideImpulse;
|
||||
|
||||
btAlignedObjectArray<btVector3> m_forwardWS;
|
||||
btAlignedObjectArray<btVector3> m_axle;
|
||||
btAlignedObjectArray<btScalar> m_forwardImpulse;
|
||||
btAlignedObjectArray<btScalar> m_sideImpulse;
|
||||
|
||||
///backwards compatibility
|
||||
int m_userConstraintType;
|
||||
int m_userConstraintId;
|
||||
///backwards compatibility
|
||||
int m_userConstraintType;
|
||||
int m_userConstraintId;
|
||||
|
||||
public:
|
||||
class btVehicleTuning
|
||||
{
|
||||
public:
|
||||
btVehicleTuning()
|
||||
: m_suspensionStiffness(btScalar(5.88)),
|
||||
m_suspensionCompression(btScalar(0.83)),
|
||||
m_suspensionDamping(btScalar(0.88)),
|
||||
m_maxSuspensionTravelCm(btScalar(500.)),
|
||||
m_frictionSlip(btScalar(10.5)),
|
||||
m_maxSuspensionForce(btScalar(6000.))
|
||||
{
|
||||
public:
|
||||
}
|
||||
btScalar m_suspensionStiffness;
|
||||
btScalar m_suspensionCompression;
|
||||
btScalar m_suspensionDamping;
|
||||
btScalar m_maxSuspensionTravelCm;
|
||||
btScalar m_frictionSlip;
|
||||
btScalar m_maxSuspensionForce;
|
||||
};
|
||||
|
||||
btVehicleTuning()
|
||||
:m_suspensionStiffness(btScalar(5.88)),
|
||||
m_suspensionCompression(btScalar(0.83)),
|
||||
m_suspensionDamping(btScalar(0.88)),
|
||||
m_maxSuspensionTravelCm(btScalar(500.)),
|
||||
m_frictionSlip(btScalar(10.5)),
|
||||
m_maxSuspensionForce(btScalar(6000.))
|
||||
{
|
||||
}
|
||||
btScalar m_suspensionStiffness;
|
||||
btScalar m_suspensionCompression;
|
||||
btScalar m_suspensionDamping;
|
||||
btScalar m_maxSuspensionTravelCm;
|
||||
btScalar m_frictionSlip;
|
||||
btScalar m_maxSuspensionForce;
|
||||
|
||||
};
|
||||
private:
|
||||
|
||||
btVehicleRaycaster* m_vehicleRaycaster;
|
||||
btScalar m_pitchControl;
|
||||
btScalar m_steeringValue;
|
||||
btVehicleRaycaster* m_vehicleRaycaster;
|
||||
btScalar m_pitchControl;
|
||||
btScalar m_steeringValue;
|
||||
btScalar m_currentVehicleSpeedKmHour;
|
||||
|
||||
btRigidBody* m_chassisBody;
|
||||
|
||||
int m_indexRightAxis;
|
||||
int m_indexUpAxis;
|
||||
int m_indexForwardAxis;
|
||||
int m_indexForwardAxis;
|
||||
|
||||
void defaultInit(const btVehicleTuning& tuning);
|
||||
|
||||
public:
|
||||
|
||||
//constructor to create a car from an existing rigidbody
|
||||
btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis, btVehicleRaycaster* raycaster );
|
||||
|
||||
virtual ~btRaycastVehicle() ;
|
||||
btRaycastVehicle(const btVehicleTuning& tuning, btRigidBody* chassis, btVehicleRaycaster* raycaster);
|
||||
|
||||
virtual ~btRaycastVehicle();
|
||||
|
||||
///btActionInterface interface
|
||||
virtual void updateAction( btCollisionWorld* collisionWorld, btScalar step)
|
||||
virtual void updateAction(btCollisionWorld* collisionWorld, btScalar step)
|
||||
{
|
||||
(void) collisionWorld;
|
||||
(void)collisionWorld;
|
||||
updateVehicle(step);
|
||||
}
|
||||
|
||||
|
||||
///btActionInterface interface
|
||||
void debugDraw(btIDebugDraw* debugDrawer);
|
||||
|
||||
void debugDraw(btIDebugDraw* debugDrawer);
|
||||
|
||||
const btTransform& getChassisWorldTransform() const;
|
||||
|
||||
|
||||
btScalar rayCast(btWheelInfo& wheel);
|
||||
|
||||
virtual void updateVehicle(btScalar step);
|
||||
|
||||
|
||||
|
||||
void resetSuspension();
|
||||
|
||||
btScalar getSteeringValue(int wheel) const;
|
||||
btScalar getSteeringValue(int wheel) const;
|
||||
|
||||
void setSteeringValue(btScalar steering,int wheel);
|
||||
void setSteeringValue(btScalar steering, int wheel);
|
||||
|
||||
void applyEngineForce(btScalar force, int wheel);
|
||||
|
||||
void applyEngineForce(btScalar force, int wheel);
|
||||
const btTransform& getWheelTransformWS(int wheelIndex) const;
|
||||
|
||||
const btTransform& getWheelTransformWS( int wheelIndex ) const;
|
||||
void updateWheelTransform(int wheelIndex, bool interpolatedTransform = true);
|
||||
|
||||
void updateWheelTransform( int wheelIndex, bool interpolatedTransform = true );
|
||||
|
||||
// void setRaycastWheelInfo( int wheelIndex , bool isInContact, const btVector3& hitPoint, const btVector3& hitNormal,btScalar depth);
|
||||
// void setRaycastWheelInfo( int wheelIndex , bool isInContact, const btVector3& hitPoint, const btVector3& hitNormal,btScalar depth);
|
||||
|
||||
btWheelInfo& addWheel( const btVector3& connectionPointCS0, const btVector3& wheelDirectionCS0,const btVector3& wheelAxleCS,btScalar suspensionRestLength,btScalar wheelRadius,const btVehicleTuning& tuning, bool isFrontWheel);
|
||||
btWheelInfo& addWheel(const btVector3& connectionPointCS0, const btVector3& wheelDirectionCS0, const btVector3& wheelAxleCS, btScalar suspensionRestLength, btScalar wheelRadius, const btVehicleTuning& tuning, bool isFrontWheel);
|
||||
|
||||
inline int getNumWheels() const {
|
||||
return int (m_wheelInfo.size());
|
||||
inline int getNumWheels() const
|
||||
{
|
||||
return int(m_wheelInfo.size());
|
||||
}
|
||||
|
||||
btAlignedObjectArray<btWheelInfo> m_wheelInfo;
|
||||
|
||||
btAlignedObjectArray<btWheelInfo> m_wheelInfo;
|
||||
|
||||
const btWheelInfo& getWheelInfo(int index) const;
|
||||
const btWheelInfo& getWheelInfo(int index) const;
|
||||
|
||||
btWheelInfo& getWheelInfo(int index);
|
||||
btWheelInfo& getWheelInfo(int index);
|
||||
|
||||
void updateWheelTransformsWS(btWheelInfo& wheel , bool interpolatedTransform = true);
|
||||
void updateWheelTransformsWS(btWheelInfo& wheel, bool interpolatedTransform = true);
|
||||
|
||||
|
||||
void setBrake(btScalar brake,int wheelIndex);
|
||||
void setBrake(btScalar brake, int wheelIndex);
|
||||
|
||||
void setPitchControl(btScalar pitch)
|
||||
void setPitchControl(btScalar pitch)
|
||||
{
|
||||
m_pitchControl = pitch;
|
||||
}
|
||||
|
||||
void updateSuspension(btScalar deltaTime);
|
||||
|
||||
virtual void updateFriction(btScalar timeStep);
|
||||
|
||||
void updateSuspension(btScalar deltaTime);
|
||||
|
||||
virtual void updateFriction(btScalar timeStep);
|
||||
|
||||
inline btRigidBody* getRigidBody()
|
||||
{
|
||||
@@ -151,7 +140,7 @@ public:
|
||||
return m_chassisBody;
|
||||
}
|
||||
|
||||
inline int getRightAxis() const
|
||||
inline int getRightAxis() const
|
||||
{
|
||||
return m_indexRightAxis;
|
||||
}
|
||||
@@ -165,46 +154,44 @@ public:
|
||||
return m_indexForwardAxis;
|
||||
}
|
||||
|
||||
|
||||
///Worldspace forward vector
|
||||
btVector3 getForwardVector() const
|
||||
{
|
||||
const btTransform& chassisTrans = getChassisWorldTransform();
|
||||
const btTransform& chassisTrans = getChassisWorldTransform();
|
||||
|
||||
btVector3 forwardW (
|
||||
chassisTrans.getBasis()[0][m_indexForwardAxis],
|
||||
chassisTrans.getBasis()[1][m_indexForwardAxis],
|
||||
chassisTrans.getBasis()[2][m_indexForwardAxis]);
|
||||
btVector3 forwardW(
|
||||
chassisTrans.getBasis()[0][m_indexForwardAxis],
|
||||
chassisTrans.getBasis()[1][m_indexForwardAxis],
|
||||
chassisTrans.getBasis()[2][m_indexForwardAxis]);
|
||||
|
||||
return forwardW;
|
||||
}
|
||||
|
||||
///Velocity of vehicle (positive if velocity vector has same direction as foward vector)
|
||||
btScalar getCurrentSpeedKmHour() const
|
||||
btScalar getCurrentSpeedKmHour() const
|
||||
{
|
||||
return m_currentVehicleSpeedKmHour;
|
||||
}
|
||||
|
||||
virtual void setCoordinateSystem(int rightIndex,int upIndex,int forwardIndex)
|
||||
virtual void setCoordinateSystem(int rightIndex, int upIndex, int forwardIndex)
|
||||
{
|
||||
m_indexRightAxis = rightIndex;
|
||||
m_indexUpAxis = upIndex;
|
||||
m_indexForwardAxis = forwardIndex;
|
||||
}
|
||||
|
||||
|
||||
///backwards compatibility
|
||||
int getUserConstraintType() const
|
||||
{
|
||||
return m_userConstraintType ;
|
||||
return m_userConstraintType;
|
||||
}
|
||||
|
||||
void setUserConstraintType(int userConstraintType)
|
||||
void setUserConstraintType(int userConstraintType)
|
||||
{
|
||||
m_userConstraintType = userConstraintType;
|
||||
};
|
||||
|
||||
void setUserConstraintId(int uid)
|
||||
void setUserConstraintId(int uid)
|
||||
{
|
||||
m_userConstraintId = uid;
|
||||
}
|
||||
@@ -213,22 +200,19 @@ public:
|
||||
{
|
||||
return m_userConstraintId;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
class btDefaultVehicleRaycaster : public btVehicleRaycaster
|
||||
{
|
||||
btDynamicsWorld* m_dynamicsWorld;
|
||||
btDynamicsWorld* m_dynamicsWorld;
|
||||
|
||||
public:
|
||||
btDefaultVehicleRaycaster(btDynamicsWorld* world)
|
||||
:m_dynamicsWorld(world)
|
||||
: m_dynamicsWorld(world)
|
||||
{
|
||||
}
|
||||
|
||||
virtual void* castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result);
|
||||
|
||||
virtual void* castRay(const btVector3& from, const btVector3& to, btVehicleRaycasterResult& result);
|
||||
};
|
||||
|
||||
|
||||
#endif //BT_RAYCASTVEHICLE_H
|
||||
|
||||
#endif //BT_RAYCASTVEHICLE_H
|
||||
|
||||
@@ -16,20 +16,18 @@
|
||||
/// btVehicleRaycaster is provides interface for between vehicle simulation and raycasting
|
||||
struct btVehicleRaycaster
|
||||
{
|
||||
virtual ~btVehicleRaycaster()
|
||||
{
|
||||
}
|
||||
virtual ~btVehicleRaycaster()
|
||||
{
|
||||
}
|
||||
struct btVehicleRaycasterResult
|
||||
{
|
||||
btVehicleRaycasterResult() :m_distFraction(btScalar(-1.)){};
|
||||
btVector3 m_hitPointInWorld;
|
||||
btVector3 m_hitNormalInWorld;
|
||||
btScalar m_distFraction;
|
||||
btVehicleRaycasterResult() : m_distFraction(btScalar(-1.)){};
|
||||
btVector3 m_hitPointInWorld;
|
||||
btVector3 m_hitNormalInWorld;
|
||||
btScalar m_distFraction;
|
||||
};
|
||||
|
||||
virtual void* castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result) = 0;
|
||||
|
||||
virtual void* castRay(const btVector3& from, const btVector3& to, btVehicleRaycasterResult& result) = 0;
|
||||
};
|
||||
|
||||
#endif //BT_VEHICLE_RAYCASTER_H
|
||||
|
||||
#endif //BT_VEHICLE_RAYCASTER_H
|
||||
|
||||
@@ -9,30 +9,26 @@
|
||||
* It is provided "as is" without express or implied warranty.
|
||||
*/
|
||||
#include "btWheelInfo.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h" // for pointvelocity
|
||||
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h" // for pointvelocity
|
||||
|
||||
btScalar btWheelInfo::getSuspensionRestLength() const
|
||||
{
|
||||
|
||||
return m_suspensionRestLength1;
|
||||
|
||||
}
|
||||
|
||||
void btWheelInfo::updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo)
|
||||
void btWheelInfo::updateWheel(const btRigidBody& chassis, RaycastInfo& raycastInfo)
|
||||
{
|
||||
(void)raycastInfo;
|
||||
|
||||
|
||||
if (m_raycastInfo.m_isInContact)
|
||||
|
||||
{
|
||||
btScalar project= m_raycastInfo.m_contactNormalWS.dot( m_raycastInfo.m_wheelDirectionWS );
|
||||
btVector3 chassis_velocity_at_contactPoint;
|
||||
btScalar project = m_raycastInfo.m_contactNormalWS.dot(m_raycastInfo.m_wheelDirectionWS);
|
||||
btVector3 chassis_velocity_at_contactPoint;
|
||||
btVector3 relpos = m_raycastInfo.m_contactPointWS - chassis.getCenterOfMassPosition();
|
||||
chassis_velocity_at_contactPoint = chassis.getVelocityInLocalPoint( relpos );
|
||||
btScalar projVel = m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint );
|
||||
if ( project >= btScalar(-0.1))
|
||||
chassis_velocity_at_contactPoint = chassis.getVelocityInLocalPoint(relpos);
|
||||
btScalar projVel = m_raycastInfo.m_contactNormalWS.dot(chassis_velocity_at_contactPoint);
|
||||
if (project >= btScalar(-0.1))
|
||||
{
|
||||
m_suspensionRelativeVelocity = btScalar(0.0);
|
||||
m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1);
|
||||
@@ -43,10 +39,9 @@ void btWheelInfo::updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInf
|
||||
m_suspensionRelativeVelocity = projVel * inv;
|
||||
m_clippedInvContactDotSuspension = inv;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
else // Not in contact : position wheel in a nice (rest length) position
|
||||
else // Not in contact : position wheel in a nice (rest length) position
|
||||
{
|
||||
m_raycastInfo.m_suspensionLength = this->getSuspensionRestLength();
|
||||
m_suspensionRelativeVelocity = btScalar(0.0);
|
||||
|
||||
@@ -18,20 +18,19 @@ class btRigidBody;
|
||||
|
||||
struct btWheelInfoConstructionInfo
|
||||
{
|
||||
btVector3 m_chassisConnectionCS;
|
||||
btVector3 m_wheelDirectionCS;
|
||||
btVector3 m_wheelAxleCS;
|
||||
btScalar m_suspensionRestLength;
|
||||
btScalar m_maxSuspensionTravelCm;
|
||||
btScalar m_wheelRadius;
|
||||
|
||||
btScalar m_suspensionStiffness;
|
||||
btScalar m_wheelsDampingCompression;
|
||||
btScalar m_wheelsDampingRelaxation;
|
||||
btScalar m_frictionSlip;
|
||||
btScalar m_maxSuspensionForce;
|
||||
btVector3 m_chassisConnectionCS;
|
||||
btVector3 m_wheelDirectionCS;
|
||||
btVector3 m_wheelAxleCS;
|
||||
btScalar m_suspensionRestLength;
|
||||
btScalar m_maxSuspensionTravelCm;
|
||||
btScalar m_wheelRadius;
|
||||
|
||||
btScalar m_suspensionStiffness;
|
||||
btScalar m_wheelsDampingCompression;
|
||||
btScalar m_wheelsDampingRelaxation;
|
||||
btScalar m_frictionSlip;
|
||||
btScalar m_maxSuspensionForce;
|
||||
bool m_bIsFrontWheel;
|
||||
|
||||
};
|
||||
|
||||
/// btWheelInfo contains information per wheel about friction and suspension.
|
||||
@@ -40,51 +39,50 @@ struct btWheelInfo
|
||||
struct RaycastInfo
|
||||
{
|
||||
//set by raycaster
|
||||
btVector3 m_contactNormalWS;//contactnormal
|
||||
btVector3 m_contactPointWS;//raycast hitpoint
|
||||
btScalar m_suspensionLength;
|
||||
btVector3 m_hardPointWS;//raycast starting point
|
||||
btVector3 m_wheelDirectionWS; //direction in worldspace
|
||||
btVector3 m_wheelAxleWS; // axle in worldspace
|
||||
bool m_isInContact;
|
||||
void* m_groundObject; //could be general void* ptr
|
||||
btVector3 m_contactNormalWS; //contactnormal
|
||||
btVector3 m_contactPointWS; //raycast hitpoint
|
||||
btScalar m_suspensionLength;
|
||||
btVector3 m_hardPointWS; //raycast starting point
|
||||
btVector3 m_wheelDirectionWS; //direction in worldspace
|
||||
btVector3 m_wheelAxleWS; // axle in worldspace
|
||||
bool m_isInContact;
|
||||
void* m_groundObject; //could be general void* ptr
|
||||
};
|
||||
|
||||
RaycastInfo m_raycastInfo;
|
||||
RaycastInfo m_raycastInfo;
|
||||
|
||||
btTransform m_worldTransform;
|
||||
|
||||
btVector3 m_chassisConnectionPointCS; //const
|
||||
btVector3 m_wheelDirectionCS;//const
|
||||
btVector3 m_wheelAxleCS; // const or modified by steering
|
||||
btScalar m_suspensionRestLength1;//const
|
||||
btScalar m_maxSuspensionTravelCm;
|
||||
btTransform m_worldTransform;
|
||||
|
||||
btVector3 m_chassisConnectionPointCS; //const
|
||||
btVector3 m_wheelDirectionCS; //const
|
||||
btVector3 m_wheelAxleCS; // const or modified by steering
|
||||
btScalar m_suspensionRestLength1; //const
|
||||
btScalar m_maxSuspensionTravelCm;
|
||||
btScalar getSuspensionRestLength() const;
|
||||
btScalar m_wheelsRadius;//const
|
||||
btScalar m_suspensionStiffness;//const
|
||||
btScalar m_wheelsDampingCompression;//const
|
||||
btScalar m_wheelsDampingRelaxation;//const
|
||||
btScalar m_frictionSlip;
|
||||
btScalar m_steering;
|
||||
btScalar m_rotation;
|
||||
btScalar m_deltaRotation;
|
||||
btScalar m_rollInfluence;
|
||||
btScalar m_maxSuspensionForce;
|
||||
btScalar m_wheelsRadius; //const
|
||||
btScalar m_suspensionStiffness; //const
|
||||
btScalar m_wheelsDampingCompression; //const
|
||||
btScalar m_wheelsDampingRelaxation; //const
|
||||
btScalar m_frictionSlip;
|
||||
btScalar m_steering;
|
||||
btScalar m_rotation;
|
||||
btScalar m_deltaRotation;
|
||||
btScalar m_rollInfluence;
|
||||
btScalar m_maxSuspensionForce;
|
||||
|
||||
btScalar m_engineForce;
|
||||
btScalar m_engineForce;
|
||||
|
||||
btScalar m_brake;
|
||||
|
||||
btScalar m_brake;
|
||||
|
||||
bool m_bIsFrontWheel;
|
||||
|
||||
void* m_clientInfo;//can be used to store pointer to sync transforms...
|
||||
|
||||
void* m_clientInfo; //can be used to store pointer to sync transforms...
|
||||
|
||||
btWheelInfo() {}
|
||||
|
||||
btWheelInfo(btWheelInfoConstructionInfo& ci)
|
||||
|
||||
{
|
||||
|
||||
m_suspensionRestLength1 = ci.m_suspensionRestLength;
|
||||
m_maxSuspensionTravelCm = ci.m_maxSuspensionTravelCm;
|
||||
|
||||
@@ -104,18 +102,15 @@ struct btWheelInfo
|
||||
m_rollInfluence = btScalar(0.1);
|
||||
m_bIsFrontWheel = ci.m_bIsFrontWheel;
|
||||
m_maxSuspensionForce = ci.m_maxSuspensionForce;
|
||||
|
||||
}
|
||||
|
||||
void updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo);
|
||||
void updateWheel(const btRigidBody& chassis, RaycastInfo& raycastInfo);
|
||||
|
||||
btScalar m_clippedInvContactDotSuspension;
|
||||
btScalar m_suspensionRelativeVelocity;
|
||||
btScalar m_clippedInvContactDotSuspension;
|
||||
btScalar m_suspensionRelativeVelocity;
|
||||
//calculated by suspension
|
||||
btScalar m_wheelsSuspensionForce;
|
||||
btScalar m_skidInfo;
|
||||
|
||||
btScalar m_wheelsSuspensionForce;
|
||||
btScalar m_skidInfo;
|
||||
};
|
||||
|
||||
#endif //BT_WHEEL_INFO_H
|
||||
|
||||
#endif //BT_WHEEL_INFO_H
|
||||
|
||||
Reference in New Issue
Block a user