diff options
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/Vehicle')
5 files changed, 380 insertions, 480 deletions
diff --git a/thirdparty/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp b/thirdparty/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp index f299aa34e8..fc70d8e637 100644 --- a/thirdparty/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp +++ b/thirdparty/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp @@ -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); - return wheel; -} - + 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 = object; + 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; - - - 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) + 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) { - 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) +{ + //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); + int numWheelsOnGround = 0; -btScalar sideFrictionStiffness2 = btScalar(1.0); -void btRaycastVehicle::updateFriction(btScalar timeStep) -{ + //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.); + } - //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); - - int numWheelsOnGround = 0; - - - //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]; - 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++) + class btRigidBody* groundObject = (class btRigidBody*)wheelInfo.m_raycastInfo.m_groundObject; + + if (groundObject) { + 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 ); - - 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; - - } - + 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; } } + } 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 x = (m_forwardImpulse[wheel] ) * fwdFactor; - btScalar y = (m_sideImpulse[wheel] ) * sideFactor; - - btScalar impulseSquared = (x*x + y*y); + 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]; + btWheelInfo& wheelInfo = m_wheelInfo[wheel]; - btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS - - m_chassisBody->getCenterOfMassPosition(); + btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS - + m_chassisBody->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; + 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 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS - - groundObject->getCenterOfMassPosition(); + btVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS - + 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. - 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; } - diff --git a/thirdparty/bullet/BulletDynamics/Vehicle/btRaycastVehicle.h b/thirdparty/bullet/BulletDynamics/Vehicle/btRaycastVehicle.h index 04656b912c..99d6894e56 100644 --- a/thirdparty/bullet/BulletDynamics/Vehicle/btRaycastVehicle.h +++ b/thirdparty/bullet/BulletDynamics/Vehicle/btRaycastVehicle.h @@ -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: - - 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: + } + btScalar m_suspensionStiffness; + btScalar m_suspensionCompression; + btScalar m_suspensionDamping; + btScalar m_maxSuspensionTravelCm; + btScalar m_frictionSlip; + btScalar m_maxSuspensionForce; + }; - btVehicleRaycaster* m_vehicleRaycaster; - btScalar m_pitchControl; - btScalar m_steeringValue; +private: + 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 diff --git a/thirdparty/bullet/BulletDynamics/Vehicle/btVehicleRaycaster.h b/thirdparty/bullet/BulletDynamics/Vehicle/btVehicleRaycaster.h index 3cc909c653..2c44ce546c 100644 --- a/thirdparty/bullet/BulletDynamics/Vehicle/btVehicleRaycaster.h +++ b/thirdparty/bullet/BulletDynamics/Vehicle/btVehicleRaycaster.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 diff --git a/thirdparty/bullet/BulletDynamics/Vehicle/btWheelInfo.cpp b/thirdparty/bullet/BulletDynamics/Vehicle/btWheelInfo.cpp index ef93c16fff..d5c12f223b 100644 --- a/thirdparty/bullet/BulletDynamics/Vehicle/btWheelInfo.cpp +++ b/thirdparty/bullet/BulletDynamics/Vehicle/btWheelInfo.cpp @@ -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); diff --git a/thirdparty/bullet/BulletDynamics/Vehicle/btWheelInfo.h b/thirdparty/bullet/BulletDynamics/Vehicle/btWheelInfo.h index f991a57b69..af88b8ff83 100644 --- a/thirdparty/bullet/BulletDynamics/Vehicle/btWheelInfo.h +++ b/thirdparty/bullet/BulletDynamics/Vehicle/btWheelInfo.h @@ -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_engineForce; - - btScalar m_brake; - + 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_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 |