initial commit

bullet/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp

@@ -0,0 +1,287 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios
+
+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.
+
+Written by: Marcus Hennix
+*/
+
+
+#include "btConeTwistConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btTransformUtil.h"
+#include "LinearMath/btSimdMinMax.h"
+#include <new>
+
+btConeTwistConstraint::btConeTwistConstraint()
+:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE)
+{
+}
+
+
+btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB, 
+											 const btTransform& rbAFrame,const btTransform& rbBFrame)
+											 :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
+											 m_angularOnly(false)
+{
+	// flip axis for correct angles
+	m_rbBFrame.getBasis()[1][0] *= btScalar(-1.);
+	m_rbBFrame.getBasis()[1][1] *= btScalar(-1.);
+	m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
+
+	m_swingSpan1 = btScalar(1e30);
+	m_swingSpan2 = btScalar(1e30);
+	m_twistSpan  = btScalar(1e30);
+	m_biasFactor = 0.3f;
+	m_relaxationFactor = 1.0f;
+
+	m_solveTwistLimit = false;
+	m_solveSwingLimit = false;
+
+}
+
+btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame)
+											:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame),
+											 m_angularOnly(false)
+{
+	m_rbBFrame = m_rbAFrame;
+	
+	// flip axis for correct angles
+	m_rbBFrame.getBasis()[1][0] *= btScalar(-1.);
+	m_rbBFrame.getBasis()[1][1] *= btScalar(-1.);
+	m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
+
+	m_rbBFrame.getBasis()[2][0] *= btScalar(-1.);
+	m_rbBFrame.getBasis()[2][1] *= btScalar(-1.);
+	m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
+	
+	m_swingSpan1 = btScalar(1e30);
+	m_swingSpan2 = btScalar(1e30);
+	m_twistSpan  = btScalar(1e30);
+	m_biasFactor = 0.3f;
+	m_relaxationFactor = 1.0f;
+
+	m_solveTwistLimit = false;
+	m_solveSwingLimit = false;
+	
+}			
+
+void	btConeTwistConstraint::buildJacobian()
+{
+	m_appliedImpulse = btScalar(0.);
+
+	//set bias, sign, clear accumulator
+	m_swingCorrection = btScalar(0.);
+	m_twistLimitSign = btScalar(0.);
+	m_solveTwistLimit = false;
+	m_solveSwingLimit = false;
+	m_accTwistLimitImpulse = btScalar(0.);
+	m_accSwingLimitImpulse = btScalar(0.);
+
+	if (!m_angularOnly)
+	{
+		btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
+		btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+		btVector3 relPos = pivotBInW - pivotAInW;
+
+		btVector3 normal[3];
+		if (relPos.length2() > SIMD_EPSILON)
+		{
+			normal[0] = relPos.normalized();
+		}
+		else
+		{
+			normal[0].setValue(btScalar(1.0),0,0);
+		}
+
+		btPlaneSpace1(normal[0], normal[1], normal[2]);
+
+		for (int i=0;i<3;i++)
+		{
+			new (&m_jac[i]) btJacobianEntry(
+				m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+				m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+				pivotAInW - m_rbA.getCenterOfMassPosition(),
+				pivotBInW - m_rbB.getCenterOfMassPosition(),
+				normal[i],
+				m_rbA.getInvInertiaDiagLocal(),
+				m_rbA.getInvMass(),
+				m_rbB.getInvInertiaDiagLocal(),
+				m_rbB.getInvMass());
+		}
+	}
+
+	btVector3 b1Axis1,b1Axis2,b1Axis3;
+	btVector3 b2Axis1,b2Axis2;
+
+	b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
+	b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);
+
+	btScalar swing1=btScalar(0.),swing2 = btScalar(0.);
+
+	// Get Frame into world space
+	if (m_swingSpan1 >= btScalar(0.05f))
+	{
+		b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
+		swing1  = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) );
+	}
+
+	if (m_swingSpan2 >= btScalar(0.05f))
+	{
+		b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);			
+		swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) );
+	}
+
+	btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1);		
+	btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2);	
+	btScalar EllipseAngle = btFabs(swing1)* RMaxAngle1Sq + btFabs(swing2) * RMaxAngle2Sq;
+
+	if (EllipseAngle > 1.0f)
+	{
+		m_swingCorrection = EllipseAngle-1.0f;
+		m_solveSwingLimit = true;
+		
+		// Calculate necessary axis & factors
+		m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3));
+		m_swingAxis.normalize();
+
+		btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
+		m_swingAxis *= swingAxisSign;
+
+		m_kSwing =  btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) +
+			getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis));
+
+	}
+
+	// Twist limits
+	if (m_twistSpan >= btScalar(0.))
+	{
+		btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1);
+		btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1);
+		btVector3 TwistRef = quatRotate(rotationArc,b2Axis2); 
+		btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );
+
+		btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
+		if (twist <= -m_twistSpan*lockedFreeFactor)
+		{
+			m_twistCorrection = -(twist + m_twistSpan);
+			m_solveTwistLimit = true;
+
+			m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
+			m_twistAxis.normalize();
+			m_twistAxis *= -1.0f;
+
+			m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
+				getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
+
+		}	else
+			if (twist >  m_twistSpan*lockedFreeFactor)
+			{
+				m_twistCorrection = (twist - m_twistSpan);
+				m_solveTwistLimit = true;
+
+				m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
+				m_twistAxis.normalize();
+
+				m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
+					getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
+
+			}
+	}
+}
+
+void	btConeTwistConstraint::solveConstraint(btScalar	timeStep)
+{
+
+	btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
+	btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+
+	btScalar tau = btScalar(0.3);
+	btScalar damping = btScalar(1.);
+
+	//linear part
+	if (!m_angularOnly)
+	{
+		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;
+
+		for (int i=0;i<3;i++)
+		{		
+			const btVector3& normal = m_jac[i].m_linearJointAxis;
+			btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
+
+			btScalar rel_vel;
+			rel_vel = normal.dot(vel);
+			//positional error (zeroth order error)
+			btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
+			btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
+			m_appliedImpulse += impulse;
+			btVector3 impulse_vector = normal * impulse;
+			m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
+			m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
+		}
+	}
+	
+	{
+		///solve angular part
+		const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
+		const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
+
+		// solve swing limit
+		if (m_solveSwingLimit)
+		{
+			btScalar amplitude = ((angVelB - angVelA).dot( m_swingAxis )*m_relaxationFactor*m_relaxationFactor + m_swingCorrection*(btScalar(1.)/timeStep)*m_biasFactor);
+			btScalar impulseMag = amplitude * m_kSwing;
+
+			// Clamp the accumulated impulse
+			btScalar temp = m_accSwingLimitImpulse;
+			m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, 0.0f );
+			impulseMag = m_accSwingLimitImpulse - temp;
+
+			btVector3 impulse = m_swingAxis * impulseMag;
+
+			m_rbA.applyTorqueImpulse(impulse);
+			m_rbB.applyTorqueImpulse(-impulse);
+
+		}
+
+		// solve twist limit
+		if (m_solveTwistLimit)
+		{
+			btScalar amplitude = ((angVelB - angVelA).dot( m_twistAxis )*m_relaxationFactor*m_relaxationFactor + m_twistCorrection*(btScalar(1.)/timeStep)*m_biasFactor );
+			btScalar impulseMag = amplitude * m_kTwist;
+
+			// Clamp the accumulated impulse
+			btScalar temp = m_accTwistLimitImpulse;
+			m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, 0.0f );
+			impulseMag = m_accTwistLimitImpulse - temp;
+
+			btVector3 impulse = m_twistAxis * impulseMag;
+
+			m_rbA.applyTorqueImpulse(impulse);
+			m_rbB.applyTorqueImpulse(-impulse);
+
+		}
+	
+	}
+
+}
+
+void	btConeTwistConstraint::updateRHS(btScalar	timeStep)
+{
+	(void)timeStep;
+
+}

bullet/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h

@@ -0,0 +1,126 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios
+
+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.
+
+Written by: Marcus Hennix
+*/
+
+
+
+#ifndef CONETWISTCONSTRAINT_H
+#define CONETWISTCONSTRAINT_H
+
+#include "../../LinearMath/btVector3.h"
+#include "btJacobianEntry.h"
+#include "btTypedConstraint.h"
+
+class btRigidBody;
+
+
+///btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc)
+class btConeTwistConstraint : public btTypedConstraint
+{
+#ifdef IN_PARALLELL_SOLVER
+public:
+#endif
+	btJacobianEntry	m_jac[3]; //3 orthogonal linear constraints
+
+	btTransform m_rbAFrame; 
+	btTransform m_rbBFrame;
+
+	btScalar	m_limitSoftness;
+	btScalar	m_biasFactor;
+	btScalar	m_relaxationFactor;
+
+	btScalar	m_swingSpan1;
+	btScalar	m_swingSpan2;
+	btScalar	m_twistSpan;
+
+	btVector3   m_swingAxis;
+	btVector3	m_twistAxis;
+
+	btScalar	m_kSwing;
+	btScalar	m_kTwist;
+
+	btScalar	m_twistLimitSign;
+	btScalar	m_swingCorrection;
+	btScalar	m_twistCorrection;
+
+	btScalar	m_accSwingLimitImpulse;
+	btScalar	m_accTwistLimitImpulse;
+
+	bool		m_angularOnly;
+	bool		m_solveTwistLimit;
+	bool		m_solveSwingLimit;
+
+	
+public:
+
+	btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,const btTransform& rbAFrame, const btTransform& rbBFrame);
+	
+	btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame);
+
+	btConeTwistConstraint();
+
+	virtual void	buildJacobian();
+
+	virtual	void	solveConstraint(btScalar	timeStep);
+
+	void	updateRHS(btScalar	timeStep);
+
+	const btRigidBody& getRigidBodyA() const
+	{
+		return m_rbA;
+	}
+	const btRigidBody& getRigidBodyB() const
+	{
+		return m_rbB;
+	}
+
+	void	setAngularOnly(bool angularOnly)
+	{
+		m_angularOnly = angularOnly;
+	}
+
+	void	setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan,  btScalar _softness = 0.8f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
+	{
+		m_swingSpan1 = _swingSpan1;
+		m_swingSpan2 = _swingSpan2;
+		m_twistSpan  = _twistSpan;
+
+		m_limitSoftness =  _softness;
+		m_biasFactor = _biasFactor;
+		m_relaxationFactor = _relaxationFactor;
+	}
+
+	const btTransform& getAFrame() { return m_rbAFrame; };	
+	const btTransform& getBFrame() { return m_rbBFrame; };
+
+	inline int getSolveTwistLimit()
+	{
+		return m_solveTwistLimit;
+	}
+
+	inline int getSolveSwingLimit()
+	{
+		return m_solveTwistLimit;
+	}
+
+	inline btScalar getTwistLimitSign()
+	{
+		return m_twistLimitSign;
+	}
+
+};
+
+#endif //CONETWISTCONSTRAINT_H

bullet/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h

@@ -0,0 +1,52 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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.
+*/
+
+#ifndef CONSTRAINT_SOLVER_H
+#define CONSTRAINT_SOLVER_H
+
+#include "LinearMath/btScalar.h"
+
+class btPersistentManifold;
+class btRigidBody;
+class btCollisionObject;
+class btTypedConstraint;
+struct btContactSolverInfo;
+struct btBroadphaseProxy;
+class btIDebugDraw;
+class btStackAlloc;
+
+/// btConstraintSolver provides solver interface
+class btConstraintSolver
+{
+
+public:
+
+	virtual ~btConstraintSolver() {}
+	
+	virtual void prepareSolve (int numBodies, int numManifolds) {;}
+
+	///solve a group of constraints
+	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc) = 0;
+
+	virtual void allSolved (const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc) {;}
+
+	///clear internal cached data and reset random seed
+	virtual	void	reset() = 0;
+};
+
+
+
+
+#endif //CONSTRAINT_SOLVER_H

bullet/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp

@@ -0,0 +1,417 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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 "btContactConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btVector3.h"
+#include "btJacobianEntry.h"
+#include "btContactSolverInfo.h"
+#include "LinearMath/btMinMax.h"
+#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
+
+#define ASSERT2 assert
+
+#define USE_INTERNAL_APPLY_IMPULSE 1
+
+
+//bilateral constraint between two dynamic objects
+void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
+                      btRigidBody& body2, const btVector3& pos2,
+                      btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep)
+{
+	(void)timeStep;
+	(void)distance;
+
+
+	btScalar normalLenSqr = normal.length2();
+	ASSERT2(btFabs(normalLenSqr) < btScalar(1.1));
+	if (normalLenSqr > btScalar(1.1))
+	{
+		impulse = btScalar(0.);
+		return;
+	}
+	btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
+	btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
+	//this jacobian entry could be re-used for all iterations
+	
+	btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
+	btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
+	btVector3 vel = vel1 - vel2;
+	
+
+	  btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
+		body2.getCenterOfMassTransform().getBasis().transpose(),
+		rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
+		body2.getInvInertiaDiagLocal(),body2.getInvMass());
+
+	btScalar jacDiagAB = jac.getDiagonal();
+	btScalar jacDiagABInv = btScalar(1.) / jacDiagAB;
+	
+	  btScalar rel_vel = jac.getRelativeVelocity(
+		body1.getLinearVelocity(),
+		body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(),
+		body2.getLinearVelocity(),
+		body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity()); 
+	btScalar a;
+	a=jacDiagABInv;
+
+
+	rel_vel = normal.dot(vel);
+	
+	//todo: move this into proper structure
+	btScalar contactDamping = btScalar(0.2);
+
+#ifdef ONLY_USE_LINEAR_MASS
+	btScalar massTerm = btScalar(1.) / (body1.getInvMass() + body2.getInvMass());
+	impulse = - contactDamping * rel_vel * massTerm;
+#else	
+	btScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
+	impulse = velocityImpulse;
+#endif
+}
+
+
+
+//response  between two dynamic objects with friction
+btScalar resolveSingleCollision(
+	btRigidBody& body1,
+	btRigidBody& body2,
+	btManifoldPoint& contactPoint,
+	const btContactSolverInfo& solverInfo)
+{
+
+	const btVector3& pos1_ = contactPoint.getPositionWorldOnA();
+	const btVector3& pos2_ = contactPoint.getPositionWorldOnB();
+   	const btVector3& normal = contactPoint.m_normalWorldOnB;
+
+	//constant over all iterations
+	btVector3 rel_pos1 = pos1_ - body1.getCenterOfMassPosition(); 
+	btVector3 rel_pos2 = pos2_ - body2.getCenterOfMassPosition();
+	
+	btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
+	btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
+	btVector3 vel = vel1 - vel2;
+	btScalar rel_vel;
+	rel_vel = normal.dot(vel);
+	
+	btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ;
+
+	// btScalar damping = solverInfo.m_damping ;
+	btScalar Kerp = solverInfo.m_erp;
+	btScalar Kcor = Kerp *Kfps;
+
+	btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
+	assert(cpd);
+	btScalar distance = cpd->m_penetration;
+	btScalar positionalError = Kcor *-distance;
+	btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
+
+	btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
+
+	btScalar	velocityImpulse = velocityError * cpd->m_jacDiagABInv;
+
+	btScalar normalImpulse = penetrationImpulse+velocityImpulse;
+	
+	// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
+	btScalar oldNormalImpulse = cpd->m_appliedImpulse;
+	btScalar sum = oldNormalImpulse + normalImpulse;
+	cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
+
+	normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
+
+#ifdef USE_INTERNAL_APPLY_IMPULSE
+	if (body1.getInvMass())
+	{
+		body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);
+	}
+	if (body2.getInvMass())
+	{
+		body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
+	}
+#else //USE_INTERNAL_APPLY_IMPULSE
+	body1.applyImpulse(normal*(normalImpulse), rel_pos1);
+	body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
+#endif //USE_INTERNAL_APPLY_IMPULSE
+
+	return normalImpulse;
+}
+
+
+btScalar resolveSingleFriction(
+	btRigidBody& body1,
+	btRigidBody& body2,
+	btManifoldPoint& contactPoint,
+	const btContactSolverInfo& solverInfo)
+{
+
+	(void)solverInfo;
+
+	const btVector3& pos1 = contactPoint.getPositionWorldOnA();
+	const btVector3& pos2 = contactPoint.getPositionWorldOnB();
+
+	btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
+	btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
+	
+	btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
+	assert(cpd);
+
+	btScalar combinedFriction = cpd->m_friction;
+	
+	btScalar limit = cpd->m_appliedImpulse * combinedFriction;
+	
+	if (cpd->m_appliedImpulse>btScalar(0.))
+	//friction
+	{
+		//apply friction in the 2 tangential directions
+		
+		// 1st tangent
+		btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
+		btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
+		btVector3 vel = vel1 - vel2;
+	
+		btScalar j1,j2;
+
+		{
+				
+			btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
+
+			// calculate j that moves us to zero relative velocity
+			j1 = -vrel * cpd->m_jacDiagABInvTangent0;
+			btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse0;
+			cpd->m_accumulatedTangentImpulse0 = oldTangentImpulse + j1;
+			GEN_set_min(cpd->m_accumulatedTangentImpulse0, limit);
+			GEN_set_max(cpd->m_accumulatedTangentImpulse0, -limit);
+			j1 = cpd->m_accumulatedTangentImpulse0 - oldTangentImpulse;
+
+		}
+		{
+			// 2nd tangent
+
+			btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
+			
+			// calculate j that moves us to zero relative velocity
+			j2 = -vrel * cpd->m_jacDiagABInvTangent1;
+			btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse1;
+			cpd->m_accumulatedTangentImpulse1 = oldTangentImpulse + j2;
+			GEN_set_min(cpd->m_accumulatedTangentImpulse1, limit);
+			GEN_set_max(cpd->m_accumulatedTangentImpulse1, -limit);
+			j2 = cpd->m_accumulatedTangentImpulse1 - oldTangentImpulse;
+		}
+
+#ifdef USE_INTERNAL_APPLY_IMPULSE
+	if (body1.getInvMass())
+	{
+		body1.internalApplyImpulse(cpd->m_frictionWorldTangential0*body1.getInvMass(),cpd->m_frictionAngularComponent0A,j1);
+		body1.internalApplyImpulse(cpd->m_frictionWorldTangential1*body1.getInvMass(),cpd->m_frictionAngularComponent1A,j2);
+	}
+	if (body2.getInvMass())
+	{
+		body2.internalApplyImpulse(cpd->m_frictionWorldTangential0*body2.getInvMass(),cpd->m_frictionAngularComponent0B,-j1);
+		body2.internalApplyImpulse(cpd->m_frictionWorldTangential1*body2.getInvMass(),cpd->m_frictionAngularComponent1B,-j2);	
+	}
+#else //USE_INTERNAL_APPLY_IMPULSE
+	body1.applyImpulse((j1 * cpd->m_frictionWorldTangential0)+(j2 * cpd->m_frictionWorldTangential1), rel_pos1);
+	body2.applyImpulse((j1 * -cpd->m_frictionWorldTangential0)+(j2 * -cpd->m_frictionWorldTangential1), rel_pos2);
+#endif //USE_INTERNAL_APPLY_IMPULSE
+
+
+	} 
+	return cpd->m_appliedImpulse;
+}
+
+
+btScalar resolveSingleFrictionOriginal(
+	btRigidBody& body1,
+	btRigidBody& body2,
+	btManifoldPoint& contactPoint,
+	const btContactSolverInfo& solverInfo)
+{
+
+	(void)solverInfo;
+
+	const btVector3& pos1 = contactPoint.getPositionWorldOnA();
+	const btVector3& pos2 = contactPoint.getPositionWorldOnB();
+
+	btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
+	btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
+	
+	btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
+	assert(cpd);
+
+	btScalar combinedFriction = cpd->m_friction;
+	
+	btScalar limit = cpd->m_appliedImpulse * combinedFriction;
+	//if (contactPoint.m_appliedImpulse>btScalar(0.))
+	//friction
+	{
+		//apply friction in the 2 tangential directions
+		
+		{
+			// 1st tangent
+			btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
+			btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
+			btVector3 vel = vel1 - vel2;
+			
+			btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
+
+			// calculate j that moves us to zero relative velocity
+			btScalar j = -vrel * cpd->m_jacDiagABInvTangent0;
+			btScalar total = cpd->m_accumulatedTangentImpulse0 + j;
+			GEN_set_min(total, limit);
+			GEN_set_max(total, -limit);
+			j = total - cpd->m_accumulatedTangentImpulse0;
+			cpd->m_accumulatedTangentImpulse0 = total;
+			body1.applyImpulse(j * cpd->m_frictionWorldTangential0, rel_pos1);
+			body2.applyImpulse(j * -cpd->m_frictionWorldTangential0, rel_pos2);
+		}
+
+				
+		{
+			// 2nd tangent
+			btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
+			btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
+			btVector3 vel = vel1 - vel2;
+
+			btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
+			
+			// calculate j that moves us to zero relative velocity
+			btScalar j = -vrel * cpd->m_jacDiagABInvTangent1;
+			btScalar total = cpd->m_accumulatedTangentImpulse1 + j;
+			GEN_set_min(total, limit);
+			GEN_set_max(total, -limit);
+			j = total - cpd->m_accumulatedTangentImpulse1;
+			cpd->m_accumulatedTangentImpulse1 = total;
+			body1.applyImpulse(j * cpd->m_frictionWorldTangential1, rel_pos1);
+			body2.applyImpulse(j * -cpd->m_frictionWorldTangential1, rel_pos2);
+		}
+	} 
+	return cpd->m_appliedImpulse;
+}
+
+
+//velocity + friction
+//response  between two dynamic objects with friction
+btScalar resolveSingleCollisionCombined(
+	btRigidBody& body1,
+	btRigidBody& body2,
+	btManifoldPoint& contactPoint,
+	const btContactSolverInfo& solverInfo)
+{
+
+	const btVector3& pos1 = contactPoint.getPositionWorldOnA();
+	const btVector3& pos2 = contactPoint.getPositionWorldOnB();
+   	const btVector3& normal = contactPoint.m_normalWorldOnB;
+
+	btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
+	btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
+	
+	btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
+	btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
+	btVector3 vel = vel1 - vel2;
+	btScalar rel_vel;
+	rel_vel = normal.dot(vel);
+	
+	btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ;
+
+	//btScalar damping = solverInfo.m_damping ;
+	btScalar Kerp = solverInfo.m_erp;
+	btScalar Kcor = Kerp *Kfps;
+
+	btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
+	assert(cpd);
+	btScalar distance = cpd->m_penetration;
+	btScalar positionalError = Kcor *-distance;
+	btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
+
+	btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
+
+	btScalar	velocityImpulse = velocityError * cpd->m_jacDiagABInv;
+
+	btScalar normalImpulse = penetrationImpulse+velocityImpulse;
+	
+	// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
+	btScalar oldNormalImpulse = cpd->m_appliedImpulse;
+	btScalar sum = oldNormalImpulse + normalImpulse;
+	cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
+
+	normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
+
+
+#ifdef USE_INTERNAL_APPLY_IMPULSE
+	if (body1.getInvMass())
+	{
+		body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);
+	}
+	if (body2.getInvMass())
+	{
+		body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
+	}
+#else //USE_INTERNAL_APPLY_IMPULSE
+	body1.applyImpulse(normal*(normalImpulse), rel_pos1);
+	body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
+#endif //USE_INTERNAL_APPLY_IMPULSE
+
+	{
+		//friction
+		btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
+		btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
+		btVector3 vel = vel1 - vel2;
+	  
+		rel_vel = normal.dot(vel);
+
+
+		btVector3 lat_vel = vel - normal * rel_vel;
+		btScalar lat_rel_vel = lat_vel.length();
+
+		btScalar combinedFriction = cpd->m_friction;
+
+		if (cpd->m_appliedImpulse > 0)
+		if (lat_rel_vel > SIMD_EPSILON)
+		{
+			lat_vel /= lat_rel_vel;
+			btVector3 temp1 = body1.getInvInertiaTensorWorld() * rel_pos1.cross(lat_vel);
+			btVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(lat_vel);
+			btScalar friction_impulse = lat_rel_vel /
+				(body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
+			btScalar normal_impulse = cpd->m_appliedImpulse * combinedFriction;
+
+			GEN_set_min(friction_impulse, normal_impulse);
+			GEN_set_max(friction_impulse, -normal_impulse);
+			body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
+			body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
+		}
+	}
+
+
+
+	return normalImpulse;
+}
+
+btScalar resolveSingleFrictionEmpty(
+	btRigidBody& body1,
+	btRigidBody& body2,
+	btManifoldPoint& contactPoint,
+	const btContactSolverInfo& solverInfo)
+{
+	(void)contactPoint;
+	(void)body1;
+	(void)body2;
+	(void)solverInfo;
+
+
+	return btScalar(0.);
+};
+

bullet/src/BulletDynamics/ConstraintSolver/btContactConstraint.h

@@ -0,0 +1,122 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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.
+*/
+
+#ifndef CONTACT_CONSTRAINT_H
+#define CONTACT_CONSTRAINT_H
+
+//todo: make into a proper class working with the iterative constraint solver
+
+class btRigidBody;
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btScalar.h"
+struct btContactSolverInfo;
+class btManifoldPoint;
+
+enum {
+	DEFAULT_CONTACT_SOLVER_TYPE=0,
+	CONTACT_SOLVER_TYPE1,
+	CONTACT_SOLVER_TYPE2,
+	USER_CONTACT_SOLVER_TYPE1,
+	MAX_CONTACT_SOLVER_TYPES
+};
+
+
+typedef btScalar (*ContactSolverFunc)(btRigidBody& body1,
+									 btRigidBody& body2,
+									 class btManifoldPoint& contactPoint,
+									 const btContactSolverInfo& info);
+
+///stores some extra information to each contact point. It is not in the contact point, because that want to keep the collision detection independent from the constraint solver.
+struct btConstraintPersistentData
+{
+	inline btConstraintPersistentData()
+	:m_appliedImpulse(btScalar(0.)),
+	m_prevAppliedImpulse(btScalar(0.)),
+	m_accumulatedTangentImpulse0(btScalar(0.)),
+	m_accumulatedTangentImpulse1(btScalar(0.)),
+	m_jacDiagABInv(btScalar(0.)),
+	m_persistentLifeTime(0),
+	m_restitution(btScalar(0.)),
+	m_friction(btScalar(0.)),
+	m_penetration(btScalar(0.)),
+	m_contactSolverFunc(0),
+	m_frictionSolverFunc(0)
+	{
+	}
+	
+					
+				/// total applied impulse during most recent frame
+			btScalar	m_appliedImpulse;
+			btScalar	m_prevAppliedImpulse;
+			btScalar	m_accumulatedTangentImpulse0;
+			btScalar	m_accumulatedTangentImpulse1;
+			
+			btScalar	m_jacDiagABInv;
+			btScalar	m_jacDiagABInvTangent0;
+			btScalar	m_jacDiagABInvTangent1;
+			int		m_persistentLifeTime;
+			btScalar	m_restitution;
+			btScalar	m_friction;
+			btScalar	m_penetration;
+			btVector3	m_frictionWorldTangential0;
+			btVector3	m_frictionWorldTangential1;
+
+			btVector3	m_frictionAngularComponent0A;
+			btVector3	m_frictionAngularComponent0B;
+			btVector3	m_frictionAngularComponent1A;
+			btVector3	m_frictionAngularComponent1B;
+
+			//some data doesn't need to be persistent over frames: todo: clean/reuse this
+			btVector3	m_angularComponentA;
+			btVector3	m_angularComponentB;
+		
+			ContactSolverFunc	m_contactSolverFunc;
+			ContactSolverFunc	m_frictionSolverFunc;
+
+};
+
+///bilateral constraint between two dynamic objects
+///positive distance = separation, negative distance = penetration
+void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
+                      btRigidBody& body2, const btVector3& pos2,
+                      btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep);
+
+
+///contact constraint resolution:
+///calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint
+///positive distance = separation, negative distance = penetration
+btScalar resolveSingleCollision(
+	btRigidBody& body1,
+	btRigidBody& body2,
+		btManifoldPoint& contactPoint,
+		 const btContactSolverInfo& info);
+
+btScalar resolveSingleFriction(
+	btRigidBody& body1,
+	btRigidBody& body2,
+	btManifoldPoint& contactPoint,
+	const btContactSolverInfo& solverInfo
+		);
+
+
+
+btScalar resolveSingleCollisionCombined(
+	btRigidBody& body1,
+	btRigidBody& body2,
+	btManifoldPoint& contactPoint,
+	const btContactSolverInfo& solverInfo
+		);
+
+#endif //CONTACT_CONSTRAINT_H

bullet/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h

@@ -0,0 +1,51 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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.
+*/
+
+#ifndef CONTACT_SOLVER_INFO
+#define CONTACT_SOLVER_INFO
+
+struct btContactSolverInfoData
+{
+	btScalar	m_tau;
+	btScalar	m_damping;
+	btScalar	m_friction;
+	btScalar	m_timeStep;
+	btScalar	m_restitution;
+	int		m_numIterations;
+	btScalar	m_maxErrorReduction;
+	btScalar	m_sor;
+	btScalar	m_erp;
+
+};
+
+struct btContactSolverInfo : public btContactSolverInfoData
+{
+
+	inline btContactSolverInfo()
+	{
+		m_tau = btScalar(0.6);
+		m_damping = btScalar(1.0);
+		m_friction = btScalar(0.3);
+		m_restitution = btScalar(0.);
+		m_maxErrorReduction = btScalar(20.);
+		m_numIterations = 10;
+		m_erp = btScalar(0.4);
+		m_sor = btScalar(1.3);
+	}
+
+	
+};
+
+#endif //CONTACT_SOLVER_INFO

bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp

@@ -0,0 +1,390 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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 "btGeneric6DofConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btTransformUtil.h"
+#include <new>
+
+static const btScalar kSign[] = { btScalar(1.0), btScalar(-1.0), btScalar(1.0) };
+static const int kAxisA[] = { 1, 0, 0 };
+static const int kAxisB[] = { 2, 2, 1 };
+#define GENERIC_D6_DISABLE_WARMSTARTING 1
+
+btGeneric6DofConstraint::btGeneric6DofConstraint()
+:btTypedConstraint(D6_CONSTRAINT_TYPE)
+{
+}
+
+btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB)
+: btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB)
+, m_frameInA(frameInA)
+, m_frameInB(frameInB)
+{
+	//free means upper < lower, 
+	//locked means upper == lower
+	//limited means upper > lower
+	//so start all locked
+	for (int i=0; i<6;++i)
+	{
+		m_lowerLimit[i] = btScalar(0.0);
+		m_upperLimit[i] = btScalar(0.0);
+		m_accumulatedImpulse[i] = btScalar(0.0);
+	}
+
+}
+
+
+void btGeneric6DofConstraint::buildJacobian()
+{
+	btVector3	localNormalInA(0,0,0);
+
+	const btVector3& pivotInA = m_frameInA.getOrigin();
+	const btVector3& pivotInB = m_frameInB.getOrigin();
+
+	btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_frameInA.getOrigin();
+	btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_frameInB.getOrigin();
+
+	btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
+	btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+
+	int i;
+	//linear part
+	for (i=0;i<3;i++)
+	{
+		if (isLimited(i))
+		{
+			localNormalInA[i] = 1;
+			btVector3 normalWorld = m_rbA.getCenterOfMassTransform().getBasis() * localNormalInA;
+
+			
+			// Create linear atom
+			new (&m_jacLinear[i]) btJacobianEntry(
+				m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+				m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+				m_rbA.getCenterOfMassTransform()*pivotInA - m_rbA.getCenterOfMassPosition(),
+				m_rbB.getCenterOfMassTransform()*pivotInB - m_rbB.getCenterOfMassPosition(),
+				normalWorld,
+				m_rbA.getInvInertiaDiagLocal(),
+				m_rbA.getInvMass(),
+				m_rbB.getInvInertiaDiagLocal(),
+				m_rbB.getInvMass());
+
+			//optionally disable warmstarting
+#ifdef GENERIC_D6_DISABLE_WARMSTARTING
+			m_accumulatedImpulse[i] = btScalar(0.);
+#endif //GENERIC_D6_DISABLE_WARMSTARTING
+
+			// Apply accumulated impulse
+			btVector3 impulse_vector = m_accumulatedImpulse[i] * normalWorld;
+
+			m_rbA.applyImpulse( impulse_vector, rel_pos1);
+			m_rbB.applyImpulse(-impulse_vector, rel_pos2);
+
+			localNormalInA[i] = 0;
+		}
+	}
+
+	// angular part
+	for (i=0;i<3;i++)
+	{
+		if (isLimited(i+3))
+		{
+			btVector3 axisA = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn( kAxisA[i] );
+			btVector3 axisB = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn( kAxisB[i] );
+
+			// Dirk: This is IMO mathematically the correct way, but we should consider axisA and axisB being near parallel maybe
+			btVector3 axis = kSign[i] * axisA.cross(axisB);
+
+			// Create angular atom
+			new (&m_jacAng[i])	btJacobianEntry(axis,
+				m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+				m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+				m_rbA.getInvInertiaDiagLocal(),
+				m_rbB.getInvInertiaDiagLocal());
+
+#ifdef GENERIC_D6_DISABLE_WARMSTARTING
+			m_accumulatedImpulse[i + 3] = btScalar(0.);
+#endif //GENERIC_D6_DISABLE_WARMSTARTING
+
+			// Apply accumulated impulse
+			btVector3 impulse_vector = m_accumulatedImpulse[i + 3] * axis;
+
+			m_rbA.applyTorqueImpulse( impulse_vector);
+			m_rbB.applyTorqueImpulse(-impulse_vector);
+		}
+	}
+}
+
+btScalar getMatrixElem(const btMatrix3x3& mat,int index)
+{
+	int row = index%3;
+	int col = index / 3;
+	return mat[row][col];
+}
+
+///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
+bool	MatrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
+{
+    // rot =  cy*cz          -cy*sz           sy
+    //        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
+    //       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
+
+///	0..8
+
+	 if (getMatrixElem(mat,2) < btScalar(1.0))
+    {
+        if (getMatrixElem(mat,2) > btScalar(-1.0))
+        {
+            xyz[0] = btAtan2(-getMatrixElem(mat,5),getMatrixElem(mat,8));
+            xyz[1] = btAsin(getMatrixElem(mat,2));
+            xyz[2] = btAtan2(-getMatrixElem(mat,1),getMatrixElem(mat,0));
+            return true;
+        }
+        else
+        {
+            // WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
+            xyz[0] = -btAtan2(getMatrixElem(mat,3),getMatrixElem(mat,4));
+            xyz[1] = -SIMD_HALF_PI;
+            xyz[2] = btScalar(0.0);
+            return false;
+        }
+    }
+    else
+    {
+        // WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
+        xyz[0] = btAtan2(getMatrixElem(mat,3),getMatrixElem(mat,4));
+        xyz[1] = SIMD_HALF_PI;
+        xyz[2] = 0.0;
+     
+    }
+	 
+	return false;
+}
+
+
+void	btGeneric6DofConstraint::solveConstraint(btScalar	timeStep)
+{
+	btScalar tau = btScalar(0.1);
+	btScalar damping = btScalar(1.0);
+
+	btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_frameInA.getOrigin();
+	btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_frameInB.getOrigin();
+
+	btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
+	btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+	
+	btVector3 localNormalInA(0,0,0);
+	int i;
+
+	// linear
+	for (i=0;i<3;i++)
+	{		
+		if (isLimited(i))
+		{
+			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;
+
+			btScalar jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal();
+
+			//velocity error (first order error)
+			btScalar rel_vel = m_jacLinear[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, 
+																	m_rbB.getLinearVelocity(),angvelB);
+		
+			//positional error (zeroth order error)
+			btScalar depth = -(pivotAInW - pivotBInW).dot(normalWorld); 
+			btScalar	lo = btScalar(-1e30);
+			btScalar	hi = btScalar(1e30);
+		
+			//handle the limits
+			if (m_lowerLimit[i] < m_upperLimit[i])
+			{
+				{
+					if (depth > m_upperLimit[i])
+					{
+						depth -= m_upperLimit[i];
+						lo = btScalar(0.);
+					
+					} else
+					{
+						if (depth < m_lowerLimit[i])
+						{
+							depth -= m_lowerLimit[i];
+							hi = btScalar(0.);
+						} else
+						{
+							continue;
+						}
+					}
+				}
+			}
+
+			btScalar normalImpulse= (tau*depth/timeStep - damping*rel_vel) * jacDiagABInv;
+			btScalar oldNormalImpulse = m_accumulatedImpulse[i];
+			btScalar sum = oldNormalImpulse + normalImpulse;
+			m_accumulatedImpulse[i] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
+			normalImpulse = m_accumulatedImpulse[i] - oldNormalImpulse;
+
+			btVector3 impulse_vector = normalWorld * normalImpulse;
+			m_rbA.applyImpulse( impulse_vector, rel_pos1);
+			m_rbB.applyImpulse(-impulse_vector, rel_pos2);
+			
+			localNormalInA[i] = 0;
+		}
+	}
+
+	btVector3	axis;
+	btScalar	angle;
+	btTransform	frameAWorld = m_rbA.getCenterOfMassTransform() * m_frameInA;
+	btTransform	frameBWorld = m_rbB.getCenterOfMassTransform() * m_frameInB;
+
+	btTransformUtil::calculateDiffAxisAngle(frameAWorld,frameBWorld,axis,angle);
+	btQuaternion diff(axis,angle);
+	btMatrix3x3 diffMat (diff);
+	btVector3 xyz;
+	///this is not perfect, we can first check which axis are limited, and choose a more appropriate order
+	MatrixToEulerXYZ(diffMat,xyz);
+
+	// angular
+	for (i=0;i<3;i++)
+	{
+		if (isLimited(i+3))
+		{
+			btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
+			btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
+		
+			btScalar jacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal();
+			
+			//velocity error (first order error)
+			btScalar rel_vel = m_jacAng[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, 
+																			m_rbB.getLinearVelocity(),angvelB);
+
+			//positional error (zeroth order error)
+			btVector3 axisA = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn( kAxisA[i] );
+			btVector3 axisB = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn( kAxisB[i] );
+
+			btScalar rel_pos = kSign[i] * axisA.dot(axisB);
+
+			btScalar	lo = btScalar(-1e30);
+			btScalar	hi = btScalar(1e30);
+		
+			//handle the twist limit
+			if (m_lowerLimit[i+3] < m_upperLimit[i+3])
+			{
+				//clamp the values
+				btScalar loLimit =  m_lowerLimit[i+3] > -3.1415 ? m_lowerLimit[i+3] : btScalar(-1e30);
+				btScalar hiLimit = m_upperLimit[i+3] < 3.1415 ? m_upperLimit[i+3] : btScalar(1e30);
+
+				btScalar projAngle  = btScalar(-1.)*xyz[i];
+				
+				if (projAngle < loLimit)
+				{
+					hi = btScalar(0.);
+					rel_pos = (loLimit - projAngle);
+				} else
+				{
+					if (projAngle > hiLimit)
+					{
+						lo = btScalar(0.);
+						rel_pos = (hiLimit - projAngle);
+					} else
+					{
+						continue;
+					}
+				}
+			}
+		
+			//impulse
+			
+			btScalar normalImpulse= -(tau*rel_pos/timeStep + damping*rel_vel) * jacDiagABInv;
+			btScalar oldNormalImpulse = m_accumulatedImpulse[i+3];
+			btScalar sum = oldNormalImpulse + normalImpulse;
+			m_accumulatedImpulse[i+3] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
+			normalImpulse = m_accumulatedImpulse[i+3] - oldNormalImpulse;
+			
+			// Dirk: Not needed - we could actually project onto Jacobian entry here (same as above)
+			btVector3 axis = kSign[i] * axisA.cross(axisB);
+			btVector3 impulse_vector = axis * normalImpulse;
+
+			m_rbA.applyTorqueImpulse( impulse_vector);
+			m_rbB.applyTorqueImpulse(-impulse_vector);
+		}
+	}
+}
+
+void	btGeneric6DofConstraint::updateRHS(btScalar	timeStep)
+{
+	(void)timeStep;
+
+}
+
+btScalar btGeneric6DofConstraint::computeAngle(int axis) const
+	{
+	btScalar angle = btScalar(0.f);
+
+	switch (axis)
+		{
+		case 0:
+			{
+			btVector3 v1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(1);
+			btVector3 v2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(1);
+			btVector3 w2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(2);
+
+			btScalar s = v1.dot(w2);
+			btScalar c = v1.dot(v2);
+
+			angle = btAtan2( s, c );
+			}
+			break;
+
+		case 1:
+			{
+			btVector3 w1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(2);
+			btVector3 w2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(2);
+			btVector3 u2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(0);
+
+			btScalar s = w1.dot(u2);
+			btScalar c = w1.dot(w2);
+
+			angle = btAtan2( s, c );
+			}
+			break;
+
+		case 2:
+			{
+			btVector3 u1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(0);
+			btVector3 u2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(0);
+			btVector3 v2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(1);
+
+			btScalar s = u1.dot(v2);
+			btScalar c = u1.dot(u2);
+
+			angle = btAtan2( s, c );
+			}
+			break;
+                  default: 
+					  btAssert ( 0 ) ; 
+					  
+					  break ;
+		}
+
+		return angle;
+	}
+

bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h

@@ -0,0 +1,120 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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.
+*/
+
+#ifndef GENERIC_6DOF_CONSTRAINT_H
+#define GENERIC_6DOF_CONSTRAINT_H
+
+#include "LinearMath/btVector3.h"
+#include "btJacobianEntry.h"
+#include "btTypedConstraint.h"
+
+class btRigidBody;
+
+
+
+/// btGeneric6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
+/// btGeneric6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'
+/// Work in progress (is still a Hinge actually)
+class btGeneric6DofConstraint : public btTypedConstraint
+{
+	btJacobianEntry	m_jacLinear[3];			// 3 orthogonal linear constraints
+	btJacobianEntry	m_jacAng[3];		// 3 orthogonal angular constraints
+
+	btTransform	m_frameInA;			// the constraint space w.r.t body A
+	btTransform	m_frameInB;			// the constraint space w.r.t body B
+
+	btScalar      m_lowerLimit[6];	// the constraint lower limits
+	btScalar      m_upperLimit[6];	// the constraint upper limits
+
+	btScalar		m_accumulatedImpulse[6];
+
+	btGeneric6DofConstraint&	operator=(btGeneric6DofConstraint&	other)
+	{
+		btAssert(0);
+		(void) other;
+		return *this;
+	}
+		
+public:
+	btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB );
+
+	btGeneric6DofConstraint();
+
+	
+	virtual void	buildJacobian();
+
+	virtual	void	solveConstraint(btScalar	timeStep);
+
+	void	updateRHS(btScalar	timeStep);
+
+	btScalar computeAngle(int axis) const;
+
+	void	setLinearLowerLimit(const btVector3& linearLower)
+	{
+		m_lowerLimit[0] = linearLower.getX();
+		m_lowerLimit[1] = linearLower.getY();
+		m_lowerLimit[2] = linearLower.getZ();
+	}
+
+	void	setLinearUpperLimit(const btVector3& linearUpper)
+	{
+		m_upperLimit[0] = linearUpper.getX();
+		m_upperLimit[1] = linearUpper.getY();
+		m_upperLimit[2] = linearUpper.getZ();
+	}
+
+	void	setAngularLowerLimit(const btVector3& angularLower)
+	{
+		m_lowerLimit[3] = angularLower.getX();
+		m_lowerLimit[4] = angularLower.getY();
+		m_lowerLimit[5] = angularLower.getZ();
+	}
+
+	void	setAngularUpperLimit(const btVector3& angularUpper)
+	{
+		m_upperLimit[3] = angularUpper.getX();
+		m_upperLimit[4] = angularUpper.getY();
+		m_upperLimit[5] = angularUpper.getZ();
+	}
+
+	//first 3 are linear, next 3 are angular
+	void SetLimit(int axis, btScalar lo, btScalar hi)
+	{
+		m_lowerLimit[axis] = lo; 
+		m_upperLimit[axis] = hi; 
+	}
+
+	//free means upper < lower, 
+	//locked means upper == lower
+	//limited means upper > lower
+	//limitIndex: first 3 are linear, next 3 are angular
+	bool	isLimited(int limitIndex)
+	{
+		return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
+	}
+
+	const btRigidBody& getRigidBodyA() const
+	{
+		return m_rbA;
+	}
+	const btRigidBody& getRigidBodyB() const
+	{
+		return m_rbB;
+	}
+	
+
+};
+
+#endif //GENERIC_6DOF_CONSTRAINT_H

bullet/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp

@@ -0,0 +1,398 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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 "btHingeConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btTransformUtil.h"
+#include "LinearMath/btSimdMinMax.h"
+#include <new>
+
+
+btHingeConstraint::btHingeConstraint()
+: btTypedConstraint (HINGE_CONSTRAINT_TYPE),
+m_enableAngularMotor(false)
+{
+}
+
+btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
+									 btVector3& axisInA,btVector3& axisInB)
+									 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
+									 m_angularOnly(false),
+									 m_enableAngularMotor(false)
+{
+	m_rbAFrame.getOrigin() = pivotInA;
+	
+	// since no frame is given, assume this to be zero angle and just pick rb transform axis
+	btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
+	btScalar projection = rbAxisA1.dot(axisInA);
+	if (projection > SIMD_EPSILON)
+		rbAxisA1 = rbAxisA1*projection - axisInA;
+	 else
+		rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
+	
+	btVector3 rbAxisA2 = rbAxisA1.cross(axisInA);
+
+	m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
+									rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
+									rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
+
+	btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
+	btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
+	btVector3 rbAxisB2 =  rbAxisB1.cross(axisInB);
+	
+	
+	m_rbBFrame.getOrigin() = pivotInB;
+	m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),-axisInB.getX(),
+									rbAxisB1.getY(),rbAxisB2.getY(),-axisInB.getY(),
+									rbAxisB1.getZ(),rbAxisB2.getZ(),-axisInB.getZ() );
+	
+	//start with free
+	m_lowerLimit = btScalar(1e30);
+	m_upperLimit = btScalar(-1e30);
+	m_biasFactor = 0.3f;
+	m_relaxationFactor = 1.0f;
+	m_limitSoftness = 0.9f;
+	m_solveLimit = false;
+
+}
+
+
+btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA)
+:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false)
+{
+
+	// since no frame is given, assume this to be zero angle and just pick rb transform axis
+	// fixed axis in worldspace
+	btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
+	btScalar projection = rbAxisA1.dot(axisInA);
+	if (projection > SIMD_EPSILON)
+		rbAxisA1 = rbAxisA1*projection - axisInA;
+	else
+		rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
+
+	btVector3 rbAxisA2 = axisInA.cross(rbAxisA1);
+
+	m_rbAFrame.getOrigin() = pivotInA;
+	m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
+									rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
+									rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
+
+
+	btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * -axisInA;
+
+	btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
+	btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
+	btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
+
+
+	m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA);
+	m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
+									rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
+									rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
+	
+	//start with free
+	m_lowerLimit = btScalar(1e30);
+	m_upperLimit = btScalar(-1e30);
+	m_biasFactor = 0.3f;
+	m_relaxationFactor = 1.0f;
+	m_limitSoftness = 0.9f;
+	m_solveLimit = false;
+}
+
+btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, 
+								     const btTransform& rbAFrame, const btTransform& rbBFrame)
+:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
+m_angularOnly(false),
+m_enableAngularMotor(false)
+{
+	// flip axis
+	m_rbBFrame.getBasis()[0][2] *= btScalar(-1.);
+	m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
+	m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
+
+	//start with free
+	m_lowerLimit = btScalar(1e30);
+	m_upperLimit = btScalar(-1e30);
+	m_biasFactor = 0.3f;
+	m_relaxationFactor = 1.0f;
+	m_limitSoftness = 0.9f;
+	m_solveLimit = false;
+}			
+
+
+
+btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame)
+:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
+m_angularOnly(false),
+m_enableAngularMotor(false)
+{
+	// flip axis
+	m_rbBFrame.getBasis()[0][2] *= btScalar(-1.);
+	m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
+	m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
+
+
+	//start with free
+	m_lowerLimit = btScalar(1e30);
+	m_upperLimit = btScalar(-1e30);	
+	m_biasFactor = 0.3f;
+	m_relaxationFactor = 1.0f;
+	m_limitSoftness = 0.9f;
+	m_solveLimit = false;
+}
+
+void	btHingeConstraint::buildJacobian()
+{
+	m_appliedImpulse = btScalar(0.);
+
+	if (!m_angularOnly)
+	{
+		btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
+		btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+		btVector3 relPos = pivotBInW - pivotAInW;
+
+		btVector3 normal[3];
+		if (relPos.length2() > SIMD_EPSILON)
+		{
+			normal[0] = relPos.normalized();
+		}
+		else
+		{
+			normal[0].setValue(btScalar(1.0),0,0);
+		}
+
+		btPlaneSpace1(normal[0], normal[1], normal[2]);
+
+		for (int i=0;i<3;i++)
+		{
+			new (&m_jac[i]) btJacobianEntry(
+				m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+				m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+				pivotAInW - m_rbA.getCenterOfMassPosition(),
+				pivotBInW - m_rbB.getCenterOfMassPosition(),
+				normal[i],
+				m_rbA.getInvInertiaDiagLocal(),
+				m_rbA.getInvMass(),
+				m_rbB.getInvInertiaDiagLocal(),
+				m_rbB.getInvMass());
+		}
+	}
+
+	//calculate two perpendicular jointAxis, orthogonal to hingeAxis
+	//these two jointAxis require equal angular velocities for both bodies
+
+	//this is unused for now, it's a todo
+	btVector3 jointAxis0local;
+	btVector3 jointAxis1local;
+	
+	btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
+
+	getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
+	btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
+	btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
+	btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
+		
+	new (&m_jacAng[0])	btJacobianEntry(jointAxis0,
+		m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+		m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+		m_rbA.getInvInertiaDiagLocal(),
+		m_rbB.getInvInertiaDiagLocal());
+
+	new (&m_jacAng[1])	btJacobianEntry(jointAxis1,
+		m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+		m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+		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());
+
+
+	// Compute limit information
+	btScalar hingeAngle = getHingeAngle();  
+
+	//set bias, sign, clear accumulator
+	m_correction = btScalar(0.);
+	m_limitSign = btScalar(0.);
+	m_solveLimit = false;
+	m_accLimitImpulse = btScalar(0.);
+
+	if (m_lowerLimit < m_upperLimit)
+	{
+		if (hingeAngle <= m_lowerLimit*m_limitSoftness)
+		{
+			m_correction = (m_lowerLimit - hingeAngle);
+			m_limitSign = 1.0f;
+			m_solveLimit = true;
+		} 
+		else if (hingeAngle >= m_upperLimit*m_limitSoftness)
+		{
+			m_correction = m_upperLimit - hingeAngle;
+			m_limitSign = -1.0f;
+			m_solveLimit = true;
+		}
+	}
+
+	//Compute K = J*W*J' for hinge axis
+	btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
+	m_kHinge =   1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
+			             getRigidBodyB().computeAngularImpulseDenominator(axisA));
+
+}
+
+void	btHingeConstraint::solveConstraint(btScalar	timeStep)
+{
+
+	btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
+	btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+
+	btScalar tau = btScalar(0.3);
+	btScalar damping = btScalar(1.);
+
+//linear part
+	if (!m_angularOnly)
+	{
+		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;
+
+		for (int i=0;i<3;i++)
+		{		
+			const btVector3& normal = m_jac[i].m_linearJointAxis;
+			btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
+
+			btScalar rel_vel;
+			rel_vel = normal.dot(vel);
+			//positional error (zeroth order error)
+			btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
+			btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
+			m_appliedImpulse += impulse;
+			btVector3 impulse_vector = normal * impulse;
+			m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
+			m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
+		}
+	}
+
+	
+	{
+		///solve angular part
+
+		// get axes in world space
+		btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
+		btVector3 axisB =  getRigidBodyB().getCenterOfMassTransform().getBasis() *  m_rbBFrame.getBasis().getColumn(2);
+
+		const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
+		const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
+
+		btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
+		btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
+
+		btVector3 angAorthog = angVelA - angVelAroundHingeAxisA;
+		btVector3 angBorthog = angVelB - angVelAroundHingeAxisB;
+		btVector3 velrelOrthog = angAorthog-angBorthog;
+		{
+			//solve orthogonal angular velocity correction
+			btScalar relaxation = btScalar(1.);
+			btScalar len = velrelOrthog.length();
+			if (len > btScalar(0.00001))
+			{
+				btVector3 normal = velrelOrthog.normalized();
+				btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
+					getRigidBodyB().computeAngularImpulseDenominator(normal);
+				// scale for mass and relaxation
+				//todo:  expose this 0.9 factor to developer
+				velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor;
+			}
+
+			//solve angular positional correction
+			btVector3 angularError = -axisA.cross(axisB) *(btScalar(1.)/timeStep);
+			btScalar len2 = angularError.length();
+			if (len2>btScalar(0.00001))
+			{
+				btVector3 normal2 = angularError.normalized();
+				btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
+						getRigidBodyB().computeAngularImpulseDenominator(normal2);
+				angularError *= (btScalar(1.)/denom2) * relaxation;
+			}
+
+			m_rbA.applyTorqueImpulse(-velrelOrthog+angularError);
+			m_rbB.applyTorqueImpulse(velrelOrthog-angularError);
+
+			// solve limit
+			if (m_solveLimit)
+			{
+				btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor  ) * m_limitSign;
+
+				btScalar impulseMag = amplitude * m_kHinge;
+
+				// Clamp the accumulated impulse
+				btScalar temp = m_accLimitImpulse;
+				m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) );
+				impulseMag = m_accLimitImpulse - temp;
+
+
+				btVector3 impulse = axisA * impulseMag * m_limitSign;
+				m_rbA.applyTorqueImpulse(impulse);
+				m_rbB.applyTorqueImpulse(-impulse);
+			}
+		}
+
+		//apply motor
+		if (m_enableAngularMotor) 
+		{
+			//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;
+
+			btScalar unclippedMotorImpulse = m_kHinge * 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);
+			
+		}
+	}
+
+}
+
+void	btHingeConstraint::updateRHS(btScalar	timeStep)
+{
+	(void)timeStep;
+
+}
+
+btScalar btHingeConstraint::getHingeAngle()
+{
+	const btVector3 refAxis0  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
+	const btVector3 refAxis1  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
+	const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1);
+
+	return btAtan2Fast( swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)  );
+}

bullet/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h

@@ -0,0 +1,130 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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.
+*/
+
+/* Hinge Constraint by Dirk Gregorius. Limits added by Marcus Hennix at Starbreeze Studios */
+
+#ifndef HINGECONSTRAINT_H
+#define HINGECONSTRAINT_H
+
+#include "LinearMath/btVector3.h"
+#include "btJacobianEntry.h"
+#include "btTypedConstraint.h"
+
+class btRigidBody;
+
+/// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
+/// axis defines the orientation of the hinge axis
+class btHingeConstraint : public btTypedConstraint
+{
+#ifdef IN_PARALLELL_SOLVER
+public:
+#endif
+	btJacobianEntry	m_jac[3]; //3 orthogonal linear constraints
+	btJacobianEntry	m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor
+
+	btTransform m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+	btTransform m_rbBFrame;
+
+	btScalar	m_motorTargetVelocity;
+	btScalar	m_maxMotorImpulse;
+
+	btScalar	m_limitSoftness; 
+	btScalar	m_biasFactor; 
+	btScalar    m_relaxationFactor; 
+
+	btScalar    m_lowerLimit;	
+	btScalar    m_upperLimit;	
+	
+	btScalar	m_kHinge;
+
+	btScalar	m_limitSign;
+	btScalar	m_correction;
+
+	btScalar	m_accLimitImpulse;
+
+	bool		m_angularOnly;
+	bool		m_enableAngularMotor;
+	bool		m_solveLimit;
+
+	
+public:
+
+	btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB);
+
+	btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA);
+	
+	btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame);
+
+	btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame);
+
+	btHingeConstraint();
+
+	virtual void	buildJacobian();
+
+	virtual	void	solveConstraint(btScalar	timeStep);
+
+	void	updateRHS(btScalar	timeStep);
+
+	const btRigidBody& getRigidBodyA() const
+	{
+		return m_rbA;
+	}
+	const btRigidBody& getRigidBodyB() const
+	{
+		return m_rbB;
+	}
+
+	void	setAngularOnly(bool angularOnly)
+	{
+		m_angularOnly = angularOnly;
+	}
+
+	void	enableAngularMotor(bool enableMotor,btScalar targetVelocity,btScalar maxMotorImpulse)
+	{
+		m_enableAngularMotor  = enableMotor;
+		m_motorTargetVelocity = targetVelocity;
+		m_maxMotorImpulse = maxMotorImpulse;
+	}
+
+	void	setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
+	{
+		m_lowerLimit = low;
+		m_upperLimit = high;
+
+		m_limitSoftness =  _softness;
+		m_biasFactor = _biasFactor;
+		m_relaxationFactor = _relaxationFactor;
+
+	}
+
+	btScalar getHingeAngle();
+
+
+	const btTransform& getAFrame() { return m_rbAFrame; };	
+	const btTransform& getBFrame() { return m_rbBFrame; };
+
+	inline int getSolveLimit()
+	{
+		return m_solveLimit;
+	}
+
+	inline btScalar getLimitSign()
+	{
+		return m_limitSign;
+	}
+		
+};
+
+#endif //HINGECONSTRAINT_H

bullet/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h

@@ -0,0 +1,156 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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.
+*/
+
+#ifndef JACOBIAN_ENTRY_H
+#define JACOBIAN_ENTRY_H
+
+#include "LinearMath/btVector3.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+
+
+//notes:
+// Another memory optimization would be to store m_1MinvJt in the remaining 3 w components
+// which makes the btJacobianEntry memory layout 16 bytes
+// if you only are interested in angular part, just feed massInvA and massInvB zero
+
+/// Jacobian entry is an abstraction that allows to describe constraints
+/// it can be used in combination with a constraint solver
+/// Can be used to relate the effect of an impulse to the constraint error
+class btJacobianEntry
+{
+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, 
+		const btScalar massInvA,
+		const btVector3& inertiaInvB,
+		const btScalar massInvB)
+		:m_linearJointAxis(jointAxis)
+	{
+		m_aJ = world2A*(rel_pos1.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);
+
+		btAssert(m_Adiag > btScalar(0.0));
+	}
+
+	//angular constraint between two different rigidbodies
+	btJacobianEntry(const btVector3& jointAxis,
+		const btMatrix3x3& world2A,
+		const btMatrix3x3& world2B,
+		const btVector3& inertiaInvA,
+		const btVector3& inertiaInvB)
+		:m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.)))
+	{
+		m_aJ= world2A*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);
+
+		btAssert(m_Adiag > btScalar(0.0));
+	}
+
+	//angular constraint between two different rigidbodies
+	btJacobianEntry(const btVector3& axisInA,
+		const btVector3& axisInB,
+		const btVector3& inertiaInvA,
+		const btVector3& inertiaInvB)
+		: m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.)))
+		, m_aJ(axisInA)
+		, m_bJ(-axisInB)
+	{
+		m_0MinvJt	= inertiaInvA * m_aJ;
+		m_1MinvJt = inertiaInvB * m_bJ;
+		m_Adiag =  m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
+
+		btAssert(m_Adiag > btScalar(0.0));
+	}
+
+	//constraint on one rigidbody
+	btJacobianEntry(
+		const btMatrix3x3& world2A,
+		const btVector3& rel_pos1,const btVector3& rel_pos2,
+		const btVector3& jointAxis,
+		const btVector3& inertiaInvA, 
+		const btScalar massInvA)
+		:m_linearJointAxis(jointAxis)
+	{
+		m_aJ= world2A*(rel_pos1.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);
+
+		btAssert(m_Adiag > btScalar(0.0));
+	}
+
+	btScalar	getDiagonal() const { return m_Adiag; }
+
+	// for two constraints on the same rigidbody (for example vehicle friction)
+	btScalar	getNonDiagonal(const btJacobianEntry& jacB, const btScalar massInvA) const
+	{
+		const btJacobianEntry& jacA = *this;
+		btScalar lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis);
+		btScalar ang = jacA.m_0MinvJt.dot(jacB.m_aJ);
+		return lin + ang;
+	}
+
+	
+
+	// for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies)
+	btScalar	getNonDiagonal(const btJacobianEntry& jacB,const btScalar massInvA,const btScalar massInvB) const
+	{
+		const btJacobianEntry& jacA = *this;
+		btVector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis;
+		btVector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ;
+		btVector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ;
+		btVector3 lin0 = massInvA * lin ;
+		btVector3 lin1 = massInvB * lin;
+		btVector3 sum = ang0+ang1+lin0+lin1;
+		return sum[0]+sum[1]+sum[2];
+	}
+
+	btScalar getRelativeVelocity(const btVector3& linvelA,const btVector3& angvelA,const btVector3& linvelB,const btVector3& angvelB)
+	{
+		btVector3 linrel = linvelA - linvelB;
+		btVector3 angvela  = angvelA * m_aJ;
+		btVector3 angvelb  = angvelB * m_bJ;
+		linrel *= m_linearJointAxis;
+		angvela += angvelb;
+		angvela += linrel;
+		btScalar rel_vel2 = angvela[0]+angvela[1]+angvela[2];
+		return rel_vel2 + SIMD_EPSILON;
+	}
+//private:
+
+	btVector3	m_linearJointAxis;
+	btVector3	m_aJ;
+	btVector3	m_bJ;
+	btVector3	m_0MinvJt;
+	btVector3	m_1MinvJt;
+	//Optimization: can be stored in the w/last component of one of the vectors
+	btScalar	m_Adiag;
+
+};
+
+#endif //JACOBIAN_ENTRY_H

bullet/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp

@@ -0,0 +1,117 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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 "btPoint2PointConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include <new>
+
+
+
+btPoint2PointConstraint::btPoint2PointConstraint()
+:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE)
+{
+}
+
+btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB)
+:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB)
+{
+
+}
+
+
+btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA)
+:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA))
+{
+	
+}
+
+void	btPoint2PointConstraint::buildJacobian()
+{
+	m_appliedImpulse = btScalar(0.);
+
+	btVector3	normal(0,0,0);
+
+	for (int i=0;i<3;i++)
+	{
+		normal[i] = 1;
+		new (&m_jac[i]) btJacobianEntry(
+			m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+			m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+			m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
+			m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
+			normal,
+			m_rbA.getInvInertiaDiagLocal(),
+			m_rbA.getInvMass(),
+			m_rbB.getInvInertiaDiagLocal(),
+			m_rbB.getInvMass());
+		normal[i] = 0;
+	}
+
+}
+
+void	btPoint2PointConstraint::solveConstraint(btScalar	timeStep)
+{
+	btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
+	btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
+
+
+	btVector3 normal(0,0,0);
+	
+
+//	btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
+//	btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
+
+	for (int i=0;i<3;i++)
+	{		
+		normal[i] = 1;
+		btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
+
+		btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
+		btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+		//this jacobian entry could be re-used for all iterations
+		
+		btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
+		btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
+		btVector3 vel = vel1 - vel2;
+		
+		btScalar rel_vel;
+		rel_vel = normal.dot(vel);
+
+	/*
+		//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); //this is the error projected on the normal
+		
+		btScalar impulse = depth*m_setting.m_tau/timeStep  * jacDiagABInv -  m_setting.m_damping * rel_vel * jacDiagABInv;
+		m_appliedImpulse+=impulse;
+		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;
+	}
+}
+
+void	btPoint2PointConstraint::updateRHS(btScalar	timeStep)
+{
+	(void)timeStep;
+
+}
+

bullet/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h

@@ -0,0 +1,80 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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.
+*/
+
+#ifndef POINT2POINTCONSTRAINT_H
+#define POINT2POINTCONSTRAINT_H
+
+#include "LinearMath/btVector3.h"
+#include "btJacobianEntry.h"
+#include "btTypedConstraint.h"
+
+class btRigidBody;
+
+struct	btConstraintSetting
+{
+	btConstraintSetting()	:
+		m_tau(btScalar(0.3)),
+		m_damping(btScalar(1.))
+	{
+	}
+	btScalar		m_tau;
+	btScalar		m_damping;
+};
+
+/// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space
+class btPoint2PointConstraint : public btTypedConstraint
+{
+#ifdef IN_PARALLELL_SOLVER
+public:
+#endif
+	btJacobianEntry	m_jac[3]; //3 orthogonal linear constraints
+	
+	btVector3	m_pivotInA;
+	btVector3	m_pivotInB;
+	
+	
+	
+public:
+
+	btConstraintSetting	m_setting;
+
+	btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB);
+
+	btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA);
+
+	btPoint2PointConstraint();
+
+	virtual void	buildJacobian();
+
+
+	virtual	void	solveConstraint(btScalar	timeStep);
+
+	void	updateRHS(btScalar	timeStep);
+
+	void	setPivotA(const btVector3& pivotA)
+	{
+		m_pivotInA = pivotA;
+	}
+
+	void	setPivotB(const btVector3& pivotB)
+	{
+		m_pivotInB = pivotB;
+	}
+
+
+
+};
+
+#endif //POINT2POINTCONSTRAINT_H

bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h

@@ -0,0 +1,114 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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.
+*/
+
+#ifndef SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
+#define SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
+
+#include "btConstraintSolver.h"
+class btIDebugDraw;
+#include "btContactConstraint.h"
+	
+
+
+/// btSequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses
+/// The approach is the 3D version of Erin Catto's GDC 2006 tutorial. See http://www.gphysics.com
+/// Although Sequential Impulse is more intuitive, it is mathematically equivalent to Projected Successive Overrelaxation (iterative LCP)
+/// Applies impulses for combined restitution and penetration recovery and to simulate friction
+class btSequentialImpulseConstraintSolver : public btConstraintSolver
+{
+
+protected:
+	btScalar solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
+	btScalar solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
+	void  prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer);
+
+	ContactSolverFunc m_contactDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];
+	ContactSolverFunc m_frictionDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];
+
+	//choose between several modes, different friction model etc.
+	int	m_solverMode;
+	///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
+	unsigned long	m_btSeed2;
+
+public:
+
+	enum	eSolverMode
+	{
+		SOLVER_RANDMIZE_ORDER = 1,
+		SOLVER_FRICTION_SEPARATE = 2,
+		SOLVER_USE_WARMSTARTING = 4,
+		SOLVER_CACHE_FRIENDLY = 8
+	};
+
+	btSequentialImpulseConstraintSolver();
+
+	///Advanced: Override the default contact solving function for contacts, for certain types of rigidbody
+	///See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType
+	void	setContactSolverFunc(ContactSolverFunc func,int type0,int type1)
+	{
+		m_contactDispatch[type0][type1] = func;
+	}
+	
+	///Advanced: Override the default friction solving function for contacts, for certain types of rigidbody
+	///See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType
+	void	SetFrictionSolverFunc(ContactSolverFunc func,int type0,int type1)
+	{
+		m_frictionDispatch[type0][type1] = func;
+	}
+
+	virtual ~btSequentialImpulseConstraintSolver() {}
+	
+	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc);
+
+	virtual btScalar solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
+
+	///clear internal cached data and reset random seed
+	virtual	void	reset();
+
+	btScalar solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
+
+
+	void	setSolverMode(int mode)
+	{
+		m_solverMode = mode;
+	}
+
+	int	getSolverMode() const
+	{
+		return m_solverMode;
+	}
+
+	unsigned long btRand2();
+
+	int btRandInt2 (int n);
+
+	void	setRandSeed(unsigned long seed)
+	{
+		m_btSeed2 = seed;
+	}
+	unsigned long	getRandSeed() const
+	{
+		return m_btSeed2;
+	}
+
+};
+
+#ifndef BT_PREFER_SIMD
+typedef btSequentialImpulseConstraintSolver btSequentialImpulseConstraintSolverPrefered;
+#endif
+
+
+#endif //SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
+

bullet/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp

@@ -0,0 +1,255 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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 "btSolve2LinearConstraint.h"
+
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btVector3.h"
+#include "btJacobianEntry.h"
+
+
+void btSolve2LinearConstraint::resolveUnilateralPairConstraint(
+												   btRigidBody* body1,
+		btRigidBody* body2,
+
+						const btMatrix3x3& world2A,
+						const btMatrix3x3& world2B,
+						
+						const btVector3& invInertiaADiag,
+						const btScalar invMassA,
+						const btVector3& linvelA,const btVector3& angvelA,
+						const btVector3& rel_posA1,
+						const btVector3& invInertiaBDiag,
+						const btScalar invMassB,
+						const btVector3& linvelB,const btVector3& angvelB,
+						const btVector3& rel_posA2,
+
+					  btScalar depthA, const btVector3& normalA, 
+					  const btVector3& rel_posB1,const btVector3& rel_posB2,
+					  btScalar depthB, const btVector3& normalB, 
+					  btScalar& imp0,btScalar& imp1)
+{
+	(void)linvelA;
+	(void)linvelB;
+	(void)angvelB;
+	(void)angvelA;
+
+
+
+	imp0 = btScalar(0.);
+	imp1 = btScalar(0.);
+
+	btScalar len = btFabs(normalA.length()) - btScalar(1.);
+	if (btFabs(len) >= SIMD_EPSILON)
+		return;
+
+	btAssert(len < SIMD_EPSILON);
+
+
+	//this jacobian entry could be re-used for all iterations
+	btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
+		invInertiaBDiag,invMassB);
+	btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
+		invInertiaBDiag,invMassB);
+	
+	//const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
+	//const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
+
+	const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1));
+	const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1));
+
+//	btScalar penetrationImpulse = (depth*contactTau*timeCorrection)  * massTerm;//jacDiagABInv
+	btScalar massTerm = btScalar(1.) / (invMassA + invMassB);
+
+
+	// calculate rhs (or error) terms
+	const btScalar dv0 = depthA  * m_tau * massTerm - vel0 * m_damping;
+	const btScalar dv1 = depthB  * m_tau * massTerm - vel1 * m_damping;
+
+
+	// dC/dv * dv = -C
+	
+	// jacobian * impulse = -error
+	//
+
+	//impulse = jacobianInverse * -error
+
+	// inverting 2x2 symmetric system (offdiagonal are equal!)
+	// 
+
+
+	btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
+	btScalar	invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag );
+	
+	//imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
+	//imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
+
+	imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
+	imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
+
+	//[a b]								  [d -c]
+	//[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc)
+
+	//[jA nD] * [imp0] = [dv0]
+	//[nD jB]   [imp1]   [dv1]
+
+}
+
+
+
+void btSolve2LinearConstraint::resolveBilateralPairConstraint(
+						btRigidBody* body1,
+						btRigidBody* body2,
+						const btMatrix3x3& world2A,
+						const btMatrix3x3& world2B,
+						
+						const btVector3& invInertiaADiag,
+						const btScalar invMassA,
+						const btVector3& linvelA,const btVector3& angvelA,
+						const btVector3& rel_posA1,
+						const btVector3& invInertiaBDiag,
+						const btScalar invMassB,
+						const btVector3& linvelB,const btVector3& angvelB,
+						const btVector3& rel_posA2,
+
+					  btScalar depthA, const btVector3& normalA, 
+					  const btVector3& rel_posB1,const btVector3& rel_posB2,
+					  btScalar depthB, const btVector3& normalB, 
+					  btScalar& imp0,btScalar& imp1)
+{
+
+	(void)linvelA;
+	(void)linvelB;
+	(void)angvelA;
+	(void)angvelB;
+
+
+
+	imp0 = btScalar(0.);
+	imp1 = btScalar(0.);
+
+	btScalar len = btFabs(normalA.length()) - btScalar(1.);
+	if (btFabs(len) >= SIMD_EPSILON)
+		return;
+
+	btAssert(len < SIMD_EPSILON);
+
+
+	//this jacobian entry could be re-used for all iterations
+	btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
+		invInertiaBDiag,invMassB);
+	btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
+		invInertiaBDiag,invMassB);
+	
+	//const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
+	//const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
+
+	const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1));
+	const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1));
+
+	// calculate rhs (or error) terms
+	const btScalar dv0 = depthA  * m_tau - vel0 * m_damping;
+	const btScalar dv1 = depthB  * m_tau - vel1 * m_damping;
+
+	// dC/dv * dv = -C
+	
+	// jacobian * impulse = -error
+	//
+
+	//impulse = jacobianInverse * -error
+
+	// inverting 2x2 symmetric system (offdiagonal are equal!)
+	// 
+
+
+	btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
+	btScalar	invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag );
+	
+	//imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
+	//imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
+
+	imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
+	imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
+
+	//[a b]								  [d -c]
+	//[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc)
+
+	//[jA nD] * [imp0] = [dv0]
+	//[nD jB]   [imp1]   [dv1]
+
+	if ( imp0 > btScalar(0.0))
+	{
+		if ( imp1 > btScalar(0.0) )
+		{
+			//both positive
+		}
+		else
+		{
+			imp1 = btScalar(0.);
+
+			// now imp0>0 imp1<0
+			imp0 = dv0 / jacA.getDiagonal();
+			if ( imp0 > btScalar(0.0) )
+			{
+			} else
+			{
+				imp0 = btScalar(0.);
+			}
+		}
+	}
+	else
+	{
+		imp0 = btScalar(0.);
+
+		imp1 = dv1 / jacB.getDiagonal();
+		if ( imp1 <= btScalar(0.0) )
+		{
+			imp1 = btScalar(0.);
+			// now imp0>0 imp1<0
+			imp0 = dv0 / jacA.getDiagonal();
+			if ( imp0 > btScalar(0.0) )
+			{
+			} else
+			{
+				imp0 = btScalar(0.);
+			}
+		} else
+		{
+		}
+	}
+}
+
+
+/*
+void btSolve2LinearConstraint::resolveAngularConstraint(	const btMatrix3x3& invInertiaAWS,
+											const btScalar invMassA,
+											const btVector3& linvelA,const btVector3& angvelA,
+											const btVector3& rel_posA1,
+											const btMatrix3x3& invInertiaBWS,
+											const btScalar invMassB,
+											const btVector3& linvelB,const btVector3& angvelB,
+											const btVector3& rel_posA2,
+
+											btScalar depthA, const btVector3& normalA, 
+											const btVector3& rel_posB1,const btVector3& rel_posB2,
+											btScalar depthB, const btVector3& normalB, 
+											btScalar& imp0,btScalar& imp1)
+{
+
+}
+*/
+

bullet/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h

@@ -0,0 +1,107 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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.
+*/
+
+#ifndef SOLVE_2LINEAR_CONSTRAINT_H
+#define SOLVE_2LINEAR_CONSTRAINT_H
+
+#include "LinearMath/btMatrix3x3.h"
+#include "LinearMath/btVector3.h"
+
+
+class btRigidBody;
+
+
+
+/// constraint class used for lateral tyre friction.
+class	btSolve2LinearConstraint
+{
+	btScalar	m_tau;
+	btScalar	m_damping;
+
+public:
+
+	btSolve2LinearConstraint(btScalar tau,btScalar damping)
+	{
+		m_tau = tau;
+		m_damping = damping;
+	}
+	//
+	// solve unilateral constraint (equality, direct method)
+	//
+	void resolveUnilateralPairConstraint(		
+														   btRigidBody* body0,
+		btRigidBody* body1,
+
+		const btMatrix3x3& world2A,
+						const btMatrix3x3& world2B,
+						
+						const btVector3& invInertiaADiag,
+						const btScalar invMassA,
+						const btVector3& linvelA,const btVector3& angvelA,
+						const btVector3& rel_posA1,
+						const btVector3& invInertiaBDiag,
+						const btScalar invMassB,
+						const btVector3& linvelB,const btVector3& angvelB,
+						const btVector3& rel_posA2,
+
+					  btScalar depthA, const btVector3& normalA, 
+					  const btVector3& rel_posB1,const btVector3& rel_posB2,
+					  btScalar depthB, const btVector3& normalB, 
+					  btScalar& imp0,btScalar& imp1);
+
+
+	//
+	// solving 2x2 lcp problem (inequality, direct solution )
+	//
+	void resolveBilateralPairConstraint(
+			btRigidBody* body0,
+						btRigidBody* body1,
+		const btMatrix3x3& world2A,
+						const btMatrix3x3& world2B,
+						
+						const btVector3& invInertiaADiag,
+						const btScalar invMassA,
+						const btVector3& linvelA,const btVector3& angvelA,
+						const btVector3& rel_posA1,
+						const btVector3& invInertiaBDiag,
+						const btScalar invMassB,
+						const btVector3& linvelB,const btVector3& angvelB,
+						const btVector3& rel_posA2,
+
+					  btScalar depthA, const btVector3& normalA, 
+					  const btVector3& rel_posB1,const btVector3& rel_posB2,
+					  btScalar depthB, const btVector3& normalB, 
+					  btScalar& imp0,btScalar& imp1);
+
+/*
+	void resolveAngularConstraint(	const btMatrix3x3& invInertiaAWS,
+						const btScalar invMassA,
+						const btVector3& linvelA,const btVector3& angvelA,
+						const btVector3& rel_posA1,
+						const btMatrix3x3& invInertiaBWS,
+						const btScalar invMassB,
+						const btVector3& linvelB,const btVector3& angvelB,
+						const btVector3& rel_posA2,
+
+					  btScalar depthA, const btVector3& normalA, 
+					  const btVector3& rel_posB1,const btVector3& rel_posB2,
+					  btScalar depthB, const btVector3& normalB, 
+					  btScalar& imp0,btScalar& imp1);
+
+*/
+
+};
+
+#endif //SOLVE_2LINEAR_CONSTRAINT_H

bullet/src/BulletDynamics/ConstraintSolver/btSolverBody.h

@@ -0,0 +1,71 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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.
+*/
+
+#ifndef BT_SOLVER_BODY_H
+#define BT_SOLVER_BODY_H
+
+class	btRigidBody;
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btMatrix3x3.h"
+
+
+
+
+ATTRIBUTE_ALIGNED16 (struct)	btSolverBody
+{
+	btVector3		m_centerOfMassPosition;
+	btVector3		m_linearVelocity;
+	btVector3		m_angularVelocity;
+	btRigidBody*	m_originalBody;
+	float			m_invMass;
+	float			m_friction;
+	float			m_angularFactor;
+
+	inline void	getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const
+	{
+		velocity = m_linearVelocity + m_angularVelocity.cross(rel_pos);
+	}
+
+	//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
+	inline void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
+	{
+		m_linearVelocity += linearComponent*impulseMagnitude;
+		m_angularVelocity += angularComponent*impulseMagnitude*m_angularFactor;
+	}
+
+	void	writebackVelocity()
+	{
+		if (m_invMass)
+		{
+			m_originalBody->setLinearVelocity(m_linearVelocity);
+			m_originalBody->setAngularVelocity(m_angularVelocity);
+		}
+	}
+
+	void	readVelocity()
+	{
+		if (m_invMass)
+		{
+			m_linearVelocity = m_originalBody->getLinearVelocity();
+			m_angularVelocity = m_originalBody->getAngularVelocity();
+		}
+	}
+
+	
+
+
+};
+
+#endif //BT_SOLVER_BODY_H

bullet/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h

@@ -0,0 +1,63 @@
+
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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.
+*/
+
+#ifndef BT_SOLVER_CONSTRAINT_H
+#define BT_SOLVER_CONSTRAINT_H
+
+class	btRigidBody;
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btMatrix3x3.h"
+
+//#define NO_FRICTION_TANGENTIALS 1
+
+///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
+ATTRIBUTE_ALIGNED16 (struct)	btSolverConstraint
+{
+	btVector3	m_relpos1CrossNormal;
+	btVector3	m_relpos2CrossNormal;
+	btVector3	m_contactNormal;
+	btVector3	m_angularComponentA;
+	btVector3	m_angularComponentB;
+
+	btScalar	m_appliedVelocityImpulse;
+	int			m_solverBodyIdA;
+	int			m_solverBodyIdB;
+	btScalar	m_friction;
+	btScalar	m_restitution;
+	btScalar	m_jacDiagABInv;
+	btScalar	m_penetration;
+	btScalar	m_appliedImpulse;
+
+	int			m_constraintType;
+	int			m_frictionIndex;
+	int			m_unusedPadding[2];
+
+	enum		btSolverConstraintType
+	{
+		BT_SOLVER_CONTACT_1D = 0,
+		BT_SOLVER_FRICTION_1D
+	};
+};
+
+
+
+
+
+
+#endif //BT_SOLVER_CONSTRAINT_H
+
+

bullet/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp

@@ -0,0 +1,56 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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 "btTypedConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+
+static btRigidBody s_fixed(0, 0,0);
+
+btTypedConstraint::btTypedConstraint(btTypedConstraintType type)
+: m_constraintType (type),
+m_userConstraintType(-1),
+m_userConstraintId(-1),
+m_rbA(s_fixed),
+m_rbB(s_fixed),
+m_appliedImpulse(btScalar(0.))
+{
+	s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
+}
+btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA)
+: m_constraintType (type),
+m_userConstraintType(-1),
+m_userConstraintId(-1),
+m_rbA(rbA),
+m_rbB(s_fixed),
+m_appliedImpulse(btScalar(0.))
+{
+		s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
+
+}
+
+
+btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB)
+: m_constraintType (type),
+m_userConstraintType(-1),
+m_userConstraintId(-1),
+m_rbA(rbA),
+m_rbB(rbB),
+m_appliedImpulse(btScalar(0.))
+{
+		s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
+
+}
+

bullet/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h

@@ -0,0 +1,112 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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.
+*/
+
+#ifndef TYPED_CONSTRAINT_H
+#define TYPED_CONSTRAINT_H
+
+class btRigidBody;
+#include "LinearMath/btScalar.h"
+
+enum btTypedConstraintType
+{
+	POINT2POINT_CONSTRAINT_TYPE,
+	HINGE_CONSTRAINT_TYPE,
+	CONETWIST_CONSTRAINT_TYPE,
+	D6_CONSTRAINT_TYPE,
+	VEHICLE_CONSTRAINT_TYPE
+};
+
+///TypedConstraint is the baseclass for Bullet constraints and vehicles
+class btTypedConstraint
+{
+	int	m_userConstraintType;
+	int	m_userConstraintId;
+
+	btTypedConstraintType m_constraintType;
+
+	btTypedConstraint&	operator=(btTypedConstraint&	other)
+	{
+		btAssert(0);
+		(void) other;
+		return *this;
+	}
+
+protected:
+	btRigidBody&	m_rbA;
+	btRigidBody&	m_rbB;
+	btScalar	m_appliedImpulse;
+
+
+public:
+
+	btTypedConstraint(btTypedConstraintType type);
+	virtual ~btTypedConstraint() {};
+	btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA);
+
+	btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB);
+
+	virtual void	buildJacobian() = 0;
+
+	virtual	void	solveConstraint(btScalar	timeStep) = 0;
+
+	const btRigidBody& getRigidBodyA() const
+	{
+		return m_rbA;
+	}
+	const btRigidBody& getRigidBodyB() const
+	{
+		return m_rbB;
+	}
+
+	btRigidBody& getRigidBodyA() 
+	{
+		return m_rbA;
+	}
+	btRigidBody& getRigidBodyB()
+	{
+		return m_rbB;
+	}
+
+	int getUserConstraintType() const
+	{
+		return m_userConstraintType ;
+	}
+
+	void	setUserConstraintType(int userConstraintType)
+	{
+		m_userConstraintType = userConstraintType;
+	};
+
+	void	setUserConstraintId(int uid)
+	{
+		m_userConstraintId = uid;
+	}
+	
+	int getUserConstraintId() const
+	{
+		return m_userConstraintId;
+	}
+	btScalar	getAppliedImpulse() const
+	{
+		return m_appliedImpulse;
+	}
+
+	btTypedConstraintType getConstraintType () const
+	{
+		return m_constraintType;
+	}
+};
+
+#endif //TYPED_CONSTRAINT_H