1 files changed, 180 insertions, 184 deletions

diff --git a/thirdparty/bullet/BulletDynamics/Character/btKinematicCharacterController.cpp b/thirdparty/bullet/BulletDynamics/Character/btKinematicCharacterController.cpp
index cb1aa71a14..2bbccb291c 100644
--- a/thirdparty/bullet/BulletDynamics/Character/btKinematicCharacterController.cpp
+++ b/thirdparty/bullet/BulletDynamics/Character/btKinematicCharacterController.cpp
@@ -13,7 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include <stdio.h>
 #include "LinearMath/btIDebugDraw.h"
 #include "BulletCollision/CollisionDispatch/btGhostObject.h"
@@ -24,20 +23,19 @@ subject to the following restrictions:
 #include "LinearMath/btDefaultMotionState.h"
 #include "btKinematicCharacterController.h"
 
-
 // static helper method
 static btVector3
 getNormalizedVector(const btVector3& v)
 {
 	btVector3 n(0, 0, 0);
 
-	if (v.length() > SIMD_EPSILON) {
+	if (v.length() > SIMD_EPSILON)
+	{
 		n = v.normalized();
 	}
 	return n;
 }
 
-
 ///@todo Interact with dynamic objects,
 ///Ride kinematicly animated platforms properly
 ///More realistic (or maybe just a config option) falling
@@ -47,18 +45,19 @@ getNormalizedVector(const btVector3& v)
 class btKinematicClosestNotMeRayResultCallback : public btCollisionWorld::ClosestRayResultCallback
 {
 public:
-	btKinematicClosestNotMeRayResultCallback (btCollisionObject* me) : btCollisionWorld::ClosestRayResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
+	btKinematicClosestNotMeRayResultCallback(btCollisionObject* me) : btCollisionWorld::ClosestRayResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
 	{
 		m_me = me;
 	}
 
-	virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace)
+	virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace)
 	{
 		if (rayResult.m_collisionObject == m_me)
 			return 1.0;
 
-		return ClosestRayResultCallback::addSingleResult (rayResult, normalInWorldSpace);
+		return ClosestRayResultCallback::addSingleResult(rayResult, normalInWorldSpace);
 	}
+
 protected:
 	btCollisionObject* m_me;
 };
@@ -66,15 +65,12 @@ protected:
 class btKinematicClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
 {
 public:
-	btKinematicClosestNotMeConvexResultCallback (btCollisionObject* me, const btVector3& up, btScalar minSlopeDot)
-	: btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
-	, m_me(me)
-	, m_up(up)
-	, m_minSlopeDot(minSlopeDot)
+	btKinematicClosestNotMeConvexResultCallback(btCollisionObject* me, const btVector3& up, btScalar minSlopeDot)
+		: btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0)), m_me(me), m_up(up), m_minSlopeDot(minSlopeDot)
 	{
 	}
 
-	virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace)
+	virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult, bool normalInWorldSpace)
 	{
 		if (convexResult.m_hitCollisionObject == m_me)
 			return btScalar(1.0);
@@ -86,19 +82,22 @@ public:
 		if (normalInWorldSpace)
 		{
 			hitNormalWorld = convexResult.m_hitNormalLocal;
-		} else
+		}
+		else
 		{
 			///need to transform normal into worldspace
-			hitNormalWorld = convexResult.m_hitCollisionObject->getWorldTransform().getBasis()*convexResult.m_hitNormalLocal;
+			hitNormalWorld = convexResult.m_hitCollisionObject->getWorldTransform().getBasis() * convexResult.m_hitNormalLocal;
 		}
 
 		btScalar dotUp = m_up.dot(hitNormalWorld);
-		if (dotUp < m_minSlopeDot) {
+		if (dotUp < m_minSlopeDot)
+		{
 			return btScalar(1.0);
 		}
 
-		return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace);
+		return ClosestConvexResultCallback::addSingleResult(convexResult, normalInWorldSpace);
 	}
+
 protected:
 	btCollisionObject* m_me;
 	const btVector3 m_up;
@@ -110,7 +109,7 @@ protected:
  *
  * from: http://www-cs-students.stanford.edu/~adityagp/final/node3.html
  */
-btVector3 btKinematicCharacterController::computeReflectionDirection (const btVector3& direction, const btVector3& normal)
+btVector3 btKinematicCharacterController::computeReflectionDirection(const btVector3& direction, const btVector3& normal)
 {
 	return direction - (btScalar(2.0) * direction.dot(normal)) * normal;
 }
@@ -118,7 +117,7 @@ btVector3 btKinematicCharacterController::computeReflectionDirection (const btVe
 /*
  * Returns the portion of 'direction' that is parallel to 'normal'
  */
-btVector3 btKinematicCharacterController::parallelComponent (const btVector3& direction, const btVector3& normal)
+btVector3 btKinematicCharacterController::parallelComponent(const btVector3& direction, const btVector3& normal)
 {
 	btScalar magnitude = direction.dot(normal);
 	return normal * magnitude;
@@ -127,29 +126,29 @@ btVector3 btKinematicCharacterController::parallelComponent (const btVector3& di
 /*
  * Returns the portion of 'direction' that is perpindicular to 'normal'
  */
-btVector3 btKinematicCharacterController::perpindicularComponent (const btVector3& direction, const btVector3& normal)
+btVector3 btKinematicCharacterController::perpindicularComponent(const btVector3& direction, const btVector3& normal)
 {
 	return direction - parallelComponent(direction, normal);
 }
 
-btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, const btVector3& up)
+btKinematicCharacterController::btKinematicCharacterController(btPairCachingGhostObject* ghostObject, btConvexShape* convexShape, btScalar stepHeight, const btVector3& up)
 {
 	m_ghostObject = ghostObject;
 	m_up.setValue(0.0f, 0.0f, 1.0f);
 	m_jumpAxis.setValue(0.0f, 0.0f, 1.0f);
 	m_addedMargin = 0.02;
-	m_walkDirection.setValue(0.0,0.0,0.0);
+	m_walkDirection.setValue(0.0, 0.0, 0.0);
 	m_AngVel.setValue(0.0, 0.0, 0.0);
-	m_useGhostObjectSweepTest = true;	
+	m_useGhostObjectSweepTest = true;
 	m_turnAngle = btScalar(0.0);
-	m_convexShape=convexShape;	
-	m_useWalkDirection = true;	// use walk direction by default, legacy behavior
+	m_convexShape = convexShape;
+	m_useWalkDirection = true;  // use walk direction by default, legacy behavior
 	m_velocityTimeInterval = 0.0;
 	m_verticalVelocity = 0.0;
 	m_verticalOffset = 0.0;
-	m_gravity = 9.8 * 3.0 ; // 3G acceleration.
-	m_fallSpeed = 55.0; // Terminal velocity of a sky diver in m/s.
-	m_jumpSpeed = 10.0; // ?
+	m_gravity = 9.8 * 3.0;  // 3G acceleration.
+	m_fallSpeed = 55.0;     // Terminal velocity of a sky diver in m/s.
+	m_jumpSpeed = 10.0;     // ?
 	m_SetjumpSpeed = m_jumpSpeed;
 	m_wasOnGround = false;
 	m_wasJumping = false;
@@ -166,7 +165,7 @@ btKinematicCharacterController::btKinematicCharacterController (btPairCachingGho
 	setMaxSlope(btRadians(45.0));
 }
 
-btKinematicCharacterController::~btKinematicCharacterController ()
+btKinematicCharacterController::~btKinematicCharacterController()
 {
 }
 
@@ -175,7 +174,7 @@ btPairCachingGhostObject* btKinematicCharacterController::getGhostObject()
 	return m_ghostObject;
 }
 
-bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld* collisionWorld)
+bool btKinematicCharacterController::recoverFromPenetration(btCollisionWorld* collisionWorld)
 {
 	// Here we must refresh the overlapping paircache as the penetrating movement itself or the
 	// previous recovery iteration might have used setWorldTransform and pushed us into an object
@@ -186,19 +185,19 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
 	// paircache and the ghostobject's internal paircache at the same time.    /BW
 
 	btVector3 minAabb, maxAabb;
-	m_convexShape->getAabb(m_ghostObject->getWorldTransform(), minAabb,maxAabb);
-	collisionWorld->getBroadphase()->setAabb(m_ghostObject->getBroadphaseHandle(), 
-						 minAabb, 
-						 maxAabb, 
-						 collisionWorld->getDispatcher());
-						 
+	m_convexShape->getAabb(m_ghostObject->getWorldTransform(), minAabb, maxAabb);
+	collisionWorld->getBroadphase()->setAabb(m_ghostObject->getBroadphaseHandle(),
+											 minAabb,
+											 maxAabb,
+											 collisionWorld->getDispatcher());
+
 	bool penetration = false;
 
 	collisionWorld->getDispatcher()->dispatchAllCollisionPairs(m_ghostObject->getOverlappingPairCache(), collisionWorld->getDispatchInfo(), collisionWorld->getDispatcher());
 
 	m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
-	
-//	btScalar maxPen = btScalar(0.0);
+
+	//	btScalar maxPen = btScalar(0.0);
 	for (int i = 0; i < m_ghostObject->getOverlappingPairCache()->getNumOverlappingPairs(); i++)
 	{
 		m_manifoldArray.resize(0);
@@ -206,25 +205,24 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
 		btBroadphasePair* collisionPair = &m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray()[i];
 
 		btCollisionObject* obj0 = static_cast<btCollisionObject*>(collisionPair->m_pProxy0->m_clientObject);
-        btCollisionObject* obj1 = static_cast<btCollisionObject*>(collisionPair->m_pProxy1->m_clientObject);
+		btCollisionObject* obj1 = static_cast<btCollisionObject*>(collisionPair->m_pProxy1->m_clientObject);
 
 		if ((obj0 && !obj0->hasContactResponse()) || (obj1 && !obj1->hasContactResponse()))
 			continue;
 
 		if (!needsCollision(obj0, obj1))
 			continue;
-		
+
 		if (collisionPair->m_algorithm)
 			collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray);
 
-		
-		for (int j=0;j<m_manifoldArray.size();j++)
+		for (int j = 0; j < m_manifoldArray.size(); j++)
 		{
 			btPersistentManifold* manifold = m_manifoldArray[j];
 			btScalar directionSign = manifold->getBody0() == m_ghostObject ? btScalar(-1.0) : btScalar(1.0);
-			for (int p=0;p<manifold->getNumContacts();p++)
+			for (int p = 0; p < manifold->getNumContacts(); p++)
 			{
-				const btManifoldPoint&pt = manifold->getContactPoint(p);
+				const btManifoldPoint& pt = manifold->getContactPoint(p);
 
 				btScalar dist = pt.getDistance();
 
@@ -239,22 +237,24 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
 					//}
 					m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * btScalar(0.2);
 					penetration = true;
-				} else {
+				}
+				else
+				{
 					//printf("touching %f\n", dist);
 				}
 			}
-			
+
 			//manifold->clearManifold();
 		}
 	}
 	btTransform newTrans = m_ghostObject->getWorldTransform();
 	newTrans.setOrigin(m_currentPosition);
 	m_ghostObject->setWorldTransform(newTrans);
-//	printf("m_touchingNormal = %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]);
+	//	printf("m_touchingNormal = %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]);
 	return penetration;
 }
 
-void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
+void btKinematicCharacterController::stepUp(btCollisionWorld* world)
 {
 	btScalar stepHeight = 0.0f;
 	if (m_verticalVelocity < 0.0)
@@ -263,8 +263,8 @@ void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
 	// phase 1: up
 	btTransform start, end;
 
-	start.setIdentity ();
-	end.setIdentity ();
+	start.setIdentity();
+	end.setIdentity();
 
 	/* FIXME: Handle penetration properly */
 	start.setOrigin(m_currentPosition);
@@ -272,7 +272,7 @@ void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
 	m_targetPosition = m_currentPosition + m_up * (stepHeight) + m_jumpAxis * ((m_verticalOffset > 0.f ? m_verticalOffset : 0.f));
 	m_currentPosition = m_targetPosition;
 
-	end.setOrigin (m_targetPosition);
+	end.setOrigin(m_targetPosition);
 
 	start.setRotation(m_currentOrientation);
 	end.setRotation(m_targetOrientation);
@@ -280,10 +280,10 @@ void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
 	btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, -m_up, m_maxSlopeCosine);
 	callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
 	callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
-	
+
 	if (m_useGhostObjectSweepTest)
 	{
-		m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration);
+		m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration);
 	}
 	else
 	{
@@ -298,7 +298,7 @@ void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
 			// we moved up only a fraction of the step height
 			m_currentStepOffset = stepHeight * callback.m_closestHitFraction;
 			if (m_interpolateUp == true)
-				m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
+				m_currentPosition.setInterpolate3(m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
 			else
 				m_currentPosition = m_targetPosition;
 		}
@@ -329,7 +329,9 @@ void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
 			m_verticalVelocity = 0.0;
 			m_currentStepOffset = m_stepHeight;
 		}
-	} else {
+	}
+	else
+	{
 		m_currentStepOffset = stepHeight;
 		m_currentPosition = m_targetPosition;
 	}
@@ -342,43 +344,44 @@ bool btKinematicCharacterController::needsCollision(const btCollisionObject* bod
 	return collides;
 }
 
-void btKinematicCharacterController::updateTargetPositionBasedOnCollision (const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag)
+void btKinematicCharacterController::updateTargetPositionBasedOnCollision(const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag)
 {
 	btVector3 movementDirection = m_targetPosition - m_currentPosition;
 	btScalar movementLength = movementDirection.length();
-	if (movementLength>SIMD_EPSILON)
+	if (movementLength > SIMD_EPSILON)
 	{
 		movementDirection.normalize();
 
-		btVector3 reflectDir = computeReflectionDirection (movementDirection, hitNormal);
+		btVector3 reflectDir = computeReflectionDirection(movementDirection, hitNormal);
 		reflectDir.normalize();
 
 		btVector3 parallelDir, perpindicularDir;
 
-		parallelDir = parallelComponent (reflectDir, hitNormal);
-		perpindicularDir = perpindicularComponent (reflectDir, hitNormal);
+		parallelDir = parallelComponent(reflectDir, hitNormal);
+		perpindicularDir = perpindicularComponent(reflectDir, hitNormal);
 
 		m_targetPosition = m_currentPosition;
-		if (0)//tangentMag != 0.0)
+		if (0)  //tangentMag != 0.0)
 		{
-			btVector3 parComponent = parallelDir * btScalar (tangentMag*movementLength);
-//			printf("parComponent=%f,%f,%f\n",parComponent[0],parComponent[1],parComponent[2]);
-			m_targetPosition +=  parComponent;
+			btVector3 parComponent = parallelDir * btScalar(tangentMag * movementLength);
+			//			printf("parComponent=%f,%f,%f\n",parComponent[0],parComponent[1],parComponent[2]);
+			m_targetPosition += parComponent;
 		}
 
 		if (normalMag != 0.0)
 		{
-			btVector3 perpComponent = perpindicularDir * btScalar (normalMag*movementLength);
-//			printf("perpComponent=%f,%f,%f\n",perpComponent[0],perpComponent[1],perpComponent[2]);
+			btVector3 perpComponent = perpindicularDir * btScalar(normalMag * movementLength);
+			//			printf("perpComponent=%f,%f,%f\n",perpComponent[0],perpComponent[1],perpComponent[2]);
 			m_targetPosition += perpComponent;
 		}
-	} else
+	}
+	else
 	{
-//		printf("movementLength don't normalize a zero vector\n");
+		//		printf("movementLength don't normalize a zero vector\n");
 	}
 }
 
-void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* collisionWorld, const btVector3& walkMove)
+void btKinematicCharacterController::stepForwardAndStrafe(btCollisionWorld* collisionWorld, const btVector3& walkMove)
 {
 	// printf("m_normalizedDirection=%f,%f,%f\n",
 	// 	m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]);
@@ -387,29 +390,28 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
 
 	m_targetPosition = m_currentPosition + walkMove;
 
-	start.setIdentity ();
-	end.setIdentity ();
-	
+	start.setIdentity();
+	end.setIdentity();
+
 	btScalar fraction = 1.0;
-	btScalar distance2 = (m_currentPosition-m_targetPosition).length2();
-//	printf("distance2=%f\n",distance2);
+	btScalar distance2 = (m_currentPosition - m_targetPosition).length2();
+	//	printf("distance2=%f\n",distance2);
 
 	int maxIter = 10;
 
 	while (fraction > btScalar(0.01) && maxIter-- > 0)
 	{
-		start.setOrigin (m_currentPosition);
-		end.setOrigin (m_targetPosition);
+		start.setOrigin(m_currentPosition);
+		end.setOrigin(m_targetPosition);
 		btVector3 sweepDirNegative(m_currentPosition - m_targetPosition);
 
 		start.setRotation(m_currentOrientation);
 		end.setRotation(m_targetOrientation);
 
-		btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, sweepDirNegative, btScalar(0.0));
+		btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, sweepDirNegative, btScalar(0.0));
 		callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
 		callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
 
-
 		btScalar margin = m_convexShape->getMargin();
 		m_convexShape->setMargin(margin + m_addedMargin);
 
@@ -426,18 +428,17 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
 		}
 		m_convexShape->setMargin(margin);
 
-		
 		fraction -= callback.m_closestHitFraction;
 
 		if (callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
-		{	
+		{
 			// we moved only a fraction
 			//btScalar hitDistance;
 			//hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
 
-//			m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
+			//			m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
 
-			updateTargetPositionBasedOnCollision (callback.m_hitNormalWorld);
+			updateTargetPositionBasedOnCollision(callback.m_hitNormalWorld);
 			btVector3 currentDir = m_targetPosition - m_currentPosition;
 			distance2 = currentDir.length2();
 			if (distance2 > SIMD_EPSILON)
@@ -448,21 +449,21 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
 				{
 					break;
 				}
-			} else
+			}
+			else
 			{
-//				printf("currentDir: don't normalize a zero vector\n");
+				//				printf("currentDir: don't normalize a zero vector\n");
 				break;
 			}
-
 		}
-        else
-        {
-            m_currentPosition = m_targetPosition;
+		else
+		{
+			m_currentPosition = m_targetPosition;
 		}
 	}
 }
 
-void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld, btScalar dt)
+void btKinematicCharacterController::stepDown(btCollisionWorld* collisionWorld, btScalar dt)
 {
 	btTransform start, end, end_double;
 	bool runonce = false;
@@ -475,64 +476,64 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
 	m_targetPosition -= (step_drop + gravity_drop);*/
 
 	btVector3 orig_position = m_targetPosition;
-	
-	btScalar downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
+
+	btScalar downVelocity = (m_verticalVelocity < 0.f ? -m_verticalVelocity : 0.f) * dt;
 
 	if (m_verticalVelocity > 0.0)
 		return;
 
-	if(downVelocity > 0.0 && downVelocity > m_fallSpeed
-		&& (m_wasOnGround || !m_wasJumping))
+	if (downVelocity > 0.0 && downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping))
 		downVelocity = m_fallSpeed;
 
 	btVector3 step_drop = m_up * (m_currentStepOffset + downVelocity);
 	m_targetPosition -= step_drop;
 
 	btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, m_up, m_maxSlopeCosine);
-        callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
-        callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
+	callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
+	callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
 
 	btKinematicClosestNotMeConvexResultCallback callback2(m_ghostObject, m_up, m_maxSlopeCosine);
-        callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
-        callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
+	callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
+	callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
 
 	while (1)
 	{
-		start.setIdentity ();
-		end.setIdentity ();
+		start.setIdentity();
+		end.setIdentity();
 
-		end_double.setIdentity ();
+		end_double.setIdentity();
 
-		start.setOrigin (m_currentPosition);
-		end.setOrigin (m_targetPosition);
+		start.setOrigin(m_currentPosition);
+		end.setOrigin(m_targetPosition);
 
 		start.setRotation(m_currentOrientation);
 		end.setRotation(m_targetOrientation);
 
 		//set double test for 2x the step drop, to check for a large drop vs small drop
-		end_double.setOrigin (m_targetPosition - step_drop);
+		end_double.setOrigin(m_targetPosition - step_drop);
 
 		if (m_useGhostObjectSweepTest)
 		{
-			m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+			m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
 
 			if (!callback.hasHit() && m_ghostObject->hasContactResponse())
 			{
 				//test a double fall height, to see if the character should interpolate it's fall (full) or not (partial)
-				m_ghostObject->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+				m_ghostObject->convexSweepTest(m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
 			}
-		} else
+		}
+		else
 		{
-			collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+			collisionWorld->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
 
 			if (!callback.hasHit() && m_ghostObject->hasContactResponse())
 			{
 				//test a double fall height, to see if the character should interpolate it's fall (large) or not (small)
-				collisionWorld->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+				collisionWorld->convexSweepTest(m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
 			}
 		}
-	
-		btScalar downVelocity2 = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
+
+		btScalar downVelocity2 = (m_verticalVelocity < 0.f ? -m_verticalVelocity : 0.f) * dt;
 		bool has_hit;
 		if (bounce_fix == true)
 			has_hit = (callback.hasHit() || callback2.hasHit()) && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject);
@@ -543,8 +544,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
 		if (m_verticalVelocity < 0.0)
 			stepHeight = m_stepHeight;
 
-		if (downVelocity2 > 0.0 && downVelocity2 < stepHeight && has_hit == true && runonce == false
-					&& (m_wasOnGround || !m_wasJumping))
+		if (downVelocity2 > 0.0 && downVelocity2 < stepHeight && has_hit == true && runonce == false && (m_wasOnGround || !m_wasJumping))
 		{
 			//redo the velocity calculation when falling a small amount, for fast stairs motion
 			//for larger falls, use the smoother/slower interpolated movement by not touching the target position
@@ -555,7 +555,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
 			step_drop = m_up * (m_currentStepOffset + downVelocity);
 			m_targetPosition -= step_drop;
 			runonce = true;
-			continue; //re-run previous tests
+			continue;  //re-run previous tests
 		}
 		break;
 	}
@@ -570,30 +570,32 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
 		if (bounce_fix == true)
 		{
 			if (full_drop == true)
-				m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
-            else
+				m_currentPosition.setInterpolate3(m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
+			else
 				//due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually
-				m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, fraction);
+				m_currentPosition.setInterpolate3(m_currentPosition, m_targetPosition, fraction);
 		}
 		else
-			m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
+			m_currentPosition.setInterpolate3(m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
 
 		full_drop = false;
 
 		m_verticalVelocity = 0.0;
 		m_verticalOffset = 0.0;
 		m_wasJumping = false;
-	} else {
+	}
+	else
+	{
 		// we dropped the full height
 
 		full_drop = true;
 
 		if (bounce_fix == true)
 		{
-			downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
+			downVelocity = (m_verticalVelocity < 0.f ? -m_verticalVelocity : 0.f) * dt;
 			if (downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping))
 			{
-				m_targetPosition += step_drop; //undo previous target change
+				m_targetPosition += step_drop;  //undo previous target change
 				downVelocity = m_fallSpeed;
 				step_drop = m_up * (m_currentStepOffset + downVelocity);
 				m_targetPosition -= step_drop;
@@ -605,30 +607,22 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
 	}
 }
 
-
-
-void btKinematicCharacterController::setWalkDirection
-(
-const btVector3& walkDirection
-)
+void btKinematicCharacterController::setWalkDirection(
+	const btVector3& walkDirection)
 {
 	m_useWalkDirection = true;
 	m_walkDirection = walkDirection;
 	m_normalizedDirection = getNormalizedVector(m_walkDirection);
 }
 
-
-
-void btKinematicCharacterController::setVelocityForTimeInterval
-(
-const btVector3& velocity,
-btScalar timeInterval
-)
+void btKinematicCharacterController::setVelocityForTimeInterval(
+	const btVector3& velocity,
+	btScalar timeInterval)
 {
-//	printf("setVelocity!\n");
-//	printf("  interval: %f\n", timeInterval);
-//	printf("  velocity: (%f, %f, %f)\n",
-//		 velocity.x(), velocity.y(), velocity.z());
+	//	printf("setVelocity!\n");
+	//	printf("  interval: %f\n", timeInterval);
+	//	printf("  velocity: (%f, %f, %f)\n",
+	//		 velocity.x(), velocity.y(), velocity.z());
 
 	m_useWalkDirection = false;
 	m_walkDirection = velocity;
@@ -661,7 +655,7 @@ void btKinematicCharacterController::setLinearVelocity(const btVector3& velocity
 			btVector3 upComponent = m_up * (btSin(SIMD_HALF_PI - btAcos(c)) * m_walkDirection.length());
 			m_walkDirection -= upComponent;
 			m_verticalVelocity = (c < 0.0f ? -1 : 1) * upComponent.length();
-			
+
 			if (c > 0.0f)
 			{
 				m_wasJumping = true;
@@ -678,46 +672,45 @@ btVector3 btKinematicCharacterController::getLinearVelocity() const
 	return m_walkDirection + (m_verticalVelocity * m_up);
 }
 
-void btKinematicCharacterController::reset ( btCollisionWorld* collisionWorld )
+void btKinematicCharacterController::reset(btCollisionWorld* collisionWorld)
 {
-    m_verticalVelocity = 0.0;
-    m_verticalOffset = 0.0;
-    m_wasOnGround = false;
-    m_wasJumping = false;
-    m_walkDirection.setValue(0,0,0);
-    m_velocityTimeInterval = 0.0;
+	m_verticalVelocity = 0.0;
+	m_verticalOffset = 0.0;
+	m_wasOnGround = false;
+	m_wasJumping = false;
+	m_walkDirection.setValue(0, 0, 0);
+	m_velocityTimeInterval = 0.0;
 
-    //clear pair cache
-    btHashedOverlappingPairCache *cache = m_ghostObject->getOverlappingPairCache();
-    while (cache->getOverlappingPairArray().size() > 0)
-    {
-            cache->removeOverlappingPair(cache->getOverlappingPairArray()[0].m_pProxy0, cache->getOverlappingPairArray()[0].m_pProxy1, collisionWorld->getDispatcher());
-    }
+	//clear pair cache
+	btHashedOverlappingPairCache* cache = m_ghostObject->getOverlappingPairCache();
+	while (cache->getOverlappingPairArray().size() > 0)
+	{
+		cache->removeOverlappingPair(cache->getOverlappingPairArray()[0].m_pProxy0, cache->getOverlappingPairArray()[0].m_pProxy1, collisionWorld->getDispatcher());
+	}
 }
 
-void btKinematicCharacterController::warp (const btVector3& origin)
+void btKinematicCharacterController::warp(const btVector3& origin)
 {
 	btTransform xform;
 	xform.setIdentity();
-	xform.setOrigin (origin);
-	m_ghostObject->setWorldTransform (xform);
+	xform.setOrigin(origin);
+	m_ghostObject->setWorldTransform(xform);
 }
 
-
-void btKinematicCharacterController::preStep (  btCollisionWorld* collisionWorld)
+void btKinematicCharacterController::preStep(btCollisionWorld* collisionWorld)
 {
 	m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
 	m_targetPosition = m_currentPosition;
 
 	m_currentOrientation = m_ghostObject->getWorldTransform().getRotation();
 	m_targetOrientation = m_currentOrientation;
-//	printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]);
+	//	printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]);
 }
 
-void btKinematicCharacterController::playerStep (  btCollisionWorld* collisionWorld, btScalar dt)
+void btKinematicCharacterController::playerStep(btCollisionWorld* collisionWorld, btScalar dt)
 {
-//	printf("playerStep(): ");
-//	printf("  dt = %f", dt);
+	//	printf("playerStep(): ");
+	//	printf("  dt = %f", dt);
 
 	if (m_AngVel.length2() > 0.0f)
 	{
@@ -744,16 +737,17 @@ void btKinematicCharacterController::playerStep (  btCollisionWorld* collisionWo
 	}
 
 	// quick check...
-	if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0)) {
-//		printf("\n");
-		return;		// no motion
+	if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0 || m_walkDirection.fuzzyZero())) 
+	{
+		//		printf("\n");
+		return;  // no motion
 	}
 
 	m_wasOnGround = onGround();
 
 	//btVector3 lvel = m_walkDirection;
 	//btScalar c = 0.0f;
-	
+
 	if (m_walkDirection.length2() > 0)
 	{
 		// apply damping
@@ -761,7 +755,7 @@ void btKinematicCharacterController::playerStep (  btCollisionWorld* collisionWo
 	}
 
 	m_verticalVelocity *= btPow(btScalar(1) - m_linearDamping, dt);
-	
+
 	// Update fall velocity.
 	m_verticalVelocity -= m_gravity * dt;
 	if (m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
@@ -777,12 +771,12 @@ void btKinematicCharacterController::playerStep (  btCollisionWorld* collisionWo
 	btTransform xform;
 	xform = m_ghostObject->getWorldTransform();
 
-//	printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]);
-//	printf("walkSpeed=%f\n",walkSpeed);
+	//	printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]);
+	//	printf("walkSpeed=%f\n",walkSpeed);
 
 	stepUp(collisionWorld);
 	//todo: Experimenting with behavior of controller when it hits a ceiling..
-	//bool hitUp = stepUp (collisionWorld);	
+	//bool hitUp = stepUp (collisionWorld);
 	//if (hitUp)
 	//{
 	//	m_verticalVelocity -= m_gravity * dt;
@@ -799,9 +793,12 @@ void btKinematicCharacterController::playerStep (  btCollisionWorld* collisionWo
 	//	xform = m_ghostObject->getWorldTransform();
 	//}
 
-	if (m_useWalkDirection) {
-		stepForwardAndStrafe (collisionWorld, m_walkDirection);
-	} else {
+	if (m_useWalkDirection)
+	{
+		stepForwardAndStrafe(collisionWorld, m_walkDirection);
+	}
+	else
+	{
 		//printf("  time: %f", m_velocityTimeInterval);
 		// still have some time left for moving!
 		btScalar dtMoving =
@@ -816,7 +813,7 @@ void btKinematicCharacterController::playerStep (  btCollisionWorld* collisionWo
 		// okay, step
 		stepForwardAndStrafe(collisionWorld, move);
 	}
-	stepDown (collisionWorld, dt);
+	stepDown(collisionWorld, dt);
 
 	//todo: Experimenting with max jump height
 	//if (m_wasJumping)
@@ -827,7 +824,7 @@ void btKinematicCharacterController::playerStep (  btCollisionWorld* collisionWo
 	//		// substract the overshoot
 	//		m_currentPosition[m_upAxis] -= ds - m_maxJumpHeight;
 
-	//		// max height was reached, so potential energy is at max 
+	//		// max height was reached, so potential energy is at max
 	//		// and kinematic energy is 0, thus velocity is 0.
 	//		if (m_verticalVelocity > 0.0)
 	//			m_verticalVelocity = 0.0;
@@ -835,8 +832,8 @@ void btKinematicCharacterController::playerStep (  btCollisionWorld* collisionWo
 	//}
 	// printf("\n");
 
-	xform.setOrigin (m_currentPosition);
-	m_ghostObject->setWorldTransform (xform);
+	xform.setOrigin(m_currentPosition);
+	m_ghostObject->setWorldTransform(xform);
 
 	int numPenetrationLoops = 0;
 	m_touchingContact = false;
@@ -852,23 +849,23 @@ void btKinematicCharacterController::playerStep (  btCollisionWorld* collisionWo
 	}
 }
 
-void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed)
+void btKinematicCharacterController::setFallSpeed(btScalar fallSpeed)
 {
 	m_fallSpeed = fallSpeed;
 }
 
-void btKinematicCharacterController::setJumpSpeed (btScalar jumpSpeed)
+void btKinematicCharacterController::setJumpSpeed(btScalar jumpSpeed)
 {
 	m_jumpSpeed = jumpSpeed;
 	m_SetjumpSpeed = m_jumpSpeed;
 }
 
-void btKinematicCharacterController::setMaxJumpHeight (btScalar maxJumpHeight)
+void btKinematicCharacterController::setMaxJumpHeight(btScalar maxJumpHeight)
 {
 	m_maxJumpHeight = maxJumpHeight;
 }
 
-bool btKinematicCharacterController::canJump () const
+bool btKinematicCharacterController::canJump() const
 {
 	return onGround();
 }
@@ -927,20 +924,20 @@ btScalar btKinematicCharacterController::getMaxPenetrationDepth() const
 	return m_maxPenetrationDepth;
 }
 
-bool btKinematicCharacterController::onGround () const
+bool btKinematicCharacterController::onGround() const
 {
 	return (fabs(m_verticalVelocity) < SIMD_EPSILON) && (fabs(m_verticalOffset) < SIMD_EPSILON);
 }
 
-void btKinematicCharacterController::setStepHeight(btScalar h) 
+void btKinematicCharacterController::setStepHeight(btScalar h)
 {
 	m_stepHeight = h;
 }
 
 btVector3* btKinematicCharacterController::getUpAxisDirections()
 {
-	static btVector3 sUpAxisDirection[3] = { btVector3(1.0f, 0.0f, 0.0f), btVector3(0.0f, 1.0f, 0.0f), btVector3(0.0f, 0.0f, 1.0f) };
-	
+	static btVector3 sUpAxisDirection[3] = {btVector3(1.0f, 0.0f, 0.0f), btVector3(0.0f, 1.0f, 0.0f), btVector3(0.0f, 0.0f, 1.0f)};
+
 	return sUpAxisDirection;
 }
 
@@ -997,4 +994,3 @@ btQuaternion btKinematicCharacterController::getRotation(btVector3& v0, btVector
 
 	return shortestArcQuatNormalize2(v0, v1);
 }
-