+ improved KinematicCharacterController

+ improved btSubsimplexConvexCast, btContinuousConvexCollision and btGjkConvexCast to support configuration that start in touching/penetration, required for 'sliding'. + added files to CMakeLists.txt for CharacterController + bump up version to 2.70 (preparation for beta)
2008-07-09 00:08:49 +00:00
parent 76bac83937
commit dcd57f333b
23 changed files with 431 additions and 243 deletions
--- a/Demos/CharacterDemo/CharacterControllerInterface.h
+++ b/Demos/CharacterDemo/CharacterControllerInterface.h
@@ -12,16 +12,16 @@ class CharacterControllerInterface
 public:
 	CharacterControllerInterface () {};
 	virtual ~CharacterControllerInterface () {};
-	virtual void setup (btDynamicsWorld* dynamicsWorld, btScalar height = 2.0, btScalar width = 0.25, btScalar stepHeight = 0.25) = 0;
-	virtual void destroy (btDynamicsWorld* dynamicsWorld) = 0;
+	virtual void setup (btScalar height = 2.0, btScalar width = 0.25, btScalar stepHeight = 0.25) = 0;
+	virtual void destroy () = 0;

 	virtual btCollisionObject* getCollisionObject () = 0;

 	virtual void reset () = 0;
 	virtual void warp (const btVector3& origin) = 0;
-
-	virtual void preStep (btDynamicsWorld* dynamicsWorld) = 0;
-	virtual void playerStep (btDynamicsWorld* dynamicsWorld, btScalar dt,
+	virtual void registerPairCacheAndDispatcher (btOverlappingPairCache* pairCache, btCollisionDispatcher* dispatcher)=0;
+	virtual void preStep (const btDynamicsWorld* dynamicsWorld) = 0;
+	virtual void playerStep (const btDynamicsWorld* dynamicsWorld, btScalar dt,
 					         int forward,
 							 int backward,
 							 int left,
--- a/Demos/CharacterDemo/CharacterDemo.cpp
+++ b/Demos/CharacterDemo/CharacterDemo.cpp
@@ -43,14 +43,25 @@ static int gLeft = 0;
 static int gRight = 0;
 static int gJump = 0;

-#define QUAKE_BSP_IMPORTING 1
+///playerStepCallback is the main function that is updating the character.
+///Register this callback using: m_dynamicsWorld->setInternalTickCallback(playerStepCallback,m_character);
+///This function will be called at the end of each internal simulation time step
+void playerStepCallback(const btDynamicsWorld* dynamicsWorld, btScalar timeStep)
+{
+	CharacterControllerInterface* characterInterface= (CharacterControllerInterface*) dynamicsWorld->getWorldUserInfo();
+	characterInterface->preStep (dynamicsWorld);
+	characterInterface->playerStep (dynamicsWorld, timeStep, gForward, gBackward, gLeft, gRight, gJump);
+}

+
+#define QUAKE_BSP_IMPORTING 1
 #ifdef QUAKE_BSP_IMPORTING
 #include "../BspDemo/BspLoader.h"
 #include "../BspDemo/BspConverter.h"



+
 class BspToBulletConverter : public BspConverter
 {
 	CharacterDemo* m_demoApp;
@@ -108,7 +119,10 @@ CharacterDemo::~CharacterDemo()
 {
 	//cleanup in the reverse order of creation/initialization
 	if (m_character)
-		m_character->destroy (m_dynamicsWorld);
+	{
+		m_dynamicsWorld->removeCollisionObject(m_character->getCollisionObject());
+		m_character->destroy ();
+	}


 	//remove the rigidbodies from the dynamics world and delete them
@@ -163,8 +177,10 @@ class MyCustomOverlappingPairCallback : public btOverlappingPairCallback
 	{
 		bool needBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const
 		{
-			// we already know that the character proxy is either proxy0 or proxy1
-			return true;
+			bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
+			collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
+			return collides;
+
 		}
 	} myCustomOverlapFilterCallback;

@@ -237,12 +253,12 @@ void CharacterDemo::initPhysics()

 	m_constraintSolver = new btSequentialImpulseConstraintSolver();
 	m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_overlappingPairCache,m_constraintSolver,m_collisionConfiguration);
+	
 	//m_dynamicsWorld->setGravity(btVector3(0,0,0));
 	btTransform tr;
 	tr.setIdentity();

-#define USE_BSP_STAGE
-#ifdef USE_BSP_STAGE
+
 #ifdef QUAKE_BSP_IMPORTING
 	char* bspfilename = "BspDemo.bsp";
 	void* memoryBuffer = 0;
@@ -286,7 +302,6 @@ void CharacterDemo::initPhysics()
 		fclose(file);
 	}

-#endif
 #else
 #define  USE_TRIMESH_GROUND 1
 #ifdef USE_TRIMESH_GROUND
@@ -477,12 +492,19 @@ const float TRIANGLE_SIZE=20.f;
 #else
 	m_character = new KinematicCharacterController ();
 #endif
-	m_character->setup (m_dynamicsWorld);
+	m_character->setup ();
+	
+	m_dynamicsWorld->setInternalTickCallback(playerStepCallback,m_character);

 	//some custom callback sample
 	m_customPairCallback = new MyCustomOverlappingPairCallback(this,m_character->getCollisionObject());
 	sweepBP->setOverlappingPairUserCallback(m_customPairCallback);
-	m_character->registerPairCache (m_customPairCallback->getOverlappingPairCache());
+	m_character->registerPairCacheAndDispatcher (m_customPairCallback->getOverlappingPairCache(), m_dispatcher);
+
+	m_dynamicsWorld->addCollisionObject(m_character->getCollisionObject(),btBroadphaseProxy::DebrisFilter, btBroadphaseProxy::StaticFilter);//AllFilter);
+
+	
+
 	clientResetScene();

 	setCameraDistance(26.f);
@@ -498,22 +520,11 @@ void CharacterDemo::renderme()
 	DemoApplication::renderme();
 }

-void CharacterDemo::clientMoveAndDisplay()
+
+
+void	CharacterDemo::debugDrawContacts()
 {
-
-	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
-
-	float dt = getDeltaTimeMicroseconds() * 0.000001f;
-
-	/* Character stuff &*/
-	if (m_character)
-	{
-		m_character->preStep (m_dynamicsWorld);
-		m_character->playerStep (m_dynamicsWorld, dt, gForward, gBackward, gLeft, gRight, gJump);
-	}
-
-#if 0
-	printf("numPairs = %d\n",m_customPairCallback->getOverlappingPairArray().size());
+//	printf("numPairs = %d\n",m_customPairCallback->getOverlappingPairArray().size());
 	{
 		btManifoldArray	manifoldArray;
 		for (int i=0;i<m_customPairCallback->getOverlappingPairArray().size();i++)
@@ -541,8 +552,23 @@ void CharacterDemo::clientMoveAndDisplay()
 			}
 		}
 	}
-#endif

+}
+
+void CharacterDemo::clientMoveAndDisplay()
+{
+
+	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+	float dt = getDeltaTimeMicroseconds() * 0.000001f;
+
+	/* Character stuff &*/
+	if (m_character)
+	{
+		
+	}
+
+	debugDrawContacts();


 	if (m_dynamicsWorld)
@@ -612,6 +638,7 @@ void CharacterDemo::displayCallback(void)
 	if (m_dynamicsWorld)
 		m_dynamicsWorld->debugDrawWorld();

+	debugDrawContacts();

 	glFlush();
 	glutSwapBuffers();
@@ -622,7 +649,8 @@ void CharacterDemo::clientResetScene()
 	m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(m_character->getCollisionObject()->getBroadphaseHandle(),getDynamicsWorld()->getDispatcher());

 	m_character->reset ();
-	m_character->warp (btVector3(0.0, 1.75, 0.0));
+	///WTF
+	m_character->warp (btVector3(0, -2.0, 0.));
 }

 void CharacterDemo::specialKeyboardUp(int key, int x, int y)
--- a/Demos/CharacterDemo/CharacterDemo.h
+++ b/Demos/CharacterDemo/CharacterDemo.h
@@ -15,7 +15,12 @@ subject to the following restrictions:
 #ifndef CHARACTER_DEMO_H
 #define CHARACTER_DEMO_H

-class CharacterController;
+
+///DYNAMIC_CHARACTER_CONTROLLER is not supported/obsolete at the moment
+//#define DYNAMIC_CHARACTER_CONTROLLER 1
+
+class CharacterControllerInterface;
+class DynamicCharacterController;
 class KinematicCharacterController;

 class btCollisionShape;
@@ -29,7 +34,7 @@ class CharacterDemo : public DemoApplication
 	public:

 #ifdef DYNAMIC_CHARACTER_CONTROLLER
-	CharacterController* m_character;
+	CharacterControllerInterface* m_character;
 #else
 	KinematicCharacterController* m_character;
 #endif
@@ -50,6 +55,7 @@ class CharacterDemo : public DemoApplication

 	btVector3*	m_vertices;

+	void	debugDrawContacts();
 	
 	float		m_cameraHeight;

@@ -67,7 +73,7 @@ class CharacterDemo : public DemoApplication

 	virtual void displayCallback();
 	
-	///a very basic camera following the vehicle
+	///a very basic camera following the character
 	virtual void updateCamera();

 	virtual void specialKeyboard(int key, int x, int y);
--- a/Demos/CharacterDemo/DynamicCharacterController.cpp
+++ b/Demos/CharacterDemo/DynamicCharacterController.cpp
@@ -21,7 +21,7 @@ DynamicCharacterController::~DynamicCharacterController ()
 {
 }

-void DynamicCharacterController::setup (btDynamicsWorld* dynamicsWorld, btScalar height, btScalar width, btScalar stepHeight)
+void DynamicCharacterController::setup (btScalar height, btScalar width, btScalar stepHeight)
 {
 	btVector3 spherePositions[2];
 	btScalar sphereRadii[2];
@@ -45,10 +45,10 @@ void DynamicCharacterController::setup (btDynamicsWorld* dynamicsWorld, btScalar
 	//m_rigidBody->setCollisionFlags( m_rigidBody->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
 	m_rigidBody->setSleepingThresholds (0.0, 0.0);
 	m_rigidBody->setAngularFactor (0.0);
-	dynamicsWorld->addRigidBody (m_rigidBody);
+	
 }

-void DynamicCharacterController::destroy (btDynamicsWorld* dynamicsWorld)
+void DynamicCharacterController::destroy ()
 {
 	if (m_shape)
 	{
@@ -57,8 +57,8 @@ void DynamicCharacterController::destroy (btDynamicsWorld* dynamicsWorld)

 	if (m_rigidBody)
 	{
-		dynamicsWorld->removeRigidBody (m_rigidBody);
 		delete m_rigidBody;
+		m_rigidBody = 0;
 	}
 }

@@ -67,7 +67,7 @@ btCollisionObject* DynamicCharacterController::getCollisionObject ()
 	return m_rigidBody;
 }

-void DynamicCharacterController::preStep (btDynamicsWorld* dynamicsWorld)
+void DynamicCharacterController::preStep (const btDynamicsWorld* dynamicsWorld)
 {
 	btTransform xform;
 	m_rigidBody->getMotionState()->getWorldTransform (xform);
@@ -118,7 +118,7 @@ void DynamicCharacterController::preStep (btDynamicsWorld* dynamicsWorld)
 	}
 }

-void DynamicCharacterController::playerStep (btScalar dt,
+void DynamicCharacterController::playerStep (const btDynamicsWorld* dynaWorld,btScalar dt,
 					 int forward,
 					 int backward,
 					 int left,
@@ -190,3 +190,15 @@ bool DynamicCharacterController::onGround () const
 {
 	return m_rayLambda[0] < btScalar(1.0);
 }
+
+void DynamicCharacterController::reset ()
+{
+}
+void DynamicCharacterController::warp (const btVector3& origin)
+{
+}
+void DynamicCharacterController::registerPairCacheAndDispatcher (btOverlappingPairCache* pairCache, btCollisionDispatcher* dispatcher)
+{
+
+}
+
--- a/Demos/CharacterDemo/DynamicCharacterController.h
+++ b/Demos/CharacterDemo/DynamicCharacterController.h
@@ -9,6 +9,7 @@ class btCollisionShape;
 class btRigidBody;
 class btDynamicsWorld;

+///DynamicCharacterController is obsolete/unsupported at the moment
 class DynamicCharacterController : public CharacterControllerInterface
 {
 protected:
@@ -29,13 +30,17 @@ protected:
 public:
 	DynamicCharacterController ();
 	~DynamicCharacterController ();
-	void setup (btDynamicsWorld* dynamicsWorld, btScalar height = 2.0, btScalar width = 0.25, btScalar stepHeight = 0.25);
-	void destroy (btDynamicsWorld* dynamicsWorld);
+	void setup (btScalar height = 2.0, btScalar width = 0.25, btScalar stepHeight = 0.25);
+	void destroy ();
+
+	virtual void reset ();
+	virtual void warp (const btVector3& origin);
+	virtual void registerPairCacheAndDispatcher (btOverlappingPairCache* pairCache, btCollisionDispatcher* dispatcher);

 	btCollisionObject* getCollisionObject ();

-	void preStep (btDynamicsWorld* dynamicsWorld);
-	void playerStep (btScalar dt,
+	void preStep (const btDynamicsWorld* dynamicsWorld);
+	void playerStep (const btDynamicsWorld* dynaWorld,btScalar dt,
 					 int forward,
 					 int backward,
 					 int left,
--- a/Demos/CharacterDemo/KinematicCharacterController.cpp
+++ b/Demos/CharacterDemo/KinematicCharacterController.cpp
@@ -87,7 +87,7 @@ btVector3 perpindicularComponent (const btVector3& direction, const btVector3& n
 KinematicCharacterController::KinematicCharacterController ()
 {
 	m_turnAngle = btScalar(0.0);
-	m_walkVelocity = btScalar(1.1) * 4.0; // 4 km/h -> 1.1 m/s
+	m_walkVelocity = btScalar(1.1) * 14.0; // 4 km/h -> 1.1 m/s
 	m_shape = NULL;
 	m_pairCache = NULL;
 	m_collisionObject = NULL;
@@ -97,15 +97,15 @@ KinematicCharacterController::~KinematicCharacterController ()
 {
 }

-void KinematicCharacterController::setup (btDynamicsWorld* dynamicsWorld, btScalar height, btScalar width, btScalar stepHeight)
+void KinematicCharacterController::setup (btScalar height, btScalar width, btScalar stepHeight)
 {
 	btVector3 spherePositions[2];
 	btScalar sphereRadii[2];
 	
 	sphereRadii[0] = width;
 	sphereRadii[1] = width;
-	spherePositions[0] = btVector3 (0.0, (height/btScalar(2.0) - width), 0.0);
-	spherePositions[1] = btVector3 (0.0, (-height/btScalar(2.0) + width), 0.0);
+	spherePositions[0] = btVector3 (0.0, (height/btScalar(2.0) ), 0.0);
+	spherePositions[1] = btVector3 (0.0, (-height/btScalar(2.0)), 0.0);

 	m_halfHeight = height/btScalar(2.0);

@@ -116,19 +116,20 @@ void KinematicCharacterController::setup (btDynamicsWorld* dynamicsWorld, btScal
 	btTransform startTransform;
 	startTransform.setIdentity ();
 	startTransform.setOrigin (btVector3(0.0, 4.0, 0.0));
-	btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
-	btRigidBody::btRigidBodyConstructionInfo cInfo(1.0, myMotionState, m_shape);
+	//btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
+	//btRigidBody::btRigidBodyConstructionInfo cInfo(1.0, myMotionState, m_shape);
 	m_collisionObject = new btCollisionObject ();
+
+	m_collisionObject->setWorldTransform(startTransform);
 	m_collisionObject->setCollisionShape (m_shape);
-	m_collisionObject->setCollisionFlags (btCollisionObject::CF_KINEMATIC_OBJECT);
-	dynamicsWorld->addCollisionObject (m_collisionObject);
+	m_collisionObject->setCollisionFlags (btCollisionObject::CF_NO_CONTACT_RESPONSE);
+	
 }

-void KinematicCharacterController::destroy (btDynamicsWorld* dynamicsWorld)
+void KinematicCharacterController::destroy ()
 {
 	if (m_collisionObject)
 	{
-		dynamicsWorld->removeCollisionObject (m_collisionObject);
 		delete m_collisionObject;
 	}

@@ -143,29 +144,33 @@ btCollisionObject* KinematicCharacterController::getCollisionObject ()
 	return m_collisionObject;
 }

-bool KinematicCharacterController::recoverFromPenetration (btDynamicsWorld* dynamicsWorld)
+bool KinematicCharacterController::recoverFromPenetration (const btDynamicsWorld* dynamicsWorld)
 {
+
 	if (m_pairCache == NULL)
 		return false;
 	
 	bool penetration = false;

-	dynamicsWorld->getDispatcher()->dispatchAllCollisionPairs (m_pairCache, dynamicsWorld->getDispatchInfo(), dynamicsWorld->getDispatcher());
+	
+	m_dispatcher->dispatchAllCollisionPairs (m_pairCache, dynamicsWorld->getDispatchInfo(), m_dispatcher);

-	btManifoldArray	manifoldArray;
+	m_currentPosition = m_collisionObject->getWorldTransform().getOrigin();
+	
 	btScalar maxPen = btScalar(0.0);
 	for (int i = 0; i < m_pairCache->getNumOverlappingPairs(); i++)
 	{
-		manifoldArray.clear();
+		m_manifoldArray.resize(0);

 		btBroadphasePair* collisionPair = &m_pairCache->getOverlappingPairArray()[i];
 		
 		if (collisionPair->m_algorithm)
-			collisionPair->m_algorithm->getAllContactManifolds(manifoldArray);
+			collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray);

-		for (int j=0;j<manifoldArray.size();j++)
+		
+		for (int j=0;j<m_manifoldArray.size();j++)
 		{
-			btPersistentManifold* manifold = manifoldArray[j];
+			btPersistentManifold* manifold = m_manifoldArray[j];
 			btScalar directionSign = manifold->getBody0() == m_collisionObject ? btScalar(-1.0) : btScalar(1.0);
 			for (int p=0;p<manifold->getNumContacts();p++)
 			{
@@ -176,7 +181,8 @@ bool KinematicCharacterController::recoverFromPenetration (btDynamicsWorld* dyna
 					if (pt.getDistance() < maxPen)
 					{
 						maxPen = pt.getDistance();
-						m_touchingNormal = pt.m_normalWorldOnB * directionSign;
+						m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
+
 					}
 					m_currentPosition += pt.m_normalWorldOnB * directionSign * pt.getDistance() * btScalar(0.2);
 					penetration = true;
@@ -184,13 +190,18 @@ bool KinematicCharacterController::recoverFromPenetration (btDynamicsWorld* dyna
 					//printf("touching %f\n", pt.getDistance());
 				}
 			}
-			manifold->clearManifold();
+			
+			//manifold->clearManifold();
 		}
 	}
+	btTransform newTrans = m_collisionObject->getWorldTransform();
+	newTrans.setOrigin(m_currentPosition);
+	m_collisionObject->setWorldTransform(newTrans);
+//	printf("m_touchingNormal = %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]);
 	return penetration;
 }

-void KinematicCharacterController::stepUp (btDynamicsWorld* dynamicsWorld)
+void KinematicCharacterController::stepUp (const btDynamicsWorld* dynamicsWorld)
 {
 	// phase 1: up
 	btTransform start, end;
@@ -222,31 +233,47 @@ void KinematicCharacterController::updateTargetPositionBasedOnCollision (const b
 {
 	btVector3 movementDirection = m_targetPosition - m_currentPosition;
 	btScalar movementLength = movementDirection.length();
-	movementDirection.normalize();
-
-	btVector3 reflectDir = computeReflectionDirection (movementDirection, hitNormal);
-	reflectDir.normalize();
-
-	btVector3 parallelDir, perpindicularDir;
-
-	parallelDir = parallelComponent (reflectDir, hitNormal);
-	perpindicularDir = perpindicularComponent (reflectDir, hitNormal);
-
-	m_targetPosition = m_currentPosition;
-	if (tangentMag != 0.0)
+	if (movementLength>SIMD_EPSILON)
 	{
-		m_targetPosition += parallelDir * btScalar (tangentMag*movementLength);
-	}
+		movementDirection.normalize();

-	if (normalMag != 0.0)
+		btVector3 reflectDir = computeReflectionDirection (movementDirection, hitNormal);
+		reflectDir.normalize();
+
+		btVector3 parallelDir, perpindicularDir;
+
+		parallelDir = parallelComponent (reflectDir, hitNormal);
+		perpindicularDir = perpindicularComponent (reflectDir, hitNormal);
+
+		m_targetPosition = m_currentPosition;
+		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;
+		}
+
+		if (normalMag != 0.0)
+		{
+			btVector3 perpComponent = perpindicularDir * btScalar (normalMag*movementLength);
+//			printf("perpComponent=%f,%f,%f\n",perpComponent[0],perpComponent[1],perpComponent[2]);
+			m_targetPosition += perpComponent;
+		}
+	} else
 	{
-		m_targetPosition += perpindicularDir * btScalar (normalMag*movementLength);
+//		printf("movementLength don't normalize a zero vector\n");
 	}
 }

-void KinematicCharacterController::stepForwardAndStrafe (btDynamicsWorld* dynamicsWorld, const btVector3& walkMove)
+void KinematicCharacterController::stepForwardAndStrafe (const btDynamicsWorld* dynamicsWorld, const btVector3& walkMove)
 {
+
 	btVector3 originalDir = walkMove.normalized();
+	if (walkMove.length() < SIMD_EPSILON)
+	{
+		originalDir.setValue(0.f,0.f,0.f);
+	}
+//	printf("originalDir=%f,%f,%f\n",originalDir[0],originalDir[1],originalDir[2]);
 	// phase 2: forward and strafe
 	btTransform start, end;
 	m_targetPosition = m_currentPosition + walkMove;
@@ -255,6 +282,7 @@ void KinematicCharacterController::stepForwardAndStrafe (btDynamicsWorld* dynami
 	
 	btScalar fraction = 1.0;
 	btScalar distance2 = (m_currentPosition-m_targetPosition).length2();
+//	printf("distance2=%f\n",distance2);

 	if (m_touchingContact)
 	{
@@ -262,44 +290,67 @@ void KinematicCharacterController::stepForwardAndStrafe (btDynamicsWorld* dynami
 			updateTargetPositionBasedOnCollision (m_touchingNormal);
 	}

-	while (fraction > btScalar(0.01))
+	int maxIter = 10;
+
+	while (fraction > btScalar(0.01) && maxIter-- > 0)
 	{
 		start.setOrigin (m_currentPosition);
 		end.setOrigin (m_targetPosition);

 		ClosestNotMeConvexResultCallback callback (m_collisionObject);
-		dynamicsWorld->convexSweepTest (m_shape, start, end, callback);

+		//btScalar margin = m_shape->getMargin();
+		//m_shape->setMargin(margin - 0.06f);
+		dynamicsWorld->convexSweepTest (m_shape, start, end, callback);
+		//m_shape->setMargin(margin);
+
+		
 		fraction -= callback.m_closestHitFraction;

 		if (callback.HasHit())
 		{	
 			// we moved only a fraction
 			btScalar hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
+			if (hitDistance<0.f)
+			{
+//				printf("neg dist?\n");
+			}

 			/* If the distance is farther than the collision margin, move */
 			if (hitDistance > 0.05)
 			{
+//				printf("callback.m_closestHitFraction=%f\n",callback.m_closestHitFraction);
 				m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
 			}

 			updateTargetPositionBasedOnCollision (callback.m_hitNormalWorld);
 			btVector3 currentDir = m_targetPosition - m_currentPosition;
 			distance2 = currentDir.length2();
-			currentDir.normalize();
-			/* Ageia's C.C. took this test from Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." */
-			if (currentDir.dot(originalDir) <= btScalar(0.0))
+			if (distance2 > SIMD_EPSILON)
 			{
+				currentDir.normalize();
+				/* See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." */
+				if (currentDir.dot(originalDir) <= btScalar(0.0))
+				{
+					break;
+				}
+			} else
+			{
+//				printf("currentDir: don't normalize a zero vector\n");
 				break;
 			}
 		} else {
 			// we moved whole way
 			m_currentPosition = m_targetPosition;
 		}
+
+	//	if (callback.m_closestHitFraction == 0.f)
+	//		break;
+
 	}
 }

-void KinematicCharacterController::stepDown (btDynamicsWorld* dynamicsWorld, btScalar dt)
+void KinematicCharacterController::stepDown (const btDynamicsWorld* dynamicsWorld, btScalar dt)
 {
 	btTransform start, end;

@@ -341,13 +392,15 @@ void KinematicCharacterController::warp (const btVector3& origin)
 	m_collisionObject->setWorldTransform (xform);
 }

-void KinematicCharacterController::registerPairCache (btOverlappingPairCache* pairCache)
+void KinematicCharacterController::registerPairCacheAndDispatcher (btOverlappingPairCache* pairCache, btCollisionDispatcher* dispatcher)
 {
 	m_pairCache = pairCache;
+	m_dispatcher = dispatcher;
 }

-void KinematicCharacterController::preStep (btDynamicsWorld* dynamicsWorld)
+void KinematicCharacterController::preStep (const btDynamicsWorld* dynamicsWorld)
 {
+	
 	int numPenetrationLoops = 0;
 	m_touchingContact = false;
 	while (recoverFromPenetration (dynamicsWorld))
@@ -356,7 +409,7 @@ void KinematicCharacterController::preStep (btDynamicsWorld* dynamicsWorld)
 		m_touchingContact = true;
 		if (numPenetrationLoops > 4)
 		{
-			printf("character could not recover from penetration = %d\n", numPenetrationLoops);
+//			printf("character could not recover from penetration = %d\n", numPenetrationLoops);
 			break;
 		}
 	}
@@ -364,6 +417,7 @@ void KinematicCharacterController::preStep (btDynamicsWorld* dynamicsWorld)
 	xform = m_collisionObject->getWorldTransform ();

 	btVector3 forwardDir = xform.getBasis()[2];
+//	printf("forwardDir=%f,%f,%f\n",forwardDir[0],forwardDir[1],forwardDir[2]);
 	btVector3 upDir = xform.getBasis()[1];
 	btVector3 strafeDir = xform.getBasis()[0];
 	forwardDir.normalize ();
@@ -376,11 +430,12 @@ void KinematicCharacterController::preStep (btDynamicsWorld* dynamicsWorld)

 	m_currentPosition = xform.getOrigin();
 	m_targetPosition = m_currentPosition;
+//	printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]);

 	
 }

-void KinematicCharacterController::playerStep (btDynamicsWorld* dynamicsWorld,
+void KinematicCharacterController::playerStep (const btDynamicsWorld* dynamicsWorld,
 					 btScalar dt,
 					 int forward,
 					 int backward,
@@ -406,6 +461,9 @@ void KinematicCharacterController::playerStep (btDynamicsWorld* dynamicsWorld,
 	btTransform xform;
 	xform = m_collisionObject->getWorldTransform ();

+//	printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]);
+//	printf("walkSpeed=%f\n",walkSpeed);
+
 	stepUp (dynamicsWorld);
 	stepForwardAndStrafe (dynamicsWorld, walkDirection * walkSpeed);
 	stepDown (dynamicsWorld, dt);
--- a/Demos/CharacterDemo/KinematicCharacterController.h
+++ b/Demos/CharacterDemo/KinematicCharacterController.h
@@ -8,7 +8,12 @@
 class btCollisionShape;
 class btRigidBody;
 class btDynamicsWorld;
+class btCollisionDispatcher;

+
+///KinematicCharacterController is a collision object with support for sliding motion in a world.
+///It uses the convex sweep test to test for upcoming collisions. This is combined with discrete collision detection to recover from penetrations.
+///Interaction between KinematicCharacterController and dynamic rigid bodies needs to be explicity implemented by the user.
 class KinematicCharacterController : public CharacterControllerInterface
 {
 protected:
@@ -16,6 +21,7 @@ protected:
 	btConvexShape* m_shape;
 	btCollisionObject* m_collisionObject;
 	btOverlappingPairCache* m_pairCache;
+	btCollisionDispatcher*	m_dispatcher;

 	btScalar m_fallSpeed;
 	btScalar m_jumpSpeed;
@@ -36,28 +42,30 @@ protected:
 	btScalar  m_currentStepOffset;
 	btVector3 m_targetPosition;

+	btManifoldArray	m_manifoldArray;
+
 	bool m_touchingContact;
 	btVector3 m_touchingNormal;
 	
-	bool recoverFromPenetration (btDynamicsWorld* dynamicsWorld);
-	void stepUp (btDynamicsWorld* dynamicsWorld);
-	void updateTargetPositionBasedOnCollision (const btVector3& hit_normal, btScalar tangentMag = btScalar(1.0), btScalar normalMag = btScalar(0.0));
-	void stepForwardAndStrafe (btDynamicsWorld* dynamicsWorld, const btVector3& walkMove);
-	void stepDown (btDynamicsWorld* dynamicsWorld, btScalar dt);
+	bool recoverFromPenetration (const btDynamicsWorld* dynamicsWorld);
+	void stepUp (const btDynamicsWorld* dynamicsWorld);
+	void updateTargetPositionBasedOnCollision (const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0));
+	void stepForwardAndStrafe (const btDynamicsWorld* dynamicsWorld, const btVector3& walkMove);
+	void stepDown (const btDynamicsWorld* dynamicsWorld, btScalar dt);
 public:
 	KinematicCharacterController ();
 	~KinematicCharacterController ();
-	void setup (btDynamicsWorld* dynamicsWorld, btScalar height = btScalar(1.75), btScalar width = btScalar(0.4), btScalar stepHeight = btScalar(0.35));
-	void destroy (btDynamicsWorld* dynamicsWorld);
+	void setup (btScalar height = btScalar(1.75), btScalar width = btScalar(0.4), btScalar stepHeight = btScalar(0.35));
+	void destroy ();

 	btCollisionObject* getCollisionObject ();

 	void reset ();
 	void warp (const btVector3& origin);

-	void registerPairCache (btOverlappingPairCache* pairCache);
-	void preStep (btDynamicsWorld* dynamicsWorld);
-	void playerStep (btDynamicsWorld* dynamicsWorld, btScalar dt,
+	virtual void registerPairCacheAndDispatcher (btOverlappingPairCache* pairCache, btCollisionDispatcher* dispatcher);
+	void preStep (const btDynamicsWorld* dynamicsWorld);
+	void playerStep (const btDynamicsWorld* dynamicsWorld, btScalar dt,
 					 int forward,
 					 int backward,
 					 int left,