moved files around

BulletDynamics/ConstraintSolver/ConstraintSolver.h

@@ -0,0 +1,41 @@
+/*
+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
+
+class PersistentManifold;
+class RigidBody;
+
+struct ContactSolverInfo;
+struct BroadphaseProxy;
+class IDebugDraw;
+
+/// ConstraintSolver provides solver interface
+class ConstraintSolver
+{
+
+public:
+
+	virtual ~ConstraintSolver() {}
+	
+	virtual float SolveGroup(PersistentManifold** manifold,int numManifolds,const ContactSolverInfo& info,class IDebugDraw* debugDrawer = 0) = 0;
+
+};
+
+
+
+
+#endif //CONSTRAINT_SOLVER_H

BulletDynamics/ConstraintSolver/ContactConstraint.cpp

@@ -0,0 +1,248 @@
+/*
+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 "ContactConstraint.h"
+#include "Dynamics/RigidBody.h"
+#include "SimdVector3.h"
+#include "JacobianEntry.h"
+#include "ContactSolverInfo.h"
+#include "GEN_MinMax.h"
+#include "NarrowPhaseCollision/ManifoldPoint.h"
+
+#define ASSERT2 assert
+
+//some values to find stable tresholds
+
+float useGlobalSettingContacts = false;//true;
+SimdScalar contactDamping = 0.2f;
+SimdScalar contactTau = .02f;//0.02f;//*0.02f;
+
+
+
+
+SimdScalar	calculateCombinedFriction(RigidBody& body0,RigidBody& body1)
+{
+	SimdScalar friction = body0.getFriction() * body1.getFriction();
+
+	const SimdScalar MAX_FRICTION  = 10.f;
+	if (friction < -MAX_FRICTION)
+		friction = -MAX_FRICTION;
+	if (friction > MAX_FRICTION)
+		friction = MAX_FRICTION;
+	return friction;
+
+}
+
+
+//bilateral constraint between two dynamic objects
+void resolveSingleBilateral(RigidBody& body1, const SimdVector3& pos1,
+                      RigidBody& body2, const SimdVector3& pos2,
+                      SimdScalar distance, const SimdVector3& normal,SimdScalar& impulse ,float timeStep)
+{
+	float normalLenSqr = normal.length2();
+	ASSERT2(fabs(normalLenSqr) < 1.1f);
+	if (normalLenSqr > 1.1f)
+	{
+		impulse = 0.f;
+		return;
+	}
+	SimdVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
+	SimdVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
+	//this jacobian entry could be re-used for all iterations
+	
+	SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
+	SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
+	SimdVector3 vel = vel1 - vel2;
+	
+
+	  JacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
+		body2.getCenterOfMassTransform().getBasis().transpose(),
+		rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
+		body2.getInvInertiaDiagLocal(),body2.getInvMass());
+
+	SimdScalar jacDiagAB = jac.getDiagonal();
+	SimdScalar jacDiagABInv = 1.f / jacDiagAB;
+	
+	  SimdScalar rel_vel = jac.getRelativeVelocity(
+		body1.getLinearVelocity(),
+		body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(),
+		body2.getLinearVelocity(),
+		body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity()); 
+	float a;
+	a=jacDiagABInv;
+
+
+	rel_vel = normal.dot(vel);
+		
+
+#ifdef ONLY_USE_LINEAR_MASS
+	SimdScalar massTerm = 1.f / (body1.getInvMass() + body2.getInvMass());
+	impulse = - contactDamping * rel_vel * massTerm;
+#else	
+	SimdScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
+	impulse = velocityImpulse;
+#endif
+}
+
+
+
+
+//velocity + friction
+//response  between two dynamic objects with friction
+float resolveSingleCollision(
+	RigidBody& body1,
+	RigidBody& body2,
+	ManifoldPoint& contactPoint,
+	const ContactSolverInfo& solverInfo
+
+		)
+{
+
+	const SimdVector3& pos1 = contactPoint.GetPositionWorldOnA();
+	const SimdVector3& pos2 = contactPoint.GetPositionWorldOnB();
+    
+	
+//	printf("distance=%f\n",distance);
+
+	const SimdVector3& normal = contactPoint.m_normalWorldOnB;
+
+	SimdVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
+	SimdVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
+	
+	SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
+	SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
+	SimdVector3 vel = vel1 - vel2;
+	SimdScalar rel_vel;
+	rel_vel = normal.dot(vel);
+	
+	float combinedRestitution = body1.getRestitution() * body2.getRestitution();
+
+	
+
+	SimdScalar Kfps = 1.f / solverInfo.m_timeStep ;
+
+	float damping = solverInfo.m_damping ;
+	float Kerp = solverInfo.m_erp;
+	
+	if (useGlobalSettingContacts)
+	{
+		damping = contactDamping;
+		Kerp = contactTau;
+	} 
+
+	float Kcor = Kerp *Kfps;
+
+	//printf("dist=%f\n",distance);
+
+		ConstraintPersistentData* cpd = (ConstraintPersistentData*) contactPoint.m_userPersistentData;
+	assert(cpd);
+
+	SimdScalar distance = cpd->m_penetration;//contactPoint.GetDistance();
+	
+
+	//distance = 0.f;
+	SimdScalar positionalError = Kcor *-distance;
+	//jacDiagABInv;
+	SimdScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
+
+	
+	SimdScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
+
+	SimdScalar	velocityImpulse = velocityError * cpd->m_jacDiagABInv;
+
+	SimdScalar normalImpulse = penetrationImpulse+velocityImpulse;
+	
+	// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
+	float oldNormalImpulse = cpd->m_appliedImpulse;
+	float sum = oldNormalImpulse + normalImpulse;
+	cpd->m_appliedImpulse = 0.f > sum ? 0.f: sum;
+
+	normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
+
+	body1.applyImpulse(normal*(normalImpulse), rel_pos1);
+	body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
+	
+	return normalImpulse;
+}
+
+
+float resolveSingleFriction(
+	RigidBody& body1,
+	RigidBody& body2,
+	ManifoldPoint& contactPoint,
+	const ContactSolverInfo& solverInfo
+
+		)
+{
+	const SimdVector3& pos1 = contactPoint.GetPositionWorldOnA();
+	const SimdVector3& pos2 = contactPoint.GetPositionWorldOnB();
+	const SimdVector3& normal = contactPoint.m_normalWorldOnB;
+
+	SimdVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
+	SimdVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
+	float combinedFriction = calculateCombinedFriction(body1,body2);
+
+	ConstraintPersistentData* cpd = (ConstraintPersistentData*) contactPoint.m_userPersistentData;
+	assert(cpd);
+
+	SimdScalar limit = cpd->m_appliedImpulse * combinedFriction;
+	//if (contactPoint.m_appliedImpulse>0.f)
+	//friction
+	{
+		//apply friction in the 2 tangential directions
+		
+		SimdScalar relaxation = solverInfo.m_damping;
+		{
+			// 1st tangent
+			SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
+			SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
+			SimdVector3 vel = vel1 - vel2;
+			
+			SimdScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
+
+			// calculate j that moves us to zero relative velocity
+			SimdScalar j = -vrel * cpd->m_jacDiagABInvTangent0;
+			float 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
+			SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
+			SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
+			SimdVector3 vel = vel1 - vel2;
+
+			SimdScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
+			
+			// calculate j that moves us to zero relative velocity
+			SimdScalar j = -vrel * cpd->m_jacDiagABInvTangent1;
+			float 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;
+}

BulletDynamics/ConstraintSolver/ContactConstraint.h

@@ -0,0 +1,83 @@
+/*
+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 RigidBody;
+#include "SimdVector3.h"
+#include "SimdScalar.h"
+struct ContactSolverInfo;
+class ManifoldPoint;
+
+struct ConstraintPersistentData
+{
+	inline ConstraintPersistentData()
+	:m_appliedImpulse(0.f),
+	m_prevAppliedImpulse(0.f),
+	m_accumulatedTangentImpulse0(0.f),
+	m_accumulatedTangentImpulse1(0.f),
+	m_jacDiagABInv(0.f),
+	m_persistentLifeTime(0),
+	m_restitution(0.f),
+	m_penetration(0.f)
+	{
+	}
+	
+					
+				/// total applied impulse during most recent frame
+			float	m_appliedImpulse;
+			float	m_prevAppliedImpulse;
+			float	m_accumulatedTangentImpulse0;
+			float	m_accumulatedTangentImpulse1;
+			
+			float	m_jacDiagABInv;
+			float	m_jacDiagABInvTangent0;
+			float	m_jacDiagABInvTangent1;
+			int		m_persistentLifeTime;
+			float	m_restitution;
+			float	m_penetration;
+			SimdVector3	m_frictionWorldTangential0;
+			SimdVector3	m_frictionWorldTangential1;
+
+
+};
+
+///bilateral constraint between two dynamic objects
+///positive distance = separation, negative distance = penetration
+void resolveSingleBilateral(RigidBody& body1, const SimdVector3& pos1,
+                      RigidBody& body2, const SimdVector3& pos2,
+                      SimdScalar distance, const SimdVector3& normal,SimdScalar& impulse ,float timeStep);
+
+
+///contact constraint resolution:
+///calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint
+///positive distance = separation, negative distance = penetration
+float resolveSingleCollision(
+	RigidBody& body1,
+	RigidBody& body2,
+		ManifoldPoint& contactPoint,
+		 const ContactSolverInfo& info);
+
+float resolveSingleFriction(
+	RigidBody& body1,
+	RigidBody& body2,
+	ManifoldPoint& contactPoint,
+	const ContactSolverInfo& solverInfo
+		);
+
+#endif //CONTACT_CONSTRAINT_H

BulletDynamics/ConstraintSolver/ContactSolverInfo.h

@@ -0,0 +1,47 @@
+/*
+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 ContactSolverInfo
+{
+
+	inline ContactSolverInfo()
+	{
+		m_tau = 0.6f;
+		m_damping = 1.0f;
+		m_friction = 0.3f;
+		m_restitution = 0.f;
+		m_maxErrorReduction = 20.f;
+		m_numIterations = 10;
+		m_erp = 0.4f;
+		m_sor = 1.3f;
+	}
+
+	float	m_tau;
+	float	m_damping;
+	float	m_friction;
+	float	m_timeStep;
+	float	m_restitution;
+	int		m_numIterations;
+	float	m_maxErrorReduction;
+	float	m_sor;
+	float	m_erp;
+
+};
+
+#endif //CONTACT_SOLVER_INFO

BulletDynamics/ConstraintSolver/HingeConstraint.cpp

@@ -0,0 +1,176 @@
+/*
+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 "HingeConstraint.h"
+#include "Dynamics/RigidBody.h"
+#include "Dynamics/MassProps.h"
+#include "SimdTransformUtil.h"
+
+
+HingeConstraint::HingeConstraint()
+{
+}
+
+HingeConstraint::HingeConstraint(RigidBody& rbA,RigidBody& rbB, const SimdVector3& pivotInA,const SimdVector3& pivotInB,
+								 SimdVector3& axisInA,SimdVector3& axisInB)
+:TypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
+m_axisInA(axisInA),
+m_axisInB(axisInB),
+m_angularOnly(false)
+{
+
+}
+
+
+HingeConstraint::HingeConstraint(RigidBody& rbA,const SimdVector3& pivotInA,SimdVector3& axisInA)
+:TypedConstraint(rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
+m_axisInA(axisInA),
+//fixed axis in worldspace
+m_axisInB(rbA.getCenterOfMassTransform().getBasis() * -axisInA),
+m_angularOnly(false)
+{
+	
+}
+
+void	HingeConstraint::BuildJacobian()
+{
+	SimdVector3	normal(0,0,0);
+
+	if (!m_angularOnly)
+	{
+		for (int i=0;i<3;i++)
+		{
+			normal[i] = 1;
+			new (&m_jac[i]) JacobianEntry(
+				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;
+		}
+	}
+
+	//calculate two perpendicular jointAxis, orthogonal to hingeAxis
+	//these two jointAxis require equal angular velocities for both bodies
+
+	//this is ununsed for now, it's a todo
+	SimdVector3 axisWorldA = GetRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
+	SimdVector3 jointAxis0;
+	SimdVector3 jointAxis1;
+	SimdPlaneSpace1(axisWorldA,jointAxis0,jointAxis1);
+	
+	new (&m_jacAng[0])	JacobianEntry(jointAxis0,
+		m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+		m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+		m_rbA.getInvInertiaDiagLocal(),
+		m_rbB.getInvInertiaDiagLocal());
+
+	new (&m_jacAng[1])	JacobianEntry(jointAxis1,
+		m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+		m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+		m_rbA.getInvInertiaDiagLocal(),
+		m_rbB.getInvInertiaDiagLocal());
+
+
+}
+
+void	HingeConstraint::SolveConstraint(SimdScalar	timeStep)
+{
+
+	SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
+	SimdVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
+
+	SimdVector3 normal(0,0,0);
+	SimdScalar tau = 0.3f;
+	SimdScalar damping = 1.f;
+
+	if (!m_angularOnly)
+	{
+		for (int i=0;i<3;i++)
+		{		
+			normal[i] = 1;
+			SimdScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal();
+
+			SimdVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
+			SimdVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+			
+			SimdVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
+			SimdVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
+			SimdVector3 vel = vel1 - vel2;
+			SimdScalar rel_vel;
+			rel_vel = normal.dot(vel);
+			//positional error (zeroth order error)
+			SimdScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
+			SimdScalar impulse = depth*tau/timeStep  * jacDiagABInv -  damping * rel_vel * jacDiagABInv * damping;
+
+			SimdVector3 impulse_vector = normal * impulse;
+			m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
+			m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
+			
+			normal[i] = 0;
+		}
+	}
+
+	///solve angular part
+
+	// get axes in world space
+	SimdVector3 axisA = GetRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
+	SimdVector3 axisB = GetRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB;
+
+	const SimdVector3& angVelA = GetRigidBodyA().getAngularVelocity();
+	const SimdVector3& angVelB = GetRigidBodyB().getAngularVelocity();
+	SimdVector3 angA = angVelA - axisA * axisA.dot(angVelA);
+	SimdVector3 angB = angVelB - axisB * axisB.dot(angVelB);
+	SimdVector3 velrel = angA-angB;
+
+	//solve angular velocity correction
+	float relaxation = 1.f;
+	float len = velrel.length();
+	if (len > 0.00001f)
+	{
+		SimdVector3 normal = velrel.normalized();
+		float denom = GetRigidBodyA().ComputeAngularImpulseDenominator(normal) +
+			GetRigidBodyB().ComputeAngularImpulseDenominator(normal);
+		// scale for mass and relaxation
+		velrel *= (1.f/denom) * 0.9;
+	}
+
+	//solve angular positional correction
+	SimdVector3 angularError = -axisA.cross(axisB) *(1.f/timeStep);
+	float len2 = angularError.length();
+	if (len2>0.00001f)
+	{
+		SimdVector3 normal2 = angularError.normalized();
+		float denom2 = GetRigidBodyA().ComputeAngularImpulseDenominator(normal2) +
+				GetRigidBodyB().ComputeAngularImpulseDenominator(normal2);
+		angularError *= (1.f/denom2) * relaxation;
+	}
+
+	m_rbA.applyTorqueImpulse(-velrel+angularError);
+	m_rbB.applyTorqueImpulse(velrel-angularError);
+
+}
+
+void	HingeConstraint::UpdateRHS(SimdScalar	timeStep)
+{
+
+}
+

BulletDynamics/ConstraintSolver/HingeConstraint.h

@@ -0,0 +1,73 @@
+/*
+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 HINGECONSTRAINT_H
+#define HINGECONSTRAINT_H
+
+#include "SimdVector3.h"
+
+#include "ConstraintSolver/JacobianEntry.h"
+#include "TypedConstraint.h"
+
+class RigidBody;
+
+
+/// 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 HingeConstraint : public TypedConstraint
+{
+	JacobianEntry	m_jac[3]; //3 orthogonal linear constraints
+	JacobianEntry	m_jacAng[2]; //2 orthogonal angular constraints
+
+	SimdVector3	m_pivotInA;
+	SimdVector3	m_pivotInB;
+	SimdVector3	m_axisInA;
+	SimdVector3	m_axisInB;
+
+	bool	m_angularOnly;
+	
+public:
+
+	HingeConstraint(RigidBody& rbA,RigidBody& rbB, const SimdVector3& pivotInA,const SimdVector3& pivotInB,SimdVector3& axisInA,SimdVector3& axisInB);
+
+	HingeConstraint(RigidBody& rbA,const SimdVector3& pivotInA,SimdVector3& axisInA);
+
+	HingeConstraint();
+
+	virtual void	BuildJacobian();
+
+	virtual	void	SolveConstraint(SimdScalar	timeStep);
+
+	void	UpdateRHS(SimdScalar	timeStep);
+
+	const RigidBody& GetRigidBodyA() const
+	{
+		return m_rbA;
+	}
+	const RigidBody& GetRigidBodyB() const
+	{
+		return m_rbB;
+	}
+
+	void	setAngularOnly(bool angularOnly)
+	{
+		m_angularOnly = angularOnly;
+	}
+
+
+
+};
+
+#endif //HINGECONSTRAINT_H

BulletDynamics/ConstraintSolver/JacobianEntry.h

@@ -0,0 +1,134 @@
+/*
+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 "SimdVector3.h"
+#include "Dynamics/RigidBody.h"
+
+
+//notes:
+// Another memory optimization would be to store m_1MinvJt in the remaining 3 w components
+// which makes the JacobianEntry 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 JacobianEntry
+{
+public:
+	JacobianEntry() {};
+	//constraint between two different rigidbodies
+	JacobianEntry(
+		const SimdMatrix3x3& world2A,
+		const SimdMatrix3x3& world2B,
+		const SimdVector3& rel_pos1,const SimdVector3& rel_pos2,
+		const SimdVector3& jointAxis,
+		const SimdVector3& inertiaInvA, 
+		const SimdScalar massInvA,
+		const SimdVector3& inertiaInvB,
+		const SimdScalar massInvB)
+		:m_jointAxis(jointAxis)
+	{
+		m_aJ = world2A*(rel_pos1.cross(m_jointAxis));
+		m_bJ = world2B*(rel_pos2.cross(-m_jointAxis));
+		m_0MinvJt	= inertiaInvA * m_aJ;
+		m_1MinvJt = inertiaInvB * m_bJ;
+		m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ);
+	}
+
+		//angular constraint between two different rigidbodies
+	JacobianEntry(const SimdVector3& jointAxis,
+		const SimdMatrix3x3& world2A,
+		const SimdMatrix3x3& world2B,
+		const SimdVector3& inertiaInvA,
+		const SimdVector3& inertiaInvB)
+		:m_jointAxis(m_jointAxis)
+	{
+		m_aJ= world2A*m_jointAxis;
+		m_bJ = world2B*-m_jointAxis;
+		m_0MinvJt	= inertiaInvA * m_aJ;
+		m_1MinvJt = inertiaInvB * m_bJ;
+		m_Adiag =  m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
+	}
+
+	//constraint on one rigidbody
+	JacobianEntry(
+		const SimdMatrix3x3& world2A,
+		const SimdVector3& rel_pos1,const SimdVector3& rel_pos2,
+		const SimdVector3& jointAxis,
+		const SimdVector3& inertiaInvA, 
+		const SimdScalar massInvA)
+		:m_jointAxis(jointAxis)
+	{
+		m_aJ= world2A*(rel_pos1.cross(m_jointAxis));
+		m_bJ = world2A*(rel_pos2.cross(-m_jointAxis));
+		m_0MinvJt	= inertiaInvA * m_aJ;
+		m_1MinvJt = SimdVector3(0.f,0.f,0.f);
+		m_Adiag = massInvA + m_0MinvJt.dot(m_aJ);
+	}
+
+	SimdScalar	getDiagonal() const { return m_Adiag; }
+
+	// for two constraints on the same rigidbody (for example vehicle friction)
+	SimdScalar	getNonDiagonal(const JacobianEntry& jacB, const SimdScalar massInvA) const
+	{
+		const JacobianEntry& jacA = *this;
+		SimdScalar lin = massInvA * jacA.m_jointAxis.dot(jacB.m_jointAxis);
+		SimdScalar 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)
+	SimdScalar	getNonDiagonal(const JacobianEntry& jacB,const SimdScalar massInvA,const SimdScalar massInvB) const
+	{
+		const JacobianEntry& jacA = *this;
+		SimdVector3 lin = jacA.m_jointAxis* jacB.m_jointAxis;
+		SimdVector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ;
+		SimdVector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ;
+		SimdVector3 lin0 = massInvA * lin ;
+		SimdVector3 lin1 = massInvB * lin;
+		SimdVector3 sum = ang0+ang1+lin0+lin1;
+		return sum[0]+sum[1]+sum[2];
+	}
+
+	SimdScalar getRelativeVelocity(const SimdVector3& linvelA,const SimdVector3& angvelA,const SimdVector3& linvelB,const SimdVector3& angvelB)
+	{
+		SimdVector3 linrel = linvelA - linvelB;
+		SimdVector3 angvela  = angvelA * m_aJ;
+		SimdVector3 angvelb  = angvelB * m_bJ;
+		linrel *= m_jointAxis;
+		angvela += angvelb;
+		angvela += linrel;
+		SimdScalar rel_vel2 = angvela[0]+angvela[1]+angvela[2];
+		return rel_vel2 + SIMD_EPSILON;
+	}
+//private:
+
+	SimdVector3	m_jointAxis;
+	SimdVector3	m_aJ;
+	SimdVector3	m_bJ;
+	SimdVector3	m_0MinvJt;
+	SimdVector3	m_1MinvJt;
+	//Optimization: can be stored in the w/last component of one of the vectors
+	SimdScalar	m_Adiag;
+
+};
+
+#endif //JACOBIAN_ENTRY_H

BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp

@@ -0,0 +1,265 @@
+/*
+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 "OdeConstraintSolver.h"
+
+#include "NarrowPhaseCollision/PersistentManifold.h"
+#include "Dynamics/RigidBody.h"
+#include "ContactConstraint.h"
+#include "Solve2LinearConstraint.h"
+#include "ContactSolverInfo.h"
+#include "Dynamics/BU_Joint.h"
+#include "Dynamics/ContactJoint.h"
+
+#include "IDebugDraw.h"
+
+#define USE_SOR_SOLVER
+
+#include "SorLcp.h"
+
+#include <math.h>
+#include <float.h>//FLT_MAX
+#ifdef WIN32
+#include <memory.h>
+#endif
+#include <string.h>
+#include <stdio.h>
+
+#if defined (WIN32)
+#include <malloc.h>
+#else
+#if defined (__FreeBSD__)
+#include <stdlib.h>
+#else
+#include <alloca.h>
+#endif
+#endif
+
+class BU_Joint;
+
+//see below
+
+OdeConstraintSolver::OdeConstraintSolver():
+m_cfm(0.f),//1e-5f),
+m_erp(0.4f)
+{
+}
+
+
+
+
+
+
+
+//iterative lcp and penalty method
+float OdeConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numManifolds,const ContactSolverInfo& infoGlobal,IDebugDraw* debugDrawer)
+{
+	m_CurBody = 0;
+	m_CurJoint = 0;
+
+
+	RigidBody* bodies [MAX_RIGIDBODIES];
+
+	int numBodies = 0;
+	BU_Joint* joints [MAX_RIGIDBODIES*4];
+	int numJoints = 0;
+
+	for (int j=0;j<numManifolds;j++)
+	{
+
+		int body0=-1,body1=-1;
+
+		PersistentManifold* manifold = manifoldPtr[j];
+		if (manifold->GetNumContacts() > 0)
+		{
+			body0 = ConvertBody((RigidBody*)manifold->GetBody0(),bodies,numBodies);
+			body1 = ConvertBody((RigidBody*)manifold->GetBody1(),bodies,numBodies);
+			ConvertConstraint(manifold,joints,numJoints,bodies,body0,body1,debugDrawer);
+		}
+	}
+
+	SolveInternal1(m_cfm,m_erp,bodies,numBodies,joints,numJoints,infoGlobal);
+
+	return 0.f;
+
+}
+
+/////////////////////////////////////////////////////////////////////////////////
+
+
+typedef SimdScalar dQuaternion[4];
+#define _R(i,j) R[(i)*4+(j)]
+
+void dRfromQ1 (dMatrix3 R, const dQuaternion q)
+{
+  // q = (s,vx,vy,vz)
+  SimdScalar qq1 = 2.f*q[1]*q[1];
+  SimdScalar qq2 = 2.f*q[2]*q[2];
+  SimdScalar qq3 = 2.f*q[3]*q[3];
+  _R(0,0) = 1.f - qq2 - qq3;
+  _R(0,1) = 2*(q[1]*q[2] - q[0]*q[3]);
+  _R(0,2) = 2*(q[1]*q[3] + q[0]*q[2]);
+  _R(0,3) = 0.f;
+	
+  _R(1,0) = 2*(q[1]*q[2] + q[0]*q[3]);
+  _R(1,1) = 1.f - qq1 - qq3;
+  _R(1,2) = 2*(q[2]*q[3] - q[0]*q[1]);
+  _R(1,3) = 0.f;
+
+  _R(2,0) = 2*(q[1]*q[3] - q[0]*q[2]);
+  _R(2,1) = 2*(q[2]*q[3] + q[0]*q[1]);
+  _R(2,2) = 1.f - qq1 - qq2;
+  _R(2,3) = 0.f;
+
+}
+
+
+
+int OdeConstraintSolver::ConvertBody(RigidBody* body,RigidBody** bodies,int& numBodies)
+{
+	if (!body || (body->getInvMass() == 0.f) )
+	{
+		return -1;
+	}
+	//first try to find
+	int i,j;
+	for (i=0;i<numBodies;i++)
+	{
+		if (bodies[i] == body)
+			return i;
+	}
+	//if not found, create a new body
+	bodies[numBodies++] = body;
+	//convert data
+
+
+	body->m_facc.setValue(0,0,0,0);
+	body->m_tacc.setValue(0,0,0,0);
+	
+	//are the indices the same ?
+	for (i=0;i<4;i++)
+	{
+		for ( j=0;j<3;j++)
+		{
+			body->m_invI[i+4*j] = 0.f;
+			body->m_I[i+4*j] = 0.f;
+		}
+	}
+	body->m_invI[0+4*0] = 	body->getInvInertiaDiagLocal()[0];
+	body->m_invI[1+4*1] = 	body->getInvInertiaDiagLocal()[1];
+	body->m_invI[2+4*2] = 	body->getInvInertiaDiagLocal()[2];
+
+	body->m_I[0+0*4] = 1.f/body->getInvInertiaDiagLocal()[0];
+	body->m_I[1+1*4] = 1.f/body->getInvInertiaDiagLocal()[1];
+	body->m_I[2+2*4] = 1.f/body->getInvInertiaDiagLocal()[2];
+	
+
+	
+	
+	dQuaternion q;
+
+	q[1] = body->getOrientation()[0];
+	q[2] = body->getOrientation()[1];
+	q[3] = body->getOrientation()[2];
+	q[0] = body->getOrientation()[3];
+	
+	dRfromQ1(body->m_R,q);
+	
+	return numBodies-1;
+}
+
+
+
+
+	
+#define MAX_JOINTS_1 8192
+
+static ContactJoint gJointArray[MAX_JOINTS_1];
+
+
+void OdeConstraintSolver::ConvertConstraint(PersistentManifold* manifold,BU_Joint** joints,int& numJoints,
+					   RigidBody** bodies,int _bodyId0,int _bodyId1,IDebugDraw* debugDrawer)
+{
+
+
+	manifold->RefreshContactPoints(((RigidBody*)manifold->GetBody0())->getCenterOfMassTransform(),
+		((RigidBody*)manifold->GetBody1())->getCenterOfMassTransform());
+
+	int bodyId0 = _bodyId0,bodyId1 = _bodyId1;
+
+	int i,numContacts = manifold->GetNumContacts();
+	
+	bool swapBodies = (bodyId0 < 0);
+
+	
+	RigidBody* body0,*body1;
+
+	if (swapBodies)
+	{
+		bodyId0 = _bodyId1;
+		bodyId1 = _bodyId0;
+
+		body0 = (RigidBody*)manifold->GetBody1();
+		body1 = (RigidBody*)manifold->GetBody0();
+
+	} else
+	{
+		body0 = (RigidBody*)manifold->GetBody0();
+		body1 = (RigidBody*)manifold->GetBody1();
+	}
+
+	assert(bodyId0 >= 0);
+
+	SimdVector3 color(0,1,0);
+	for (i=0;i<numContacts;i++)
+	{
+
+		if (debugDrawer)
+		{
+			const ManifoldPoint& cp = manifold->GetContactPoint(i);
+
+			debugDrawer->DrawContactPoint(
+				cp.m_positionWorldOnB,
+				cp.m_normalWorldOnB,
+				cp.GetDistance(),
+				cp.GetLifeTime(),
+				color);
+
+		}
+		assert (m_CurJoint < MAX_JOINTS_1);
+
+//		if (manifold->GetContactPoint(i).GetDistance() < 0.0f)
+		{
+			ContactJoint* cont = new (&gJointArray[m_CurJoint++]) ContactJoint( manifold ,i, swapBodies,body0,body1);
+
+			cont->node[0].joint = cont;
+			cont->node[0].body = bodyId0 >= 0 ? bodies[bodyId0] : 0;
+			
+			cont->node[1].joint = cont;
+			cont->node[1].body = bodyId1 >= 0 ? bodies[bodyId1] : 0;
+			
+			joints[numJoints++] = cont;
+			for (int i=0;i<6;i++)
+				cont->lambda[i] = 0.f;
+
+			cont->flags = 0;
+		}
+	}
+
+	//create a new contact constraint
+};
+

BulletDynamics/ConstraintSolver/OdeConstraintSolver.h

@@ -0,0 +1,66 @@
+/*
+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 ODE_CONSTRAINT_SOLVER_H
+#define ODE_CONSTRAINT_SOLVER_H
+
+#include "ConstraintSolver.h"
+
+class RigidBody;
+class BU_Joint;
+
+/// OdeConstraintSolver is one of the available solvers for Bullet dynamics framework
+/// It uses the the unmodified version of quickstep solver from the open dynamics project
+class OdeConstraintSolver : public ConstraintSolver
+{
+private:
+
+	int m_CurBody;
+	int m_CurJoint;
+
+	float	m_cfm;
+	float	m_erp;
+	
+
+	int ConvertBody(RigidBody* body,RigidBody** bodies,int& numBodies);
+	void ConvertConstraint(PersistentManifold* manifold,BU_Joint** joints,int& numJoints,
+					   RigidBody** bodies,int _bodyId0,int _bodyId1,IDebugDraw* debugDrawer);
+
+public:
+
+	OdeConstraintSolver();
+
+	virtual ~OdeConstraintSolver() {}
+	
+	virtual float SolveGroup(PersistentManifold** manifold,int numManifolds,const ContactSolverInfo& info,IDebugDraw* debugDrawer = 0);
+
+	///setConstraintForceMixing, the cfm adds some positive value to the main diagonal
+	///This can improve convergence (make matrix positive semidefinite), but it can make the simulation look more 'springy'
+	void	setConstraintForceMixing(float cfm) { 
+		m_cfm  = cfm;
+	}
+
+	///setErrorReductionParamter sets the maximum amount of error reduction
+	///which limits energy addition during penetration depth recovery
+	void	setErrorReductionParamter(float erp)
+	{
+		m_erp = erp;
+	}
+};
+
+
+
+
+#endif //ODE_CONSTRAINT_SOLVER_H

BulletDynamics/ConstraintSolver/Point2PointConstraint.cpp

@@ -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.
+*/
+
+
+#include "Point2PointConstraint.h"
+#include "Dynamics/RigidBody.h"
+#include "Dynamics/MassProps.h"
+
+
+
+
+Point2PointConstraint::Point2PointConstraint()
+{
+}
+
+Point2PointConstraint::Point2PointConstraint(RigidBody& rbA,RigidBody& rbB, const SimdVector3& pivotInA,const SimdVector3& pivotInB)
+:TypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB)
+{
+
+}
+
+
+Point2PointConstraint::Point2PointConstraint(RigidBody& rbA,const SimdVector3& pivotInA)
+:TypedConstraint(rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA))
+{
+	
+}
+
+void	Point2PointConstraint::BuildJacobian()
+{
+	SimdVector3	normal(0,0,0);
+
+	for (int i=0;i<3;i++)
+	{
+		normal[i] = 1;
+		new (&m_jac[i]) JacobianEntry(
+			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	Point2PointConstraint::SolveConstraint(SimdScalar	timeStep)
+{
+	SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
+	SimdVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
+
+
+	SimdVector3 normal(0,0,0);
+	
+
+//	SimdVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
+//	SimdVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
+
+	for (int i=0;i<3;i++)
+	{		
+		normal[i] = 1;
+		SimdScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal();
+
+		SimdVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
+		SimdVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+		//this jacobian entry could be re-used for all iterations
+		
+		SimdVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
+		SimdVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
+		SimdVector3 vel = vel1 - vel2;
+		
+		SimdScalar rel_vel;
+		rel_vel = normal.dot(vel);
+
+	/*
+		//velocity error (first order error)
+		SimdScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
+														m_rbB.getLinearVelocity(),angvelB);
+	*/
+	
+		//positional error (zeroth order error)
+		SimdScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
+		
+		SimdScalar impulse = depth*m_setting.m_tau/timeStep  * jacDiagABInv -  m_setting.m_damping * rel_vel * jacDiagABInv;
+
+		SimdVector3 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	Point2PointConstraint::UpdateRHS(SimdScalar	timeStep)
+{
+
+}
+

BulletDynamics/ConstraintSolver/Point2PointConstraint.h

@@ -0,0 +1,78 @@
+/*
+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 "SimdVector3.h"
+
+#include "ConstraintSolver/JacobianEntry.h"
+#include "TypedConstraint.h"
+
+class RigidBody;
+
+struct	ConstraintSetting
+{
+	ConstraintSetting()	:
+		m_tau(0.3f),
+		m_damping(1.f)
+	{
+	}
+	float		m_tau;
+	float		m_damping;
+};
+
+/// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space
+class Point2PointConstraint : public TypedConstraint
+{
+	JacobianEntry	m_jac[3]; //3 orthogonal linear constraints
+	
+	SimdVector3	m_pivotInA;
+	SimdVector3	m_pivotInB;
+	
+	
+	
+public:
+
+	ConstraintSetting	m_setting;
+
+	Point2PointConstraint(RigidBody& rbA,RigidBody& rbB, const SimdVector3& pivotInA,const SimdVector3& pivotInB);
+
+	Point2PointConstraint(RigidBody& rbA,const SimdVector3& pivotInA);
+
+	Point2PointConstraint();
+
+	virtual void	BuildJacobian();
+
+
+	virtual	void	SolveConstraint(SimdScalar	timeStep);
+
+	void	UpdateRHS(SimdScalar	timeStep);
+
+	void	SetPivotA(const SimdVector3& pivotA)
+	{
+		m_pivotInA = pivotA;
+	}
+
+	void	SetPivotB(const SimdVector3& pivotB)
+	{
+		m_pivotInB = pivotB;
+	}
+
+
+
+};
+
+#endif //POINT2POINTCONSTRAINT_H

BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp

@@ -0,0 +1,329 @@
+/*
+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 "SimpleConstraintSolver.h"
+#include "NarrowPhaseCollision/PersistentManifold.h"
+#include "Dynamics/RigidBody.h"
+#include "ContactConstraint.h"
+#include "Solve2LinearConstraint.h"
+#include "ContactSolverInfo.h"
+#include "Dynamics/BU_Joint.h"
+#include "Dynamics/ContactJoint.h"
+#include "IDebugDraw.h"
+#include "JacobianEntry.h"
+#include "GEN_MinMax.h"
+
+#ifdef USE_PROFILE
+#include "quickprof.h"
+#endif //USE_PROFILE
+
+int totalCpd = 0;
+
+
+
+bool  MyContactDestroyedCallback(void* userPersistentData)
+{
+	assert (userPersistentData);
+	ConstraintPersistentData* cpd = (ConstraintPersistentData*)userPersistentData;
+	delete cpd;
+	totalCpd--;
+	//printf("totalCpd = %i. DELETED Ptr %x\n",totalCpd,userPersistentData);
+	return true;
+}
+
+
+SimpleConstraintSolver::SimpleConstraintSolver()
+{
+	gContactCallback = &MyContactDestroyedCallback;
+}
+
+
+/// SimpleConstraintSolver Sequentially applies impulses
+float SimpleConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numManifolds,const ContactSolverInfo& infoGlobal,IDebugDraw* debugDrawer)
+{
+	
+	ContactSolverInfo info = infoGlobal;
+
+	int numiter = infoGlobal.m_numIterations;
+#ifdef USE_PROFILE
+	Profiler::beginBlock("Solve");
+#endif //USE_PROFILE
+
+	//should traverse the contacts random order...
+	int i;
+	for ( i = 0;i<numiter;i++)
+	{
+		int j;
+		for (j=0;j<numManifolds;j++)
+		{
+			int k=j;
+			if (i&1)
+				k=numManifolds-j-1;
+
+			Solve(manifoldPtr[k],info,i,debugDrawer);
+		}
+		
+	}
+#ifdef USE_PROFILE
+	Profiler::endBlock("Solve");
+
+	Profiler::beginBlock("SolveFriction");
+#endif //USE_PROFILE
+
+	//now solve the friction		
+	for (i = 0;i<numiter;i++)
+	{
+		int j;
+	for (j=0;j<numManifolds;j++)
+		{
+			int k = j;
+			if (i&1)
+				k=numManifolds-j-1;
+			SolveFriction(manifoldPtr[k],info,i,debugDrawer);
+		}
+	}
+#ifdef USE_PROFILE
+	Profiler::endBlock("SolveFriction");
+#endif //USE_PROFILE
+
+	return 0.f;
+}
+
+
+float penetrationResolveFactor = 0.9f;
+SimdScalar restitutionCurve(SimdScalar rel_vel, SimdScalar restitution)
+{
+	SimdScalar rest = restitution * -rel_vel;
+	return rest;
+}
+
+
+
+
+float SimpleConstraintSolver::Solve(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer)
+{
+
+	RigidBody* body0 = (RigidBody*)manifoldPtr->GetBody0();
+	RigidBody* body1 = (RigidBody*)manifoldPtr->GetBody1();
+
+	float maxImpulse = 0.f;
+
+	//only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
+	if (iter == 0)
+	{
+		manifoldPtr->RefreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform());
+		
+		int numpoints = manifoldPtr->GetNumContacts();
+
+		SimdVector3 color(0,1,0);
+		for (int i=0;i<numpoints ;i++)
+		{
+			ManifoldPoint& cp = manifoldPtr->GetContactPoint(i);
+			if (cp.GetDistance() <= 0.f)
+			{
+				const SimdVector3& pos1 = cp.GetPositionWorldOnA();
+				const SimdVector3& pos2 = cp.GetPositionWorldOnB();
+
+				SimdVector3 rel_pos1 = pos1 - body0->getCenterOfMassPosition(); 
+				SimdVector3 rel_pos2 = pos2 - body1->getCenterOfMassPosition();
+				
+
+				//this jacobian entry is re-used for all iterations
+				JacobianEntry jac(body0->getCenterOfMassTransform().getBasis().transpose(),
+					body1->getCenterOfMassTransform().getBasis().transpose(),
+					rel_pos1,rel_pos2,cp.m_normalWorldOnB,body0->getInvInertiaDiagLocal(),body0->getInvMass(),
+					body1->getInvInertiaDiagLocal(),body1->getInvMass());
+
+				
+				SimdScalar jacDiagAB = jac.getDiagonal();
+
+				ConstraintPersistentData* cpd = (ConstraintPersistentData*) cp.m_userPersistentData;
+				if (cpd)
+				{
+					//might be invalid
+					cpd->m_persistentLifeTime++;
+					if (cpd->m_persistentLifeTime != cp.GetLifeTime())
+					{
+						//printf("Invalid: cpd->m_persistentLifeTime = %i cp.GetLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.GetLifeTime());
+						new (cpd) ConstraintPersistentData;
+						cpd->m_persistentLifeTime = cp.GetLifeTime();
+
+					} else
+					{
+						//printf("Persistent: cpd->m_persistentLifeTime = %i cp.GetLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.GetLifeTime());
+						
+					}
+				} else
+				{
+						
+					cpd = new ConstraintPersistentData();
+					totalCpd ++;
+					//printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);
+					cp.m_userPersistentData = cpd;
+					cpd->m_persistentLifeTime = cp.GetLifeTime();
+					//printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.GetLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.GetLifeTime());
+					
+				}
+				assert(cpd);
+
+				cpd->m_jacDiagABInv = 1.f / jacDiagAB;
+
+
+				SimdVector3 vel1 = body0->getVelocityInLocalPoint(rel_pos1);
+				SimdVector3 vel2 = body1->getVelocityInLocalPoint(rel_pos2);
+				SimdVector3 vel = vel1 - vel2;
+				SimdScalar rel_vel;
+				rel_vel = cp.m_normalWorldOnB.dot(vel);
+				
+				float combinedRestitution = body0->getRestitution() * body1->getRestitution();
+
+				cpd->m_penetration = cp.GetDistance();
+
+				cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution);
+				if (cpd->m_restitution <= 0.) //0.f)
+				{
+					cpd->m_restitution = 0.0f;
+
+				};
+				
+				//restitution and penetration work in same direction so
+				//rel_vel 
+				
+				SimdScalar penVel = -cpd->m_penetration/info.m_timeStep;
+
+				if (cpd->m_restitution >= penVel)
+				{
+					cpd->m_penetration = 0.f;
+				} 				
+				
+
+				float relaxation = info.m_damping;
+				cpd->m_appliedImpulse *= relaxation;
+				//for friction
+				cpd->m_prevAppliedImpulse = cpd->m_appliedImpulse;
+				
+				//re-calculate friction direction every frame, todo: check if this is really needed
+				SimdPlaneSpace1(cp.m_normalWorldOnB,cpd->m_frictionWorldTangential0,cpd->m_frictionWorldTangential1);
+
+
+#define NO_FRICTION_WARMSTART 1
+
+	#ifdef NO_FRICTION_WARMSTART
+				cpd->m_accumulatedTangentImpulse0 = 0.f;
+				cpd->m_accumulatedTangentImpulse1 = 0.f;
+	#endif //NO_FRICTION_WARMSTART
+				float denom0 = body0->ComputeImpulseDenominator(pos1,cpd->m_frictionWorldTangential0);
+				float denom1 = body1->ComputeImpulseDenominator(pos2,cpd->m_frictionWorldTangential0);
+				float denom = relaxation/(denom0+denom1);
+				cpd->m_jacDiagABInvTangent0 = denom;
+
+
+				denom0 = body0->ComputeImpulseDenominator(pos1,cpd->m_frictionWorldTangential1);
+				denom1 = body1->ComputeImpulseDenominator(pos2,cpd->m_frictionWorldTangential1);
+				denom = relaxation/(denom0+denom1);
+				cpd->m_jacDiagABInvTangent1 = denom;
+
+				SimdVector3 totalImpulse = 
+	#ifndef NO_FRICTION_WARMSTART
+					cp.m_frictionWorldTangential0*cp.m_accumulatedTangentImpulse0+
+					cp.m_frictionWorldTangential1*cp.m_accumulatedTangentImpulse1+
+	#endif //NO_FRICTION_WARMSTART
+					cp.m_normalWorldOnB*cpd->m_appliedImpulse;
+
+				//apply previous frames impulse on both bodies
+				body0->applyImpulse(totalImpulse, rel_pos1);
+				body1->applyImpulse(-totalImpulse, rel_pos2);
+			}
+			
+		}
+	}
+
+	{
+		const int numpoints = manifoldPtr->GetNumContacts();
+
+		SimdVector3 color(0,1,0);
+		for (int i=0;i<numpoints ;i++)
+		{
+
+			int j=i;
+			if (iter % 2)
+				j = numpoints-1-i;
+			else
+				j=i;
+
+			ManifoldPoint& cp = manifoldPtr->GetContactPoint(j);
+				if (cp.GetDistance() <= 0.f)
+			{
+
+				if (iter == 0)
+				{
+					if (debugDrawer)
+						debugDrawer->DrawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.GetDistance(),cp.GetLifeTime(),color);
+				}
+
+				{
+
+
+					//float dist =  cp.GetDistance();
+					//printf("dist(%i)=%f\n",j,dist);
+					float impulse = resolveSingleCollision(
+						*body0,*body1,
+						cp,
+						info);
+					
+					if (maxImpulse < impulse)
+						maxImpulse  = impulse;
+
+				}
+			}
+		}
+	}
+	return maxImpulse;
+}
+
+float SimpleConstraintSolver::SolveFriction(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer)
+{
+	RigidBody* body0 = (RigidBody*)manifoldPtr->GetBody0();
+	RigidBody* body1 = (RigidBody*)manifoldPtr->GetBody1();
+
+
+	{
+		const int numpoints = manifoldPtr->GetNumContacts();
+
+		SimdVector3 color(0,1,0);
+		for (int i=0;i<numpoints ;i++)
+		{
+
+			int j=i;
+			//if (iter % 2)
+			//	j = numpoints-1-i;
+
+			ManifoldPoint& cp = manifoldPtr->GetContactPoint(j);
+				if (cp.GetDistance() <= 0.f)
+			{
+
+				resolveSingleFriction(
+					*body0,*body1,
+					cp,
+					info);
+				
+			}
+		}
+
+	
+	}
+	return 0.f;
+}

BulletDynamics/ConstraintSolver/SimpleConstraintSolver.h

@@ -0,0 +1,43 @@
+/*
+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 SIMPLE_CONSTRAINT_SOLVER_H
+#define SIMPLE_CONSTRAINT_SOLVER_H
+
+#include "ConstraintSolver.h"
+class IDebugDraw;
+
+/// SimpleConstraintSolver 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 SimpleConstraintSolver : public ConstraintSolver
+{
+	float Solve(PersistentManifold* manifold, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer);
+	float SolveFriction(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer);
+	
+
+public:
+
+	SimpleConstraintSolver();
+
+	virtual ~SimpleConstraintSolver() {}
+	
+	virtual float SolveGroup(PersistentManifold** manifold,int numManifolds,const ContactSolverInfo& info, IDebugDraw* debugDrawer=0);
+
+};
+
+#endif //SIMPLE_CONSTRAINT_SOLVER_H
+

BulletDynamics/ConstraintSolver/Solve2LinearConstraint.cpp

@@ -0,0 +1,241 @@
+/*
+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 "Solve2LinearConstraint.h"
+
+#include "Dynamics/RigidBody.h"
+#include "SimdVector3.h"
+#include "JacobianEntry.h"
+
+
+void Solve2LinearConstraint::resolveUnilateralPairConstraint(
+												   RigidBody* body1,
+		RigidBody* body2,
+
+						const SimdMatrix3x3& world2A,
+						const SimdMatrix3x3& world2B,
+						
+						const SimdVector3& invInertiaADiag,
+						const SimdScalar invMassA,
+						const SimdVector3& linvelA,const SimdVector3& angvelA,
+						const SimdVector3& rel_posA1,
+						const SimdVector3& invInertiaBDiag,
+						const SimdScalar invMassB,
+						const SimdVector3& linvelB,const SimdVector3& angvelB,
+						const SimdVector3& rel_posA2,
+
+					  SimdScalar depthA, const SimdVector3& normalA, 
+					  const SimdVector3& rel_posB1,const SimdVector3& rel_posB2,
+					  SimdScalar depthB, const SimdVector3& normalB, 
+					  SimdScalar& imp0,SimdScalar& imp1)
+{
+
+	imp0 = 0.f;
+	imp1 = 0.f;
+
+	SimdScalar len = fabs(normalA.length())-1.f;
+	if (fabs(len) >= SIMD_EPSILON)
+		return;
+
+	ASSERT(len < SIMD_EPSILON);
+
+
+	//this jacobian entry could be re-used for all iterations
+	JacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
+		invInertiaBDiag,invMassB);
+	JacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
+		invInertiaBDiag,invMassB);
+	
+	//const SimdScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
+	//const SimdScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
+
+	const SimdScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1));
+	const SimdScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1));
+
+//	SimdScalar penetrationImpulse = (depth*contactTau*timeCorrection)  * massTerm;//jacDiagABInv
+	SimdScalar massTerm = 1.f / (invMassA + invMassB);
+
+
+	// calculate rhs (or error) terms
+	const SimdScalar dv0 = depthA  * m_tau * massTerm - vel0 * m_damping;
+	const SimdScalar 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!)
+	// 
+
+
+	SimdScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
+	SimdScalar	invDet = 1.0f / (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 Solve2LinearConstraint::resolveBilateralPairConstraint(
+						RigidBody* body1,
+						RigidBody* body2,
+						const SimdMatrix3x3& world2A,
+						const SimdMatrix3x3& world2B,
+						
+						const SimdVector3& invInertiaADiag,
+						const SimdScalar invMassA,
+						const SimdVector3& linvelA,const SimdVector3& angvelA,
+						const SimdVector3& rel_posA1,
+						const SimdVector3& invInertiaBDiag,
+						const SimdScalar invMassB,
+						const SimdVector3& linvelB,const SimdVector3& angvelB,
+						const SimdVector3& rel_posA2,
+
+					  SimdScalar depthA, const SimdVector3& normalA, 
+					  const SimdVector3& rel_posB1,const SimdVector3& rel_posB2,
+					  SimdScalar depthB, const SimdVector3& normalB, 
+					  SimdScalar& imp0,SimdScalar& imp1)
+{
+
+	imp0 = 0.f;
+	imp1 = 0.f;
+
+	SimdScalar len = fabs(normalA.length())-1.f;
+	if (fabs(len) >= SIMD_EPSILON)
+		return;
+
+	ASSERT(len < SIMD_EPSILON);
+
+
+	//this jacobian entry could be re-used for all iterations
+	JacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
+		invInertiaBDiag,invMassB);
+	JacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
+		invInertiaBDiag,invMassB);
+	
+	//const SimdScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
+	//const SimdScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
+
+	const SimdScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1));
+	const SimdScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1));
+
+	// calculate rhs (or error) terms
+	const SimdScalar dv0 = depthA  * m_tau - vel0 * m_damping;
+	const SimdScalar dv1 = depthB  * m_tau - vel1 * m_damping;
+
+	// dC/dv * dv = -C
+	
+	// jacobian * impulse = -error
+	//
+
+	//impulse = jacobianInverse * -error
+
+	// inverting 2x2 symmetric system (offdiagonal are equal!)
+	// 
+
+
+	SimdScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
+	SimdScalar	invDet = 1.0f / (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 > 0.0f)
+	{
+		if ( imp1 > 0.0f )
+		{
+			//both positive
+		}
+		else
+		{
+			imp1 = 0.f;
+
+			// now imp0>0 imp1<0
+			imp0 = dv0 / jacA.getDiagonal();
+			if ( imp0 > 0.0f )
+			{
+			} else
+			{
+				imp0 = 0.f;
+			}
+		}
+	}
+	else
+	{
+		imp0 = 0.f;
+
+		imp1 = dv1 / jacB.getDiagonal();
+		if ( imp1 <= 0.0f )
+		{
+			imp1 = 0.f;
+			// now imp0>0 imp1<0
+			imp0 = dv0 / jacA.getDiagonal();
+			if ( imp0 > 0.0f )
+			{
+			} else
+			{
+				imp0 = 0.f;
+			}
+		} else
+		{
+		}
+	}
+}
+
+
+
+void Solve2LinearConstraint::resolveAngularConstraint(	const SimdMatrix3x3& invInertiaAWS,
+											const SimdScalar invMassA,
+											const SimdVector3& linvelA,const SimdVector3& angvelA,
+											const SimdVector3& rel_posA1,
+											const SimdMatrix3x3& invInertiaBWS,
+											const SimdScalar invMassB,
+											const SimdVector3& linvelB,const SimdVector3& angvelB,
+											const SimdVector3& rel_posA2,
+
+											SimdScalar depthA, const SimdVector3& normalA, 
+											const SimdVector3& rel_posB1,const SimdVector3& rel_posB2,
+											SimdScalar depthB, const SimdVector3& normalB, 
+											SimdScalar& imp0,SimdScalar& imp1)
+{
+
+}
+

BulletDynamics/ConstraintSolver/Solve2LinearConstraint.h

@@ -0,0 +1,106 @@
+/*
+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 "SimdMatrix3x3.h"
+#include "SimdVector3.h"
+
+
+class RigidBody;
+
+
+
+/// constraint class used for lateral tyre friction.
+class	Solve2LinearConstraint
+{
+	SimdScalar	m_tau;
+	SimdScalar	m_damping;
+
+public:
+
+	Solve2LinearConstraint(SimdScalar tau,SimdScalar damping)
+	{
+		m_tau = tau;
+		m_damping = damping;
+	}
+	//
+	// solve unilateral constraint (equality, direct method)
+	//
+	void resolveUnilateralPairConstraint(		
+														   RigidBody* body0,
+		RigidBody* body1,
+
+		const SimdMatrix3x3& world2A,
+						const SimdMatrix3x3& world2B,
+						
+						const SimdVector3& invInertiaADiag,
+						const SimdScalar invMassA,
+						const SimdVector3& linvelA,const SimdVector3& angvelA,
+						const SimdVector3& rel_posA1,
+						const SimdVector3& invInertiaBDiag,
+						const SimdScalar invMassB,
+						const SimdVector3& linvelB,const SimdVector3& angvelB,
+						const SimdVector3& rel_posA2,
+
+					  SimdScalar depthA, const SimdVector3& normalA, 
+					  const SimdVector3& rel_posB1,const SimdVector3& rel_posB2,
+					  SimdScalar depthB, const SimdVector3& normalB, 
+					  SimdScalar& imp0,SimdScalar& imp1);
+
+
+	//
+	// solving 2x2 lcp problem (inequality, direct solution )
+	//
+	void resolveBilateralPairConstraint(
+			RigidBody* body0,
+						RigidBody* body1,
+		const SimdMatrix3x3& world2A,
+						const SimdMatrix3x3& world2B,
+						
+						const SimdVector3& invInertiaADiag,
+						const SimdScalar invMassA,
+						const SimdVector3& linvelA,const SimdVector3& angvelA,
+						const SimdVector3& rel_posA1,
+						const SimdVector3& invInertiaBDiag,
+						const SimdScalar invMassB,
+						const SimdVector3& linvelB,const SimdVector3& angvelB,
+						const SimdVector3& rel_posA2,
+
+					  SimdScalar depthA, const SimdVector3& normalA, 
+					  const SimdVector3& rel_posB1,const SimdVector3& rel_posB2,
+					  SimdScalar depthB, const SimdVector3& normalB, 
+					  SimdScalar& imp0,SimdScalar& imp1);
+
+
+	void resolveAngularConstraint(	const SimdMatrix3x3& invInertiaAWS,
+						const SimdScalar invMassA,
+						const SimdVector3& linvelA,const SimdVector3& angvelA,
+						const SimdVector3& rel_posA1,
+						const SimdMatrix3x3& invInertiaBWS,
+						const SimdScalar invMassB,
+						const SimdVector3& linvelB,const SimdVector3& angvelB,
+						const SimdVector3& rel_posA2,
+
+					  SimdScalar depthA, const SimdVector3& normalA, 
+					  const SimdVector3& rel_posB1,const SimdVector3& rel_posB2,
+					  SimdScalar depthB, const SimdVector3& normalB, 
+					  SimdScalar& imp0,SimdScalar& imp1);
+
+
+};
+
+#endif //SOLVE_2LINEAR_CONSTRAINT_H

BulletDynamics/ConstraintSolver/SorLcp.cpp

@@ -0,0 +1,849 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
+ * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
+ *                                                                       *
+ * This library is free software; you can redistribute it and/or         *
+ * modify it under the terms of EITHER:                                  *
+ *   (1) The GNU Lesser General Public License as published by the Free  *
+ *       Software Foundation; either version 2.1 of the License, or (at  *
+ *       your option) any later version. The text of the GNU Lesser      *
+ *       General Public License is included with this library in the     *
+ *       file LICENSE.TXT.                                               *
+ *   (2) The BSD-style license that is included with this library in     *
+ *       the file LICENSE-BSD.TXT.                                       *
+ *                                                                       *
+ * This library is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#include "SorLcp.h"
+
+#ifdef USE_SOR_SOLVER
+
+// SOR LCP taken from ode quickstep, 
+// todo: write own successive overrelaxation gauss-seidel, or jacobi iterative solver
+
+
+#include "SimdScalar.h"
+
+#include "Dynamics/RigidBody.h"
+#include <math.h>
+#include <float.h>//FLT_MAX
+#ifdef WIN32
+#include <memory.h>
+#endif
+#include <string.h>
+#include <stdio.h>
+
+#if defined (WIN32)
+#include <malloc.h>
+#else
+#if defined (__FreeBSD__)
+#include <stdlib.h>
+#else
+#include <alloca.h>
+#endif
+#endif
+
+#include "Dynamics/BU_Joint.h"
+#include "ContactSolverInfo.h"
+
+////////////////////////////////////////////////////////////////////
+//math stuff
+
+typedef SimdScalar dVector4[4];
+typedef SimdScalar dMatrix3[4*3];
+#define dInfinity FLT_MAX
+
+
+
+#define dRecip(x) ((float)(1.0f/(x)))				/* reciprocal */
+
+
+
+#define dMULTIPLY0_331NEW(A,op,B,C) \
+{\
+	float tmp[3];\
+	tmp[0] = C.getX();\
+	tmp[1] = C.getY();\
+	tmp[2] = C.getZ();\
+	dMULTIPLYOP0_331(A,op,B,tmp);\
+}
+
+#define dMULTIPLY0_331(A,B,C) dMULTIPLYOP0_331(A,=,B,C)
+#define dMULTIPLYOP0_331(A,op,B,C) \
+  (A)[0] op dDOT1((B),(C)); \
+  (A)[1] op dDOT1((B+4),(C)); \
+  (A)[2] op dDOT1((B+8),(C));
+
+#define dAASSERT ASSERT
+#define dIASSERT ASSERT
+
+#define REAL float
+#define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)])
+SimdScalar dDOT1  (const SimdScalar *a, const SimdScalar *b) { return dDOTpq(a,b,1,1); }
+#define dDOT14(a,b) dDOTpq(a,b,1,4)
+
+#define dCROSS(a,op,b,c) \
+  (a)[0] op ((b)[1]*(c)[2] - (b)[2]*(c)[1]); \
+  (a)[1] op ((b)[2]*(c)[0] - (b)[0]*(c)[2]); \
+  (a)[2] op ((b)[0]*(c)[1] - (b)[1]*(c)[0]);
+
+
+#define dMULTIPLYOP2_333(A,op,B,C) \
+  (A)[0] op dDOT1((B),(C)); \
+  (A)[1] op dDOT1((B),(C+4)); \
+  (A)[2] op dDOT1((B),(C+8)); \
+  (A)[4] op dDOT1((B+4),(C)); \
+  (A)[5] op dDOT1((B+4),(C+4)); \
+  (A)[6] op dDOT1((B+4),(C+8)); \
+  (A)[8] op dDOT1((B+8),(C)); \
+  (A)[9] op dDOT1((B+8),(C+4)); \
+  (A)[10] op dDOT1((B+8),(C+8));
+#define dMULTIPLYOP0_333(A,op,B,C) \
+  (A)[0] op dDOT14((B),(C)); \
+  (A)[1] op dDOT14((B),(C+1)); \
+  (A)[2] op dDOT14((B),(C+2)); \
+  (A)[4] op dDOT14((B+4),(C)); \
+  (A)[5] op dDOT14((B+4),(C+1)); \
+  (A)[6] op dDOT14((B+4),(C+2)); \
+  (A)[8] op dDOT14((B+8),(C)); \
+  (A)[9] op dDOT14((B+8),(C+1)); \
+  (A)[10] op dDOT14((B+8),(C+2));
+
+#define dMULTIPLY2_333(A,B,C) dMULTIPLYOP2_333(A,=,B,C)
+#define dMULTIPLY0_333(A,B,C) dMULTIPLYOP0_333(A,=,B,C)
+#define dMULTIPLYADD0_331(A,B,C) dMULTIPLYOP0_331(A,+=,B,C)
+
+
+////////////////////////////////////////////////////////////////////
+#define EFFICIENT_ALIGNMENT 16
+#define dEFFICIENT_SIZE(x) ((((x)-1)|(EFFICIENT_ALIGNMENT-1))+1)
+/* alloca aligned to the EFFICIENT_ALIGNMENT. note that this can waste
+ * up to 15 bytes per allocation, depending on what alloca() returns.
+ */
+
+#define dALLOCA16(n) \
+  ((char*)dEFFICIENT_SIZE(((size_t)(alloca((n)+(EFFICIENT_ALIGNMENT-1))))))
+
+
+
+/////////////////////////////////////////////////////////////////////
+/////////////////////////////////////////////////////////////////////
+
+#ifdef DEBUG
+#define ANSI_FTOL 1
+
+extern "C" { 
+    __declspec(naked) void _ftol2() {
+        __asm    {
+#if ANSI_FTOL
+            fnstcw   WORD PTR [esp-2]
+            mov      ax, WORD PTR [esp-2]
+			
+            OR AX,	 0C00h
+			
+            mov      WORD PTR [esp-4], ax
+            fldcw    WORD PTR [esp-4]
+            fistp    QWORD PTR [esp-12]
+            fldcw    WORD PTR [esp-2]
+            mov      eax, DWORD PTR [esp-12]
+            mov      edx, DWORD PTR [esp-8]
+#else
+            fistp    DWORD PTR [esp-12]
+            mov		 eax, DWORD PTR [esp-12]
+            mov		 ecx, DWORD PTR [esp-8]
+#endif
+            ret
+        }
+    }
+}
+#endif //DEBUG
+
+
+
+  
+
+
+#define ALLOCA dALLOCA16
+
+typedef const SimdScalar *dRealPtr;
+typedef SimdScalar *dRealMutablePtr;
+#define dRealArray(name,n) SimdScalar name[n];
+#define dRealAllocaArray(name,n) SimdScalar *name = (SimdScalar*) ALLOCA ((n)*sizeof(SimdScalar));
+
+void dSetZero1 (SimdScalar *a, int n)
+{
+  dAASSERT (a && n >= 0);
+  while (n > 0) {
+    *(a++) = 0;
+    n--;
+  }
+}
+
+void dSetValue1 (SimdScalar *a, int n, SimdScalar value)
+{
+  dAASSERT (a && n >= 0);
+  while (n > 0) {
+    *(a++) = value;
+    n--;
+  }
+}
+
+
+//***************************************************************************
+// configuration
+
+// for the SOR and CG methods:
+// uncomment the following line to use warm starting. this definitely
+// help for motor-driven joints. unfortunately it appears to hurt
+// with high-friction contacts using the SOR method. use with care
+
+//#define WARM_STARTING 1
+
+// for the SOR method:
+// uncomment the following line to randomly reorder constraint rows
+// during the solution. depending on the situation, this can help a lot
+// or hardly at all, but it doesn't seem to hurt.
+
+#define RANDOMLY_REORDER_CONSTRAINTS 1
+
+
+
+//***************************************************************************
+// various common computations involving the matrix J
+
+// compute iMJ = inv(M)*J'
+
+static void compute_invM_JT (int m, dRealMutablePtr J, dRealMutablePtr iMJ, int *jb,
+	RigidBody * const *body, dRealPtr invI)
+{
+	int i,j;
+	dRealMutablePtr iMJ_ptr = iMJ;
+	dRealMutablePtr J_ptr = J;
+	for (i=0; i<m; i++) {
+		int b1 = jb[i*2];	
+		int b2 = jb[i*2+1];
+		SimdScalar k = body[b1]->getInvMass();
+		for (j=0; j<3; j++) iMJ_ptr[j] = k*J_ptr[j];
+		dMULTIPLY0_331 (iMJ_ptr + 3, invI + 12*b1, J_ptr + 3);
+		if (b2 >= 0) {
+			k = body[b2]->getInvMass();
+			for (j=0; j<3; j++) iMJ_ptr[j+6] = k*J_ptr[j+6];
+			dMULTIPLY0_331 (iMJ_ptr + 9, invI + 12*b2, J_ptr + 9);
+		}
+		J_ptr += 12;
+		iMJ_ptr += 12;
+	}
+}
+
+#if 0
+static void multiply_invM_JTSpecial (int m, int nb, dRealMutablePtr iMJ, int *jb,
+	dRealMutablePtr in, dRealMutablePtr out,int onlyBody1,int onlyBody2)
+{
+	int i,j;
+
+		
+
+	dRealMutablePtr out_ptr1 = out + onlyBody1*6;
+	
+	for (j=0; j<6; j++) 
+		out_ptr1[j] = 0;
+
+	if (onlyBody2 >= 0)
+	{
+		out_ptr1 = out + onlyBody2*6;
+
+		for (j=0; j<6; j++) 
+			out_ptr1[j] = 0;
+	}
+
+	dRealPtr iMJ_ptr = iMJ;
+	for (i=0; i<m; i++) {
+
+		int b1 = jb[i*2];	
+
+		dRealMutablePtr out_ptr = out + b1*6;
+		if ((b1 == onlyBody1) || (b1 == onlyBody2))
+		{
+			for (j=0; j<6; j++) 
+				out_ptr[j] += iMJ_ptr[j] * in[i] ;
+		}
+			
+		iMJ_ptr += 6;
+
+		int b2 = jb[i*2+1];
+		if ((b2 == onlyBody1) || (b2 == onlyBody2))
+		{
+			if (b2 >= 0) 
+			{
+					out_ptr = out + b2*6;
+					for (j=0; j<6; j++) 
+						out_ptr[j] += iMJ_ptr[j] * in[i];
+			}
+		}
+		
+		iMJ_ptr += 6;
+		
+	}
+}
+#endif
+
+
+// compute out = inv(M)*J'*in.
+
+#if 0
+static void multiply_invM_JT (int m, int nb, dRealMutablePtr iMJ, int *jb,
+	dRealMutablePtr in, dRealMutablePtr out)
+{
+	int i,j;
+	dSetZero1 (out,6*nb);
+	dRealPtr iMJ_ptr = iMJ;
+	for (i=0; i<m; i++) {
+		int b1 = jb[i*2];	
+		int b2 = jb[i*2+1];
+		dRealMutablePtr out_ptr = out + b1*6;
+		for (j=0; j<6; j++) 
+			out_ptr[j] += iMJ_ptr[j] * in[i];
+		iMJ_ptr += 6;
+		if (b2 >= 0) {
+			out_ptr = out + b2*6;
+			for (j=0; j<6; j++) out_ptr[j] += iMJ_ptr[j] * in[i];
+		}
+		iMJ_ptr += 6;
+	}
+}
+#endif
+
+
+// compute out = J*in.
+
+static void multiply_J (int m, dRealMutablePtr J, int *jb,
+	dRealMutablePtr in, dRealMutablePtr out)
+{
+	int i,j;
+	dRealPtr J_ptr = J;
+	for (i=0; i<m; i++) {
+		int b1 = jb[i*2];	
+		int b2 = jb[i*2+1];
+		SimdScalar sum = 0;
+		dRealMutablePtr in_ptr = in + b1*6;
+		for (j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j];				
+		J_ptr += 6;
+		if (b2 >= 0) {
+			in_ptr = in + b2*6;
+			for (j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j];				
+		}
+		J_ptr += 6;
+		out[i] = sum;
+	}
+}
+
+//***************************************************************************
+// SOR-LCP method
+
+// nb is the number of bodies in the body array.
+// J is an m*12 matrix of constraint rows
+// jb is an array of first and second body numbers for each constraint row
+// invI is the global frame inverse inertia for each body (stacked 3x3 matrices)
+//
+// this returns lambda and fc (the constraint force).
+// note: fc is returned as inv(M)*J'*lambda, the constraint force is actually J'*lambda
+//
+// b, lo and hi are modified on exit
+
+
+struct IndexError {
+	SimdScalar error;		// error to sort on
+	int findex;
+	int index;		// row index
+};
+
+static unsigned long seed2 = 0;
+
+unsigned long dRand2()
+{
+  seed2 = (1664525L*seed2 + 1013904223L) & 0xffffffff;
+  return seed2;
+}
+
+int dRandInt2 (int n)
+{
+  float a = float(n) / 4294967296.0f;
+  return (int) (float(dRand2()) * a);
+}
+
+
+static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb, RigidBody * const *body,
+	dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr invMforce, dRealMutablePtr rhs,
+	dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex,
+	int numiter,float overRelax)
+{
+	const int num_iterations = numiter;
+	const float sor_w = overRelax;		// SOR over-relaxation parameter
+
+	int i,j;
+
+#ifdef WARM_STARTING
+	// for warm starting, this seems to be necessary to prevent
+	// jerkiness in motor-driven joints. i have no idea why this works.
+	for (i=0; i<m; i++) lambda[i] *= 0.9;
+#else
+	dSetZero1 (lambda,m);
+#endif
+
+	// the lambda computed at the previous iteration.
+	// this is used to measure error for when we are reordering the indexes.
+	dRealAllocaArray (last_lambda,m);
+
+	// a copy of the 'hi' vector in case findex[] is being used
+	dRealAllocaArray (hicopy,m);
+	memcpy (hicopy,hi,m*sizeof(float));
+
+	// precompute iMJ = inv(M)*J'
+	dRealAllocaArray (iMJ,m*12);
+	compute_invM_JT (m,J,iMJ,jb,body,invI);
+
+	// compute fc=(inv(M)*J')*lambda. we will incrementally maintain fc
+	// as we change lambda.
+#ifdef WARM_STARTING
+	multiply_invM_JT (m,nb,iMJ,jb,lambda,fc);
+#else
+	dSetZero1 (invMforce,nb*6);
+#endif
+
+	// precompute 1 / diagonals of A
+	dRealAllocaArray (Ad,m);
+	dRealPtr iMJ_ptr = iMJ;
+	dRealMutablePtr J_ptr = J;
+	for (i=0; i<m; i++) {
+		float sum = 0;
+		for (j=0; j<6; j++) sum += iMJ_ptr[j] * J_ptr[j];
+		if (jb[i*2+1] >= 0) {
+			for (j=6; j<12; j++) sum += iMJ_ptr[j] * J_ptr[j];
+		}
+		iMJ_ptr += 12;
+		J_ptr += 12;
+		Ad[i] = sor_w / sum;//(sum + cfm[i]);
+	}
+
+	// scale J and b by Ad
+	J_ptr = J;
+	for (i=0; i<m; i++) {
+		for (j=0; j<12; j++) {
+			J_ptr[0] *= Ad[i];
+			J_ptr++;
+		}
+		rhs[i] *= Ad[i];
+	}
+
+	// scale Ad by CFM
+	for (i=0; i<m; i++) 
+		Ad[i] *= cfm[i];
+
+	// order to solve constraint rows in
+	IndexError *order = (IndexError*) alloca (m*sizeof(IndexError));
+
+#ifndef REORDER_CONSTRAINTS
+	// make sure constraints with findex < 0 come first.
+	j=0;
+	for (i=0; i<m; i++) 
+		if (findex[i] < 0) 
+			order[j++].index = i;
+	for (i=0; i<m; i++) 
+		if (findex[i] >= 0) 
+			order[j++].index = i;
+	dIASSERT (j==m);
+#endif
+
+	for (int iteration=0; iteration < num_iterations; iteration++) {
+
+#ifdef REORDER_CONSTRAINTS
+		// constraints with findex < 0 always come first.
+		if (iteration < 2) {
+			// for the first two iterations, solve the constraints in
+			// the given order
+			for (i=0; i<m; i++) {
+				order[i].error = i;
+				order[i].findex = findex[i];
+				order[i].index = i;
+			}
+		}
+		else {
+			// sort the constraints so that the ones converging slowest
+			// get solved last. use the absolute (not relative) error.
+			for (i=0; i<m; i++) {
+				float v1 = dFabs (lambda[i]);
+				float v2 = dFabs (last_lambda[i]);
+				float max = (v1 > v2) ? v1 : v2;
+				if (max > 0) {
+					//@@@ relative error: order[i].error = dFabs(lambda[i]-last_lambda[i])/max;
+					order[i].error = dFabs(lambda[i]-last_lambda[i]);
+				}
+				else {
+					order[i].error = dInfinity;
+				}
+				order[i].findex = findex[i];
+				order[i].index = i;
+			}
+		}
+		qsort (order,m,sizeof(IndexError),&compare_index_error);
+#endif
+#ifdef RANDOMLY_REORDER_CONSTRAINTS
+                if ((iteration & 7) == 0) {
+			for (i=1; i<m; ++i) {
+				IndexError tmp = order[i];
+				int swapi = dRandInt2(i+1);
+				order[i] = order[swapi];
+				order[swapi] = tmp;
+			}
+                }
+#endif
+
+		//@@@ potential optimization: swap lambda and last_lambda pointers rather
+		//    than copying the data. we must make sure lambda is properly
+		//    returned to the caller
+		memcpy (last_lambda,lambda,m*sizeof(float));
+
+		for (int i=0; i<m; i++) {
+			// @@@ potential optimization: we could pre-sort J and iMJ, thereby
+			//     linearizing access to those arrays. hmmm, this does not seem
+			//     like a win, but we should think carefully about our memory
+			//     access pattern.
+		
+			int index = order[i].index;
+			J_ptr = J + index*12;
+			iMJ_ptr = iMJ + index*12;
+		
+			// set the limits for this constraint. note that 'hicopy' is used.
+			// this is the place where the QuickStep method differs from the
+			// direct LCP solving method, since that method only performs this
+			// limit adjustment once per time step, whereas this method performs
+			// once per iteration per constraint row.
+			// the constraints are ordered so that all lambda[] values needed have
+			// already been computed.
+			if (findex[index] >= 0) {
+				hi[index] = SimdFabs (hicopy[index] * lambda[findex[index]]);
+				lo[index] = -hi[index];
+			}
+
+			int b1 = jb[index*2];
+			int b2 = jb[index*2+1];
+			float delta = rhs[index] - lambda[index]*Ad[index];
+			dRealMutablePtr fc_ptr = invMforce + 6*b1;
+			
+			// @@@ potential optimization: SIMD-ize this and the b2 >= 0 case
+			delta -=fc_ptr[0] * J_ptr[0] + fc_ptr[1] * J_ptr[1] +
+				fc_ptr[2] * J_ptr[2] + fc_ptr[3] * J_ptr[3] +
+				fc_ptr[4] * J_ptr[4] + fc_ptr[5] * J_ptr[5];
+			// @@@ potential optimization: handle 1-body constraints in a separate
+			//     loop to avoid the cost of test & jump?
+			if (b2 >= 0) {
+				fc_ptr = invMforce + 6*b2;
+				delta -=fc_ptr[0] * J_ptr[6] + fc_ptr[1] * J_ptr[7] +
+					fc_ptr[2] * J_ptr[8] + fc_ptr[3] * J_ptr[9] +
+					fc_ptr[4] * J_ptr[10] + fc_ptr[5] * J_ptr[11];
+			}
+
+			// compute lambda and clamp it to [lo,hi].
+			// @@@ potential optimization: does SSE have clamping instructions
+			//     to save test+jump penalties here?
+			float new_lambda = lambda[index] + delta;
+			if (new_lambda < lo[index]) {
+				delta = lo[index]-lambda[index];
+				lambda[index] = lo[index];
+			}
+			else if (new_lambda > hi[index]) {
+				delta = hi[index]-lambda[index];
+				lambda[index] = hi[index];
+			}
+			else {
+				lambda[index] = new_lambda;
+			}
+
+			//@@@ a trick that may or may not help
+			//float ramp = (1-((float)(iteration+1)/(float)num_iterations));
+			//delta *= ramp;
+		
+			// update invMforce.
+			// @@@ potential optimization: SIMD for this and the b2 >= 0 case
+			fc_ptr = invMforce + 6*b1;
+			fc_ptr[0] += delta * iMJ_ptr[0];
+			fc_ptr[1] += delta * iMJ_ptr[1];
+			fc_ptr[2] += delta * iMJ_ptr[2];
+			fc_ptr[3] += delta * iMJ_ptr[3];
+			fc_ptr[4] += delta * iMJ_ptr[4];
+			fc_ptr[5] += delta * iMJ_ptr[5];
+			// @@@ potential optimization: handle 1-body constraints in a separate
+			//     loop to avoid the cost of test & jump?
+			if (b2 >= 0) {
+				fc_ptr = invMforce + 6*b2;
+				fc_ptr[0] += delta * iMJ_ptr[6];
+				fc_ptr[1] += delta * iMJ_ptr[7];
+				fc_ptr[2] += delta * iMJ_ptr[8];
+				fc_ptr[3] += delta * iMJ_ptr[9];
+				fc_ptr[4] += delta * iMJ_ptr[10];
+				fc_ptr[5] += delta * iMJ_ptr[11];
+			}
+		}
+	}
+}
+
+
+
+void SolveInternal1 (float global_cfm,
+					 float global_erp,
+					 RigidBody * const *body, int nb,
+					BU_Joint * const *_joint, 
+					int nj, 
+					const ContactSolverInfo& solverInfo)
+{
+
+	int numIter = solverInfo.m_numIterations;
+	float sOr = solverInfo.m_sor;
+
+	int i,j;
+	
+	SimdScalar stepsize1 = dRecip(solverInfo.m_timeStep);
+
+	// number all bodies in the body list - set their tag values
+	for (i=0; i<nb; i++) 
+		body[i]->m_odeTag = i;
+	
+	// make a local copy of the joint array, because we might want to modify it.
+	// (the "BU_Joint *const*" declaration says we're allowed to modify the joints
+	// but not the joint array, because the caller might need it unchanged).
+	//@@@ do we really need to do this? we'll be sorting constraint rows individually, not joints
+	BU_Joint **joint = (BU_Joint**) alloca (nj * sizeof(BU_Joint*));
+	memcpy (joint,_joint,nj * sizeof(BU_Joint*));
+	
+	// for all bodies, compute the inertia tensor and its inverse in the global
+	// frame, and compute the rotational force and add it to the torque
+	// accumulator. I and invI are a vertical stack of 3x4 matrices, one per body.
+	dRealAllocaArray (I,3*4*nb);
+	dRealAllocaArray (invI,3*4*nb);
+/*	for (i=0; i<nb; i++) {
+		dMatrix3 tmp;
+		// compute inertia tensor in global frame
+		dMULTIPLY2_333 (tmp,body[i]->m_I,body[i]->m_R);
+		// compute inverse inertia tensor in global frame
+		dMULTIPLY2_333 (tmp,body[i]->m_invI,body[i]->m_R);
+		dMULTIPLY0_333 (invI+i*12,body[i]->m_R,tmp);
+		// compute rotational force
+		dCROSS (body[i]->m_tacc,-=,body[i]->getAngularVelocity(),tmp);
+	}
+*/
+	for (i=0; i<nb; i++) {
+		dMatrix3 tmp;
+		// compute inertia tensor in global frame
+		dMULTIPLY2_333 (tmp,body[i]->m_I,body[i]->m_R);
+		dMULTIPLY0_333 (I+i*12,body[i]->m_R,tmp);
+
+		// compute inverse inertia tensor in global frame
+		dMULTIPLY2_333 (tmp,body[i]->m_invI,body[i]->m_R);
+		dMULTIPLY0_333 (invI+i*12,body[i]->m_R,tmp);
+		// compute rotational force
+		dMULTIPLY0_331 (tmp,I+i*12,body[i]->getAngularVelocity());
+		dCROSS (body[i]->m_tacc,-=,body[i]->getAngularVelocity(),tmp);
+	}
+
+
+
+
+	// get joint information (m = total constraint dimension, nub = number of unbounded variables).
+	// joints with m=0 are inactive and are removed from the joints array
+	// entirely, so that the code that follows does not consider them.
+	//@@@ do we really need to save all the info1's
+	BU_Joint::Info1 *info = (BU_Joint::Info1*) alloca (nj*sizeof(BU_Joint::Info1));
+	for (i=0, j=0; j<nj; j++) {	// i=dest, j=src
+		joint[j]->GetInfo1 (info+i);
+		dIASSERT (info[i].m >= 0 && info[i].m <= 6 && info[i].nub >= 0 && info[i].nub <= info[i].m);
+		if (info[i].m > 0) {
+			joint[i] = joint[j];
+			i++;
+		}
+	}
+	nj = i;
+
+	// create the row offset array
+	int m = 0;
+	int *ofs = (int*) alloca (nj*sizeof(int));
+	for (i=0; i<nj; i++) {
+		ofs[i] = m;
+		m += info[i].m;
+	}
+
+	// if there are constraints, compute the constraint force
+	dRealAllocaArray (J,m*12);
+	int *jb = (int*) alloca (m*2*sizeof(int));
+	if (m > 0) {
+		// create a constraint equation right hand side vector `c', a constraint
+		// force mixing vector `cfm', and LCP low and high bound vectors, and an
+		// 'findex' vector.
+		dRealAllocaArray (c,m);
+		dRealAllocaArray (cfm,m);
+		dRealAllocaArray (lo,m);
+		dRealAllocaArray (hi,m);
+		int *findex = (int*) alloca (m*sizeof(int));
+		dSetZero1 (c,m);
+		dSetValue1 (cfm,m,global_cfm);
+		dSetValue1 (lo,m,-dInfinity);
+		dSetValue1 (hi,m, dInfinity);
+		for (i=0; i<m; i++) findex[i] = -1;
+		
+		// get jacobian data from constraints. an m*12 matrix will be created
+		// to store the two jacobian blocks from each constraint. it has this
+		// format:
+		//
+		//   l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 \    .
+		//   l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2  }-- jacobian for joint 0, body 1 and body 2 (3 rows)
+		//   l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 /
+		//   l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 }--- jacobian for joint 1, body 1 and body 2 (3 rows)
+		//   etc...
+		//
+		//   (lll) = linear jacobian data
+		//   (aaa) = angular jacobian data
+		//
+		dSetZero1 (J,m*12);
+		BU_Joint::Info2 Jinfo;
+		Jinfo.rowskip = 12;
+		Jinfo.fps = stepsize1;
+		Jinfo.erp = global_erp;
+		for (i=0; i<nj; i++) {
+			Jinfo.J1l = J + ofs[i]*12;
+			Jinfo.J1a = Jinfo.J1l + 3;
+			Jinfo.J2l = Jinfo.J1l + 6;
+			Jinfo.J2a = Jinfo.J1l + 9;
+			Jinfo.c = c + ofs[i];
+			Jinfo.cfm = cfm + ofs[i];
+			Jinfo.lo = lo + ofs[i];
+			Jinfo.hi = hi + ofs[i];
+			Jinfo.findex = findex + ofs[i];
+			joint[i]->GetInfo2 (&Jinfo);
+
+			if (Jinfo.c[0] > solverInfo.m_maxErrorReduction)
+				Jinfo.c[0] = solverInfo.m_maxErrorReduction;
+
+
+
+
+			// adjust returned findex values for global index numbering
+			for (j=0; j<info[i].m; j++) {
+				if (findex[ofs[i] + j] >= 0) 
+					findex[ofs[i] + j] += ofs[i];
+			}
+		}
+
+		// create an array of body numbers for each joint row
+		int *jb_ptr = jb;
+		for (i=0; i<nj; i++) {
+			int b1 = (joint[i]->node[0].body) ? (joint[i]->node[0].body->m_odeTag) : -1;
+			int b2 = (joint[i]->node[1].body) ? (joint[i]->node[1].body->m_odeTag) : -1;
+			for (j=0; j<info[i].m; j++) {
+				jb_ptr[0] = b1;
+				jb_ptr[1] = b2;
+				jb_ptr += 2;
+			}
+		}
+		dIASSERT (jb_ptr == jb+2*m);
+
+		// compute the right hand side `rhs'
+		dRealAllocaArray (tmp1,nb*6);
+		// put v/h + invM*fe into tmp1
+		for (i=0; i<nb; i++) {
+			SimdScalar body_invMass = body[i]->getInvMass();
+			for (j=0; j<3; j++) 
+				tmp1[i*6+j] = body[i]->m_facc[j] * body_invMass + body[i]->getLinearVelocity()[j] * stepsize1;
+			dMULTIPLY0_331NEW (tmp1 + i*6 + 3,=,invI + i*12,body[i]->m_tacc);
+			for (j=0; j<3; j++) 
+				tmp1[i*6+3+j] += body[i]->getAngularVelocity()[j] * stepsize1;
+		}
+
+		// put J*tmp1 into rhs
+		dRealAllocaArray (rhs,m);
+		multiply_J (m,J,jb,tmp1,rhs);
+
+		// complete rhs
+		for (i=0; i<m; i++) rhs[i] = c[i]*stepsize1 - rhs[i];
+
+		// scale CFM
+		for (i=0; i<m; i++) 
+			cfm[i] *= stepsize1;
+
+		// load lambda from the value saved on the previous iteration
+		dRealAllocaArray (lambda,m);
+#ifdef WARM_STARTING
+		dSetZero1 (lambda,m);	//@@@ shouldn't be necessary
+		for (i=0; i<nj; i++) {
+			memcpy (lambda+ofs[i],joint[i]->lambda,info[i].m * sizeof(SimdScalar));
+		}
+#endif
+
+		// solve the LCP problem and get lambda and invM*constraint_force
+		dRealAllocaArray (cforce,nb*6);
+
+		SOR_LCP (m,nb,J,jb,body,invI,lambda,cforce,rhs,lo,hi,cfm,findex,numIter,sOr);
+
+#ifdef WARM_STARTING
+		// save lambda for the next iteration
+		//@@@ note that this doesn't work for contact joints yet, as they are
+		// recreated every iteration
+		for (i=0; i<nj; i++) {
+			memcpy (joint[i]->lambda,lambda+ofs[i],info[i].m * sizeof(SimdScalar));
+		}
+#endif
+
+	
+		// note that the SOR method overwrites rhs and J at this point, so
+		// they should not be used again.
+		
+		// add stepsize * cforce to the body velocity
+		for (i=0; i<nb; i++) {
+			SimdVector3 linvel = body[i]->getLinearVelocity();
+			SimdVector3 angvel = body[i]->getAngularVelocity();
+			
+			for (j=0; j<3; j++) 
+				linvel[j] += solverInfo.m_timeStep* cforce[i*6+j];
+			for (j=0; j<3; j++) 
+				angvel[j] += solverInfo.m_timeStep* cforce[i*6+3+j];
+		
+			body[i]->setLinearVelocity(linvel);
+			body[i]->setAngularVelocity(angvel);
+
+		}
+		
+	}
+
+
+
+	// compute the velocity update:
+	// add stepsize * invM * fe to the body velocity
+
+	for (i=0; i<nb; i++) {
+		SimdScalar body_invMass = body[i]->getInvMass();
+		SimdVector3 linvel = body[i]->getLinearVelocity();
+		SimdVector3 angvel = body[i]->getAngularVelocity();
+
+		for (j=0; j<3; j++) 
+		{
+			linvel[j] += solverInfo.m_timeStep * body_invMass * body[i]->m_facc[j];
+		}
+		for (j=0; j<3; j++) 
+		{
+			body[i]->m_tacc[j] *= solverInfo.m_timeStep;	
+		}
+		dMULTIPLY0_331NEW(angvel,+=,invI + i*12,body[i]->m_tacc);
+		body[i]->setAngularVelocity(angvel);
+
+	}
+
+
+
+}
+
+
+#endif //USE_SOR_SOLVER

BulletDynamics/ConstraintSolver/SorLcp.h

@@ -0,0 +1,45 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
+ * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
+ *                                                                       *
+ * This library is free software; you can redistribute it and/or         *
+ * modify it under the terms of EITHER:                                  *
+ *   (1) The GNU Lesser General Public License as published by the Free  *
+ *       Software Foundation; either version 2.1 of the License, or (at  *
+ *       your option) any later version. The text of the GNU Lesser      *
+ *       General Public License is included with this library in the     *
+ *       file LICENSE.TXT.                                               *
+ *   (2) The BSD-style license that is included with this library in     *
+ *       the file LICENSE-BSD.TXT.                                       *
+ *                                                                       *
+ * This library is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#define USE_SOR_SOLVER
+#ifdef USE_SOR_SOLVER
+
+#ifndef SOR_LCP_H
+#define SOR_LCP_H
+class RigidBody;
+class BU_Joint;
+#include "SimdScalar.h"
+
+struct ContactSolverInfo;
+
+void SolveInternal1 (float global_cfm,
+					 float global_erp,
+					 RigidBody * const *body, int nb,
+		     BU_Joint * const *_joint, int nj, const ContactSolverInfo& info);
+
+int dRandInt2 (int n);
+
+
+#endif //SOR_LCP_H
+
+#endif //USE_SOR_SOLVER
+

BulletDynamics/ConstraintSolver/TypedConstraint.cpp

@@ -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.
+*/
+
+
+#include "TypedConstraint.h"
+#include "Dynamics/RigidBody.h"
+#include "Dynamics/MassProps.h"
+
+static RigidBody s_fixed(MassProps(0,SimdVector3(0.f,0.f,0.f)),0.f,0.f,1.f,1.f);
+
+TypedConstraint::TypedConstraint()
+: m_userConstraintType(-1),
+m_userConstraintId(-1),
+m_rbA(s_fixed),
+m_rbB(s_fixed)
+{
+	s_fixed.setMassProps(0.f,SimdVector3(0.f,0.f,0.f));
+}
+TypedConstraint::TypedConstraint(RigidBody& rbA)
+: m_userConstraintType(-1),
+m_userConstraintId(-1),
+m_rbA(rbA),
+m_rbB(s_fixed)
+{
+		s_fixed.setMassProps(0.f,SimdVector3(0.f,0.f,0.f));
+
+}
+
+
+TypedConstraint::TypedConstraint(RigidBody& rbA,RigidBody& rbB)
+: m_userConstraintType(-1),
+m_userConstraintId(-1),
+m_rbA(rbA),
+m_rbB(rbB)
+{
+		s_fixed.setMassProps(0.f,SimdVector3(0.f,0.f,0.f));
+
+}
+

BulletDynamics/ConstraintSolver/TypedConstraint.h

@@ -0,0 +1,74 @@
+/*
+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 RigidBody;
+#include "SimdScalar.h"
+
+//TypedConstraint is the baseclass for Bullet constraints and vehicles
+class TypedConstraint
+{
+	int	m_userConstraintType;
+	int	m_userConstraintId;
+
+protected:
+	RigidBody&	m_rbA;
+	RigidBody&	m_rbB;
+
+public:
+
+	TypedConstraint();
+	virtual ~TypedConstraint() {};
+	TypedConstraint(RigidBody& rbA);
+
+	TypedConstraint(RigidBody& rbA,RigidBody& rbB);
+
+	virtual void	BuildJacobian() = 0;
+
+	virtual	void	SolveConstraint(SimdScalar	timeStep) = 0;
+
+	const RigidBody& GetRigidBodyA() const
+	{
+		return m_rbA;
+	}
+	const RigidBody& GetRigidBodyB() const
+	{
+		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()
+	{
+		return m_userConstraintId;
+	}
+};
+
+#endif //TYPED_CONSTRAINT_H