+ need to reset rigid body using 'setCenterOfMassTransform' to reset world inertia tensor

+ fixes in compound algorithm -> recompute contact points
+ add debug drawing to some demos.
+ revert btJacobianEntry change
+ replace dCROSSMAT by btVector3::getSkewSymmetricMatrix

Demos/ForkLiftDemo/main.cpp

@@ -1,7 +1,9 @@
 
 #include "ForkLiftDemo.h"
 #include "GlutStuff.h"
-
+#include "GLDebugDrawer.h"
+#include "btBulletDynamicsCommon.h"
+GLDebugDrawer	gDebugDrawer;
 
 int main(int argc,char** argv)
 {
@@ -9,6 +11,7 @@ int main(int argc,char** argv)
         ForkLiftDemo* pForkLiftDemo = new ForkLiftDemo;
 
         pForkLiftDemo->initPhysics(); 
+		pForkLiftDemo->getDynamicsWorld()->setDebugDrawer(&gDebugDrawer);
 
         return glutmain(argc, argv,640,480,"Bullet ForkLift Demo. http://www.continuousphysics.com/Bullet/phpBB2/", pForkLiftDemo);
 }

Demos/OpenGL/DemoApplication.cpp

@@ -608,7 +608,7 @@ btVector3	DemoApplication::getRayTo(int x,int y)
 	return rayTo;
 }
 
-btScalar mousePickClamping = 3.f;
+btScalar mousePickClamping = 30.f;
 
 
 void DemoApplication::mouseFunc(int button, int state, int x, int y)
@@ -1249,7 +1249,7 @@ void	DemoApplication::clientResetScene()
 			{
 				btDefaultMotionState* myMotionState = (btDefaultMotionState*)body->getMotionState();
 				myMotionState->m_graphicsWorldTrans = myMotionState->m_startWorldTrans;
-				colObj->setWorldTransform( myMotionState->m_graphicsWorldTrans );
+				body->setCenterOfMassTransform( myMotionState->m_graphicsWorldTrans );
 				colObj->setInterpolationWorldTransform( myMotionState->m_startWorldTrans );
 				colObj->activate();
 				//colObj->setActivationState(WANTS_DEACTIVATION);

Demos/RagdollDemo/main.cpp

@@ -1,3 +1,17 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2008 Erwin Coumans  http://bulletphysics.com
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
 
 #include "RagdollDemo.h"
 #include "GlutStuff.h"

Demos/SliderConstraintDemo/main.cpp

@@ -5,13 +5,18 @@ April 04, 2008
 #include "SliderConstraintDemo.h"
 #include "GlutStuff.h"
 
+#include "GLDebugDrawer.h"
+#include "btBulletDynamicsCommon.h"
+
+GLDebugDrawer	gDebugDrawer;
+
 int main(int argc,char** argv)
 {
 
         SliderConstraintDemo* sliderConstraintDemo = new SliderConstraintDemo();
 
         sliderConstraintDemo->initPhysics();
-
+		sliderConstraintDemo->getDynamicsWorld()->setDebugDrawer(&gDebugDrawer);
        
 
         return glutmain(argc, argv,640,480,"Slider Constraint Demo. http://www.continuousphysics.com/Bullet/phpBB2/", sliderConstraintDemo);

Demos/VehicleDemo/main.cpp

@@ -1,7 +1,9 @@
 
 #include "VehicleDemo.h"
 #include "GlutStuff.h"
-
+#include "GLDebugDrawer.h"
+#include "btBulletDynamicsCommon.h"
+GLDebugDrawer	gDebugDrawer;
 
 int main(int argc,char** argv)
 {
@@ -9,6 +11,7 @@ int main(int argc,char** argv)
         VehicleDemo* vehicleDemo = new VehicleDemo;
 
         vehicleDemo->initPhysics(); 
+		vehicleDemo->getDynamicsWorld()->setDebugDrawer(&gDebugDrawer);
 
         return glutmain(argc, argv,640,480,"Bullet Vehicle Demo. http://www.continuousphysics.com/Bullet/phpBB2/", vehicleDemo);
 }

src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp

@@ -19,6 +19,7 @@ subject to the following restrictions:
 #include "BulletCollision/BroadphaseCollision/btDbvt.h"
 #include "LinearMath/btIDebugDraw.h"
 #include "LinearMath/btAabbUtil2.h"
+#include "btManifoldResult.h"
 
 btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped)
 :btActivatingCollisionAlgorithm(ci,body0,body1),
@@ -174,6 +175,31 @@ void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,bt
 	//use a dynamic aabb tree to cull potential child-overlaps
 	btCompoundLeafCallback  callback(colObj,otherObj,m_dispatcher,dispatchInfo,resultOut,&m_childCollisionAlgorithms[0],m_sharedManifold);
 
+	///we need to refresh all contact manifolds
+	///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep
+	///so we should add a 'refreshManifolds' in the btCollisionAlgorithm
+	{
+		int numChildren = m_childCollisionAlgorithms.size();
+		int i;
+		btManifoldArray manifoldArray;
+		for (i=0;i<m_childCollisionAlgorithms.size();i++)
+		{
+			if (m_childCollisionAlgorithms[i])
+			{
+				m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray);
+				for (int m=0;m<manifoldArray.size();m++)
+				{
+					if (manifoldArray[m]->getNumContacts())
+					{
+						resultOut->setPersistentManifold(manifoldArray[m]);
+						resultOut->refreshContactPoints();
+						resultOut->setPersistentManifold(0);//??necessary?
+					}
+				}
+				manifoldArray.clear();
+			}
+		}
+	}
 
 	if (tree)
 	{

src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp

@@ -118,13 +118,15 @@ void	btConeTwistConstraint::buildJacobian()
 			for (int i=0;i<3;i++)
 			{
 				new (&m_jac[i]) btJacobianEntry(
-					pivotAInW - m_rbA.getCenterOfMassPosition(),
+				m_rbA.getCenterOfMassTransform().getBasis().transpose(),
-					pivotBInW - m_rbB.getCenterOfMassPosition(),
+				m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-					normal[i],
+				pivotAInW - m_rbA.getCenterOfMassPosition(),
-					m_rbA.getInvInertiaDiagLocal(),
+				pivotBInW - m_rbB.getCenterOfMassPosition(),
-					m_rbA.getInvMass(),
+				normal[i],
-					m_rbB.getInvInertiaDiagLocal(),
+				m_rbA.getInvInertiaDiagLocal(),
-					m_rbB.getInvMass());
+				m_rbA.getInvMass(),
+				m_rbB.getInvInertiaDiagLocal(),
+				m_rbB.getInvMass());
 			}
 		}
 

src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp

@@ -52,7 +52,8 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
 	btVector3 vel = vel1 - vel2;
 	
 
-	  btJacobianEntry jac(
+	   btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
+		body2.getCenterOfMassTransform().getBasis().transpose(),
 		rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
 		body2.getInvInertiaDiagLocal(),body2.getInvMass());
 

src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp

@@ -44,17 +44,6 @@ m_useSolveConstraintObsolete(true)
 }
 
 
-
-#define dCROSSMAT(A,a,skip,plus,minus) \
-{ \
-  (A)[1] = minus (a)[2]; \
-  (A)[2] = plus (a)[1]; \
-  (A)[(skip)+0] = plus (a)[2]; \
-  (A)[(skip)+2] = minus (a)[0]; \
-  (A)[2*(skip)+0] = minus (a)[1]; \
-  (A)[2*(skip)+1] = plus (a)[0]; \
-} 
-
 #define GENERIC_D6_DISABLE_WARMSTARTING 1
 
 btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
@@ -374,25 +363,25 @@ void btGeneric6DofConstraint::buildLinearJacobian(
 	const btVector3 & pivotAInW,const btVector3 & pivotBInW)
 {
 	new (&jacLinear) btJacobianEntry(
-
+        m_rbA.getCenterOfMassTransform().getBasis().transpose(),
-		pivotAInW - m_rbA.getCenterOfMassPosition(),
+        m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-		pivotBInW - m_rbB.getCenterOfMassPosition(),
+        pivotAInW - m_rbA.getCenterOfMassPosition(),
-		normalWorld,
+        pivotBInW - m_rbB.getCenterOfMassPosition(),
-		m_rbA.getInvInertiaDiagLocal(),
+        normalWorld,
-		m_rbA.getInvMass(),
+        m_rbA.getInvInertiaDiagLocal(),
-		m_rbB.getInvInertiaDiagLocal(),
+        m_rbA.getInvMass(),
-		m_rbB.getInvMass());
+        m_rbB.getInvInertiaDiagLocal(),
-
+        m_rbB.getInvMass());
 }
 
 void btGeneric6DofConstraint::buildAngularJacobian(
 	btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
 {
-	new (&jacAngular)	btJacobianEntry(jointAxisW,
+	 new (&jacAngular)	btJacobianEntry(jointAxisW,
-		m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+                                      m_rbA.getCenterOfMassTransform().getBasis().transpose(),
-		m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+                                      m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-		m_rbA.getInvInertiaDiagLocal(),
+                                      m_rbA.getInvInertiaDiagLocal(),
-		m_rbB.getInvInertiaDiagLocal());
+                                      m_rbB.getInvInertiaDiagLocal());
 
 }
 
@@ -517,12 +506,34 @@ int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info)
 	info->m_J1linearAxis[s+1] = 1;
 	info->m_J1linearAxis[2*s+2] = 1;
 
+	/*for (int i=0;i<3;i++)
+	{
+		if (m_useLinearReferenceFrameA)
+		{
+			btVector3* linear_axis = (btVector3* )&info->m_J1linearAxis[s*i];
+			*linear_axis = m_calculatedTransformA.getBasis().getColumn(i);
+		}
+		else
+		{
+			btVector3* linear_axis = (btVector3* )&info->m_J1linearAxis[s*i];
+			*linear_axis = m_calculatedTransformB.getBasis().getColumn(i);
+		}
+	}
+	*/
+
+
 
 	btVector3 a1,a2;
 
 	a1 = body0_trans.getBasis()*d6constraint->getFrameOffsetA().getOrigin();
-	//dMULTIPLY0_331 (a1, body0_mat,m_constraint->m_pivotInA);
+
-	dCROSSMAT (info->m_J1angularAxis,a1,s,-,+);
+	{
+		btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
+		btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
+		btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
+		btVector3 a1neg = -a1;
+		a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+	}
 
 	/*info->m_J2linearAxis[0] = -1;
 	info->m_J2linearAxis[s+1] = -1;
@@ -530,15 +541,19 @@ int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info)
 	*/
 
 	a2 = body1_trans.getBasis()*d6constraint->getFrameOffsetB().getOrigin();
-	//dMULTIPLY0_331 (a2,body1_mat,m_constraint->m_pivotInB);
+
-	dCROSSMAT (info->m_J2angularAxis,a2,s,+,-);
+	{
+		btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
+		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
+		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
+		a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
+	}
 
 	// set right hand side
 	btScalar k = info->fps * info->erp;
 	for (int j=0; j<3; j++)
 	{
-		info->m_constraintError[s*j] = k * (a2[j] + body1_trans.getOrigin()[j] -
+		info->m_constraintError[s*j] = k * (a2[j] + body1_trans.getOrigin()[j] -	a1[j] - body0_trans.getOrigin()[j]);
-			a1[j] - body0_trans.getOrigin()[j]);
 	}
 
 	return 3;

src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp

@@ -189,13 +189,15 @@ void	btHingeConstraint::buildJacobian()
 			for (int i=0;i<3;i++)
 			{
 				new (&m_jac[i]) btJacobianEntry(
-					pivotAInW - m_rbA.getCenterOfMassPosition(),
+				m_rbA.getCenterOfMassTransform().getBasis().transpose(),
-					pivotBInW - m_rbB.getCenterOfMassPosition(),
+				m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-					normal[i],
+				pivotAInW - m_rbA.getCenterOfMassPosition(),
-					m_rbA.getInvInertiaDiagLocal(),
+				pivotBInW - m_rbB.getCenterOfMassPosition(),
-					m_rbA.getInvMass(),
+				normal[i],
-					m_rbB.getInvInertiaDiagLocal(),
+				m_rbA.getInvInertiaDiagLocal(),
-					m_rbB.getInvMass());
+				m_rbA.getInvMass(),
+				m_rbB.getInvInertiaDiagLocal(),
+				m_rbB.getInvMass());
 			}
 		}
 

src/BulletDynamics/ConstraintSolver/btJacobianEntry.h

@@ -34,6 +34,8 @@ public:
 	btJacobianEntry() {};
 	//constraint between two different rigidbodies
 	btJacobianEntry(
+		const btMatrix3x3& world2A,
+		const btMatrix3x3& world2B,
 		const btVector3& rel_pos1,const btVector3& rel_pos2,
 		const btVector3& jointAxis,
 		const btVector3& inertiaInvA, 
@@ -42,8 +44,8 @@ public:
 		const btScalar massInvB)
 		:m_linearJointAxis(jointAxis)
 	{
-		m_aJ = (rel_pos1.cross(m_linearJointAxis));
+		m_aJ = world2A*(rel_pos1.cross(m_linearJointAxis));
-		m_bJ = (rel_pos2.cross(-m_linearJointAxis));
+		m_bJ = world2B*(rel_pos2.cross(-m_linearJointAxis));
 		m_0MinvJt	= inertiaInvA * m_aJ;
 		m_1MinvJt = inertiaInvB * m_bJ;
 		m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ);
@@ -59,8 +61,8 @@ public:
 		const btVector3& inertiaInvB)
 		:m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.)))
 	{
-		m_aJ= jointAxis;
+		m_aJ= world2A*jointAxis;
-		m_bJ = -jointAxis;
+		m_bJ = world2B*-jointAxis;
 		m_0MinvJt	= inertiaInvA * m_aJ;
 		m_1MinvJt = inertiaInvB * m_bJ;
 		m_Adiag =  m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
@@ -93,8 +95,8 @@ public:
 		const btScalar massInvA)
 		:m_linearJointAxis(jointAxis)
 	{
-		m_aJ= (rel_pos1.cross(jointAxis));
+		m_aJ= world2A*(rel_pos1.cross(jointAxis));
-		m_bJ = (rel_pos2.cross(-jointAxis));
+		m_bJ = world2A*(rel_pos2.cross(-jointAxis));
 		m_0MinvJt	= inertiaInvA * m_aJ;
 		m_1MinvJt = btVector3(btScalar(0.),btScalar(0.),btScalar(0.));
 		m_Adiag = massInvA + m_0MinvJt.dot(m_aJ);
@@ -128,7 +130,7 @@ public:
 		return sum[0]+sum[1]+sum[2];
 	}
 
-	btScalar getRelativeVelocity(const btVector3& linvelA,const btVector3& angvelA,const btVector3& linvelB,const btVector3& angvelB) const
+	btScalar getRelativeVelocity(const btVector3& linvelA,const btVector3& angvelA,const btVector3& linvelB,const btVector3& angvelB)
 	{
 		btVector3 linrel = linvelA - linvelB;
 		btVector3 angvela  = angvelA * m_aJ;

src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp

@@ -43,7 +43,7 @@ m_useSolveConstraintObsolete(false)
 
 void	btPoint2PointConstraint::buildJacobian()
 {
-	if (m_useSolveConstraintObsolete)
+	///we need it for both methods
 	{
 		m_appliedImpulse = btScalar(0.);
 
@@ -53,15 +53,16 @@ void	btPoint2PointConstraint::buildJacobian()
 		{
 			normal[i] = 1;
 			new (&m_jac[i]) btJacobianEntry(
-
+			m_rbA.getCenterOfMassTransform().getBasis().transpose(),
-				m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
+			m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-				m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
+			m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
-				normal,
+			m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
-				m_rbA.getInvInertiaDiagLocal(),
+			normal,
-				m_rbA.getInvMass(),
+			m_rbA.getInvInertiaDiagLocal(),
-				m_rbB.getInvInertiaDiagLocal(),
+			m_rbA.getInvMass(),
-				m_rbB.getInvMass());
+			m_rbB.getInvInertiaDiagLocal(),
-			normal[i] = 0;
+			m_rbB.getInvMass());
+		normal[i] = 0;
 		}
 	}
 
@@ -80,16 +81,6 @@ void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info)
 		info->nub = 3;
 	}
 }
-#define dCROSSMAT(A,a,skip,plus,minus) \
-{ \
-  (A)[1] = minus (a)[2]; \
-  (A)[2] = plus (a)[1]; \
-  (A)[(skip)+0] = plus (a)[2]; \
-  (A)[(skip)+2] = minus (a)[0]; \
-  (A)[2*(skip)+0] = minus (a)[1]; \
-  (A)[2*(skip)+1] = plus (a)[0]; \
-} 
-#include <stdio.h>
 
 void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info)
 {
@@ -102,48 +93,55 @@ void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info)
 	body1_trans = m_rbB.getCenterOfMassTransform();
 
 	// anchor points in global coordinates with respect to body PORs.
-    int s = info->rowskip;
+   
-
     // set jacobian
     info->m_J1linearAxis[0] = 1;
-    info->m_J1linearAxis[s+1] = 1;
+    info->m_J1linearAxis[info->rowskip+1] = 1;
-    info->m_J1linearAxis[2*s+2] = 1;
+    info->m_J1linearAxis[2*info->rowskip+2] = 1;
 
-    btVector3 a1,a2;
+	btVector3 a1 = body0_trans.getBasis()*getPivotInA();
-
+	{
-    a1 = body0_trans.getBasis()*getPivotInA();
+		btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
-    //dMULTIPLY0_331 (a1, body0_mat,m_constraint->m_pivotInA);
+		btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
-    dCROSSMAT (info->m_J1angularAxis,a1,s,-,+);
+		btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
+		btVector3 a1neg = -a1;
+		a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+	}
     
 	/*info->m_J2linearAxis[0] = -1;
     info->m_J2linearAxis[s+1] = -1;
     info->m_J2linearAxis[2*s+2] = -1;
 	*/
-
+	
-    a2 = body1_trans.getBasis()*getPivotInB();
+	btVector3 a2 = body1_trans.getBasis()*getPivotInB();
-	//dMULTIPLY0_331 (a2,body1_mat,m_constraint->m_pivotInB);
+   
-    //dCROSSMAT (info->m_J2angularAxis,a2,s,+,-);
+	{
-	dCROSSMAT (info->m_J2angularAxis,a2,s,+,-);
+		btVector3 a2n = -a2;
-
+		btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
+		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
+		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
+		a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
+	}
     
 
+
     // set right hand side
     btScalar k = info->fps * info->erp;
     int j;
 
 	for (j=0; j<3; j++)
     {
-        info->m_constraintError[j*s] = k * (a2[j] + body1_trans.getOrigin()[j] -                     a1[j] - body0_trans.getOrigin()[j]);
+        info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] -                     a1[j] - body0_trans.getOrigin()[j]);
 		//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
     }
 
-	btScalar impulseClamp = m_setting.m_impulseClamp;
+	btScalar impulseClamp = m_setting.m_impulseClamp;//
 	for (j=0; j<3; j++)
     {
-		if (impulseClamp > 0)
+		if (m_setting.m_impulseClamp > 0)
 		{
-			info->m_lowerLimit[j*s] = -impulseClamp;
+			info->m_lowerLimit[j*info->rowskip] = -impulseClamp;
-			info->m_upperLimit[j*s] = impulseClamp;
+			info->m_upperLimit[j*info->rowskip] = impulseClamp;
 		}
 	}
 	
@@ -190,24 +188,33 @@ void	btPoint2PointConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolv
 			//positional error (zeroth order error)
 			btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
 			
-			btScalar impulse = depth*m_setting.m_tau/timeStep  * jacDiagABInv -  m_setting.m_damping * rel_vel * jacDiagABInv;
+			btScalar deltaImpulse = depth*m_setting.m_tau/timeStep  * jacDiagABInv -  m_setting.m_damping * rel_vel * jacDiagABInv;
 
 			btScalar impulseClamp = m_setting.m_impulseClamp;
-			if (impulseClamp > 0)
+			
+			const btScalar sum = btScalar(m_appliedImpulse) + deltaImpulse;
+			if (sum < -impulseClamp)
 			{
-				if (impulse < -impulseClamp) 
+				deltaImpulse = -impulseClamp-m_appliedImpulse;
-					impulse = -impulseClamp;
+				m_appliedImpulse = -impulseClamp;
-				if (impulse > impulseClamp) 
+			}
-					impulse = impulseClamp;
+			else if (sum > impulseClamp) 
+			{
+				deltaImpulse = impulseClamp-m_appliedImpulse;
+				m_appliedImpulse = impulseClamp;
+			}
+			else
+			{
+				m_appliedImpulse = sum;
 			}
 
-			m_appliedImpulse+=impulse;
+			
-			btVector3 impulse_vector = normal * impulse;
+			btVector3 impulse_vector = normal * deltaImpulse;
 			
 			btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
 			btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
-			bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
+			bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,deltaImpulse);
-			bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
+			bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-deltaImpulse);
 
 
 			normal[i] = 0;

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -382,6 +382,16 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 		return 0.f;
 	}
 	
+	if (1)
+	{
+		int j;
+		for (j=0;j<numConstraints;j++)
+		{
+			btTypedConstraint* constraint = constraints[j];
+			constraint->buildJacobian();
+		}
+	}
+
 	btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
 	initSolverBody(&fixedBody,0);
 
@@ -406,10 +416,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 			btTypedConstraint::btConstraintInfo1 info1;
 			info1.m_numConstraintRows = 0;
 			
-			if (m_tmpSolverNonContactConstraintPool.size()>3)
+			
-			{
-				printf("dsadas\n");
-			}
 			///setup the btSolverConstraints
 			int currentRow = 0;
 
@@ -492,7 +499,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 							sum += iMJlB.dot(solverConstraint.m_contactNormal);
 							sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
 
-							solverConstraint.m_jacDiagABInv = 1./sum;
+							solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
 						}
 
 
@@ -551,6 +558,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 					
 					///this is a bad test and results in jitter -> always solve for those zero-distanc contacts! 
 					///-> if (cp.getDistance() <= btScalar(0.))
+					//if (cp.getDistance() <= manifold->getContactBreakingThreshold())
 					{
 						
 						const btVector3& pos1 = cp.getPositionWorldOnA();
@@ -772,15 +780,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 	
 	btContactSolverInfo info = infoGlobal;
 
-	if (1)
+	
-	{
-		int j;
-		for (j=0;j<numConstraints;j++)
-		{
-			btTypedConstraint* constraint = constraints[j];
-			constraint->buildJacobian();
-		}
-	}
 	
 	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
 	int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();

src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp

@@ -119,6 +119,8 @@ void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, co
     {
 		normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
 		new (&m_jacLin[i]) btJacobianEntry(
+			rbA.getCenterOfMassTransform().getBasis().transpose(),
+			rbB.getCenterOfMassTransform().getBasis().transpose(),
 			m_relPosA,
 			m_relPosB,
 			normalWorld,

src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp

@@ -61,10 +61,9 @@ void btSolve2LinearConstraint::resolveUnilateralPairConstraint(
 
 
 	//this jacobian entry could be re-used for all iterations
-	btJacobianEntry jacA(
+	btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
-		rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
 		invInertiaBDiag,invMassB);
-	btJacobianEntry jacB(rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
+	btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
 		invInertiaBDiag,invMassB);
 	
 	//const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
@@ -151,9 +150,9 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint(
 
 
 	//this jacobian entry could be re-used for all iterations
-	btJacobianEntry jacA(rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
+	btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
 		invInertiaBDiag,invMassB);
-	btJacobianEntry jacB(rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
+	btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
 		invInertiaBDiag,invMassB);
 	
 	//const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);

src/LinearMath/btVector3.h

@@ -309,6 +309,13 @@ public:
 			m_floats[3] = 0.f;
 		}
 
+		void	getSkewSymmetricMatrix(btVector3* v0,btVector3* v1,btVector3* v2) const
+		{
+			v0->setValue(0.		,-z()		,y());
+			v1->setValue(z()	,0.			,-x());
+			v2->setValue(-y()	,x()	,0.);
+		}
+
 };
 
 /**@brief Return the sum of two vectors (Point symantics)*/