- added linear limits to btGeneric6DofConstraint and made sure the linear axis are in local space of objectA

- use microseconds instead of milliseconds for deltatime

12 files changed, 163 insertions, 39 deletions

diff --git a/Demos/BasicDemo/BasicDemo.cpp b/Demos/BasicDemo/BasicDemo.cpp
index 0673e0636..8f4801759 100644
--- a/Demos/BasicDemo/BasicDemo.cpp
+++ b/Demos/BasicDemo/BasicDemo.cpp
@@ -61,14 +61,14 @@ void BasicDemo::clientMoveAndDisplay()
 	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 

 

 	//simple dynamics world doesn't handle fixed-time-stepping

-	float ms = m_clock.getTimeMilliseconds();

+	float ms = m_clock.getTimeMicroseconds();

 	m_clock.reset();

-	float minFPS = 1000.f/60.f;

+	float minFPS = 1000000.f/60.f;

 	if (ms > minFPS)

 		ms = minFPS;

 

 	if (m_dynamicsWorld)

-		m_dynamicsWorld->stepSimulation(ms / 1000.f);

+		m_dynamicsWorld->stepSimulation(ms / 1000000.f);

 		

 	renderme(); 

 

diff --git a/Demos/BspDemo/BspDemo.cpp b/Demos/BspDemo/BspDemo.cpp
index e40cdcd2d..43b514d3e 100644
--- a/Demos/BspDemo/BspDemo.cpp
+++ b/Demos/BspDemo/BspDemo.cpp
@@ -185,7 +185,7 @@ void BspDemo::clientMoveAndDisplay()
 {

 	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 

 

-	float dt = m_clock.getTimeMilliseconds() * 0.001f;

+	float dt = m_clock.getTimeMicroseconds() * 0.000001f;

 	m_clock.reset();

 	m_dynamicsWorld->stepSimulation(dt);

 

diff --git a/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp b/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp
index 27aa231a3..16acde51b 100644
--- a/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp
+++ b/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp
@@ -160,7 +160,7 @@ void CcdPhysicsDemo::clientMoveAndDisplay()
 
 #endif //USE_KINEMATIC_GROUND
 
-	float dt = m_clock.getTimeMilliseconds() * 0.001f;
+	float dt = m_clock.getTimeMicroseconds() * 0.000001f;
 	m_clock.reset();
 	printf("dt = %f: ",dt);
 	
diff --git a/Demos/ColladaDemo/ColladaDemo.cpp b/Demos/ColladaDemo/ColladaDemo.cpp
index 0e7956271..6e0d848ca 100644
--- a/Demos/ColladaDemo/ColladaDemo.cpp
+++ b/Demos/ColladaDemo/ColladaDemo.cpp
@@ -188,7 +188,7 @@ void ColladaDemo::clientMoveAndDisplay()
 {

 	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 

 

-	float dt = m_clock.getTimeMilliseconds() * 0.001f;

+	float dt = m_clock.getTimeMicroseconds() * 0.000001f;

 	m_clock.reset();

 

 	m_dynamicsWorld->stepSimulation(dt);

diff --git a/Demos/ConcaveDemo/ConcavePhysicsDemo.cpp b/Demos/ConcaveDemo/ConcavePhysicsDemo.cpp
index 007d7fa1f..8efcec3d6 100644
--- a/Demos/ConcaveDemo/ConcavePhysicsDemo.cpp
+++ b/Demos/ConcaveDemo/ConcavePhysicsDemo.cpp
@@ -183,7 +183,7 @@ void ConcaveDemo::clientMoveAndDisplay()
 {
 	 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 
 
-	float dt = m_clock.getTimeMilliseconds() * 0.001f;
+	float dt = m_clock.getTimeMicroseconds() * 0.000001f;
 	m_clock.reset();
 	
 	m_dynamicsWorld->stepSimulation(dt);
diff --git a/Demos/ConstraintDemo/ConstraintDemo.cpp b/Demos/ConstraintDemo/ConstraintDemo.cpp
index a2ba68381..6c34f3acc 100644
--- a/Demos/ConstraintDemo/ConstraintDemo.cpp
+++ b/Demos/ConstraintDemo/ConstraintDemo.cpp
@@ -14,6 +14,7 @@ subject to the following restrictions:
 */
 
 
+
 #include "btBulletDynamicsCommon.h"
 #include "LinearMath/btIDebugDraw.h"
 
@@ -45,8 +46,21 @@ int main(int argc,char** argv)
 	return glutmain(argc, argv,640,480,"Constraint Demo. http://www.continuousphysics.com/Bullet/phpBB2/",constraintDemo);
 }
 
+btTransform sliderTransform;
+btVector3 lowerSliderLimit = btVector3(-20,0,0);
+btVector3 hiSliderLimit = btVector3(10,0,0);
 
-
+void	drawLimit()
+{
+		btVector3 from = sliderTransform*lowerSliderLimit;
+		btVector3 to = sliderTransform*hiSliderLimit;
+		btVector3 color(255,0,0);
+		glBegin(GL_LINES);
+		glColor3f(color.getX(), color.getY(), color.getZ());
+		glVertex3d(from.getX(), from.getY(), from.getZ());
+		glVertex3d(to.getX(), to.getY(), to.getZ());
+		glEnd();
+}
 
 void	ConstraintDemo::initPhysics()
 {
@@ -56,6 +70,7 @@ void	ConstraintDemo::initPhysics()
 	//btOverlappingPairCache* broadphase = new btSimpleBroadphase();
 
 	m_dynamicsWorld = new btDiscreteDynamicsWorld();
+	m_dynamicsWorld->setDebugDrawer(&debugDrawer);
 
 	btCollisionShape* shape = new btBoxShape(btVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS));
 	btTransform trans;
@@ -63,17 +78,16 @@ void	ConstraintDemo::initPhysics()
 	trans.setOrigin(btVector3(0,20,0));
 
 	float mass = 0.f;
-	btRigidBody* body0 = localCreateRigidBody( mass,trans,shape);
-	trans.setOrigin(btVector3(2*CUBE_HALF_EXTENTS,20,0));
+	//point to point constraint (ball socket)
+	{
+		btRigidBody* body0 = localCreateRigidBody( mass,trans,shape);
+		trans.setOrigin(btVector3(2*CUBE_HALF_EXTENTS,20,0));
 
-	mass = 1.f;
-	btRigidBody* body1 = localCreateRigidBody( mass,trans,shape);
-	body1->setDamping(0.3,0.3);
+		mass = 1.f;
+		btRigidBody* body1 = localCreateRigidBody( mass,trans,shape);
+		body1->setActivationState(DISABLE_DEACTIVATION);
+		body1->setDamping(0.3,0.3);
 
-	
-	clientResetScene();
-
-	{
 		btVector3 pivotInA(CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS);
 		btVector3 axisInA(0,0,1);
 
@@ -87,21 +101,80 @@ void	ConstraintDemo::initPhysics()
 		m_dynamicsWorld->addConstraint(p2p);
 
 	}
+
+	
+
+	//create a slider, using the generic D6 constraint
+	{
+		mass = 1.f;
+		btVector3 sliderWorldPos(0,10,0);
+		btVector3 sliderAxis(0,0,1);
+		btScalar angle=SIMD_RADS_PER_DEG * 10.f;
+		btMatrix3x3 sliderOrientation(btQuaternion(sliderAxis ,angle));
+		trans.setOrigin(sliderWorldPos);
+		trans.setBasis(sliderOrientation);
+		sliderTransform = trans;
+
+		btRigidBody* body0 = localCreateRigidBody( mass,trans,shape);
+		body0->setActivationState(DISABLE_DEACTIVATION);
+		btRigidBody* fixedBody1 = localCreateRigidBody(0,trans,0);
+
+		btTransform frameInA, frameInB;
+		frameInA = btTransform::getIdentity();
+		frameInB = btTransform::getIdentity();
+		
+		btGeneric6DofConstraint* slider = new btGeneric6DofConstraint(*body0,*fixedBody1,frameInA,frameInB);
+		slider->setLinearLowerLimit(lowerSliderLimit);
+		slider->setLinearUpperLimit(hiSliderLimit);
+		slider->setAngularLowerLimit(btVector3(1e30,0,0));
+		slider->setAngularUpperLimit(btVector3(-1e30,0,0));
+
+		m_dynamicsWorld->addConstraint(slider);
+
+	}
+
 }
 
 
 void ConstraintDemo::clientMoveAndDisplay()
 {
 	
-	 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 
+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 
 
-	float dt = m_clock.getTimeMilliseconds() * 0.001f;
+ 	float dt = float(m_clock.getTimeMicroseconds()) * 0.000001f;
 	m_clock.reset();
 
-
-	m_dynamicsWorld->stepSimulation(dt);
+	//printf("dt = %f: ",dt);
+	
+ {
+	 	//during idle mode, just run 1 simulation step maximum
+		int maxSimSubSteps = m_idle ? 1 : 1;
+		if (m_idle)
+			dt = 1.0/420.f;
+
+		int numSimSteps = m_dynamicsWorld->stepSimulation(dt,maxSimSubSteps);
+		bool verbose = false;
+		if (verbose)
+		{
+			if (!numSimSteps)
+				printf("Interpolated transforms\n");
+			else
+			{
+				if (numSimSteps > maxSimSubSteps)
+				{
+					//detect dropping frames
+					printf("Dropped (%i) simulation steps out of %i\n",numSimSteps - maxSimSubSteps,numSimSteps);
+				} else
+				{
+					printf("Simulated (%i) steps\n",numSimSteps);
+				}
+			}
+		}
+ }
 	renderme();
 
+	drawLimit();
+
     glFlush();
     glutSwapBuffers();
 }
@@ -113,6 +186,11 @@ void ConstraintDemo::displayCallback(void) {
 
     glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 
 
+	
+	m_dynamicsWorld->updateAabbs();
+	
+	drawLimit();
+
 	renderme();
 
     glFlush();
diff --git a/Demos/ConvexDecompositionDemo/ConvexDecompositionDemo.cpp b/Demos/ConvexDecompositionDemo/ConvexDecompositionDemo.cpp
index 00c878d35..da1296fe4 100644
--- a/Demos/ConvexDecompositionDemo/ConvexDecompositionDemo.cpp
+++ b/Demos/ConvexDecompositionDemo/ConvexDecompositionDemo.cpp
@@ -294,7 +294,7 @@ void ConvexDecompositionDemo::clientMoveAndDisplay()
 {

 	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 

 

-	float dt = m_clock.getTimeMilliseconds() * 0.001f;

+	float dt = m_clock.getTimeMicroseconds() * 0.000001f;

 	m_clock.reset();

 

 	m_dynamicsWorld->stepSimulation(dt);

diff --git a/Demos/OpenGL/DemoApplication.cpp b/Demos/OpenGL/DemoApplication.cpp
index 2a711fe86..3055afe96 100644
--- a/Demos/OpenGL/DemoApplication.cpp
+++ b/Demos/OpenGL/DemoApplication.cpp
@@ -822,6 +822,10 @@ void DemoApplication::renderme()
 

 void	DemoApplication::clientResetScene()

 {

+	if (m_dynamicsWorld)

+	{

+		m_dynamicsWorld->stepSimulation(1.f/60.f,0);

+	}

 	int numObjects = m_dynamicsWorld->getNumCollisionObjects();

 	

 	for (int i=0;i<numObjects;i++)

diff --git a/Demos/UserCollisionAlgorithm/UserCollisionAlgorithm.cpp b/Demos/UserCollisionAlgorithm/UserCollisionAlgorithm.cpp
index 8a87fed98..d22ab4d29 100644
--- a/Demos/UserCollisionAlgorithm/UserCollisionAlgorithm.cpp
+++ b/Demos/UserCollisionAlgorithm/UserCollisionAlgorithm.cpp
@@ -142,7 +142,7 @@ void UserCollisionAlgorithm::clientMoveAndDisplay()
 	 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 

 

 	

-	float dt = m_clock.getTimeMilliseconds() * 0.001f;

+	float dt = m_clock.getTimeMicroseconds() * 0.000001f;

 	m_clock.reset();

 	

 

diff --git a/Demos/VehicleDemo/VehicleDemo.cpp b/Demos/VehicleDemo/VehicleDemo.cpp
index 3f7c16f32..afbcf01aa 100644
--- a/Demos/VehicleDemo/VehicleDemo.cpp
+++ b/Demos/VehicleDemo/VehicleDemo.cpp
@@ -247,7 +247,7 @@ void VehicleDemo::clientMoveAndDisplay()
 

 	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 

 

-	float dt = m_clock.getTimeMilliseconds() * 0.001f;

+	float dt = m_clock.getTimeMicroseconds() * 0.000001f;

 	m_clock.reset();

 		

 	if (m_dynamicsWorld)

diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
index a8ab1bbad..be6afe0e1 100644
--- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
@@ -21,6 +21,7 @@ subject to the following restrictions:
 static const btScalar kSign[] = { 1.0f, -1.0f, 1.0f };
 static const int kAxisA[] = { 1, 0, 0 };
 static const int kAxisB[] = { 2, 2, 1 };
+#define GENERIC_D6_DISABLE_WARMSTARTING 1
 
 btGeneric6DofConstraint::btGeneric6DofConstraint()
 {
@@ -47,7 +48,7 @@ btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody&
 
 void btGeneric6DofConstraint::buildJacobian()
 {
-	btVector3	normal(0,0,0);
+	btVector3	localNormalInA(0,0,0);
 
 	const btVector3& pivotInA = m_frameInA.getOrigin();
 	const btVector3& pivotInB = m_frameInB.getOrigin();
@@ -64,7 +65,9 @@ void btGeneric6DofConstraint::buildJacobian()
 	{
 		if (isLimited(i))
 		{
-			normal[i] = 1;
+			localNormalInA[i] = 1;
+			btVector3 normalWorld = m_rbA.getCenterOfMassTransform().getBasis() * localNormalInA;
+
 			
 			// Create linear atom
 			new (&m_jacLinear[i]) btJacobianEntry(
@@ -72,19 +75,24 @@ void btGeneric6DofConstraint::buildJacobian()
 				m_rbB.getCenterOfMassTransform().getBasis().transpose(),
 				m_rbA.getCenterOfMassTransform()*pivotInA - m_rbA.getCenterOfMassPosition(),
 				m_rbB.getCenterOfMassTransform()*pivotInB - m_rbB.getCenterOfMassPosition(),
-				normal,
+				normalWorld,
 				m_rbA.getInvInertiaDiagLocal(),
 				m_rbA.getInvMass(),
 				m_rbB.getInvInertiaDiagLocal(),
 				m_rbB.getInvMass());
 
+			//optionally disable warmstarting
+#ifdef GENERIC_D6_DISABLE_WARMSTARTING
+			m_accumulatedImpulse[i] = 0.f;
+#endif //GENERIC_D6_DISABLE_WARMSTARTING
+
 			// Apply accumulated impulse
-			btVector3 impulse_vector = m_accumulatedImpulse[i] * normal;
+			btVector3 impulse_vector = m_accumulatedImpulse[i] * normalWorld;
 
 			m_rbA.applyImpulse( impulse_vector, rel_pos1);
 			m_rbB.applyImpulse(-impulse_vector, rel_pos2);
 
-			normal[i] = 0;
+			localNormalInA[i] = 0;
 		}
 	}
 
@@ -106,6 +114,10 @@ void btGeneric6DofConstraint::buildJacobian()
 				m_rbA.getInvInertiaDiagLocal(),
 				m_rbB.getInvInertiaDiagLocal());
 
+#ifdef GENERIC_D6_DISABLE_WARMSTARTING
+			m_accumulatedImpulse[i + 3] = 0.f;
+#endif //GENERIC_D6_DISABLE_WARMSTARTING
+
 			// Apply accumulated impulse
 			btVector3 impulse_vector = m_accumulatedImpulse[i + 3] * axis;
 
@@ -126,7 +138,7 @@ void	btGeneric6DofConstraint::solveConstraint(btScalar	timeStep)
 	btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
 	btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
 	
-	btVector3 normal(0,0,0);
+	btVector3 localNormalInA(0,0,0);
 	int i;
 
 	// linear
@@ -137,8 +149,10 @@ void	btGeneric6DofConstraint::solveConstraint(btScalar	timeStep)
 			btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
 			btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
 		
+			localNormalInA.setValue(0,0,0);
+			localNormalInA[i] = 1;
+			btVector3 normalWorld = m_rbA.getCenterOfMassTransform().getBasis() * localNormalInA;
 
-			normal[i] = 1;
 			btScalar jacDiagABInv = 1.f / m_jacLinear[i].getDiagonal();
 
 			//velocity error (first order error)
@@ -146,16 +160,37 @@ void	btGeneric6DofConstraint::solveConstraint(btScalar	timeStep)
 																	m_rbB.getLinearVelocity(),angvelB);
 		
 			//positional error (zeroth order error)
-			btScalar depth = -(pivotAInW - pivotBInW).dot(normal); 
+			btScalar depth = -(pivotAInW - pivotBInW).dot(normalWorld); 
+
+			//handle the limits
+			if (m_lowerLimit[i] < m_upperLimit[i])
+			{
+				{
+					if (depth > m_upperLimit[i])
+					{
+						depth -= m_upperLimit[i];
+					} else
+					{
+						if (depth < m_lowerLimit[i])
+						{
+							depth -= m_lowerLimit[i];
+						} else
+						{
+							continue;
+						}
+					}
+				}
+			}
 			
 			btScalar impulse = (tau*depth/timeStep - damping*rel_vel) * jacDiagABInv;
+			
 			m_accumulatedImpulse[i] += impulse;
 
-			btVector3 impulse_vector = normal * impulse;
+			btVector3 impulse_vector = normalWorld * impulse;
 			m_rbA.applyImpulse( impulse_vector, rel_pos1);
 			m_rbB.applyImpulse(-impulse_vector, rel_pos2);
 			
-			normal[i] = 0;
+			localNormalInA[i] = 0;
 		}
 	}
 
diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
index e5ce5141b..d605198c6 100644
--- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
+++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
@@ -269,12 +269,19 @@ void	btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body)
 

 void	btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body)

 {

-	body->setGravity(m_gravity);

-	bool isDynamic = !(body->isStaticObject() || body->isKinematicObject());

-	short collisionFilterGroup = isDynamic? btBroadphaseProxy::DefaultFilter : btBroadphaseProxy::StaticFilter;

-	short collisionFilterMask = isDynamic? 	btBroadphaseProxy::AllFilter : 	btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter;

+	if (!body->isStaticOrKinematicObject())

+	{

+		body->setGravity(m_gravity);

+	}

 

-	addCollisionObject(body,collisionFilterGroup,collisionFilterMask);

+	if (body->getCollisionShape())

+	{

+		bool isDynamic = !(body->isStaticObject() || body->isKinematicObject());

+		short collisionFilterGroup = isDynamic? btBroadphaseProxy::DefaultFilter : btBroadphaseProxy::StaticFilter;

+		short collisionFilterMask = isDynamic? 	btBroadphaseProxy::AllFilter : 	btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter;

+

+		addCollisionObject(body,collisionFilterGroup,collisionFilterMask);

+	}

 }