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

2 files changed, 38 insertions, 7 deletions

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)),