added very basic debug drawing for vehicle wheels, and updated raycast in world to do ray-aabb instead of aabb-aabb

Demos/VehicleDemo/VehicleDemo.cpp

@@ -34,17 +34,17 @@ const int maxOverlap = 65535;
 ///notice that for higher-quality slow-moving vehicles, another approach might be better
 ///implementing explicit hinged-wheel constraints with cylinder collision, rather then raycasts
 float	gEngineForce = 0.f;
-float	maxEngineForce = 100.f;
+float	maxEngineForce = 3000.f;
 float	gVehicleSteering = 0.f;
-float	steeringIncrement = 0.1f;
+float	steeringIncrement = 0.04f;
 float	steeringClamp = 0.3f;
 float	wheelRadius = 0.5f;
-float	wheelWidth = 0.2f;
+float	wheelWidth = 0.4f;
-float	wheelFriction = 100.f;
+float	wheelFriction = 1e30f;//1000;//1e30f;
-float	suspensionStiffness = 10.f;
+float	suspensionStiffness = 20.f;
-float	suspensionDamping = 1.3f;
+float	suspensionDamping = 2.3f;
-float	suspensionCompression = 2.4f;
+float	suspensionCompression = 4.4f;
-float	rollInfluence = 0.1f;
+float	rollInfluence = 0.1f;//1.0f;
 btVector3 wheelDirectionCS0(0,-1,0);
 btVector3 wheelAxleCS(1,0,0);
 btScalar suspensionRestLength(0.6);
@@ -76,6 +76,7 @@ m_cameraHeight(4.f),
 m_minCameraDistance(3.f),
 m_maxCameraDistance(10.f)
 {
+	m_vehicle = 0;
 	m_cameraPosition = btVector3(30,30,30);
 }
 
@@ -86,6 +87,7 @@ void VehicleDemo::setupPhysics()
 
 	btCollisionShape* groundShape = new btBoxShape(btVector3(50,3,50));
 	m_dynamicsWorld = new btDiscreteDynamicsWorld();
+	m_dynamicsWorld->setDebugDrawer(&debugDrawer);
 
 
 #define  USE_TRIMESH_GROUND 1
@@ -113,7 +115,9 @@ const float TRIANGLE_SIZE=20.f;
 	{
 		for (int j=0;j<NUM_VERTS_Y;j++)
 		{
-			gVertices[i+j*NUM_VERTS_X].setValue((i-NUM_VERTS_X*0.5f)*TRIANGLE_SIZE,2.f*sinf((float)i)*cosf((float)j),(j-NUM_VERTS_Y*0.5f)*TRIANGLE_SIZE);
+			float wl = .2f;
+			float height = 20.f*sinf(float(i)*wl)*cosf(float(j)*wl);
+			gVertices[i+j*NUM_VERTS_X].setValue((i-NUM_VERTS_X*0.5f)*TRIANGLE_SIZE,height,(j-NUM_VERTS_Y*0.5f)*TRIANGLE_SIZE);
 		}
 	}
 
@@ -150,9 +154,18 @@ const float TRIANGLE_SIZE=20.f;
 	localCreateRigidBody(0,tr,groundShape);
 
 	btCollisionShape* chassisShape = new btBoxShape(btVector3(1.f,0.5f,2.f));
+	btCompoundShape* compound = new btCompoundShape();
+	btTransform localTrans;
+	localTrans.setIdentity();
+	//localTrans effectively shifts the center of mass with respect to the chassis
+	localTrans.setOrigin(btVector3(0,1,0));
+	compound->addChildShape(localTrans,chassisShape);
+
 	tr.setOrigin(btVector3(0,0.f,0));
 
-	m_carChassis = localCreateRigidBody(800,tr,chassisShape);
+	m_carChassis = localCreateRigidBody(800,tr,compound);//chassisShape);
+	m_carChassis->setDamping(0.2,0.2);
+	
 	
 	clientResetScene();
 
@@ -167,46 +180,34 @@ const float TRIANGLE_SIZE=20.f;
 
 		m_dynamicsWorld->addVehicle(m_vehicle);
 
+		float connectionHeight = 1.2f;
 
-		btVector3 connectionPointCS0(CUBE_HALF_EXTENTS-(0.3*wheelWidth),0,2*CUBE_HALF_EXTENTS-wheelRadius);
+	
+		btVector3 connectionPointCS0(CUBE_HALF_EXTENTS-(0.3*wheelWidth),connectionHeight,2*CUBE_HALF_EXTENTS-wheelRadius);
 		bool isFrontWheel=true;
 		int rightIndex = 0;
 		int upIndex = 1;
 		int forwardIndex = 2;
 
 		m_vehicle->setCoordinateSystem(rightIndex,upIndex,forwardIndex);
-
 		m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel);
-		connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),0,2*CUBE_HALF_EXTENTS-wheelRadius);
+		connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),connectionHeight,2*CUBE_HALF_EXTENTS-wheelRadius);
 		m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel);
-		connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),0,-2*CUBE_HALF_EXTENTS+wheelRadius);
+		connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),connectionHeight,-2*CUBE_HALF_EXTENTS+wheelRadius);
 		isFrontWheel = false;
 		m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel);
-		connectionPointCS0 = btVector3(CUBE_HALF_EXTENTS-(0.3*wheelWidth),0,-2*CUBE_HALF_EXTENTS+wheelRadius);
+		connectionPointCS0 = btVector3(CUBE_HALF_EXTENTS-(0.3*wheelWidth),connectionHeight,-2*CUBE_HALF_EXTENTS+wheelRadius);
 		m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel);
 		
-/*		gVehicleConstraint->SetSuspensionStiffness(suspensionStiffness,0);
+		for (int i=0;i<m_vehicle->getNumWheels();i++)
-		gVehicleConstraint->SetSuspensionStiffness(suspensionStiffness,1);
+		{
-		gVehicleConstraint->SetSuspensionStiffness(suspensionStiffness,2);
+			btWheelInfo& wheel = m_vehicle->getWheelInfo(i);
-		gVehicleConstraint->SetSuspensionStiffness(suspensionStiffness,3);
+			wheel.m_suspensionStiffness = suspensionStiffness;
-		
+			wheel.m_wheelsDampingRelaxation = suspensionDamping;
-		gVehicleConstraint->SetSuspensionDamping(suspensionDamping,0);
+			wheel.m_wheelsDampingCompression = suspensionCompression;
-		gVehicleConstraint->SetSuspensionDamping(suspensionDamping,1);
+			wheel.m_frictionSlip = wheelFriction;
-		gVehicleConstraint->SetSuspensionDamping(suspensionDamping,2);
+			wheel.m_rollInfluence = rollInfluence;
-		gVehicleConstraint->SetSuspensionDamping(suspensionDamping,3);
+		}
-		
-		gVehicleConstraint->SetSuspensionCompression(suspensionCompression,0);
-		gVehicleConstraint->SetSuspensionCompression(suspensionCompression,1);
-		gVehicleConstraint->SetSuspensionCompression(suspensionCompression,2);
-		gVehicleConstraint->SetSuspensionCompression(suspensionCompression,3);
-
-		gVehicleConstraint->SetWheelFriction(wheelFriction,0);
-		gVehicleConstraint->SetWheelFriction(wheelFriction,1);
-		gVehicleConstraint->SetWheelFriction(wheelFriction,2);
-		gVehicleConstraint->SetWheelFriction(wheelFriction,3);
-		*/
-
-		
 	}
 
 	
@@ -338,6 +339,19 @@ void VehicleDemo::clientResetScene()
 	gEngineForce = 0.f;
 	gVehicleSteering = 0.f;
 	m_carChassis->setCenterOfMassTransform(btTransform::getIdentity());
+	m_carChassis->setLinearVelocity(btVector3(0,0,0));
+	m_carChassis->setAngularVelocity(btVector3(0,0,0));
+	m_dynamicsWorld->getBroadphase()->cleanProxyFromPairs(m_carChassis->getBroadphaseHandle());
+	if (m_vehicle)
+	{
+		m_vehicle->resetSuspension();
+		for (int i=0;i<m_vehicle->getNumWheels();i++)
+		{
+			//synchronize the wheels with the (interpolated) chassis worldtransform
+			m_vehicle->updateWheelTransform(i);
+		}
+	}
+
 }
 
 

src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp

@@ -320,15 +320,7 @@ void	btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& r
 	
 	rayToTrans.setOrigin(rayToWorld);
 
-	//do culling based on aabb (rayFrom/rayTo)
+	/// go over all objects, and if the ray intersects their aabb, do a ray-shape query using convexCaster (CCD)
-	btVector3 rayAabbMin = rayFromWorld;
-	btVector3 rayAabbMax = rayFromWorld;
-	rayAabbMin.setMin(rayToWorld);
-	rayAabbMax.setMax(rayToWorld);
-
-
-	/// brute force go over all objects. Once there is a broadphase, use that, or
-	/// add a raycast against aabb first.
 	
 	std::vector<btCollisionObject*>::iterator iter;
 	
@@ -342,11 +334,9 @@ void	btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& r
 		btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
 		collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
 
-		//check aabb overlap
+		float hitLambda = 1.f; //could use resultCallback.m_closestHitFraction, but needs testing
-
-		float hitLambda = 1.f;
 		btVector3 hitNormal;
-		if (btRayAabb(rayAabbMin,rayAabbMax,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
+		if (btRayAabb(rayFromWorld,rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
 		{
 			rayTestSingle(rayFromTrans,rayToTrans,
 				collisionObject,

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

@@ -118,6 +118,43 @@ void	btDiscreteDynamicsWorld::saveKinematicState(float timeStep)
 
 void	btDiscreteDynamicsWorld::synchronizeMotionStates()
 {
+	//debug vehicle wheels
+	
+	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
+	{
+		for (unsigned int i=0;i<this->m_vehicles.size();i++)
+		{
+			for (int v=0;v<m_vehicles[i]->getNumWheels();v++)
+			{
+				btVector3 wheelColor(0,255,255);
+				if (m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_isInContact)
+				{
+					wheelColor.setValue(0,0,255);
+				} else
+				{
+					wheelColor.setValue(255,0,255);
+				}
+
+				//synchronize the wheels with the (interpolated) chassis worldtransform
+				m_vehicles[i]->updateWheelTransform(v);
+					
+				btVector3 wheelPosWS = m_vehicles[i]->getWheelInfo(v).m_worldTransform.getOrigin();
+
+				btVector3 axle = btVector3(	
+					m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[0][m_vehicles[i]->getRightAxis()],
+					m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[1][m_vehicles[i]->getRightAxis()],
+					m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[2][m_vehicles[i]->getRightAxis()]);
+
+
+				//m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS
+				//debug wheels (cylinders)
+				m_debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor);
+				m_debugDrawer->drawLine(wheelPosWS,m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor);
+
+			}
+		}
+	}
+	{
 		//todo: iterate over awake simulation islands!
 		for (unsigned int i=0;i<m_collisionObjects.size();i++)
 		{
@@ -157,6 +194,8 @@ void	btDiscreteDynamicsWorld::synchronizeMotionStates()
 				}
 			}
 		}
+	}
+
 
 }
 

src/LinearMath/btAabbUtil2.h

@@ -66,6 +66,7 @@ SIMD_FORCE_INLINE int	btOutcode(const btVector3& p,const btVector3& halfExtent)
 		   (p.getZ() >  halfExtent.getZ() ? 0x20 : 0x0);
 }
 
+
 SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom, 
 								 const btVector3& rayTo, 
 								 const btVector3& aabbMin,