added anisotropic friction support for Bullet. Both for static and dynamic objects

4 files changed, 58 insertions, 8 deletions

diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp
index eebd0c99fcb..846c9b9b989 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp
@@ -17,7 +17,9 @@ subject to the following restrictions:
 #include "btCollisionObject.h"
 
 btCollisionObject::btCollisionObject()
-	:	m_broadphaseHandle(0),
+	:	m_anisotropicFriction(1.f,1.f,1.f),
+	m_hasAnisotropicFriction(false),
+		m_broadphaseHandle(0),
 		m_collisionShape(0),
 		m_rootCollisionShape(0),
 		m_collisionFlags(btCollisionObject::CF_STATIC_OBJECT),
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h
index 0961f3e75c9..8442868cf89 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h
@@ -49,6 +49,9 @@ protected:
 	//without destroying the continuous interpolated motion (which uses this interpolation velocities)
 	btVector3	m_interpolationLinearVelocity;
 	btVector3	m_interpolationAngularVelocity;
+	btVector3		m_anisotropicFriction;
+	bool	m_hasAnisotropicFriction;
+
 	btBroadphaseProxy*		m_broadphaseHandle;
 	btCollisionShape*		m_collisionShape;
 	
@@ -119,6 +122,20 @@ public:
 		return  ((m_collisionFlags & (CF_STATIC_OBJECT | CF_KINEMATIC_OBJECT | CF_NO_CONTACT_RESPONSE) )==0);
 	}
 
+	const btVector3& getAnisotropicFriction() const
+	{
+		return m_anisotropicFriction;
+	}
+	void	setAnisotropicFriction(const btVector3& anisotropicFriction)
+	{
+		m_anisotropicFriction = anisotropicFriction;
+		m_hasAnisotropicFriction = (anisotropicFriction[0]!=1.f) || (anisotropicFriction[1]!=1.f) || (anisotropicFriction[2]!=1.f);
+	}
+	bool	hasAnisotropicFriction() const
+	{
+		return m_hasAnisotropicFriction;
+	}
+
 
 	SIMD_FORCE_INLINE bool		isStaticObject() const {
 		return (m_collisionFlags & CF_STATIC_OBJECT) != 0;
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
index b8afbd6aac5..2911d52e5ea 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
@@ -149,6 +149,7 @@ void	initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject
 		solverBody->m_originalBody = 0;
 		solverBody->m_angularFactor = 1.f;
 	}
+	
 	solverBody->m_pushVelocity.setValue(0.f,0.f,0.f);
 	solverBody->m_turnVelocity.setValue(0.f,0.f,0.f);
 }
@@ -292,7 +293,7 @@ btScalar resolveSingleCollisionCombinedCacheFriendly(
 	return normalImpulse;
 }
 
-
+//#define NO_FRICTION_TANGENTIALS 1
 #ifndef NO_FRICTION_TANGENTIALS
 
 btScalar resolveSingleFrictionCacheFriendly(
@@ -396,7 +397,7 @@ btScalar resolveSingleFrictionCacheFriendly(
 			return 0.f;
 
 
-		body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
+		body1.getVelocityInLocalPoint(contactConstraint.m_relpos1CrossNormal,vel1);
 		body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
 		btVector3 vel = vel1 - vel2;
 		btScalar rel_vel;
@@ -421,9 +422,9 @@ btScalar resolveSingleFrictionCacheFriendly(
 				(body1.m_invMass + body2.m_invMass + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
 			btScalar normal_impulse = contactConstraint.m_appliedImpulse * combinedFriction;
 
-			GEN_set_min(friction_impulse, normal_impulse);
-			GEN_set_max(friction_impulse, -normal_impulse);
-			body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
+			btSetMin(friction_impulse, normal_impulse);
+			btSetMin(friction_impulse, -normal_impulse);
+			body1.internalApplyImpulse(lat_vel * -friction_impulse, rel_pos1);
 			body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
 		}
 	}
@@ -495,6 +496,23 @@ void	btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3&
 }
 
 
+void	applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection);
+void	applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection)
+{
+	if (colObj && colObj->hasAnisotropicFriction())
+	{
+		// transform to local coordinates
+		btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis();
+		const btVector3& friction_scaling = colObj->getAnisotropicFriction();
+		//apply anisotropic friction
+		loc_lateral *= friction_scaling;
+		// ... and transform it back to global coordinates
+		frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral;
+	}
+}
+
+
+
 
 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
 {
@@ -755,19 +773,31 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 							if (!cp.m_lateralFrictionInitialized)
 							{
 								cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
+								
+								//scale anisotropic friction
+								
+								applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
+								applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
+
 								btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
+
+
 								if (lat_rel_vel > SIMD_EPSILON)//0.0f)
 								{
 									cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
 									addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 									cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
-									cp.m_lateralFrictionDir2.normalize();//??
+									cp.m_lateralFrictionDir2.normalize();
+									applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
+									applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
+
 									addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 								} else
 								{
 									//re-calculate friction direction every frame, todo: check if this is really needed
-									
 									btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
+									applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
+									applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
 									addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 									addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 								}
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp
index e2afb687ac6..93d70de39d8 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp
@@ -45,6 +45,7 @@ void	btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo&
 	m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
 	m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
 	m_angularFactor = btScalar(1.);
+	m_anisotropicFriction.setValue(1.f,1.f,1.f);
 	m_gravity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
 	m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
 	m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)),