added some new Bullet files, and upgraded to latest Bullet 2.x

Please make sure to have extern/bullet/src/LinearMath/btAlignedAllocator.cpp in your build, if you add the files by name, instead of wildcard *.cpp

6 files changed, 128 insertions, 138 deletions

diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
index e073797f989..ce90f798c04 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
@@ -18,7 +18,7 @@ subject to the following restrictions:
 
 class btPersistentManifold;
 class btRigidBody;
-
+class btTypedConstraint;
 struct btContactSolverInfo;
 struct btBroadphaseProxy;
 class btIDebugDraw;
@@ -31,7 +31,7 @@ public:
 
 	virtual ~btConstraintSolver() {}
 	
-	virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info,class btIDebugDraw* debugDrawer = 0) = 0;
+	virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer = 0) = 0;
 
 };
 
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
index b5783f824d4..b2132a8d4f3 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
@@ -289,7 +289,7 @@ void	btGeneric6DofConstraint::solveConstraint(btScalar	timeStep)
 				btScalar loLimit =  m_upperLimit[i+3] > -3.1415 ? m_lowerLimit[i+3] : -1e30f;
 				btScalar hiLimit = m_upperLimit[i+3] < 3.1415 ? m_upperLimit[i+3] : 1e30f;
 
-				float projAngle  = -2.*xyz[i];
+				float projAngle  = -2.f*xyz[i];
 				
 				if (projAngle < loLimit)
 				{
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
index 7b2a9ba6244..f72278e2cbf 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
@@ -19,7 +19,8 @@ subject to the following restrictions:
 #include "LinearMath/btTransformUtil.h"
 
 
-btHingeConstraint::btHingeConstraint()
+btHingeConstraint::btHingeConstraint():
+m_enableAngularMotor(false)
 {
 }
 
@@ -28,7 +29,8 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt
 :btTypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
 m_axisInA(axisInA),
 m_axisInB(-axisInB),
-m_angularOnly(false)
+m_angularOnly(false),
+m_enableAngularMotor(false)
 {
 
 }
@@ -39,7 +41,8 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,
 m_axisInA(axisInA),
 //fixed axis in worldspace
 m_axisInB(rbA.getCenterOfMassTransform().getBasis() * -axisInA),
-m_angularOnly(false)
+m_angularOnly(false),
+m_enableAngularMotor(false)
 {
 	
 }
@@ -73,11 +76,16 @@ void	btHingeConstraint::buildJacobian()
 	//these two jointAxis require equal angular velocities for both bodies
 
 	//this is unused for now, it's a todo
-	btVector3 axisWorldA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
-	btVector3 jointAxis0;
-	btVector3 jointAxis1;
-	btPlaneSpace1(axisWorldA,jointAxis0,jointAxis1);
+	btVector3 jointAxis0local;
+	btVector3 jointAxis1local;
 	
+	btPlaneSpace1(m_axisInA,jointAxis0local,jointAxis1local);
+
+	getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
+	btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
+	btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
+	btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
+		
 	new (&m_jacAng[0])	btJacobianEntry(jointAxis0,
 		m_rbA.getCenterOfMassTransform().getBasis().transpose(),
 		m_rbB.getCenterOfMassTransform().getBasis().transpose(),
@@ -90,107 +98,18 @@ void	btHingeConstraint::buildJacobian()
 		m_rbA.getInvInertiaDiagLocal(),
 		m_rbB.getInvInertiaDiagLocal());
 
+	new (&m_jacAng[2])	btJacobianEntry(hingeAxisWorld,
+		m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+		m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+		m_rbA.getInvInertiaDiagLocal(),
+		m_rbB.getInvInertiaDiagLocal());
+
+
 
 }
 
 void	btHingeConstraint::solveConstraint(btScalar	timeStep)
 {
-//#define NEW_IMPLEMENTATION
-
-#ifdef NEW_IMPLEMENTATION
-	btScalar tau = 0.3f;
-	btScalar damping = 1.f;
-
-	btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
-	btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
-
-	// Dirk: Don't we need to update this after each applied impulse
-	btVector3 angvelA; // = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
-    btVector3 angvelB; // = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
-
-
-	if (!m_angularOnly)
-	{
-		btVector3 normal(0,0,0);
-
-		for (int i=0;i<3;i++)
-		{		
-			normal[i] = 1;
-			btScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal();
-
-			btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
-			btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
-			
-			btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
-			btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
-			btVector3 vel = vel1 - vel2;
-
-			// Dirk: Get new angular velocity since it changed after applying an impulse
-			angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
-			angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
-	
-			//velocity error (first order error)
-			btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, 
-																	m_rbB.getLinearVelocity(),angvelB);
-		
-			//positional error (zeroth order error)
-			btScalar depth = -(pivotAInW - pivotBInW).dot(normal); 
-			
-			btScalar impulse = tau*depth/timeStep * jacDiagABInv -  damping * rel_vel * jacDiagABInv;
-
-			btVector3 impulse_vector = normal * impulse;
-			m_rbA.applyImpulse( impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
-			m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
-			
-			normal[i] = 0;
-		}
-	}
-
-	///solve angular part
-
-	// get axes in world space
-	btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
-	btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB;
-
-	// constraint axes in world space
-	btVector3 jointAxis0;
-	btVector3 jointAxis1;
-	btPlaneSpace1(axisA,jointAxis0,jointAxis1);
-
-
-	// Dirk: Get new angular velocity since it changed after applying an impulse
-	angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
-    angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
-	
-	btScalar jacDiagABInv0 = 1.f / m_jacAng[0].getDiagonal();
-	btScalar rel_vel0 = m_jacAng[0].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, 
-																	m_rbB.getLinearVelocity(),angvelB);
-	float tau1 = tau;//0.f;
-
-	btScalar impulse0 = (tau1 * axisB.dot(jointAxis1) / timeStep - damping * rel_vel0) * jacDiagABInv0;
-	btVector3 angular_impulse0 = jointAxis0 * impulse0;
-
-	m_rbA.applyTorqueImpulse( angular_impulse0);
-	m_rbB.applyTorqueImpulse(-angular_impulse0);
-
-
-
-	// Dirk: Get new angular velocity since it changed after applying an impulse	
-	angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
-    angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
-
-	btScalar jacDiagABInv1 = 1.f / m_jacAng[1].getDiagonal();
-	btScalar rel_vel1 = m_jacAng[1].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, 
-																	m_rbB.getLinearVelocity(),angvelB);;
-
-	btScalar impulse1 = -(tau1 * axisB.dot(jointAxis0) / timeStep + damping * rel_vel1) * jacDiagABInv1;
-	btVector3 angular_impulse1 = jointAxis1 * impulse1;
-
-	m_rbA.applyTorqueImpulse( angular_impulse1);
-	m_rbB.applyTorqueImpulse(-angular_impulse1);
-
-#else
-
 
 	btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
 	btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
@@ -237,37 +156,68 @@ void	btHingeConstraint::solveConstraint(btScalar	timeStep)
 
 		const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
 		const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
-		btVector3 angA = angVelA - axisA * axisA.dot(angVelA);
-		btVector3 angB = angVelB - axisB * axisB.dot(angVelB);
-		btVector3 velrel = angA-angB;
-
-		//solve angular velocity correction
-		float relaxation = 1.f;
-		float len = velrel.length();
-		if (len > 0.00001f)
+
+		btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
+		btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
+
+		btVector3 angAorthog = angVelA - angVelAroundHingeAxisA;
+		btVector3 angBorthog = angVelB - angVelAroundHingeAxisB;
+		btVector3 velrelOrthog = angAorthog-angBorthog;
 		{
-			btVector3 normal = velrel.normalized();
-			float denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
-				getRigidBodyB().computeAngularImpulseDenominator(normal);
-			// scale for mass and relaxation
-			velrel *= (1.f/denom) * 0.9;
+			//solve orthogonal angular velocity correction
+			float relaxation = 1.f;
+			float len = velrelOrthog.length();
+			if (len > 0.00001f)
+			{
+				btVector3 normal = velrelOrthog.normalized();
+				float denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
+					getRigidBodyB().computeAngularImpulseDenominator(normal);
+				// scale for mass and relaxation
+				//todo:  expose this 0.9 factor to developer
+				velrelOrthog *= (1.f/denom) * 0.9f;
+			}
+
+			//solve angular positional correction
+			btVector3 angularError = -axisA.cross(axisB) *(1.f/timeStep);
+			float len2 = angularError.length();
+			if (len2>0.00001f)
+			{
+				btVector3 normal2 = angularError.normalized();
+				float denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
+						getRigidBodyB().computeAngularImpulseDenominator(normal2);
+				angularError *= (1.f/denom2) * relaxation;
+			}
+
+			m_rbA.applyTorqueImpulse(-velrelOrthog+angularError);
+			m_rbB.applyTorqueImpulse(velrelOrthog-angularError);
 		}
 
-		//solve angular positional correction
-		btVector3 angularError = -axisA.cross(axisB) *(1.f/timeStep);
-		float len2 = angularError.length();
-		if (len2>0.00001f)
+		//apply motor
+		if (m_enableAngularMotor)
 		{
-			btVector3 normal2 = angularError.normalized();
-			float denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
-					getRigidBodyB().computeAngularImpulseDenominator(normal2);
-			angularError *= (1.f/denom2) * relaxation;
-		}
+			//todo: add limits too
+			btVector3 angularLimit(0,0,0);
+
+			btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
+			btScalar projRelVel = velrel.dot(axisA);
+
+			btScalar desiredMotorVel = m_motorTargetVelocity;
+			btScalar motor_relvel = desiredMotorVel - projRelVel;
 
-		m_rbA.applyTorqueImpulse(-velrel+angularError);
-		m_rbB.applyTorqueImpulse(velrel-angularError);
+			float denom3 = getRigidBodyA().computeAngularImpulseDenominator(axisA) +
+					getRigidBodyB().computeAngularImpulseDenominator(axisA);
+
+			btScalar unclippedMotorImpulse = (1.f/denom3) * motor_relvel;;
+			//todo: should clip against accumulated impulse
+			btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse;
+			clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
+			btVector3 motorImp = clippedMotorImpulse * axisA;
+
+			m_rbA.applyTorqueImpulse(motorImp+angularLimit);
+			m_rbB.applyTorqueImpulse(-motorImp-angularLimit);
+			
+		}
 	}
-#endif
 
 }
 
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
index 3bade2b091d..553ec135c8a 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
@@ -29,14 +29,18 @@ class btRigidBody;
 class btHingeConstraint : public btTypedConstraint
 {
 	btJacobianEntry	m_jac[3]; //3 orthogonal linear constraints
-	btJacobianEntry	m_jacAng[2]; //2 orthogonal angular constraints
+	btJacobianEntry	m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor
 
 	btVector3	m_pivotInA;
 	btVector3	m_pivotInB;
 	btVector3	m_axisInA;
 	btVector3	m_axisInB;
 
-	bool	m_angularOnly;
+	bool		m_angularOnly;
+
+	float		m_motorTargetVelocity;
+	float		m_maxMotorImpulse;
+	bool		m_enableAngularMotor;
 	
 public:
 
@@ -66,7 +70,12 @@ public:
 		m_angularOnly = angularOnly;
 	}
 
-
+	void	enableAngularMotor(bool enableMotor,float targetVelocity,float maxMotorImpulse)
+	{
+		m_enableAngularMotor  = enableMotor;
+		m_motorTargetVelocity = targetVelocity;
+		m_maxMotorImpulse = maxMotorImpulse;
+	}
 
 };
 
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
index ce85fc398be..94eece73e7c 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
@@ -23,6 +23,8 @@ subject to the following restrictions:
 #include "LinearMath/btIDebugDraw.h"
 #include "btJacobianEntry.h"
 #include "LinearMath/btMinMax.h"
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+
 
 #ifdef USE_PROFILE
 #include "LinearMath/btQuickprof.h"
@@ -123,7 +125,7 @@ btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
 }
 
 /// btSequentialImpulseConstraintSolver Sequentially applies impulses
-float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
 {
 	
 	btContactSolverInfo info = infoGlobal;
@@ -151,6 +153,15 @@ float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** ma
 			}
 		}
 	}
+
+	{
+		int j;
+		for (j=0;j<numConstraints;j++)
+		{
+			btTypedConstraint* constraint = constraints[j];
+			constraint->buildJacobian();
+		}
+	}
 	
 	//should traverse the contacts random order...
 	int iteration;
@@ -171,6 +182,12 @@ float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** ma
 				}
 			}
 
+			for (j=0;j<numConstraints;j++)
+			{
+				btTypedConstraint* constraint = constraints[j];
+				constraint->solveConstraint(info.m_timeStep);
+			}
+
 			for (j=0;j<totalPoints;j++)
 			{
 				btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
@@ -197,7 +214,7 @@ float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** ma
 
 
 /// btSequentialImpulseConstraintSolver Sequentially applies impulses
-float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
 {
 	
 	btContactSolverInfo info = infoGlobal;
@@ -222,6 +239,14 @@ float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** man
 			}
 		}
 	}
+	{
+		int j;
+		for (j=0;j<numConstraints;j++)
+		{
+			btTypedConstraint* constraint = constraints[j];
+			constraint->buildJacobian();
+		}
+	}
 	
 	//should traverse the contacts random order...
 	int iteration;
@@ -230,6 +255,12 @@ float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** man
 	{
 		int j;
 
+		for (j=0;j<numConstraints;j++)
+		{
+			btTypedConstraint* constraint = constraints[j];
+			constraint->solveConstraint(info.m_timeStep);
+		}
+
 		for (j=0;j<numManifolds;j++)
 		{
 			btPersistentManifold* manifold = manifoldPtr[j];
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
index ec56af14dac..0989a86e2cd 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
@@ -68,7 +68,7 @@ public:
 
 	virtual ~btSequentialImpulseConstraintSolver() {}
 	
-	virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0);
+	virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0);
 
 	void	setSolverMode(int mode)
 	{
@@ -88,7 +88,7 @@ public:
 
 	btSequentialImpulseConstraintSolver3();
 
-	virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0);
+	virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0);
 
 
 };