Merge branch 'master' of https://github.com/erwincoumans/bullet3

examples/ExampleBrowser/OpenGLExampleBrowser.cpp

@@ -157,6 +157,7 @@ void deleteDemo()
 }
 
 const char* gPngFileName = 0;
+int gPngSkipFrames = 0;
 
 
 
@@ -763,7 +764,7 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[])
 	loadCurrentSettings(startFileName, args);
 
 	args.GetCmdLineArgument("fixed_timestep",gFixedTimeStep);
-	
+	args.GetCmdLineArgument("png_skip_frames", gPngSkipFrames);	
 	///The OpenCL rigid body pipeline is experimental and 
 	///most OpenCL drivers and OpenCL compilers have issues with our kernels.
 	///If you have a high-end desktop GPU such as AMD 7970 or better, or NVIDIA GTX 680 with up-to-date drivers
@@ -1101,24 +1102,6 @@ void OpenGLExampleBrowser::update(float deltaTime)
 				//printf("---------------------------------------------------\n");
 				//printf("Framecount = %d\n",frameCount);
 
-				if (gPngFileName)
-				{
-					
-					static int skip = 0;
-					skip++;
-					if (skip>4)
-					{
-						skip=0;
-						//printf("gPngFileName=%s\n",gPngFileName);
-						static int s_frameCount = 100;
-						
-						sprintf(staticPngFileName,"%s%d.png",gPngFileName,s_frameCount++);
-						//b3Printf("Made screenshot %s",staticPngFileName);
-						s_app->dumpNextFrameToPng(staticPngFileName);
-						 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 
-					}
-				}
-				
 
 				if (gFixedTimeStep>0)
 				{
@@ -1153,6 +1136,25 @@ void OpenGLExampleBrowser::update(float deltaTime)
             }
 		}
 
+				if (gPngFileName)
+				{
+
+					static int skip = 0;
+					skip--;
+					if (skip<0)
+					{
+						skip=gPngSkipFrames;
+						//printf("gPngFileName=%s\n",gPngFileName);
+						static int s_frameCount = 100;
+
+						sprintf(staticPngFileName,"%s%d.png",gPngFileName,s_frameCount++);
+						//b3Printf("Made screenshot %s",staticPngFileName);
+						s_app->dumpNextFrameToPng(staticPngFileName);
+						 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+					}
+				}
+
+
 		{
 			
 			if (gui2 && s_guiHelper && s_guiHelper->getRenderInterface() && s_guiHelper->getRenderInterface()->getActiveCamera())

examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp

@@ -28,6 +28,7 @@ subject to the following restrictions:
 #include "btMatrix4x4.h"
 
 
+#define MAX_VISUAL_SHAPES 512
 
 
 struct VertexSource
@@ -288,6 +289,8 @@ void readLibraryGeometries(TiXmlDocument& doc, btAlignedObjectArray<GLInstanceGr
 		}//for each mesh
 		
 		int shapeIndex = visualShapes.size();
+        if (shapeIndex<MAX_VISUAL_SHAPES)
+        {
             GLInstanceGraphicsShape& visualShape = visualShapes.expand();
             {
                 visualShape.m_vertices = new b3AlignedObjectArray<GLInstanceVertex>;
@@ -323,7 +326,10 @@ void readLibraryGeometries(TiXmlDocument& doc, btAlignedObjectArray<GLInstanceGr
             }
             //b3Printf("geometry name=%s\n",geometryName);
             name2Shape.insert(geometryName,shapeIndex);
-		
+        } else
+        {
+            b3Warning("DAE exceeds number of visual shapes (%d/%d)",shapeIndex, MAX_VISUAL_SHAPES);
+        }
 
 	}//for each geometry
 }
@@ -557,7 +563,7 @@ void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray<GLIn
 //	GLInstanceGraphicsShape* instance = 0;
 	
 	//usually COLLADA files don't have that many visual geometries/shapes
-	visualShapes.reserve(32);
+	visualShapes.reserve(MAX_VISUAL_SHAPES);
 
 	float extraScaling = 1;//0.01;
 	btHashMap<btHashString, int> name2ShapeIndex;

examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp

@@ -67,7 +67,7 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string&
 
                     } else
                     {
-                        b3Warning("not found %s\n",relativeFileName);
+                        b3Warning("not found [%s]\n",relativeFileName);
                     }
 				}
 			}

examples/InverseKinematics/InverseKinematicsExample.cpp

@@ -84,7 +84,7 @@ void Reset(Tree &tree, Jacobian* m_ikJacobian)
 // Update target positions
 
 void UpdateTargets( double T2, Tree & treeY) {
-	double T = T2 / 20.;
+	double T = T2 / 5.;
 	target1[0].Set(0.6*b3Sin(0), 0.6*b3Cos(0), 0.5+0.4*b3Sin(3 * T));
 }
 

examples/OpenGLWindow/SimpleOpenGL3App.cpp

@@ -750,11 +750,13 @@ static void writeTextureToFile(int textureWidth, int textureHeight, const char*
 
 void SimpleOpenGL3App::swapBuffer()
 {
-	m_window->endRendering();
+
 	if (m_data->m_frameDumpPngFileName)
     {
-        writeTextureToFile((int)m_window->getRetinaScale()*m_instancingRenderer->getScreenWidth(),
+        int width = (int)m_window->getRetinaScale()*m_instancingRenderer->getScreenWidth();
-                          (int) m_window->getRetinaScale()*this->m_instancingRenderer->getScreenHeight(),m_data->m_frameDumpPngFileName,
+        int height = (int) m_window->getRetinaScale()*this->m_instancingRenderer->getScreenHeight();
+        writeTextureToFile(width,
+                          height,m_data->m_frameDumpPngFileName,
                           m_data->m_ffmpegFile);
         m_data->m_renderTexture->disable();
         if (m_data->m_ffmpegFile==0)
@@ -762,6 +764,7 @@ void SimpleOpenGL3App::swapBuffer()
 			m_data->m_frameDumpPngFileName = 0;
         }
     }
+ m_window->endRendering();
         m_window->startRendering();
 }
 
@@ -774,11 +777,14 @@ void SimpleOpenGL3App::dumpFramesToVideo(const char* mp4FileName)
 
 #ifdef _WIN32
 	sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
-    		"-y -crf 0  -b:v 1500000 -an -vcodec h264 -vf vflip  %s", width, height, mp4FileName);
+		"-threads 0 -y -b 50000k  -t 20 -c:v libx264 -preset slow -crf 22 -an   -pix_fmt yuv420p -vf vflip %s", width, height, mp4FileName);
+
+    //sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba   -s %dx%d -i - "
+    //		"-y -crf 0  -b:v 1500000 -an -vcodec h264 -vf vflip  %s", width, height, mp4FileName);
 #else
    
    sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
-    		"-threads 0 -y -crf 0 -b 50000k -vf vflip %s", width, height, mp4FileName);
+    		"-threads 0 -y -b 50000k  -t 20 -c:v libx264 -preset slow -crf 22 -an   -pix_fmt yuv420p -vf vflip %s", width, height, mp4FileName);
 #endif
     
     //sprintf(cmd,"ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "

examples/OpenGLWindow/X11OpenGLWindow.cpp

@@ -516,6 +516,9 @@ void    X11OpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
 
     m_data->m_dpy = MyXOpenDisplay(NULL);
 
+    m_data->m_glWidth = ci.m_width;
+    m_data->m_glHeight = ci.m_height;
+
     if(m_data->m_dpy == NULL) {
         printf("\n\tcannot connect to X server\n\n");
             exit(0);

examples/ThirdPartyLibs/BussIK/LinearR4.h

@@ -580,8 +580,11 @@ inline Matrix4x4::Matrix4x4 ( const Matrix4x4& A)
 
 inline void Matrix4x4::SetIdentity ( )
 {
+          m12 =  m13 = m14 = 
+    m21 =        m23 = m24 = 
+    m31 = m32 =        m34 =
+    m41 = m42 = m43 =        0.0;
     m11 = m22 = m33 =  m44 = 1.0;
-	m12 = m13 = m14 = m21 = m23 = m24 = m13 = m23 = m41= m42 = m43 = 0.0;
 }
 
 inline void Matrix4x4::Set( const VectorR4& u, const VectorR4& v, 

examples/ThirdPartyLibs/BussIK/MatrixRmn.h

@@ -399,4 +399,4 @@ inline void MatrixRmn::AddArrayScale( long length, const double* from, long from
 
 
 
-#endif MATRIX_RMN_H
+#endif //MATRIX_RMN_H

examples/ThirdPartyLibs/BussIK/VectorRn.h

@@ -235,4 +235,4 @@ inline double Dot( const VectorRn& u, const VectorRn& v )
 	return res;
 }
 
-#endif VECTOR_RN_H
+#endif //VECTOR_RN_H

examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp

@@ -366,6 +366,8 @@ std::string LoadMtl (
       continue;
     }
 
+    linebuf = linebuf.substr(0, linebuf.find_last_not_of(" \t") + 1);
+
     // Skip leading space.
     const char* token = linebuf.c_str();
     token += strspn(token, " \t");

examples/pybullet/pybullet.c

@@ -294,16 +294,16 @@ pybullet_resetSimulation(PyObject* self, PyObject* args)
 
 static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
 {
-    //todo(erwincoumans): set max forces, kp, kd
-
     int size;
     int bodyIndex, jointIndex, controlMode;
     double targetValue=0;
+    double targetPosition=0;
+    double targetVelocity=0;
     double maxForce=100000;
-    double gains=0.1;
+    double kp=0.1;
+    double kd=1.0;
     int valid = 0;
 
-     
     if (0==sm)
     {
         PyErr_SetString(SpamError, "Not connected to physics server.");
@@ -311,10 +311,10 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
     }
 
     size= PySequence_Size(args);
-    
     if (size==4)
     {
-        
+        // for CONTROL_MODE_VELOCITY targetValue -> velocity
+        // for CONTROL_MODE_TORQUE targetValue -> force torque
         if (!PyArg_ParseTuple(args, "iiid", &bodyIndex, &jointIndex, &controlMode, &targetValue))
         {
             PyErr_SetString(SpamError, "Error parsing arguments");
@@ -324,7 +324,8 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
     }
     if (size==5)
     {
-        
+        // for CONTROL_MODE_VELOCITY targetValue -> velocity
+        // for CONTROL_MODE_TORQUE targetValue -> force torque
         if (!PyArg_ParseTuple(args, "iiidd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce))
         {
             PyErr_SetString(SpamError, "Error parsing arguments");
@@ -334,8 +335,17 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
     }
     if (size==6)
     {
-        
+        if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce, &kd))
-        if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce, &gains))
+        {
+            PyErr_SetString(SpamError, "Error parsing arguments");
+            return NULL;
+        }
+        valid = 1;
+    }
+    if (size==8)
+    {
+        // only applicable for CONTROL_MODE_POSITION_VELOCITY_PD.
+        if (!PyArg_ParseTuple(args, "iiiddddd", &bodyIndex, &jointIndex, &controlMode, &targetPosition, &targetVelocity, &maxForce, &kp, &kd))
         {
             PyErr_SetString(SpamError, "Error parsing arguments");
             return NULL;
@@ -343,9 +353,20 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
         valid = 1;
     }
 
+    if (size==8 && controlMode!=CONTROL_MODE_POSITION_VELOCITY_PD)
+    {
+        PyErr_SetString(SpamError, "8 argument call only applicable for control mode CONTROL_MODE_POSITION_VELOCITY_PD");
+        return NULL;
+    }
+    if (controlMode==CONTROL_MODE_POSITION_VELOCITY_PD && size!=8)
+    {
+        PyErr_SetString(SpamError, "For CONTROL_MODE_POSITION_VELOCITY_PD please call with explicit targetPosition & targetVelocity");
+        return NULL;
+    }
 
     if (valid)
     {
+
         int numJoints;
         b3SharedMemoryCommandHandle commandHandle;
         b3SharedMemoryStatusHandle statusHandle;
@@ -374,7 +395,6 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
         {
             case CONTROL_MODE_VELOCITY:
             {
-				double kd = gains;
                 b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetValue);
                 b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
                 b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce);
@@ -389,9 +409,10 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
 
             case CONTROL_MODE_POSITION_VELOCITY_PD:
             {
-				double kp = gains;
+                b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, targetPosition);
-                b3JointControlSetDesiredPosition( commandHandle, info.m_qIndex, targetValue);
                 b3JointControlSetKp(commandHandle, info.m_uIndex, kp);
+                b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity);
+                b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
                 b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce);
                 break;
             }
@@ -405,12 +426,10 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
         Py_INCREF(Py_None);
         return Py_None;
     }
-    PyErr_SetString(SpamError, "error in setJointControl.");
+    PyErr_SetString(SpamError, "Invalid number of args passed to setJointControl.");
     return NULL;
 }
 
-
-
 static PyObject *
 pybullet_setRealTimeSimulation(PyObject* self, PyObject* args)
 {

src/BulletDynamics/Character/btCharacterControllerInterface.h

@@ -37,7 +37,7 @@ public:
 	virtual void	preStep ( btCollisionWorld* collisionWorld) = 0;
 	virtual void	playerStep (btCollisionWorld* collisionWorld, btScalar dt) = 0;
 	virtual bool	canJump () const = 0;
-	virtual void	jump () = 0;
+	virtual void	jump(const btVector3& dir = btVector3()) = 0;
 
 	virtual bool	onGround () const = 0;
 	virtual void	setUpInterpolate (bool value) = 0;

src/BulletDynamics/Character/btKinematicCharacterController.cpp

@@ -132,30 +132,37 @@ btVector3 btKinematicCharacterController::perpindicularComponent (const btVector
 	return direction - parallelComponent(direction, normal);
 }
 
-btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis)
+btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, const btVector3& up)
 {
-	m_upAxis = upAxis;
+	m_up.setValue(0.0f, 0.0f, 1.0f);
+	m_jumpAxis.setValue(0.0f, 0.0f, 1.0f);
+	setUp(up);
+	setStepHeight(stepHeight);
 	m_addedMargin = 0.02;
-	m_walkDirection.setValue(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_ghostObject = ghostObject;
-	m_stepHeight = stepHeight;
 	m_turnAngle = btScalar(0.0);
 	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 ; // 3G acceleration.
+	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;
 	m_interpolateUp = true;
 	setMaxSlope(btRadians(45.0));
-	m_currentStepOffset = 0;
+	m_currentStepOffset = 0.0;
+	m_maxPenetrationDepth = 0.2;
 	full_drop = false;
 	bounce_fix = false;
+	m_linearDamping = btScalar(0.0);
+	m_angularDamping = btScalar(0.0);
 }
 
 btKinematicCharacterController::~btKinematicCharacterController ()
@@ -190,7 +197,7 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
 
 	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);
@@ -203,6 +210,9 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
 		if ((obj0 && !obj0->hasContactResponse()) || (obj1 && !obj1->hasContactResponse()))
 			continue;
 
+		if (!needsCollision(obj0, obj1))
+			continue;
+		
 		if (collisionPair->m_algorithm)
 			collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray);
 
@@ -217,14 +227,15 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
 
 				btScalar dist = pt.getDistance();
 
-				if (dist < 0.0)
+				if (dist < -m_maxPenetrationDepth)
 				{
-					if (dist < maxPen)
+					// TODO: cause problems on slopes, not sure if it is needed
-					{
+					//if (dist < maxPen)
-						maxPen = dist;
+					//{
-						m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
+					//	maxPen = dist;
+					//	m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
 
-					}
+					//}
 					m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * btScalar(0.2);
 					penetration = true;
 				} else {
@@ -244,18 +255,28 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
 
 void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
 {
+	btScalar stepHeight = 0.0f;
+	if (m_verticalVelocity < 0.0)
+		stepHeight = m_stepHeight;
+
 	// phase 1: up
 	btTransform start, end;
-	m_targetPosition = m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_stepHeight + (m_verticalOffset > 0.f?m_verticalOffset:0.f));
 
 	start.setIdentity ();
 	end.setIdentity ();
 
 	/* FIXME: Handle penetration properly */
-	start.setOrigin (m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_convexShape->getMargin() + m_addedMargin));
+	start.setOrigin(m_currentPosition);
+
+	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);
 
-	btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, -getUpAxisDirections()[m_upAxis], btScalar(0.7071));
+	start.setRotation(m_currentOrientation);
+	end.setRotation(m_targetOrientation);
+
+	btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, -m_up, m_maxSlopeCosine);
 	callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
 	callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
 	
@@ -265,29 +286,61 @@ void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
 	}
 	else
 	{
-		world->convexSweepTest (m_convexShape, start, end, callback);
+		world->convexSweepTest(m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration);
 	}
 
-	if (callback.hasHit())
+	if (callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
 	{
 		// Only modify the position if the hit was a slope and not a wall or ceiling.
-		if(callback.m_hitNormalWorld.dot(getUpAxisDirections()[m_upAxis]) > 0.0)
+		if (callback.m_hitNormalWorld.dot(m_up) > 0.0)
 		{
 			// we moved up only a fraction of the step height
-			m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction;
+			m_currentStepOffset = stepHeight * callback.m_closestHitFraction;
 			if (m_interpolateUp == true)
 				m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
 			else
 				m_currentPosition = m_targetPosition;
 		}
-		m_verticalVelocity = 0.0;
+
+		btTransform& xform = m_ghostObject->getWorldTransform();
+		xform.setOrigin(m_currentPosition);
+		m_ghostObject->setWorldTransform(xform);
+
+		// fix penetration if we hit a ceiling for example
+		int numPenetrationLoops = 0;
+		m_touchingContact = false;
+		while (recoverFromPenetration(world))
+		{
+			numPenetrationLoops++;
+			m_touchingContact = true;
+			if (numPenetrationLoops > 4)
+			{
+				//printf("character could not recover from penetration = %d\n", numPenetrationLoops);
+				break;
+			}
+		}
+		m_targetPosition = m_ghostObject->getWorldTransform().getOrigin();
+		m_currentPosition = m_targetPosition;
+
+		if (m_verticalOffset > 0)
+		{
 			m_verticalOffset = 0.0;
-	} else {
+			m_verticalVelocity = 0.0;
 			m_currentStepOffset = m_stepHeight;
+		}
+	} else {
+		m_currentStepOffset = stepHeight;
 		m_currentPosition = m_targetPosition;
 	}
 }
 
+bool btKinematicCharacterController::needsCollision(const btCollisionObject* body0, const btCollisionObject* body1)
+{
+	bool collides = (body0->getBroadphaseHandle()->m_collisionFilterGroup & body1->getBroadphaseHandle()->m_collisionFilterMask) != 0;
+	collides = collides && (body1->getBroadphaseHandle()->m_collisionFilterGroup & body0->getBroadphaseHandle()->m_collisionFilterMask);
+	return collides;
+}
+
 void btKinematicCharacterController::updateTargetPositionBasedOnCollision (const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag)
 {
 	btVector3 movementDirection = m_targetPosition - m_currentPosition;
@@ -330,6 +383,7 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
 	// 	m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]);
 	// phase 2: forward and strafe
 	btTransform start, end;
+
 	m_targetPosition = m_currentPosition + walkMove;
 
 	start.setIdentity ();
@@ -339,15 +393,6 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
 	btScalar distance2 = (m_currentPosition-m_targetPosition).length2();
 //	printf("distance2=%f\n",distance2);
 
-	if (m_touchingContact)
-	{
-		if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0))
-		{
-			//interferes with step movement
-			//updateTargetPositionBasedOnCollision (m_touchingNormal);
-		}
-	}
-
 	int maxIter = 10;
 
 	while (fraction > btScalar(0.01) && maxIter-- > 0)
@@ -356,6 +401,9 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
 		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));
 		callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
 		callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
@@ -364,21 +412,23 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
 		btScalar margin = m_convexShape->getMargin();
 		m_convexShape->setMargin(margin + m_addedMargin);
 
-
+		if (!(start == end))
+		{
 			if (m_useGhostObjectSweepTest)
 			{
 				m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
-		} else
+			}
+			else
 			{
 				collisionWorld->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
 			}
-		
+		}
 		m_convexShape->setMargin(margin);
 
 		
 		fraction -= callback.m_closestHitFraction;
 
-		if (callback.hasHit())
+		if (callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
 		{	
 			// we moved only a fraction
 			//btScalar hitDistance;
@@ -404,9 +454,11 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
 			}
 
 		} else {
+//			if (!m_wasJumping)
 				// we moved whole way
 				m_currentPosition = m_targetPosition;
 		}
+		m_currentPosition = m_targetPosition;
 
 	//	if (callback.m_closestHitFraction == 0.f)
 	//		break;
@@ -421,27 +473,30 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
 
 	// phase 3: down
 	/*btScalar additionalDownStep = (m_wasOnGround && !onGround()) ? m_stepHeight : 0.0;
-	btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + additionalDownStep);
+	btVector3 step_drop = m_up * (m_currentStepOffset + additionalDownStep);
 	btScalar downVelocity = (additionalDownStep == 0.0 && m_verticalVelocity<0.0?-m_verticalVelocity:0.0) * dt;
-	btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downVelocity; 
+	btVector3 gravity_drop = m_up * downVelocity; 
 	m_targetPosition -= (step_drop + gravity_drop);*/
 
 	btVector3 orig_position = m_targetPosition;
 	
 	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))
 		downVelocity = m_fallSpeed;
 
-	btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
+	btVector3 step_drop = m_up * (m_currentStepOffset + downVelocity);
 	m_targetPosition -= step_drop;
 
-	btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
+	btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, m_up, m_maxSlopeCosine);
         callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
         callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
 
-        btKinematicClosestNotMeConvexResultCallback callback2 (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
+	btKinematicClosestNotMeConvexResultCallback callback2(m_ghostObject, m_up, m_maxSlopeCosine);
         callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
         callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
 
@@ -455,6 +510,9 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
 		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);
 
@@ -462,7 +520,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
 		{
 			m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
 
-			if (!callback.hasHit())
+			if (!callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
 			{
 				//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);
@@ -471,7 +529,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
 		{
 			collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
 
-			if (!callback.hasHit())
+			if (!callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
 			{
 				//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);
@@ -481,20 +539,24 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
 		btScalar downVelocity2 = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
 		bool has_hit = false;
 		if (bounce_fix == true)
-			has_hit = callback.hasHit() || callback2.hasHit();
+			has_hit = (callback.hasHit() || callback2.hasHit()) && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject);
 		else
-			has_hit = callback2.hasHit();
+			has_hit = callback2.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject);
 
-		if(downVelocity2 > 0.0 && downVelocity2 < m_stepHeight && has_hit == true && runonce == false
+		btScalar stepHeight = 0.0f;
+		if (m_verticalVelocity < 0.0)
+			stepHeight = m_stepHeight;
+
+		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
 
 			m_targetPosition = orig_position;
-					downVelocity = m_stepHeight;
+			downVelocity = stepHeight;
 
-				btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
+			btVector3 step_drop = m_up * (m_currentStepOffset + downVelocity);
 			m_targetPosition -= step_drop;
 			runonce = true;
 			continue; //re-run previous tests
@@ -502,10 +564,9 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
 		break;
 	}
 
-	if (callback.hasHit() || runonce == true)
+	if ((callback.hasHit() || runonce == true) && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
 	{
 		// we dropped a fraction of the height -> hit floor
-
 		btScalar fraction = (m_currentPosition.getY() - callback.m_hitPointWorld.getY()) / 2;
 
 		//printf("hitpoint: %g - pos %g\n", callback.m_hitPointWorld.getY(), m_currentPosition.getY());
@@ -538,7 +599,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
 			{
 				m_targetPosition += step_drop; //undo previous target change
 				downVelocity = m_fallSpeed;
-				step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
+				step_drop = m_up * (m_currentStepOffset + downVelocity);
 				m_targetPosition -= step_drop;
 			}
 		}
@@ -579,6 +640,48 @@ btScalar timeInterval
 	m_velocityTimeInterval += timeInterval;
 }
 
+void btKinematicCharacterController::setAngularVelocity(const btVector3& velocity)
+{
+	m_AngVel = velocity;
+}
+
+const btVector3& btKinematicCharacterController::getAngularVelocity() const
+{
+	return m_AngVel;
+}
+
+void btKinematicCharacterController::setLinearVelocity(const btVector3& velocity)
+{
+	m_walkDirection = velocity;
+
+	// HACK: if we are moving in the direction of the up, treat it as a jump :(
+	if (m_walkDirection.length2() > 0)
+	{
+		btVector3 w = velocity.normalized();
+		btScalar c = w.dot(m_up);
+		if (c != 0)
+		{
+			//there is a component in walkdirection for vertical velocity
+			btVector3 upComponent = m_up * (sinf(SIMD_HALF_PI - acosf(c)) * m_walkDirection.length());
+			m_walkDirection -= upComponent;
+			m_verticalVelocity = (c < 0.0f ? -1 : 1) * upComponent.length();
+			
+			if (c > 0.0f)
+			{
+				m_wasJumping = true;
+				m_jumpPosition = m_ghostObject->getWorldTransform().getOrigin();
+			}
+		}
+	}
+	else
+		m_verticalVelocity = 0.0f;
+}
+
+btVector3 btKinematicCharacterController::getLinearVelocity() const
+{
+	return m_walkDirection + (m_verticalVelocity * m_up);
+}
+
 void btKinematicCharacterController::reset ( btCollisionWorld* collisionWorld )
 {
     m_verticalVelocity = 0.0;
@@ -607,42 +710,62 @@ void btKinematicCharacterController::warp (const btVector3& origin)
 
 void btKinematicCharacterController::preStep (  btCollisionWorld* collisionWorld)
 {
-	
-	int numPenetrationLoops = 0;
-	m_touchingContact = false;
-	while (recoverFromPenetration (collisionWorld))
-	{
-		numPenetrationLoops++;
-		m_touchingContact = true;
-		if (numPenetrationLoops > 4)
-		{
-			//printf("character could not recover from penetration = %d\n", numPenetrationLoops);
-			break;
-		}
-	}
-
 	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]);
-
-	
 }
 
-#include <stdio.h>
-
 void btKinematicCharacterController::playerStep (  btCollisionWorld* collisionWorld, btScalar dt)
 {
 //	printf("playerStep(): ");
 //	printf("  dt = %f", dt);
 
+	if (m_AngVel.length2() > 0.0f)
+	{
+		m_AngVel *= btPow(btScalar(1) - m_angularDamping, dt);
+	}
+
+	// integrate for angular velocity
+	if (m_AngVel.length2() > 0.0f)
+	{
+		btTransform xform;
+		xform = m_ghostObject->getWorldTransform();
+
+		btQuaternion rot(m_AngVel.normalized(), m_AngVel.length() * dt);
+
+		btQuaternion orn = rot * xform.getRotation();
+
+		xform.setRotation(orn);
+		m_ghostObject->setWorldTransform(xform);
+
+		m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
+		m_targetPosition = m_currentPosition;
+		m_currentOrientation = m_ghostObject->getWorldTransform().getRotation();
+		m_targetOrientation = m_currentOrientation;
+	}
+
 	// quick check...
-	if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0 || m_walkDirection.fuzzyZero())) {
+	if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0)) {
 //		printf("\n");
 		return;		// no motion
 	}
 
 	m_wasOnGround = onGround();
 
+	//btVector3 lvel = m_walkDirection;
+	btScalar c = 0.0f;
+	
+	if (m_walkDirection.length2() > 0)
+	{
+		// apply damping
+		m_walkDirection *= btPow(btScalar(1) - m_linearDamping, dt);
+	}
+
+	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)
@@ -655,7 +778,6 @@ void btKinematicCharacterController::playerStep (  btCollisionWorld* collisionWo
 	}
 	m_verticalOffset = m_verticalVelocity * dt;
 
-
 	btTransform xform;
 	xform = m_ghostObject->getWorldTransform();
 
@@ -663,6 +785,24 @@ void btKinematicCharacterController::playerStep (  btCollisionWorld* collisionWo
 //	printf("walkSpeed=%f\n",walkSpeed);
 
 	stepUp(collisionWorld);
+	//todo: Experimenting with behavior of controller when it hits a ceiling..
+	//bool hitUp = stepUp (collisionWorld);	
+	//if (hitUp)
+	//{
+	//	m_verticalVelocity -= m_gravity * dt;
+	//	if (m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
+	//	{
+	//		m_verticalVelocity = m_jumpSpeed;
+	//	}
+	//	if (m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
+	//	{
+	//		m_verticalVelocity = -btFabs(m_fallSpeed);
+	//	}
+	//	m_verticalOffset = m_verticalVelocity * dt;
+
+	//	xform = m_ghostObject->getWorldTransform();
+	//}
+
 	if (m_useWalkDirection) {
 		stepForwardAndStrafe (collisionWorld, m_walkDirection);
 	} else {
@@ -682,10 +822,38 @@ void btKinematicCharacterController::playerStep (  btCollisionWorld* collisionWo
 	}
 	stepDown (collisionWorld, dt);
 
+	//todo: Experimenting with max jump height
+	//if (m_wasJumping)
+	//{
+	//	btScalar ds = m_currentPosition[m_upAxis] - m_jumpPosition[m_upAxis];
+	//	if (ds > m_maxJumpHeight)
+	//	{
+	//		// substract the overshoot
+	//		m_currentPosition[m_upAxis] -= ds - m_maxJumpHeight;
+
+	//		// 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;
+	//	}
+	//}
 	// printf("\n");
 
 	xform.setOrigin (m_currentPosition);
 	m_ghostObject->setWorldTransform (xform);
+
+	int numPenetrationLoops = 0;
+	m_touchingContact = false;
+	while (recoverFromPenetration(collisionWorld))
+	{
+		numPenetrationLoops++;
+		m_touchingContact = true;
+		if (numPenetrationLoops > 4)
+		{
+			//printf("character could not recover from penetration = %d\n", numPenetrationLoops);
+			break;
+		}
+	}
 }
 
 void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed)
@@ -696,6 +864,7 @@ void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed)
 void btKinematicCharacterController::setJumpSpeed (btScalar jumpSpeed)
 {
 	m_jumpSpeed = jumpSpeed;
+	m_SetjumpSpeed = m_jumpSpeed;
 }
 
 void btKinematicCharacterController::setMaxJumpHeight (btScalar maxJumpHeight)
@@ -708,14 +877,16 @@ bool btKinematicCharacterController::canJump () const
 	return onGround();
 }
 
-void btKinematicCharacterController::jump ()
+void btKinematicCharacterController::jump(const btVector3& v)
 {
-	if (!canJump())
+	m_jumpSpeed = v.length2() == 0 ? m_SetjumpSpeed : v.length();
-		return;
-
 	m_verticalVelocity = m_jumpSpeed;
 	m_wasJumping = true;
 
+	m_jumpAxis = v.length2() == 0 ? m_up : v.normalized();
+
+	m_jumpPosition = m_ghostObject->getWorldTransform().getOrigin();
+
 #if 0
 	currently no jumping.
 	btTransform xform;
@@ -727,14 +898,16 @@ void btKinematicCharacterController::jump ()
 #endif
 }
 
-void btKinematicCharacterController::setGravity(btScalar gravity)
+void btKinematicCharacterController::setGravity(const btVector3& gravity)
 {
-	m_gravity = gravity;
+	if (gravity.length2() > 0) setUpVector(-gravity);
+
+	m_gravity = gravity.length();
 }
 
-btScalar btKinematicCharacterController::getGravity() const
+btVector3 btKinematicCharacterController::getGravity() const
 {
-	return m_gravity;
+	return -m_gravity * m_up;
 }
 
 void btKinematicCharacterController::setMaxSlope(btScalar slopeRadians)
@@ -748,11 +921,25 @@ btScalar btKinematicCharacterController::getMaxSlope() const
 	return m_maxSlopeRadians;
 }
 
-bool btKinematicCharacterController::onGround () const
+void btKinematicCharacterController::setMaxPenetrationDepth(btScalar d)
 {
-	return m_verticalVelocity == 0.0 && m_verticalOffset == 0.0;
+	m_maxPenetrationDepth = d;
 }
 
+btScalar btKinematicCharacterController::getMaxPenetrationDepth() const
+{
+	return m_maxPenetrationDepth;
+}
+
+bool btKinematicCharacterController::onGround () const
+{
+	return (fabs(m_verticalVelocity) < SIMD_EPSILON) && (fabs(m_verticalOffset) < SIMD_EPSILON);
+}
+
+void btKinematicCharacterController::setStepHeight(btScalar h) 
+{
+	m_stepHeight = h;
+}
 
 btVector3* btKinematicCharacterController::getUpAxisDirections()
 {
@@ -769,3 +956,49 @@ void btKinematicCharacterController::setUpInterpolate(bool value)
 {
 	m_interpolateUp = value;
 }
+
+void btKinematicCharacterController::setUp(const btVector3& up)
+{
+	if (up.length2() > 0 && m_gravity > 0.0f)
+	{
+		setGravity(-m_gravity * up.normalized());
+		return;
+	}
+
+	setUpVector(up);
+}
+
+void btKinematicCharacterController::setUpVector(const btVector3& up)
+{
+	if (m_up == up)
+		return;
+
+	btVector3 u = m_up;
+
+	if (up.length2() > 0)
+		m_up = up.normalized();
+	else
+		m_up = btVector3(0.0, 0.0, 0.0);
+
+	if (!m_ghostObject) return;
+	btQuaternion rot = getRotation(m_up, u);
+
+	//set orientation with new up
+	btTransform xform;
+	xform = m_ghostObject->getWorldTransform();
+	btQuaternion orn = rot.inverse() * xform.getRotation();
+	xform.setRotation(orn);
+	m_ghostObject->setWorldTransform(xform);
+}
+
+btQuaternion btKinematicCharacterController::getRotation(btVector3& v0, btVector3& v1) const
+{
+	if (v0.length2() == 0.0f || v1.length2() == 0.0f)
+	{
+		btQuaternion q;
+		return q;
+	}
+
+	return shortestArcQuatNormalize2(v0, v1);
+}
+

src/BulletDynamics/Character/btKinematicCharacterController.h

@@ -43,10 +43,12 @@ protected:
 	btPairCachingGhostObject* m_ghostObject;
 	btConvexShape*	m_convexShape;//is also in m_ghostObject, but it needs to be convex, so we store it here to avoid upcast
 	
+	btScalar m_maxPenetrationDepth;
 	btScalar m_verticalVelocity;
 	btScalar m_verticalOffset;
 	btScalar m_fallSpeed;
 	btScalar m_jumpSpeed;
+	btScalar m_SetjumpSpeed;
 	btScalar m_maxJumpHeight;
 	btScalar m_maxSlopeRadians; // Slope angle that is set (used for returning the exact value)
 	btScalar m_maxSlopeCosine;  // Cosine equivalent of m_maxSlopeRadians (calculated once when set, for optimization)
@@ -61,24 +63,34 @@ protected:
 	///this is the desired walk direction, set by the user
 	btVector3	m_walkDirection;
 	btVector3	m_normalizedDirection;
+	btVector3	m_AngVel;
+
+	btVector3	m_jumpPosition;
 
 	//some internal variables
 	btVector3 m_currentPosition;
 	btScalar  m_currentStepOffset;
 	btVector3 m_targetPosition;
 
+	btQuaternion m_currentOrientation;
+	btQuaternion m_targetOrientation;
+
 	///keep track of the contact manifolds
 	btManifoldArray	m_manifoldArray;
 
 	bool m_touchingContact;
 	btVector3 m_touchingNormal;
 
+	btScalar m_linearDamping;
+	btScalar m_angularDamping;
+
 	bool  m_wasOnGround;
 	bool  m_wasJumping;
 	bool	m_useGhostObjectSweepTest;
 	bool	m_useWalkDirection;
 	btScalar	m_velocityTimeInterval;
-	int m_upAxis;
+	btVector3 m_up;
+	btVector3 m_jumpAxis;
 
 	static btVector3* getUpAxisDirections();
 	bool  m_interpolateUp;
@@ -94,11 +106,18 @@ protected:
 	void updateTargetPositionBasedOnCollision (const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0));
 	void stepForwardAndStrafe (btCollisionWorld* collisionWorld, const btVector3& walkMove);
 	void stepDown (btCollisionWorld* collisionWorld, btScalar dt);
+
+	virtual bool needsCollision(const btCollisionObject* body0, const btCollisionObject* body1);
+
+	void setUpVector(const btVector3& up);
+
+	btQuaternion getRotation(btVector3& v0, btVector3& v1) const;
+
 public:
 
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis = 1);
+	btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, const btVector3& up = btVector3(1.0,0.0,0.0));
 	~btKinematicCharacterController ();
 	
 
@@ -112,14 +131,9 @@ public:
 	///btActionInterface interface
 	void	debugDraw(btIDebugDraw* debugDrawer);
 
-	void setUpAxis (int axis)
+	void setUp(const btVector3& up);
-	{
+
-		if (axis < 0)
+	const btVector3& getUp() { return m_up; }
-			axis = 0;
-		if (axis > 2)
-			axis = 2;
-		m_upAxis = axis;
-	}
 
 	/// This should probably be called setPositionIncrementPerSimulatorStep.
 	/// This is neither a direction nor a velocity, but the amount to
@@ -136,27 +150,47 @@ public:
 	virtual void setVelocityForTimeInterval(const btVector3& velocity,
 				btScalar timeInterval);
 
+	virtual void setAngularVelocity(const btVector3& velocity);
+	virtual const btVector3& getAngularVelocity() const;
+
+	virtual void setLinearVelocity(const btVector3& velocity);
+	virtual btVector3 getLinearVelocity() const;
+
+	void setLinearDamping(btScalar d) { m_linearDamping = btClamped(d, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); }
+	btScalar getLinearDamping() const { return  m_linearDamping; }
+	void setAngularDamping(btScalar d) { m_angularDamping = btClamped(d, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); }
+	btScalar getAngularDamping() const { return  m_angularDamping; }
+
 	void reset ( btCollisionWorld* collisionWorld );
 	void warp (const btVector3& origin);
 
 	void preStep (  btCollisionWorld* collisionWorld);
 	void playerStep ( btCollisionWorld* collisionWorld, btScalar dt);
 
+	void setStepHeight(btScalar h);
+	btScalar getStepHeight() const { return m_stepHeight; }
 	void setFallSpeed (btScalar fallSpeed);
+	btScalar getFallSpeed() const { return m_fallSpeed; }
 	void setJumpSpeed (btScalar jumpSpeed);
+	btScalar getJumpSpeed() const { return m_jumpSpeed; }
 	void setMaxJumpHeight (btScalar maxJumpHeight);
 	bool canJump () const;
 
-	void jump ();
+	void jump(const btVector3& v = btVector3());
 
-	void setGravity(btScalar gravity);
+	void applyImpulse(const btVector3& v) { jump(v); }
-	btScalar getGravity() const;
+
+	void setGravity(const btVector3& gravity);
+	btVector3 getGravity() const;
 
 	/// The max slope determines the maximum angle that the controller can walk up.
 	/// The slope angle is measured in radians.
 	void setMaxSlope(btScalar slopeRadians);
 	btScalar getMaxSlope() const;
 
+	void setMaxPenetrationDepth(btScalar d);
+	btScalar getMaxPenetrationDepth() const;
+
 	btPairCachingGhostObject* getGhostObject();
 	void	setUseGhostSweepTest(bool useGhostObjectSweepTest)
 	{

src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -299,7 +299,7 @@ void btMultiBody::setupPlanar(int i,
     m_links[i].m_eVector = parentComToThisComOffset;
 
 	//
-	static btVector3 vecNonParallelToRotAxis(1, 0, 0);
+	btVector3 vecNonParallelToRotAxis(1, 0, 0);
 	if(rotationAxis.normalized().dot(vecNonParallelToRotAxis) > 0.999)
 		vecNonParallelToRotAxis.setValue(0, 1, 0);
 	//
@@ -714,15 +714,15 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 	btScalar * Y = &scratch_r[0];
 	//
 	//aux variables	
-	static btSpatialMotionVector spatJointVel;					//spatial velocity due to the joint motion (i.e. without predecessors' influence)
+	btSpatialMotionVector spatJointVel;					//spatial velocity due to the joint motion (i.e. without predecessors' influence)
-	static btScalar D[36];										//"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do	
+	btScalar D[36];										//"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do	
-	static btScalar invD_times_Y[6];							//D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies	
+	btScalar invD_times_Y[6];							//D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies	
-	static btSpatialMotionVector result;							//holds results of the SolveImatrix op; it is a spatial motion vector (accel)
+	btSpatialMotionVector result;							//holds results of the SolveImatrix op; it is a spatial motion vector (accel)
-	static btScalar Y_minus_hT_a[6];							//Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough	
+	btScalar Y_minus_hT_a[6];							//Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough	
-	static btSpatialForceVector spatForceVecTemps[6];				//6 temporary spatial force vectors
+	btSpatialForceVector spatForceVecTemps[6];				//6 temporary spatial force vectors
-	static btSpatialTransformationMatrix fromParent;				//spatial transform from parent to child
+	btSpatialTransformationMatrix fromParent;				//spatial transform from parent to child
-	static btSymmetricSpatialDyad dyadTemp;						//inertia matrix temp
+	btSymmetricSpatialDyad dyadTemp;						//inertia matrix temp
-	static btSpatialTransformationMatrix fromWorld;
+	btSpatialTransformationMatrix fromWorld;
 	fromWorld.m_trnVec.setZero();
 	/////////////////
 
@@ -919,8 +919,8 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 			case btMultibodyLink::eSpherical:
 			case btMultibodyLink::ePlanar:
 			{
-				static btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
+				btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
-				static btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse();
+				btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse();
 
 				//unroll the loop?
 				for(int row = 0; row < 3; ++row)
@@ -1323,11 +1323,11 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
 	btScalar * Y = r_ptr; 
 	////////////////
 	//aux variables
-	static btScalar invD_times_Y[6];							//D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
+	btScalar invD_times_Y[6];							//D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
-	static btSpatialMotionVector result;							//holds results of the SolveImatrix op; it is a spatial motion vector (accel)
+	btSpatialMotionVector result;							//holds results of the SolveImatrix op; it is a spatial motion vector (accel)
-	static btScalar Y_minus_hT_a[6];							//Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough	
+	btScalar Y_minus_hT_a[6];							//Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough	
-	static btSpatialForceVector spatForceVecTemps[6];				//6 temporary spatial force vectors
+	btSpatialForceVector spatForceVecTemps[6];				//6 temporary spatial force vectors
-	static btSpatialTransformationMatrix fromParent;	
+	btSpatialTransformationMatrix fromParent;	
 	/////////////////
 
     // First 'upward' loop.
@@ -1522,8 +1522,8 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
 	btScalar *pBaseQuat = pq ? pq : m_baseQuat;	
 	btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0];		//note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
 	//
-	static btQuaternion baseQuat; baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
+	btQuaternion baseQuat; baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
-	static btVector3 baseOmega; baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
+	btVector3 baseOmega; baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
 	pQuatUpdateFun(baseOmega, baseQuat, true, dt);
 	pBaseQuat[0] = baseQuat.x();
 	pBaseQuat[1] = baseQuat.y();
@@ -1557,8 +1557,8 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
 			}
 			case btMultibodyLink::eSpherical:
 			{
-				static btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
+				btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
-				static btQuaternion jointOri; jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
+				btQuaternion jointOri; jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
 				pQuatUpdateFun(jointVel, jointOri, false, dt);
 				pJointPos[0] = jointOri.x(); pJointPos[1] = jointOri.y(); pJointPos[2] = jointOri.z(); pJointPos[3] = jointOri.w();
 				break;