added physics-debugging

didn't check every single file, so might have broken some gameengine stuff, will fix it this weekend!

24 files changed, 256 insertions, 114 deletions

diff --git a/extern/bullet/Bullet/CollisionShapes/BoxShape.cpp b/extern/bullet/Bullet/CollisionShapes/BoxShape.cpp
index 4edd8cf9a12..0fa063a2e17 100644
--- a/extern/bullet/Bullet/CollisionShapes/BoxShape.cpp
+++ b/extern/bullet/Bullet/CollisionShapes/BoxShape.cpp
@@ -11,6 +11,14 @@
 
 #include "BoxShape.h"
 
+SimdVector3 BoxShape::GetHalfExtents() const
+{ 
+	SimdVector3 scalingCorrectedHalfExtents = m_boxHalfExtents1 * m_localScaling;
+	SimdVector3 marginCorrection (CONVEX_DISTANCE_MARGIN,CONVEX_DISTANCE_MARGIN,CONVEX_DISTANCE_MARGIN);
+	return (scalingCorrectedHalfExtents - marginCorrection).absolute();
+}
+
+
 
 void BoxShape::GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& aabbMax) const
 {
@@ -25,8 +33,9 @@ void BoxShape::GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3&
 
 
 	//todo: this is a quick fix, we need to enlarge the aabb dependent on several criteria
-	//extent += SimdVector3(.2f,.2f,.2f);
-
+//	SimdVector3 extra(1,1,1);
+//	extent += extra;
+	
 	aabbMin = center - extent;
 	aabbMax = center + extent;
 
diff --git a/extern/bullet/Bullet/CollisionShapes/BoxShape.h b/extern/bullet/Bullet/CollisionShapes/BoxShape.h
index 620ddd7e4f6..9d7806fc522 100644
--- a/extern/bullet/Bullet/CollisionShapes/BoxShape.h
+++ b/extern/bullet/Bullet/CollisionShapes/BoxShape.h
@@ -32,10 +32,7 @@ public:
 
 	}
 
-	SimdVector3 GetHalfExtents() const{ return m_boxHalfExtents1 * m_localScaling;}
- 	//const SimdVector3& GetHalfExtents() const{ return m_boxHalfExtents1;}
-
-
+	SimdVector3 GetHalfExtents() const;
 	
 	virtual int	GetShapeType() const { return BOX_SHAPE_PROXYTYPE;}
 
diff --git a/extern/bullet/Bullet/CollisionShapes/ConvexHullShape.cpp b/extern/bullet/Bullet/CollisionShapes/ConvexHullShape.cpp
index 0206324fa04..8e493ba5bc0 100644
--- a/extern/bullet/Bullet/CollisionShapes/ConvexHullShape.cpp
+++ b/extern/bullet/Bullet/CollisionShapes/ConvexHullShape.cpp
@@ -118,13 +118,13 @@ void ConvexHullShape::GetEdge(int i,SimdPoint3& pa,SimdPoint3& pb) const
 
 	int index0 = i%m_points.size();
 	int index1 = i/m_points.size();
-	pa = m_points[index0];
-	pb = m_points[index1];
+	pa = m_points[index0]*m_localScaling;
+	pb = m_points[index1]*m_localScaling;
 }
 
 void ConvexHullShape::GetVertex(int i,SimdPoint3& vtx) const
 {
-	vtx = m_points[i];
+	vtx = m_points[i]*m_localScaling;
 }
 
 int	ConvexHullShape::GetNumPlanes() const
diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/CollisionMargin.h b/extern/bullet/Bullet/NarrowPhaseCollision/CollisionMargin.h
index 7e51dc7f2b3..41009dcd110 100644
--- a/extern/bullet/Bullet/NarrowPhaseCollision/CollisionMargin.h
+++ b/extern/bullet/Bullet/NarrowPhaseCollision/CollisionMargin.h
@@ -3,7 +3,7 @@
 
 //used by Gjk and some other algorithms
 
-#define CONVEX_DISTANCE_MARGIN 0.05f// 0.1f//;//0.01f
+#define CONVEX_DISTANCE_MARGIN 0.04f// 0.1f//;//0.01f
 
 
 
diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/ConvexCast.h b/extern/bullet/Bullet/NarrowPhaseCollision/ConvexCast.h
index f32887f6472..2e7fa0e8eea 100644
--- a/extern/bullet/Bullet/NarrowPhaseCollision/ConvexCast.h
+++ b/extern/bullet/Bullet/NarrowPhaseCollision/ConvexCast.h
@@ -16,6 +16,7 @@
 #include <SimdVector3.h>
 #include <SimdScalar.h>
 class MinkowskiSumShape;
+#include "IDebugDraw.h"
 
 /// ConvexCast is an interface for Casting
 class ConvexCast
@@ -30,18 +31,23 @@ public:
 	struct	CastResult
 	{
 		//virtual bool	addRayResult(const SimdVector3& normal,SimdScalar	fraction) = 0;
-
+				
 		virtual void	DebugDraw(SimdScalar	fraction) {}
 		virtual void	DrawCoordSystem(const SimdTransform& trans) {}
+
 		CastResult()
-			:m_fraction(1e30f)
+			:m_fraction(1e30f),
+			m_debugDrawer(0)
 		{
 		}
 
 		SimdVector3	m_normal;
 		SimdScalar	m_fraction;
 		SimdTransform	m_hitTransformA;
-		SimdTransform	m_hitTransformB;		
+		SimdTransform	m_hitTransformB;
+
+		IDebugDraw* m_debugDrawer;
+
 	};
 
 
diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.cpp b/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.cpp
index 9085ffa0ef0..29b13f67b8f 100644
--- a/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.cpp
+++ b/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.cpp
@@ -111,6 +111,10 @@ void PersistentManifold::AddManifoldPoint(const ManifoldPoint& newPoint)
 	ReplaceContactPoint(newPoint,insertIndex);
 }
 
+float	PersistentManifold::GetManifoldMargin() const
+{
+	return 0.02f;
+}
 
 void PersistentManifold::RefreshContactPoints(const SimdTransform& trA,const SimdTransform& trB)
 {
diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.h b/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.h
index c1d0314069e..96f91d5900e 100644
--- a/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.h
+++ b/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.h
@@ -37,6 +37,8 @@ class PersistentManifold
 	/// sort cached points so most isolated points come first
 	int	SortCachedPoints(const ManifoldPoint& pt);
 
+	int		FindContactPoint(const ManifoldPoint* unUsed, int numUnused,const ManifoldPoint& pt);
+
 public:
 
 	int m_index1;
@@ -77,11 +79,8 @@ public:
 	}
 
 	/// todo: get this margin from the current physics / collision environment
-	inline float	GetManifoldMargin() const
-	{
-		return 0.02f;
-	}
-
+	float	GetManifoldMargin() const;
+	
 	int GetCacheEntry(const ManifoldPoint& newPoint) const;
 
 	void AddManifoldPoint( const ManifoldPoint& newPoint);
diff --git a/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConvexAlgorithm.cpp b/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConvexAlgorithm.cpp
index 42387ba271e..793c94804c0 100644
--- a/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConvexAlgorithm.cpp
+++ b/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConvexAlgorithm.cpp
@@ -175,6 +175,8 @@ m_collisionImpulse = 0.f;
 		input.m_maximumDistanceSquared = min0->GetMargin() + min1->GetMargin() + m_manifoldPtr->GetManifoldMargin();
 		input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
 	}
+
+	input.m_maximumDistanceSquared = 1e30;//
 	
 	input.m_transformA = body0->getCenterOfMassTransform();
 	input.m_transformB = body1->getCenterOfMassTransform();
diff --git a/extern/bullet/BulletDynamics/CollisionDispatch/ToiContactDispatcher.cpp b/extern/bullet/BulletDynamics/CollisionDispatch/ToiContactDispatcher.cpp
index fab0d7e384d..ec566349bad 100644
--- a/extern/bullet/BulletDynamics/CollisionDispatch/ToiContactDispatcher.cpp
+++ b/extern/bullet/BulletDynamics/CollisionDispatch/ToiContactDispatcher.cpp
@@ -86,18 +86,15 @@ void ToiContactDispatcher::ReleaseManifold(PersistentManifold* manifold)
 //
 // todo: this is random access, it can be walked 'cache friendly'!
 //
-void ToiContactDispatcher::SolveConstraints(float timeStep, int numIterations,int numRigidBodies) 
+void ToiContactDispatcher::SolveConstraints(float timeStep, int numIterations,int numRigidBodies,IDebugDraw* debugDrawer) 
 {
 	int i;
 
-	int numManifolds;
 
 	for (int islandId=0;islandId<numRigidBodies;islandId++)
 	{
-		numManifolds = 0;
-
-		PersistentManifold* manifolds[MAX_MANIFOLDS];
 
+		std::vector<PersistentManifold*>  islandmanifold;
 		
 		//int numSleeping = 0;
 
@@ -116,16 +113,15 @@ void ToiContactDispatcher::SolveConstraints(float timeStep, int numIterations,in
 					allSleeping = false;
 				}
 
-				manifolds[numManifolds] = manifold;
-				numManifolds++;
+				islandmanifold.push_back(manifold);
 			}
 		}
 		if (allSleeping)
 		{
 			//tag all as 'ISLAND_SLEEPING'
-			for (i=0;i<numManifolds;i++)
+			for (i=0;i<islandmanifold.size();i++)
 			{
-				 PersistentManifold* manifold = manifolds[i];
+				 PersistentManifold* manifold = islandmanifold[i];
 				if (((RigidBody*)manifold->GetBody0()))	
 				{
 					((RigidBody*)manifold->GetBody0())->SetActivationState( ISLAND_SLEEPING );
@@ -140,9 +136,9 @@ void ToiContactDispatcher::SolveConstraints(float timeStep, int numIterations,in
 		{
 
 			//tag all as 'ISLAND_SLEEPING'
-			for (i=0;i<numManifolds;i++)
+			for (i=0;i<islandmanifold.size();i++)
 			{
-				 PersistentManifold* manifold = manifolds[i];
+				 PersistentManifold* manifold = islandmanifold[i];
 				if (((RigidBody*)manifold->GetBody0()))	
 				{
 					if ( ((RigidBody*)manifold->GetBody0())->GetActivationState() == ISLAND_SLEEPING)
@@ -169,19 +165,8 @@ void ToiContactDispatcher::SolveConstraints(float timeStep, int numIterations,in
 			info.m_tau = 0.4f;
 			info.m_restitution = 0.1f;//m_restitution;
 
-			/*
-			int numManifolds = 0;
 
-			for (i=0;i<m_firstFreeManifold;i++)
-			{
-				PersistentManifold* manifold = &m_manifolds[m_freeManifolds[i]];
-				//ASSERT(((RigidBody*)manifold->GetBody0()));
-				//ASSERT(((RigidBody*)manifold->GetBody1()));
-				manifolds[i] = manifold;
-				numManifolds++;
-			}
-*/
-			m_solver->SolveGroup( manifolds, numManifolds,info );
+			m_solver->SolveGroup( &islandmanifold[0], islandmanifold.size(),info,debugDrawer );
 
 		}
 	}
diff --git a/extern/bullet/BulletDynamics/CollisionDispatch/ToiContactDispatcher.h b/extern/bullet/BulletDynamics/CollisionDispatch/ToiContactDispatcher.h
index 818901cb901..97897a69dc4 100644
--- a/extern/bullet/BulletDynamics/CollisionDispatch/ToiContactDispatcher.h
+++ b/extern/bullet/BulletDynamics/CollisionDispatch/ToiContactDispatcher.h
@@ -7,13 +7,13 @@
 #include "BroadphaseCollision/BroadphaseProxy.h"
 
 class ConstraintSolver;
+class IDebugDraw;
 
 //island management
 #define ACTIVE_TAG 1
 #define ISLAND_SLEEPING 2
 #define WANTS_DEACTIVATION 3
 
-#define MAX_MANIFOLDS 512
 
 struct CollisionAlgorithmCreateFunc
 {
@@ -40,8 +40,6 @@ class ToiContactDispatcher : public Dispatcher
 	
 	std::vector<PersistentManifold*>	m_manifoldsPtr;
 
-//	PersistentManifold	m_manifolds[MAX_MANIFOLDS];
-//	int	m_freeManifolds[MAX_MANIFOLDS];
 
 	UnionFind m_unionFind;
 	ConstraintSolver*	m_solver;
@@ -86,7 +84,7 @@ public:
 	//
 	// todo: this is random access, it can be walked 'cache friendly'!
 	//
-	virtual void SolveConstraints(float timeStep, int numIterations,int numRigidBodies);
+	virtual void SolveConstraints(float timeStep, int numIterations,int numRigidBodies,IDebugDraw* debugDrawer);
 	
 	
 	CollisionAlgorithm* FindAlgorithm(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1)
diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/ConstraintSolver.h b/extern/bullet/BulletDynamics/ConstraintSolver/ConstraintSolver.h
index e1512ae1756..84015929586 100644
--- a/extern/bullet/BulletDynamics/ConstraintSolver/ConstraintSolver.h
+++ b/extern/bullet/BulletDynamics/ConstraintSolver/ConstraintSolver.h
@@ -16,6 +16,7 @@ class RigidBody;
 
 struct ContactSolverInfo;
 struct BroadphaseProxy;
+class IDebugDraw;
 
 /// ConstraintSolver provides solver interface
 class ConstraintSolver
@@ -25,7 +26,7 @@ public:
 
 	virtual ~ConstraintSolver() {}
 	
-	virtual float SolveGroup(PersistentManifold** manifold,int numManifolds,const ContactSolverInfo& info) = 0;
+	virtual float SolveGroup(PersistentManifold** manifold,int numManifolds,const ContactSolverInfo& info,class IDebugDraw* debugDrawer = 0) = 0;
 
 };
 
diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp
index e3603ccec67..a23d5c13f7f 100644
--- a/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp
+++ b/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp
@@ -23,16 +23,29 @@ static SimdScalar ContactThreshold = -10.0f;
 float useGlobalSettingContacts = false;//true;
 
 SimdScalar contactDamping = 0.9f;
-SimdScalar contactTau = .2f;//0.02f;//*0.02f;
+SimdScalar contactTau = .02f;//0.02f;//*0.02f;
 
 
 
 SimdScalar restitutionCurve(SimdScalar rel_vel, SimdScalar restitution)
 {
-	return restitution;
+	return 0.f;
 //	return restitution * GEN_min(1.0f, rel_vel / ContactThreshold);
 }
 
+float MAX_FRICTION = 100.f;
+
+SimdScalar	calculateCombinedFriction(RigidBody& body0,RigidBody& body1)
+{
+	SimdScalar friction = body0.getFriction() * body1.getFriction();
+	if (friction < 0.f)
+		friction = 0.f;
+	if (friction > MAX_FRICTION)
+		friction = MAX_FRICTION;
+	return friction;
+
+}
+
 void	applyFrictionInContactPointOld(RigidBody& body1, const SimdVector3& pos1,
                       RigidBody& body2, const SimdVector3& pos2,
 		const SimdVector3& normal,float normalImpulse, 
@@ -51,6 +64,8 @@ void	applyFrictionInContactPointOld(RigidBody& body1, const SimdVector3& pos1,
 	
 		SimdScalar rel_vel = normal.dot(vel);
 
+		float combinedFriction = calculateCombinedFriction(body1,body2);
+
 #define PER_CONTACT_FRICTION
 #ifdef PER_CONTACT_FRICTION
 		SimdVector3 lat_vel = vel - normal * rel_vel;
@@ -63,7 +78,7 @@ void	applyFrictionInContactPointOld(RigidBody& body1, const SimdVector3& pos1,
 			SimdVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(lat_vel); 
 			SimdScalar frictionMaxImpulse = lat_rel_vel / 
 				(body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
-			SimdScalar frictionImpulse = (normalImpulse) * solverInfo.m_friction;
+			SimdScalar frictionImpulse = (normalImpulse) * combinedFriction;
 			GEN_set_min(frictionImpulse,frictionMaxImpulse );
 			
 			body1.applyImpulse(lat_vel * -frictionImpulse, rel_pos1);
@@ -129,7 +144,6 @@ void resolveSingleBilateral(RigidBody& body1, const SimdVector3& pos1,
 		
 	SimdScalar massTerm = 1.f / (body1.getInvMass() + body2.getInvMass());
 
-	float contactDamping = 0.9f;
 	impulse = - contactDamping * rel_vel * massTerm;//jacDiagABInv;
 
 	//SimdScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
@@ -167,9 +181,11 @@ float resolveSingleCollision(RigidBody& body1, const SimdVector3& pos1,
 	
 //	if (rel_vel< 0.f)//-SIMD_EPSILON) 
 //	{
-	SimdScalar rest = restitutionCurve(rel_vel, solverInfo.m_restitution);
+	float combinedRestitution = body1.getRestitution() * body2.getRestitution();
+
+	SimdScalar rest = restitutionCurve(rel_vel, combinedRestitution);
 
-	SimdScalar timeCorrection =  1.f;///timeStep;
+	SimdScalar timeCorrection =  360.f*solverInfo.m_timeStep;
 	float damping = solverInfo.m_damping ;
 	float tau = solverInfo.m_tau;
 
@@ -252,7 +268,9 @@ SimdScalar rel_vel;
 	
 //	if (rel_vel< 0.f)//-SIMD_EPSILON) 
 //	{
-	SimdScalar rest = restitutionCurve(rel_vel, solverInfo.m_restitution);
+	float combinedRestitution = body1.getRestitution() * body2.getRestitution();
+
+	SimdScalar rest = restitutionCurve(rel_vel, combinedRestitution);
 //	SimdScalar massTerm = 1.f / (body1.getInvMass() + body2.getInvMass());
 
 	SimdScalar timeCorrection = 0.5f / solverInfo.m_timeStep ;
@@ -302,6 +320,8 @@ SimdScalar rel_vel;
 		SimdVector3 lat_vel = vel - normal * rel_vel;
 		SimdScalar lat_rel_vel = lat_vel.length();
 
+		float combinedFriction = calculateCombinedFriction(body1,body2);
+
 		if (lat_rel_vel > SIMD_EPSILON)
 		{
 			lat_vel /= lat_rel_vel;
@@ -310,7 +330,7 @@ SimdScalar rel_vel;
 			 friction_impulse = lat_rel_vel / 
 				(body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
 			SimdScalar normal_impulse = (penetrationImpulse+
-				velocityImpulse) * solverInfo.m_friction;
+				velocityImpulse) * combinedFriction;
 			GEN_set_min(friction_impulse, normal_impulse);
 			
 			body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp
index 8dc666d588f..37b3668bbf7 100644
--- a/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp
+++ b/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp
@@ -19,6 +19,8 @@
 #include "Dynamics/BU_Joint.h"
 #include "Dynamics/ContactJoint.h"
 
+#include "IDebugDraw.h"
+
 #define USE_SOR_SOLVER
 
 #include "SorLcp.h"
@@ -41,8 +43,7 @@ class BU_Joint;
 
 //see below
 
-int gCurBody = 0;
-int gCurJoint= 0;
+
 
 int ConvertBody(RigidBody* body,RigidBody** bodies,int& numBodies);
 void ConvertConstraint(PersistentManifold* manifold,BU_Joint** joints,int& numJoints,
@@ -53,10 +54,10 @@ void ConvertConstraint(PersistentManifold* manifold,BU_Joint** joints,int& numJo
 
 
 //iterative lcp and penalty method
-float OdeConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numManifolds,const ContactSolverInfo& infoGlobal)
+float OdeConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numManifolds,const ContactSolverInfo& infoGlobal,IDebugDraw* debugDrawer)
 {
-	gCurBody = 0;
-	gCurJoint = 0;
+	m_CurBody = 0;
+	m_CurJoint = 0;
 
 	float cfm = 1e-5f;
 	float erp = 0.2f;
@@ -77,7 +78,7 @@ float OdeConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numM
 		{
 			body0 = ConvertBody((RigidBody*)manifold->GetBody0(),bodies,numBodies);
 			body1 = ConvertBody((RigidBody*)manifold->GetBody1(),bodies,numBodies);
-			ConvertConstraint(manifold,joints,numJoints,bodies,body0,body1);
+			ConvertConstraint(manifold,joints,numJoints,bodies,body0,body1,debugDrawer);
 		}
 	}
 
@@ -112,7 +113,7 @@ void dRfromQ1 (dMatrix3 R, const dQuaternion q)
 
 
 
-int ConvertBody(RigidBody* body,RigidBody** bodies,int& numBodies)
+int OdeConstraintSolver::ConvertBody(RigidBody* body,RigidBody** bodies,int& numBodies)
 {
 	if (!body || (body->getInvMass() == 0.f) )
 	{
@@ -195,8 +196,8 @@ int ConvertBody(RigidBody* body,RigidBody** bodies,int& numBodies)
 static ContactJoint gJointArray[MAX_JOINTS_1];
 
 
-void ConvertConstraint(PersistentManifold* manifold,BU_Joint** joints,int& numJoints,
-					   RigidBody** bodies,int _bodyId0,int _bodyId1)
+void OdeConstraintSolver::ConvertConstraint(PersistentManifold* manifold,BU_Joint** joints,int& numJoints,
+					   RigidBody** bodies,int _bodyId0,int _bodyId1,IDebugDraw* debugDrawer)
 {
 
 
@@ -228,14 +229,21 @@ void ConvertConstraint(PersistentManifold* manifold,BU_Joint** joints,int& numJo
 
 	assert(bodyId0 >= 0);
 
+	SimdVector3 color(0,1,0);
 	for (i=0;i<numContacts;i++)
 	{
-		
-		assert (gCurJoint < MAX_JOINTS_1);
+
+		if (debugDrawer)
+		{
+			debugDrawer->DrawLine(manifold->GetContactPoint(i).m_positionWorldOnA,manifold->GetContactPoint(i).m_positionWorldOnB,color);
+			debugDrawer->DrawLine(manifold->GetContactPoint(i).m_positionWorldOnA,manifold->GetContactPoint(i).m_positionWorldOnB,color);
+
+		}
+		assert (m_CurJoint < MAX_JOINTS_1);
 
 		if (manifold->GetContactPoint(i).GetDistance() < 0.f)
 		{
-			ContactJoint* cont = new (&gJointArray[gCurJoint++]) ContactJoint( manifold ,i, swapBodies,body0,body1);
+			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;
diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.h b/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.h
index 7ae8d0eaa49..354a444caa8 100644
--- a/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.h
+++ b/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.h
@@ -13,15 +13,28 @@
 
 #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;
+
+
+	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:
 
 	virtual ~OdeConstraintSolver() {}
 	
-	virtual float SolveGroup(PersistentManifold** manifold,int numManifolds,const ContactSolverInfo& info);
+	virtual float SolveGroup(PersistentManifold** manifold,int numManifolds,const ContactSolverInfo& info,IDebugDraw* debugDrawer = 0);
 
 };
 
diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/Point2PointConstraint.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/Point2PointConstraint.cpp
index 89b554cdad9..0c46b8fc47d 100644
--- a/extern/bullet/BulletDynamics/ConstraintSolver/Point2PointConstraint.cpp
+++ b/extern/bullet/BulletDynamics/ConstraintSolver/Point2PointConstraint.cpp
@@ -13,7 +13,7 @@
 #include "Dynamics/MassProps.h"
 
 
-static RigidBody s_fixed(MassProps(0,SimdVector3(0.f,0.f,0.f)),0.f,0.f);
+static RigidBody s_fixed(MassProps(0,SimdVector3(0.f,0.f,0.f)),0.f,0.f,1.f,1.f);
 
 Point2PointConstraint::Point2PointConstraint():
 m_rbA(s_fixed),m_rbB(s_fixed)
diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp
index 3243140b823..501b0a763e2 100644
--- a/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp
+++ b/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp
@@ -16,6 +16,7 @@
 #include "ContactSolverInfo.h"
 #include "Dynamics/BU_Joint.h"
 #include "Dynamics/ContactJoint.h"
+#include "IDebugDraw.h"
 
 //debugging
 bool doApplyImpulse = true;
@@ -28,7 +29,7 @@ bool useImpulseFriction = true;//true;//false;
 
 
 //iterative lcp and penalty method
-float SimpleConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numManifolds,const ContactSolverInfo& infoGlobal)
+float SimpleConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numManifolds,const ContactSolverInfo& infoGlobal,IDebugDraw* debugDrawer)
 {
 
 	ContactSolverInfo info = infoGlobal;
@@ -41,7 +42,7 @@ float SimpleConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int n
 	{
 		for (int j=0;j<numManifolds;j++)
 		{
-			Solve(manifoldPtr[j],info,i);
+			Solve(manifoldPtr[j],info,i,debugDrawer);
 		}
 	}
 	return 0.f;
@@ -51,7 +52,7 @@ float SimpleConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int n
 float penetrationResolveFactor = 0.9f;
 
 
-float SimpleConstraintSolver::Solve(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter)
+float SimpleConstraintSolver::Solve(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer)
 {
 
 	RigidBody* body0 = (RigidBody*)manifoldPtr->GetBody0();
@@ -73,6 +74,7 @@ float SimpleConstraintSolver::Solve(PersistentManifold* manifoldPtr, const Conta
 	{
 		const int numpoints = manifoldPtr->GetNumContacts();
 
+		SimdVector3 color(0,1,0);
 		for (int i=0;i<numpoints ;i++)
 		{
 
@@ -83,6 +85,10 @@ float SimpleConstraintSolver::Solve(PersistentManifold* manifoldPtr, const Conta
 				j=i;
 
 			ManifoldPoint& cp = manifoldPtr->GetContactPoint(j);
+			
+			if (debugDrawer)
+				debugDrawer->DrawLine(cp.m_positionWorldOnA,cp.m_positionWorldOnB,color);
+
 
 			{
 
diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.h b/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.h
index 6bdcf3a2ac2..a85683b041a 100644
--- a/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.h
+++ b/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.h
@@ -12,17 +12,19 @@
 #define SIMPLE_CONSTRAINT_SOLVER_H
 
 #include "ConstraintSolver.h"
+class IDebugDraw;
 
-
+/// SimpleConstraintSolver uses a Propagation Method
+/// 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);
+	float Solve(PersistentManifold* manifold, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer);
 
 public:
 
 	virtual ~SimpleConstraintSolver() {}
 	
-	virtual float SolveGroup(PersistentManifold** manifold,int numManifolds,const ContactSolverInfo& info);
+	virtual float SolveGroup(PersistentManifold** manifold,int numManifolds,const ContactSolverInfo& info, IDebugDraw* debugDrawer=0);
 
 };
 
diff --git a/extern/bullet/BulletDynamics/Dynamics/ContactJoint.cpp b/extern/bullet/BulletDynamics/Dynamics/ContactJoint.cpp
index 130a6b98402..7ac19a6d6a2 100644
--- a/extern/bullet/BulletDynamics/Dynamics/ContactJoint.cpp
+++ b/extern/bullet/BulletDynamics/Dynamics/ContactJoint.cpp
@@ -15,7 +15,7 @@ m_body1(body1)
 
 int m_numRows = 3;
 
-float gContactFrictionFactor  = 30.5f;//100.f;//1e30f;//2.9f;
+//float gContactFrictionFactor  = 0.f;//12.f;//30.5f;//100.f;//1e30f;//2.9f;
 
 
 
@@ -149,6 +149,9 @@ void ContactJoint::GetInfo2(Info2 *info)
 	c2[0] = ccc2[0];
 	c2[1] = ccc2[1];
 	c2[2] = ccc2[2];
+	
+	float friction = m_body0->getFriction() * m_body1->getFriction();
+	
 	// first friction direction
 	if (m_numRows >= 2) 
 	{
@@ -175,8 +178,9 @@ void ContactJoint::GetInfo2(Info2 *info)
 		// mode
 		//1e30f
 		
-		info->lo[1] = -gContactFrictionFactor;//-j->contact.surface.mu;
-		info->hi[1] = gContactFrictionFactor;//j->contact.surface.mu;
+		
+		info->lo[1] = -friction;//-j->contact.surface.mu;
+		info->hi[1] = friction;//j->contact.surface.mu;
 		if (0)//j->contact.surface.mode & dContactApprox1_1) 
 			info->findex[1] = 0;
 		
@@ -210,8 +214,8 @@ void ContactJoint::GetInfo2(Info2 *info)
 			//info->hi[2] = j->contact.surface.mu2;
 		}
 		else {
-			info->lo[2] = -gContactFrictionFactor;
-			info->hi[2] = gContactFrictionFactor;
+			info->lo[2] = -friction;
+			info->hi[2] = friction;
 		}
 		if (0)//j->contact.surface.mode & dContactApprox1_2) 
 			
diff --git a/extern/bullet/BulletDynamics/Dynamics/RigidBody.cpp b/extern/bullet/BulletDynamics/Dynamics/RigidBody.cpp
index 10dccd3dd20..31d20930a10 100644
--- a/extern/bullet/BulletDynamics/Dynamics/RigidBody.cpp
+++ b/extern/bullet/BulletDynamics/Dynamics/RigidBody.cpp
@@ -7,7 +7,7 @@
 float gRigidBodyDamping = 0.5f;
 static int uniqueId = 0;
 
-RigidBody::RigidBody( const MassProps& massProps,SimdScalar linearDamping,SimdScalar angularDamping)
+RigidBody::RigidBody( const MassProps& massProps,SimdScalar linearDamping,SimdScalar angularDamping,SimdScalar friction,SimdScalar restitution)
 : m_collisionShape(0),
 	m_activationState1(1),
 	m_hitFraction(1.f),
@@ -17,7 +17,9 @@ RigidBody::RigidBody( const MassProps& massProps,SimdScalar linearDamping,SimdSc
 	m_totalForce(0.0f, 0.0f, 0.0f),
 	m_totalTorque(0.0f, 0.0f, 0.0f),
 	m_linearVelocity(0.0f, 0.0f, 0.0f),
-	m_angularVelocity(0.f,0.f,0.f)
+	m_angularVelocity(0.f,0.f,0.f),
+	m_restitution(restitution),
+	m_friction(friction)
 
 {
 	m_debugBodyId = uniqueId++;
diff --git a/extern/bullet/BulletDynamics/Dynamics/RigidBody.h b/extern/bullet/BulletDynamics/Dynamics/RigidBody.h
index ec7eb9a8678..527e37bc57d 100644
--- a/extern/bullet/BulletDynamics/Dynamics/RigidBody.h
+++ b/extern/bullet/BulletDynamics/Dynamics/RigidBody.h
@@ -15,7 +15,7 @@ typedef SimdScalar dMatrix3[4*3];
 class RigidBody  {
 public:
 
-	RigidBody(const MassProps& massProps,SimdScalar linearDamping,SimdScalar angularDamping);
+	RigidBody(const MassProps& massProps,SimdScalar linearDamping,SimdScalar angularDamping,SimdScalar friction,SimdScalar restitution);
 
 	void			proceedToTransform(const SimdTransform& newTrans); 
 	
@@ -123,6 +123,22 @@ public:
 	int	GetActivationState() const { return m_activationState1;}
 	void SetActivationState(int newState);
 
+	void	setRestitution(float rest)
+	{
+		m_restitution = rest;
+	}
+	float	getRestitution() const
+	{
+		return m_restitution;
+	}
+	void	setFriction(float frict)
+	{
+		m_friction = frict;
+	}
+	float	getFriction() const
+	{
+		return m_friction;
+	}
 
 private:
 	SimdTransform	m_worldTransform;
@@ -141,6 +157,8 @@ private:
 	SimdScalar		m_angularDamping;
 	SimdScalar		m_inverseMass;
 
+	SimdScalar		m_friction;
+	SimdScalar		m_restitution;
 
 	CollisionShape*	m_collisionShape;
 
diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp
index 376a75f40d3..8b423204bec 100644
--- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp
+++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp
@@ -7,7 +7,7 @@
 
 class BP_Proxy;
 
-bool gEnableSleeping = true;//false;//true;
+bool gEnableSleeping = false;//false;//true;
 #include "Dynamics/MassProps.h"
 
 SimdVector3 startVel(0,0,0);//-10000);
diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp
index 2a89cd0cf4a..885304983a6 100644
--- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp
+++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp
@@ -14,6 +14,7 @@
 #include "ConstraintSolver/OdeConstraintSolver.h"
 #include "ConstraintSolver/SimpleConstraintSolver.h"
 
+#include "IDebugDraw.h"
 
 
 #include "CollisionDispatch/ToiContactDispatcher.h"
@@ -32,7 +33,7 @@ bool useIslands = true;
 //#include "BroadphaseCollision/QueryBox.h"
 //todo: change this to allow dynamic registration of types!
 
-unsigned long gNumIterations = 10;
+unsigned long gNumIterations = 1;
 
 #ifdef WIN32
 void DrawRasterizerLine(const float* from,const float* to,int color);
@@ -47,6 +48,43 @@ void DrawRasterizerLine(const float* from,const float* to,int color);
 
 
 
+
+static void DrawAabb(IDebugDraw* debugDrawer,const SimdVector3& from,const SimdVector3& to,const SimdVector3& color)
+{
+	SimdVector3 halfExtents = (to-from)* 0.5f;
+	SimdVector3 center = (to+from) *0.5f;
+	int i,j;
+
+	SimdVector3 edgecoord(1.f,1.f,1.f),pa,pb;
+	for (i=0;i<4;i++)
+	{
+		for (j=0;j<3;j++)
+		{
+			pa = SimdVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],		
+				edgecoord[2]*halfExtents[2]);
+			pa+=center;
+			
+			int othercoord = j%3;
+			edgecoord[othercoord]*=-1.f;
+			pb = SimdVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],	
+				edgecoord[2]*halfExtents[2]);
+			pb+=center;
+			
+			debugDrawer->DrawLine(pa,pb,color);
+		}
+		edgecoord = SimdVector3(-1.f,-1.f,-1.f);
+		if (i<3)
+			edgecoord[i]*=-1.f;
+	}
+
+
+}
+
+
+
+
+
+
 CcdPhysicsEnvironment::CcdPhysicsEnvironment(ToiContactDispatcher* dispatcher,BroadphaseInterface* bp)
 :m_dispatcher(dispatcher),
 m_broadphase(bp),
@@ -276,10 +314,6 @@ bool	CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
 		//m_scalingPropagated = true;
 	}
 
-#ifdef EXTRA_PHYSICS_PROFILE
-	cpuProfile.begin("integrate force");
-#endif //EXTRA_PHYSICS_PROFILE
-
 
 
 	{
@@ -301,9 +335,6 @@ bool	CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
 			
 		}
 	}
-#ifdef EXTRA_PHYSICS_PROFILE
-	cpuProfile.end("integrate force");
-#endif //EXTRA_PHYSICS_PROFILE
 	BroadphaseInterface*	scene = m_broadphase;
 	
 	
@@ -321,33 +352,21 @@ bool	CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
 	DispatcherInfo dispatchInfo;
 	dispatchInfo.m_timeStep = timeStep;
 	dispatchInfo.m_stepCount = 0;
-#ifdef EXTRA_PHYSICS_PROFILE
-	cpuProfile.begin("cd");
-#endif //EXTRA_PHYSICS_PROFILE
 
 	scene->DispatchAllCollisionPairs(*m_dispatcher,dispatchInfo);///numsubstep,g);
 
-#ifdef EXTRA_PHYSICS_PROFILE
-	cpuProfile.end("cd");
-#endif //EXTRA_PHYSICS_PROFILE
 
 
 	
 		
-#ifdef EXTRA_PHYSICS_PROFILE
-	cpuProfile.begin("solver");
-#endif //EXTRA_PHYSICS_PROFILE
 	
 	int numRigidBodies = m_controllers.size();
 	
 	UpdateActivationState();
 
 	//contacts
-	m_dispatcher->SolveConstraints(timeStep, gNumIterations ,numRigidBodies);
-	
-#ifdef EXTRA_PHYSICS_PROFILE
-	cpuProfile.end("solver");
-#endif //EXTRA_PHYSICS_PROFILE
+
+	m_dispatcher->SolveConstraints(timeStep, gNumIterations ,numRigidBodies,m_debugDrawer);
 
 	for (int g=0;g<numsubstep;g++)
 	{
@@ -402,10 +421,16 @@ bool	CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
 				
 				SimdPoint3 minAabb,maxAabb;
 				CollisionShape* shapeinterface = ctrl->GetCollisionShape();
+
+
+
 				shapeinterface->CalculateTemporalAabb(body->getCenterOfMassTransform(),
 					body->getLinearVelocity(),body->getAngularVelocity(),
 					timeStep,minAabb,maxAabb);
 
+				shapeinterface->GetAabb(body->getCenterOfMassTransform(),
+					minAabb,maxAabb);
+
 				
 				BroadphaseProxy* bp = (BroadphaseProxy*) ctrl->m_broadphaseHandle;
 				if (bp)
@@ -413,8 +438,12 @@ bool	CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
 					
 #ifdef WIN32
 					SimdVector3 color (1,0,0);
-					if (m_debugDrawer)
-						m_debugDrawer->DrawLine(minAabb,maxAabb,color);
+					if (0)//m_debugDrawer)
+					{	
+						//draw aabb
+
+						DrawAabb(m_debugDrawer,minAabb,maxAabb,color);
+					}
 #endif
 					scene->SetAabb(bp,minAabb,maxAabb);
 				}
diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h
index 88cf4b495cf..fc3786885b7 100644
--- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h
+++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h
@@ -6,10 +6,6 @@
 class CcdPhysicsController;
 #include "SimdVector3.h"
 
-struct	PHY_IPhysicsDebugDraw
-{
-	virtual void	DrawLine(const SimdVector3& from,const SimdVector3& to,const SimdVector3& color)=0;
-};
 
 
 class Point2PointConstraint;
@@ -20,6 +16,7 @@ class Dispatcher;
 class Vehicle;
 class PersistentManifold;
 class BroadphaseInterface;
+class IDebugDraw;
 
 /// Physics Environment takes care of stepping the simulation and is a container for physics entities.
 /// It stores rigidbodies,constraints, materials etc.
@@ -28,7 +25,7 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
 {
 	SimdVector3 m_gravity;
 	BroadphaseInterface*	m_broadphase;
-	PHY_IPhysicsDebugDraw*	m_debugDrawer;
+	IDebugDraw*	m_debugDrawer;
 	
 	public:
 		CcdPhysicsEnvironment(ToiContactDispatcher* dispatcher=0, BroadphaseInterface* broadphase=0);
@@ -41,7 +38,7 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
 
 		/// Perform an integration step of duration 'timeStep'.
 
-		virtual void setDebugDrawer(PHY_IPhysicsDebugDraw* debugDrawer)
+		virtual void setDebugDrawer(IDebugDraw* debugDrawer)
 		{
 			m_debugDrawer = debugDrawer;
 		}
diff --git a/extern/bullet/LinearMath/IDebugDraw.h b/extern/bullet/LinearMath/IDebugDraw.h
new file mode 100644
index 00000000000..b5cb24f74e5
--- /dev/null
+++ b/extern/bullet/LinearMath/IDebugDraw.h
@@ -0,0 +1,42 @@
+/*
+Copyright (c) 2005 Gino van den Bergen / Erwin Coumans <www.erwincoumans.com>
+
+Permission is hereby granted, free of charge, to any person or organization
+obtaining a copy of the software and accompanying documentation covered by
+this license (the "Software") to use, reproduce, display, distribute,
+execute, and transmit the Software, and to prepare derivative works of the
+Software, and to permit third-parties to whom the Software is furnished to
+do so, all subject to the following:
+
+The copyright notices in the Software and this entire statement, including
+the above license grant, this restriction and the following disclaimer,
+must be included in all copies of the Software, in whole or in part, and
+all derivative works of the Software, unless such copies or derivative
+works are solely in the form of machine-executable object code generated by
+a source language processor.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
+SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
+FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
+ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+DEALINGS IN THE SOFTWARE.
+*/
+
+
+#ifndef IDEBUG_DRAW__H
+#define IDEBUG_DRAW__H
+
+#include "SimdVector3.h"
+
+class	IDebugDraw
+{
+public:
+	virtual void	DrawLine(const SimdVector3& from,const SimdVector3& to,const SimdVector3& color)=0;
+
+	virtual void	SetDebugMode(int debugMode) =0;
+
+};
+
+#endif //IDEBUG_DRAW__H
\ No newline at end of file