upgrade to latest Bullet 2.53. cross the fingers it doesn't break one of the exotic or less exotic platforms

1 files changed, 127 insertions, 46 deletions

diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
index c85fead5334..d53de7f3687 100644
--- a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
+++ b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
@@ -18,6 +18,7 @@
 #include "BulletDynamics/Dynamics/btDynamicsWorld.h"
 #include "btVehicleRaycaster.h"
 #include "btWheelInfo.h"
+#include "LinearMath/btMinMax.h"
 
 
 #include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
@@ -28,7 +29,7 @@ static btRigidBody s_fixedObject( 0,0,0);
 
 btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis,	btVehicleRaycaster* raycaster )
 :m_vehicleRaycaster(raycaster),
-m_pitchControl(0.f)
+m_pitchControl(btScalar(0.))
 {
 	m_chassisBody = chassis;
 	m_indexRightAxis = 0;
@@ -40,8 +41,9 @@ m_pitchControl(0.f)
 
 void btRaycastVehicle::defaultInit(const btVehicleTuning& tuning)
 {
-	m_currentVehicleSpeedKmHour = 0.f;
-	m_steeringValue = 0.f;
+	(void)tuning;
+	m_currentVehicleSpeedKmHour = btScalar(0.);
+	m_steeringValue = btScalar(0.);
 	
 }
 
@@ -105,12 +107,12 @@ void	btRaycastVehicle::updateWheelTransform( int wheelIndex , bool interpolatedT
 //	up.normalize();
 
 	//rotate around steering over de wheelAxleWS
-	float steering = wheel.m_steering;
+	btScalar 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(
@@ -133,11 +135,11 @@ void btRaycastVehicle::resetSuspension()
 	{
 			btWheelInfo& wheel = m_wheelInfo[i];
 			wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength();
-			wheel.m_suspensionRelativeVelocity = 0.0f;
+			wheel.m_suspensionRelativeVelocity = btScalar(0.0);
 			
 			wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS;
-			//wheel_info.setContactFriction(0.0f);
-			wheel.m_clippedInvContactDotSuspension = 1.0f;
+			//wheel_info.setContactFriction(btScalar(0.0));
+			wheel.m_clippedInvContactDotSuspension = btScalar(1.0);
 	}
 }
 
@@ -170,7 +172,7 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
 	wheel.m_raycastInfo.m_contactPointWS = source + rayvector;
 	const btVector3& target = wheel.m_raycastInfo.m_contactPointWS;
 
-	btScalar param = 0.f;
+	btScalar param = btScalar(0.);
 	
 	btVehicleRaycaster::btVehicleRaycasterResult	rayResults;
 
@@ -195,8 +197,8 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
 		wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelsRadius;
 		//clamp on max suspension travel
 
-		float  minSuspensionLength = wheel.getSuspensionRestLength() - wheel.m_maxSuspensionTravelCm*0.01f;
-		float maxSuspensionLength = wheel.getSuspensionRestLength()+ wheel.m_maxSuspensionTravelCm*0.01f;
+		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;
@@ -217,14 +219,14 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
 
 		btScalar projVel = wheel.m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint );
 
-		if ( denominator >= -0.1f)
+		if ( denominator >= btScalar(-0.1))
 		{
-			wheel.m_suspensionRelativeVelocity = 0.0f;
-			wheel.m_clippedInvContactDotSuspension = 1.0f / 0.1f;
+			wheel.m_suspensionRelativeVelocity = btScalar(0.0);
+			wheel.m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1);
 		}
 		else
 		{
-			btScalar inv = -1.f / denominator;
+			btScalar inv = btScalar(-1.) / denominator;
 			wheel.m_suspensionRelativeVelocity = projVel * inv;
 			wheel.m_clippedInvContactDotSuspension = inv;
 		}
@@ -233,9 +235,9 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
 	{
 		//put wheel info as in rest position
 		wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength();
-		wheel.m_suspensionRelativeVelocity = 0.0f;
+		wheel.m_suspensionRelativeVelocity = btScalar(0.0);
 		wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS;
-		wheel.m_clippedInvContactDotSuspension = 1.0f;
+		wheel.m_clippedInvContactDotSuspension = btScalar(1.0);
 	}
 
 	return depth;
@@ -267,7 +269,7 @@ void btRaycastVehicle::updateVehicle( btScalar step )
 	}
 
 
-	m_currentVehicleSpeedKmHour = 3.6f * getRigidBody()->getLinearVelocity().length();
+	m_currentVehicleSpeedKmHour = btScalar(3.6) * getRigidBody()->getLinearVelocity().length();
 	
 	const btTransform& chassisTrans = getChassisWorldTransform();
 
@@ -276,9 +278,9 @@ void btRaycastVehicle::updateVehicle( btScalar step )
 		chassisTrans.getBasis()[1][m_indexForwardAxis],
 		chassisTrans.getBasis()[2][m_indexForwardAxis]);
 
-	if (forwardW.dot(getRigidBody()->getLinearVelocity()) < 0.f)
+	if (forwardW.dot(getRigidBody()->getLinearVelocity()) < btScalar(0.))
 	{
-		m_currentVehicleSpeedKmHour *= -1.f;
+		m_currentVehicleSpeedKmHour *= btScalar(-1.);
 	}
 
 	//
@@ -300,9 +302,9 @@ void btRaycastVehicle::updateVehicle( btScalar step )
 		//apply suspension force
 		btWheelInfo& wheel = m_wheelInfo[i];
 		
-		float suspensionForce = wheel.m_wheelsSuspensionForce;
+		btScalar suspensionForce = wheel.m_wheelsSuspensionForce;
 		
-		float gMaxSuspensionForce = 6000.f;
+		btScalar gMaxSuspensionForce = btScalar(6000.);
 		if (suspensionForce > gMaxSuspensionForce)
 		{
 			suspensionForce = gMaxSuspensionForce;
@@ -347,7 +349,7 @@ void btRaycastVehicle::updateVehicle( btScalar step )
 			wheel.m_rotation += wheel.m_deltaRotation;
 		}
 		
-		wheel.m_deltaRotation *= 0.99f;//damping of rotation when not in contact
+		wheel.m_deltaRotation *= btScalar(0.99);//damping of rotation when not in contact
 
 	}
 
@@ -394,17 +396,18 @@ btWheelInfo&	btRaycastVehicle::getWheelInfo(int index)
 	return m_wheelInfo[index];
 }
 
-void btRaycastVehicle::setBrake(float brake,int wheelIndex)
+void btRaycastVehicle::setBrake(btScalar brake,int wheelIndex)
 {
 	btAssert((wheelIndex >= 0) && (wheelIndex < 	getNumWheels()));
-	getWheelInfo(wheelIndex).m_brake;
+	getWheelInfo(wheelIndex).m_brake = brake;
 }
 
 
 void	btRaycastVehicle::updateSuspension(btScalar deltaTime)
 {
+	(void)deltaTime;
 
-	btScalar chassisMass = 1.f / m_chassisBody->getInvMass();
+	btScalar chassisMass = btScalar(1.) / m_chassisBody->getInvMass();
 	
 	for (int w_it=0; w_it<getNumWheels(); w_it++)
 	{
@@ -429,7 +432,7 @@ void	btRaycastVehicle::updateSuspension(btScalar deltaTime)
 				btScalar projected_rel_vel = wheel_info.m_suspensionRelativeVelocity;
 				{
 					btScalar	susp_damping;
-					if ( projected_rel_vel < 0.0f )
+					if ( projected_rel_vel < btScalar(0.0) )
 					{
 						susp_damping = wheel_info.m_wheelsDampingCompression;
 					}
@@ -443,20 +446,77 @@ void	btRaycastVehicle::updateSuspension(btScalar deltaTime)
 
 			// RESULT
 			wheel_info.m_wheelsSuspensionForce = force * chassisMass;
-			if (wheel_info.m_wheelsSuspensionForce < 0.f)
+			if (wheel_info.m_wheelsSuspensionForce < btScalar(0.))
 			{
-				wheel_info.m_wheelsSuspensionForce = 0.f;
+				wheel_info.m_wheelsSuspensionForce = btScalar(0.);
 			}
 		}
 		else
 		{
-			wheel_info.m_wheelsSuspensionForce = 0.0f;
+			wheel_info.m_wheelsSuspensionForce = btScalar(0.0);
 		}
 	}
 
 }
 
-float sideFrictionStiffness2 = 1.0f;
+
+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)
+	{
+		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)
+{
+
+	btScalar j1=0.f;
+
+	const btVector3& contactPosWorld = contactPoint.m_frictionPositionWorld;
+
+	btVector3 rel_pos1 = contactPosWorld - contactPoint.m_body0->getCenterOfMassPosition(); 
+	btVector3 rel_pos2 = contactPosWorld - contactPoint.m_body1->getCenterOfMassPosition();
+	
+	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;
+
+	btScalar vrel = contactPoint.m_frictionDirectionWorld.dot(vel);
+
+	// calculate j that moves us to zero relative velocity
+	j1 = -vrel * contactPoint.m_jacDiagABInv;
+	GEN_set_min(j1, maxImpulse);
+	GEN_set_max(j1, -maxImpulse);
+
+	return j1;
+}
+
+
+
+
+btScalar sideFrictionStiffness2 = btScalar(1.0);
 void	btRaycastVehicle::updateFriction(btScalar	timeStep)
 {
 
@@ -481,8 +541,8 @@ void	btRaycastVehicle::updateFriction(btScalar	timeStep)
 			class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
 			if (groundObject)
 				numWheelsOnGround++;
-			sideImpulse[i] = 0.f;
-			forwardImpulse[i] = 0.f;
+			sideImpulse[i] = btScalar(0.);
+			forwardImpulse[i] = btScalar(0.);
 
 		}
 	
@@ -517,7 +577,7 @@ void	btRaycastVehicle::updateFriction(btScalar	timeStep)
 				
 					resolveSingleBilateral(*m_chassisBody, wheelInfo.m_raycastInfo.m_contactPointWS,
 							  *groundObject, wheelInfo.m_raycastInfo.m_contactPointWS,
-							  0.f, axle[i],sideImpulse[i],timeStep);
+							  btScalar(0.), axle[i],sideImpulse[i],timeStep);
 
 					sideImpulse[i] *= sideFrictionStiffness2;
 						
@@ -527,7 +587,7 @@ void	btRaycastVehicle::updateFriction(btScalar	timeStep)
 			}
 		}
 
-	btScalar sideFactor = 1.f;
+	btScalar sideFactor = btScalar(1.);
 	btScalar fwdFactor = 0.5;
 
 	bool sliding = false;
@@ -537,25 +597,46 @@ void	btRaycastVehicle::updateFriction(btScalar	timeStep)
 			btWheelInfo& wheelInfo = m_wheelInfo[wheel];
 			class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
 
+			btScalar	rollingFriction = 0.f;
 
-			forwardImpulse[wheel] = 0.f;
-			m_wheelInfo[wheel].m_skidInfo= 1.f;
+			if (groundObject)
+			{
+				if (wheelInfo.m_engineForce != 0.f)
+				{
+					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,forwardWS[wheel],maxImpulse);
+					rollingFriction = calcRollingFriction(contactPt);
+				}
+			}
+
+			//switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break)
+			
+
+
+
+			forwardImpulse[wheel] = btScalar(0.);
+			m_wheelInfo[wheel].m_skidInfo= btScalar(1.);
 
 			if (groundObject)
 			{
-				m_wheelInfo[wheel].m_skidInfo= 1.f;
+				m_wheelInfo[wheel].m_skidInfo= btScalar(1.);
 				
 				btScalar maximp = wheelInfo.m_wheelsSuspensionForce * timeStep * wheelInfo.m_frictionSlip;
 				btScalar maximpSide = maximp;
 
 				btScalar maximpSquared = maximp * maximpSide;
+			
 
-				forwardImpulse[wheel] = wheelInfo.m_engineForce* timeStep;
+				forwardImpulse[wheel] = rollingFriction;//wheelInfo.m_engineForce* timeStep;
 
-				float x = (forwardImpulse[wheel] ) * fwdFactor;
-				float y = (sideImpulse[wheel] ) * sideFactor;
+				btScalar x = (forwardImpulse[wheel] ) * fwdFactor;
+				btScalar y = (sideImpulse[wheel] ) * sideFactor;
 				
-				float impulseSquared = (x*x + y*y);
+				btScalar impulseSquared = (x*x + y*y);
 
 				if (impulseSquared > maximpSquared)
 				{
@@ -577,9 +658,9 @@ void	btRaycastVehicle::updateFriction(btScalar	timeStep)
 		{
 			for (int wheel = 0;wheel < getNumWheels(); wheel++)
 			{
-				if (sideImpulse[wheel] != 0.f)
+				if (sideImpulse[wheel] != btScalar(0.))
 				{
-					if (m_wheelInfo[wheel].m_skidInfo< 1.f)
+					if (m_wheelInfo[wheel].m_skidInfo< btScalar(1.))
 					{
 						forwardImpulse[wheel] *=	m_wheelInfo[wheel].m_skidInfo;
 						sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
@@ -597,11 +678,11 @@ void	btRaycastVehicle::updateFriction(btScalar	timeStep)
 				btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS - 
 						m_chassisBody->getCenterOfMassPosition();
 
-				if (forwardImpulse[wheel] != 0.f)
+				if (forwardImpulse[wheel] != btScalar(0.))
 				{
 					m_chassisBody->applyImpulse(forwardWS[wheel]*(forwardImpulse[wheel]),rel_pos);
 				}
-				if (sideImpulse[wheel] != 0.f)
+				if (sideImpulse[wheel] != btScalar(0.))
 				{
 					class btRigidBody* groundObject = (class btRigidBody*) m_wheelInfo[wheel].m_raycastInfo.m_groundObject;