Merge remote-tracking branch 'bp/master'

examples/CommonInterfaces/CommonGraphicsAppInterface.h

@@ -66,9 +66,9 @@ struct CommonGraphicsApp
 		m_mouseYpos(0.f),
 		m_mouseInitialized(false)
 	{
-		m_backgroundColorRGB[0] = 0.9;
+		m_backgroundColorRGB[0] = 0.7;
-		m_backgroundColorRGB[1] = 0.9;
+		m_backgroundColorRGB[1] = 0.7;
-		m_backgroundColorRGB[2] = 1;
+		m_backgroundColorRGB[2] = 0.8;
 	}
 	virtual ~CommonGraphicsApp()
 	{

examples/ExampleBrowser/OpenGLExampleBrowser.cpp

@@ -123,6 +123,7 @@ static bool enable_experimental_opencl = false;
 
 int gDebugDrawFlags = 0;
 static bool pauseSimulation=false;
+static bool singleStepSimulation = false;
 int midiBaseIndex = 176;
 extern bool gDisableDeactivation;
 
@@ -227,6 +228,12 @@ void MyKeyboardCallback(int key, int state)
 	{
 		pauseSimulation = !pauseSimulation;
 	}
+	if (key == 'o' && state)
+	{
+		singleStepSimulation = true;
+	}
+
+
 #ifndef NO_OPENGL3
 	if (key=='s' && state)
 	{
@@ -490,7 +497,7 @@ void	MyComboBoxCallback(int comboId, const char* item)
 void MyGuiPrintf(const char* msg)
 {
 	printf("b3Printf: %s\n",msg);
-	if (gui2)
+	if (!gDisableDemoSelection)
 	{
 		gui2->textOutput(msg);
 		gui2->forceUpdateScrollBars();
@@ -502,7 +509,7 @@ void MyGuiPrintf(const char* msg)
 void MyStatusBarPrintf(const char* msg)
 {
 	printf("b3Printf: %s\n", msg);
-	if (gui2)
+	if (!gDisableDemoSelection)
 	{
 		bool isLeft = true;
 		gui2->setStatusBarMessage(msg,isLeft);
@@ -513,7 +520,7 @@ void MyStatusBarPrintf(const char* msg)
 void MyStatusBarError(const char* msg)
 {
 	printf("Warning: %s\n", msg);
-	if (gui2)
+	if (!gDisableDemoSelection)
 	{
 		bool isLeft = false;
 		gui2->setStatusBarMessage(msg,isLeft);
@@ -1103,15 +1110,33 @@ void OpenGLExampleBrowser::update(float deltaTime)
             s_app->drawText(bla,10,10);
 		}
 
+    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 (sCurrentDemo)
 		{
-			if (!pauseSimulation)
+			if (!pauseSimulation || singleStepSimulation)
 			{
+				singleStepSimulation = false;
 				//printf("---------------------------------------------------\n");
 				//printf("Framecount = %d\n",frameCount);
 				
-
 				if (gFixedTimeStep>0)
 				{
 					sCurrentDemo->stepSimulation(gFixedTimeStep);
@@ -1145,23 +1170,6 @@ 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);
-					}
-				}
     
 
 		{

examples/ExampleBrowser/OpenGLGuiHelper.cpp

@@ -84,7 +84,10 @@ public:
 
 	virtual void	drawContactPoint(const btVector3& PointOnB,const btVector3& normalOnB,btScalar distance,int lifeTime,const btVector3& color)
 	{
-        drawLine(PointOnB,PointOnB+normalOnB,color);
+        drawLine(PointOnB,PointOnB+normalOnB*distance,color);
+		btVector3 red(0.3, 1., 0.3);
+		drawLine(PointOnB, PointOnB + normalOnB*0.01, red);
+		
 	}
      
 

examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp

@@ -36,7 +36,8 @@ class ImportUrdfSetup : public CommonMultiBodyBase
     struct ImportUrdfInternalData* m_data;
 	bool m_useMultiBody;
 	btAlignedObjectArray<std::string* > m_nameMemory;
-
+	btScalar m_grav;
+	int m_upAxis;
 public:
     ImportUrdfSetup(struct GUIHelperInterface* helper, int option, const char* fileName);
     virtual ~ImportUrdfSetup();
@@ -87,7 +88,9 @@ struct ImportUrdfInternalData
 
 
 ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option, const char* fileName)
-	:CommonMultiBodyBase(helper)
+	:CommonMultiBodyBase(helper),
+	m_grav(0),
+	m_upAxis(2)
 {
 	m_data = new ImportUrdfInternalData;
 
@@ -186,8 +189,8 @@ void ImportUrdfSetup::setFileName(const char* urdfFileName)
 void ImportUrdfSetup::initPhysics()
 {
 
-	int upAxis = 2;
+	
-	m_guiHelper->setUpAxis(upAxis);
+	m_guiHelper->setUpAxis(m_upAxis);
 	
 	this->createEmptyDynamicsWorld();
 	//m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
@@ -199,10 +202,14 @@ void ImportUrdfSetup::initPhysics()
         );//+btIDebugDraw::DBG_DrawConstraintLimits);
 
 
-	btVector3 gravity(0,0,0);
+	if (m_guiHelper->getParameterInterface())
-	gravity[upAxis]=-9.8;
+	{
+		SliderParams slider("Gravity", &m_grav);
+		slider.m_minVal = -10;
+		slider.m_maxVal = 10;
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+	}
 	
-	m_dynamicsWorld->setGravity(gravity);
 
 	BulletURDFImporter u2b(m_guiHelper, 0);
 	
@@ -350,7 +357,7 @@ void ImportUrdfSetup::initPhysics()
 		if (createGround)
 		{
 			btVector3 groundHalfExtents(20,20,20);
-			groundHalfExtents[upAxis]=1.f;
+			groundHalfExtents[m_upAxis]=1.f;
 			btBoxShape* box = new btBoxShape(groundHalfExtents);
 			m_collisionShapes.push_back(box);
 			box->initializePolyhedralFeatures();
@@ -358,7 +365,7 @@ void ImportUrdfSetup::initPhysics()
 			m_guiHelper->createCollisionShapeGraphicsObject(box);
 			btTransform start; start.setIdentity();
 			btVector3 groundOrigin(0,0,0);
-			groundOrigin[upAxis]=-2.5;
+			groundOrigin[m_upAxis]=-2.5;
 			start.setOrigin(groundOrigin);
 			btRigidBody* body =  createRigidBody(0,start,box);
 			//m_dynamicsWorld->removeRigidBody(body);
@@ -388,6 +395,10 @@ void ImportUrdfSetup::stepSimulation(float deltaTime)
 {
 	if (m_dynamicsWorld)
 	{
+		btVector3 gravity(0, 0, 0);
+		gravity[m_upAxis] = m_grav;
+		m_dynamicsWorld->setGravity(gravity);
+
         for (int i=0;i<m_data->m_numMotors;i++)
         {
 			if (m_data->m_jointMotors[i])

examples/Importers/ImportURDFDemo/URDF2Bullet.cpp

@@ -299,7 +299,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
                     {
                         //b3Printf("Fixed joint\n");
 						
-						btGeneric6DofSpring2Constraint* dof6 = creation.createFixedJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
+						btGeneric6DofSpring2Constraint* dof6 = creation.createFixedJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody,  offsetInB, offsetInA);
                        
                         if (enableConstraints)
                             world1->addConstraint(dof6,true);
@@ -324,7 +324,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
                     } else
                     {
 
-						btGeneric6DofSpring2Constraint* dof6 = creation.createRevoluteJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit);
+						btGeneric6DofSpring2Constraint* dof6 = creation.createRevoluteJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit);
 
 						if (enableConstraints)
                                     world1->addConstraint(dof6,true);
@@ -350,7 +350,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
                     } else
                     {
                         
-						btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit,jointUpperLimit);
+						btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,jointAxisInJointSpace,jointLowerLimit,jointUpperLimit);
 						
                        
                         if (enableConstraints)
@@ -455,7 +455,9 @@ void ConvertURDF2Bullet(const URDFImporterInterface& u2b, MultiBodyCreationInter
 		mb->setHasSelfCollision(false);
 		mb->finalizeMultiDof();
 
-		mb->setBaseWorldTransform(rootTransformInWorldSpace);
+		btTransform localInertialFrameRoot = cache.m_urdfLinkLocalInertialFrames[urdfLinkIndex];
+
+		mb->setBaseWorldTransform(rootTransformInWorldSpace*localInertialFrameRoot);
 		btAlignedObjectArray<btQuaternion> scratch_q;
 		btAlignedObjectArray<btVector3> scratch_m;
 		mb->forwardKinematics(scratch_q,scratch_m);

examples/RoboticsLearning/GripperGraspExample.cpp

@@ -58,7 +58,7 @@ public:
     
     virtual void physicsDebugDraw(int debugDrawMode)
     {
-        
+		m_robotSim.debugDraw(debugDrawMode);
     }
     virtual void    initPhysics()
     {
@@ -81,6 +81,15 @@ public:
                 slider.m_maxVal=1;
                 m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
             }
+			{
+				b3RobotSimLoadFileArgs args("");
+				b3RobotSimLoadFileResults results;
+				args.m_fileName = "cube_small.urdf";
+				args.m_startPosition.setValue(0, 0, .107);
+				args.m_startOrientation.setEulerZYX(0, 0, 0);
+				args.m_useMultiBody = true;
+				m_robotSim.loadFile(args, results);
+			}
 
             {
                 b3RobotSimLoadFileArgs args("");
@@ -129,15 +138,7 @@ public:
                     }
                 }
             }
-            {
+           
-                b3RobotSimLoadFileArgs args("");
-                b3RobotSimLoadFileResults results;
-                args.m_fileName = "cube_small.urdf";
-                args.m_startPosition.setValue(0,0,.107);
-                args.m_startOrientation.setEulerZYX(0,0,0);
-                args.m_useMultiBody = false;
-                m_robotSim.loadFile(args,results);
-            }
             if (1)
             {
                 b3RobotSimLoadFileArgs args("");
@@ -179,13 +180,14 @@ public:
                 m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs);
             }
         }
-	m_robotSim.stepSimulation();
+		m_robotSim.stepSimulation();
     }
     virtual void	renderScene()
     {
 		m_robotSim.renderScene();
 
 		//m_app->m_renderer->renderScene();
+		
     }
 
 	

examples/RoboticsLearning/b3RobotSimAPI.cpp

@@ -7,15 +7,15 @@
 //#include "../CommonInterfaces/CommonExampleInterface.h"
 #include "../CommonInterfaces/CommonGUIHelperInterface.h"
 #include "../SharedMemory/PhysicsServerSharedMemory.h"
+#include "../SharedMemory/PhysicsServerSharedMemory.h"
 #include "../SharedMemory/PhysicsClientC_API.h"
+#include "../SharedMemory/PhysicsDirectC_API.h"
+#include "../SharedMemory/PhysicsDirect.h"
+
 #include <string>
-
-
-
-
-
-
 #include "../Utils/b3Clock.h"
+
+
 #include "../MultiThreading/b3ThreadSupportInterface.h"
 
 
@@ -406,24 +406,27 @@ public:
 
 
 
-
-
-
 struct b3RobotSimAPI_InternalData
 {
 	//GUIHelperInterface* m_guiHelper;
 	PhysicsServerSharedMemory	m_physicsServer;
 	b3PhysicsClientHandle m_physicsClient;
 
+
 	b3ThreadSupportInterface* m_threadSupport;
 	RobotSimArgs m_args[MAX_ROBOT_NUM_THREADS];
 	MultiThreadedOpenGLGuiHelper2* m_multiThreadedHelper;
+	PhysicsDirect* m_clientServerDirect;
 
+	bool m_useMultiThreading;
 	bool m_connected;
 
 	b3RobotSimAPI_InternalData()
-		:m_multiThreadedHelper(0),
+		:m_threadSupport(0),
+		m_multiThreadedHelper(0),
+		m_clientServerDirect(0),
 		m_physicsClient(0),
+		m_useMultiThreading(false),
 		m_connected(false)
 	{
 	}
@@ -464,6 +467,9 @@ b3RobotSimAPI::~b3RobotSimAPI()
 
 void b3RobotSimAPI::processMultiThreadedGraphicsRequests()
 {
+	if (0==m_data->m_multiThreadedHelper)
+		return;
+
 	switch (m_data->m_multiThreadedHelper->getCriticalSection()->getSharedParam(1))
 	{
 	case eRobotSimGUIHelperCreateCollisionShapeGraphicsObject:
@@ -563,27 +569,6 @@ void b3RobotSimAPI::processMultiThreadedGraphicsRequests()
 		}
 	}
 	
-
-
-
-	#if 0
-	if (m_options == PHYSICS_SERVER_USE_RTC_CLOCK)
-	{
-		btClock rtc;
-		btScalar endTime = rtc.getTimeMilliseconds() + deltaTime*btScalar(800);
-
-		while (rtc.getTimeMilliseconds()<endTime)
-		{
-			m_physicsServer.processClientCommands();
-		}
-	} else
-	{
-        //for (int i=0;i<10;i++)
-			m_physicsServer.processClientCommands();
-	}
-	#endif
-
-	
 }
 
 b3SharedMemoryStatusHandle b3RobotSimAPI::submitClientCommandAndWaitStatusMultiThreaded(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle)
@@ -733,94 +718,124 @@ bool b3RobotSimAPI::loadFile(const struct b3RobotSimLoadFileArgs& args, b3RobotS
 
 bool b3RobotSimAPI::connect(GUIHelperInterface* guiHelper)
 {
-	m_data->m_multiThreadedHelper  = new MultiThreadedOpenGLGuiHelper2(guiHelper->getAppInterface(),guiHelper);
+	if (m_data->m_useMultiThreading)
-	
-	MultiThreadedOpenGLGuiHelper2* guiHelperWrapper = new MultiThreadedOpenGLGuiHelper2(guiHelper->getAppInterface(),guiHelper);
-
-	
-	
-
-	m_data->m_threadSupport = createRobotSimThreadSupport(MAX_ROBOT_NUM_THREADS);
-
-	for (int i=0;i<m_data->m_threadSupport->getNumTasks();i++)
 	{
-		RobotSimThreadLocalStorage* storage = (RobotSimThreadLocalStorage*) m_data->m_threadSupport->getThreadLocalMemory(i);
+		m_data->m_multiThreadedHelper = new MultiThreadedOpenGLGuiHelper2(guiHelper->getAppInterface(), guiHelper);
-		b3Assert(storage);
+
-		storage->threadId = i;
+		MultiThreadedOpenGLGuiHelper2* guiHelperWrapper = new MultiThreadedOpenGLGuiHelper2(guiHelper->getAppInterface(), guiHelper);
-		//storage->m_sharedMem = data->m_sharedMem;
-	}
 
 
 
 
-	for (int w=0;w<MAX_ROBOT_NUM_THREADS;w++)
+		m_data->m_threadSupport = createRobotSimThreadSupport(MAX_ROBOT_NUM_THREADS);
-	{
-		m_data->m_args[w].m_cs = m_data->m_threadSupport->createCriticalSection();
-		m_data->m_args[w].m_cs->setSharedParam(0,eRobotSimIsUnInitialized);
-		int numMoving = 0;
- 		m_data->m_args[w].m_positions.resize(numMoving);
-		m_data->m_args[w].m_physicsServerPtr = &m_data->m_physicsServer;
-		int index = 0;
 
-		m_data->m_threadSupport->runTask(B3_THREAD_SCHEDULE_TASK, (void*) &m_data->m_args[w], w);
+		for (int i = 0; i < m_data->m_threadSupport->getNumTasks(); i++)
-		while (m_data->m_args[w].m_cs->getSharedParam(0)==eRobotSimIsUnInitialized)
 		{
+			RobotSimThreadLocalStorage* storage = (RobotSimThreadLocalStorage*)m_data->m_threadSupport->getThreadLocalMemory(i);
+			b3Assert(storage);
+			storage->threadId = i;
+			//storage->m_sharedMem = data->m_sharedMem;
 		}
+
+
+
+
+		for (int w = 0; w < MAX_ROBOT_NUM_THREADS; w++)
+		{
+			m_data->m_args[w].m_cs = m_data->m_threadSupport->createCriticalSection();
+			m_data->m_args[w].m_cs->setSharedParam(0, eRobotSimIsUnInitialized);
+			int numMoving = 0;
+			m_data->m_args[w].m_positions.resize(numMoving);
+			m_data->m_args[w].m_physicsServerPtr = &m_data->m_physicsServer;
+			int index = 0;
+
+			m_data->m_threadSupport->runTask(B3_THREAD_SCHEDULE_TASK, (void*)&m_data->m_args[w], w);
+			while (m_data->m_args[w].m_cs->getSharedParam(0) == eRobotSimIsUnInitialized)
+			{
+			}
+		}
+
+		m_data->m_args[0].m_cs->setSharedParam(1, eRobotSimGUIHelperIdle);
+		m_data->m_multiThreadedHelper->setCriticalSection(m_data->m_args[0].m_cs);
+
+		bool serverConnected = m_data->m_physicsServer.connectSharedMemory(m_data->m_multiThreadedHelper);
+		b3Assert(serverConnected);
+
+		m_data->m_physicsClient = b3ConnectSharedMemory(SHARED_MEMORY_KEY);
+	}
+	else
+	{
+		m_data->m_clientServerDirect = new PhysicsDirect();
+		bool connected = m_data->m_clientServerDirect->connect(guiHelper);
+		m_data->m_physicsClient = (b3PhysicsClientHandle)m_data->m_clientServerDirect;
+
 	}
 
-	m_data->m_args[0].m_cs->setSharedParam(1,eRobotSimGUIHelperIdle);
+	//client connected
-	m_data->m_multiThreadedHelper->setCriticalSection(m_data->m_args[0].m_cs);
+	m_data->m_connected = b3CanSubmitCommand(m_data->m_physicsClient);
 	
-	m_data->m_connected = m_data->m_physicsServer.connectSharedMemory( m_data->m_multiThreadedHelper);
+	b3Assert(m_data->m_connected);
-
+	return m_data->m_connected && m_data->m_connected;
-		b3Assert(m_data->m_connected);
-
-	m_data->m_physicsClient = b3ConnectSharedMemory(SHARED_MEMORY_KEY);
-	int canSubmit = b3CanSubmitCommand(m_data->m_physicsClient);
-	b3Assert(canSubmit);
-	return m_data->m_connected && canSubmit;
 }
 
 void b3RobotSimAPI::disconnect()
 {
-
+	if (m_data->m_useMultiThreading)
-	for (int i=0;i<MAX_ROBOT_NUM_THREADS;i++)
 	{
-		m_data->m_args[i].m_cs->lock();
+		for (int i = 0; i < MAX_ROBOT_NUM_THREADS; i++)
-		m_data->m_args[i].m_cs->setSharedParam(0,eRequestTerminateRobotSim);
+		{
-		m_data->m_args[i].m_cs->unlock();
+			m_data->m_args[i].m_cs->lock();
+			m_data->m_args[i].m_cs->setSharedParam(0, eRequestTerminateRobotSim);
+			m_data->m_args[i].m_cs->unlock();
+		}
+		int numActiveThreads = MAX_ROBOT_NUM_THREADS;
+
+		while (numActiveThreads)
+		{
+			int arg0, arg1;
+			if (m_data->m_threadSupport->isTaskCompleted(&arg0, &arg1, 0))
+			{
+				numActiveThreads--;
+				printf("numActiveThreads = %d\n", numActiveThreads);
+
+			}
+			else
+			{
+			}
+		};
+
+		printf("stopping threads\n");
+
+		delete m_data->m_threadSupport;
+		m_data->m_threadSupport = 0;
 	}
-	int numActiveThreads = MAX_ROBOT_NUM_THREADS;
-
-	while (numActiveThreads)
-            {
-		int arg0,arg1;
-                    if (m_data->m_threadSupport->isTaskCompleted(&arg0,&arg1,0))
-                    {
-                            numActiveThreads--;
-                            printf("numActiveThreads = %d\n",numActiveThreads);
-
-                    } else
-                    {
-                    }
-            };
-
-	printf("stopping threads\n");
-
-	delete m_data->m_threadSupport;   
-	m_data->m_threadSupport = 0;
-
 	b3DisconnectSharedMemory(m_data->m_physicsClient);
 	m_data->m_physicsServer.disconnectSharedMemory(true);
+	
 	m_data->m_connected = false;
 }
 
+
+void b3RobotSimAPI::debugDraw(int debugDrawMode)
+{
+	if (m_data->m_clientServerDirect)
+	{
+		m_data->m_clientServerDirect->debugDraw(debugDrawMode);
+	}
+}
 void b3RobotSimAPI::renderScene()
 {
-	if (m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface())
+	if (m_data->m_useMultiThreading)
 	{
-		m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->writeTransforms();
+		if (m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface())
+		{
+			m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->writeTransforms();
+		}
 	}
 
+	if (m_data->m_clientServerDirect)
+	{
+		m_data->m_clientServerDirect->renderScene();
+	}
 	m_data->m_physicsServer.renderScene();
 }

examples/RoboticsLearning/b3RobotSimAPI.h

@@ -95,6 +95,7 @@ public:
 	void setGravity(const b3Vector3& gravityAcceleration);
 
 	void renderScene();
+	void debugDraw(int debugDrawMode);
 };
 
 #endif //B3_ROBOT_SIM_API_H

examples/SharedMemory/PhysicsClientExample.cpp

@@ -239,7 +239,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
             b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.urdf");
             //setting the initial position, orientation and other arguments are optional
             double startPosX = 0;
-            static double startPosY = 1;
+            static double startPosY = 0;
             double startPosZ = 0;
             b3LoadUrdfCommandSetStartPosition(commandHandle, startPosX,startPosY,startPosZ);
 			startPosY += 2.f;

examples/SharedMemory/PhysicsClientSharedMemory.cpp

@@ -81,7 +81,6 @@ int PhysicsClientSharedMemory::getNumJoints(int bodyIndex) const
 
 		return bodyJoints->m_jointInfo.size(); 
 	}
-	btAssert(0);
 	return 0;
 	
 }

examples/SharedMemory/PhysicsDirect.cpp

@@ -69,9 +69,27 @@ PhysicsDirect::~PhysicsDirect()
 bool PhysicsDirect::connect()
 {
 	m_data->m_commandProcessor->setGuiHelper(&m_data->m_noGfx);
+
 	return true;
 }
 
+// return true if connection succesfull, can also check 'isConnected'
+bool PhysicsDirect::connect(struct GUIHelperInterface* guiHelper)
+{
+	m_data->m_commandProcessor->setGuiHelper(guiHelper);
+	
+	return true;
+}
+
+void PhysicsDirect::renderScene()
+{
+	m_data->m_commandProcessor->renderScene();
+}
+void PhysicsDirect::debugDraw(int debugDrawMode)
+{
+	m_data->m_commandProcessor->physicsDebugDraw(debugDrawMode);
+}
+
 ////todo: rename to 'disconnect'
 void PhysicsDirect::disconnectSharedMemory()
 {

examples/SharedMemory/PhysicsDirect.h

@@ -29,7 +29,8 @@ public:
     
     virtual ~PhysicsDirect();
 
-	  // return true if connection succesfull, can also check 'isConnected'
+	// return true if connection succesfull, can also check 'isConnected'
+	//it is OK to pass a null pointer for the gui helper
     virtual bool connect();
 	
 	////todo: rename to 'disconnect'
@@ -63,6 +64,11 @@ public:
 
 	virtual void getCachedCameraImage(b3CameraImageData* cameraData);
 
+	//those 2 APIs are for internal use for visualization
+	virtual bool connect(struct GUIHelperInterface* guiHelper);
+	virtual void renderScene();
+	virtual void debugDraw(int debugDrawMode);
+
 };
 
 #endif //PHYSICS_DIRECT_H

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -696,10 +696,12 @@ void	PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb)
 
 		if (supportsJointMotor(mb,mbLinkIndex))
 		{
-			float maxMotorImpulse = 0.f;
+			float maxMotorImpulse = 10000.f;
 			int dof = 0;
 			btScalar desiredVelocity = 0.f;
 			btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,dof,desiredVelocity,maxMotorImpulse);
+			motor->setPositionTarget(0, 0);
+			motor->setVelocityTarget(0, 1);
 			//motor->setMaxAppliedImpulse(0);
             mb->getLink(mbLinkIndex).m_userPtr = motor;
 			m_data->m_dynamicsWorld->addMultiBodyConstraint(motor);
@@ -1416,7 +1418,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 											b3Printf("Using CONTROL_MODE_TORQUE");
 										}
                                       //  mb->clearForcesAndTorques();
-                                        int torqueIndex = 0;
+                                        int torqueIndex = 6;
                                         if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
                                         {
                                             for (int link=0;link<mb->getNumLinks();link++)
@@ -1602,6 +1604,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 							int totalDegreeOfFreedomQ = 0;
 							int totalDegreeOfFreedomU = 0;
 
+							if (mb->getNumLinks()>= MAX_DEGREE_OF_FREEDOM)
+							{
+								serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED;
+								hasStatus = true;
+								break;
+							}
 
 							//always add the base, even for static (non-moving objects)
 							//so that we can easily move the 'fixed' base when needed
@@ -2437,7 +2445,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
 			loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &bufferServerToClient[0], bufferServerToClient.size());
 		}
 
-		m_data->m_dynamicsWorld->stepSimulation(dtInSec);
+		m_data->m_dynamicsWorld->stepSimulation(dtInSec,10,1./240.);
 	}
 }
 

examples/SharedMemory/PhysicsServerExample.cpp

@@ -872,7 +872,22 @@ void PhysicsServerExample::renderScene()
 		static int frameCount=0;
 		frameCount++;
 		char bla[1024];
-		sprintf(bla,"VR sub-title text test, frame %d", frameCount/2);
+
+		static btScalar prevTime = m_clock.getTimeSeconds();
+		btScalar curTime = m_clock.getTimeSeconds();
+		static btScalar deltaTime = 0.f;
+		static int count = 10;
+		if (count-- < 0)
+		{
+			count = 10;
+			deltaTime = curTime - prevTime;
+		}
+		if (deltaTime == 0)
+			deltaTime = 1000;
+
+		prevTime = curTime;
+		
+		sprintf(bla,"VR sub-title text test,fps = %f, frame %d", 1./deltaTime, frameCount/2);
 		float pos[4];
 		m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->getCameraTargetPosition(pos);
 		btTransform viewTr;

examples/SharedMemory/SharedMemoryCommands.h

@@ -27,7 +27,7 @@
 #define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (256*1024)
 
 #define SHARED_MEMORY_SERVER_TEST_C
-#define MAX_DEGREE_OF_FREEDOM 64
+#define MAX_DEGREE_OF_FREEDOM 128
 #define MAX_NUM_SENSORS 256
 #define MAX_URDF_FILENAME_LENGTH 1024
 #define MAX_SDF_FILENAME_LENGTH 1024

examples/TinyRenderer/TinyRenderer.cpp

@@ -268,7 +268,7 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData)
 	renderData.m_viewportMatrix = viewport(0,0,width, height);
 	
     b3AlignedObjectArray<float>& zbuffer = renderData.m_depthBuffer;
-    int* segmentationMaskBufferPtr = &renderData.m_segmentationMaskBufferPtr->at(0);
+    int* segmentationMaskBufferPtr = renderData.m_segmentationMaskBufferPtr?&renderData.m_segmentationMaskBufferPtr->at(0):0;
     
     TGAImage& frame = renderData.m_rgbColorBuffer;
 

examples/Utils/b3Clock.cpp

@@ -239,7 +239,7 @@ void b3Clock::usleep(int microSeconds)
 	Sleep(millis);
 #else
 
-    usleep(microSeconds); 
+    ::usleep(microSeconds); 
 	//struct timeval tv;
 	//tv.tv_sec = microSeconds/1000000L;
 	//tv.tv_usec = microSeconds%1000000L;

examples/pybullet/pybullet.c

@@ -296,10 +296,11 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
 {
     int size;
     int bodyIndex, jointIndex, controlMode;
-    double targetValue=0;
+    
     double targetPosition=0;
     double targetVelocity=0;
     double maxForce=100000;
+	double appliedForce = 0;
     double kp=0.1;
     double kd=1.0;
     int valid = 0;
@@ -313,34 +314,107 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
     size= PySequence_Size(args);
     if (size==4)
     {
-        // for CONTROL_MODE_VELOCITY targetValue -> velocity
+		double targetValue = 0;
-        // for CONTROL_MODE_TORQUE targetValue -> force torque
+        // see switch statement below for convertsions dependent on controlMode
         if (!PyArg_ParseTuple(args, "iiid", &bodyIndex, &jointIndex, &controlMode, &targetValue))
         {
             PyErr_SetString(SpamError, "Error parsing arguments");
             return NULL;
         }
-        valid = 1;
+		valid = 1;
+		switch (controlMode)
+		{
+		case CONTROL_MODE_POSITION_VELOCITY_PD:
+		{
+			targetPosition = targetValue;
+			break;
+		}
+		case CONTROL_MODE_VELOCITY:
+		{
+			targetVelocity = targetValue;
+			break;
+		}
+		case CONTROL_MODE_TORQUE:
+		{
+			appliedForce = targetValue;
+			break;
+		}
+		default:
+		{
+			valid = 0;
+		}
+		}
+       
     }
     if (size==5)
     {
-        // for CONTROL_MODE_VELOCITY targetValue -> velocity
+		double targetValue = 0;
-        // for CONTROL_MODE_TORQUE targetValue -> force torque
+        //See switch statement for conversions
         if (!PyArg_ParseTuple(args, "iiidd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce))
         {
             PyErr_SetString(SpamError, "Error parsing arguments");
             return NULL;
         }
         valid = 1;
+
+		switch (controlMode)
+		{
+		case CONTROL_MODE_POSITION_VELOCITY_PD:
+		{
+			targetPosition = targetValue;
+			break;
+		}
+		case CONTROL_MODE_VELOCITY:
+		{
+			targetVelocity = targetValue;
+			break;
+		}
+		case CONTROL_MODE_TORQUE:
+		{
+			valid = 0;
+			break;
+		}
+		default:
+		{
+			valid = 0;
+		}
+		}
     }
     if (size==6)
     {
-        if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce, &kd))
+		double gain;
+		double targetValue = 0;
+        if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce, &gain))
         {
             PyErr_SetString(SpamError, "Error parsing arguments");
             return NULL;
         }
-        valid = 1;
+		valid = 1;
+
+		switch (controlMode)
+		{
+		case CONTROL_MODE_POSITION_VELOCITY_PD:
+		{
+			targetPosition = targetValue;
+			kp = gain;
+			break;
+		}
+		case CONTROL_MODE_VELOCITY:
+		{
+			targetVelocity = targetValue;
+			kd = gain;
+			break;
+		}
+		case CONTROL_MODE_TORQUE:
+		{
+			valid = 0;
+			break;
+		}
+		default:
+		{
+			valid = 0;
+		}
+		}
     }
     if (size==8)
     {
@@ -353,16 +427,7 @@ 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)
     {
@@ -395,25 +460,25 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
         {
             case CONTROL_MODE_VELOCITY:
             {
-                b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetValue);
+                b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity);
-                b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
+				b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
-                b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce);
+				b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce);
                 break;
             }
 
             case CONTROL_MODE_TORQUE:
             {
-                b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, targetValue);
+                b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, appliedForce);
                 break;
             }
 
             case CONTROL_MODE_POSITION_VELOCITY_PD:
             {
                 b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, targetPosition);
-                b3JointControlSetKp(commandHandle, info.m_uIndex, kp);
+				b3JointControlSetKp(commandHandle, info.m_uIndex, kp);
-                b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity);
+				b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity);
-                b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
+				b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
-                b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce);
+				b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce);
                 break;
             }
             default:
@@ -426,7 +491,7 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
         Py_INCREF(Py_None);
         return Py_None;
     }
-    PyErr_SetString(SpamError, "Invalid number of args passed to setJointControl.");
+    PyErr_SetString(SpamError, "Error parsing arguments in setJointControl.");
     return NULL;
 }
 
@@ -532,7 +597,7 @@ pybullet_setTimeStep(PyObject* self, PyObject* args)
 
 // Internal function used to get the base position and orientation
 // Orientation is returned in quaternions
-static void pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double basePosition[3],double baseOrientation[3])
+static int pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double basePosition[3],double baseOrientation[3])
 {
     basePosition[0] = 0.;
     basePosition[1] = 0.;
@@ -552,7 +617,11 @@ static void pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double
                     b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
 
             const int status_type = b3GetStatusType(status_handle);
-            
+			if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
+			{
+				PyErr_SetString(SpamError, "getBasePositionAndOrientation failed.");
+				return 0;
+			}
             const double* actualStateQ;
             // const double* jointReactionForces[];
             int i;
@@ -579,6 +648,7 @@ static void pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double
             
 		}
 	}
+	return 1;
 }
 
 // Get the positions (x,y,z) and orientation (x,y,z,w) in quaternion
@@ -606,7 +676,11 @@ pybullet_getBasePositionAndOrientation(PyObject* self, PyObject* args)
         return NULL;
     }
 
-    pybullet_internalGetBasePositionAndOrientation(bodyIndex,basePosition,baseOrientation);
+	if (0==pybullet_internalGetBasePositionAndOrientation(bodyIndex, basePosition, baseOrientation))
+	{
+		PyErr_SetString(SpamError, "GetBasePositionAndOrientation failed (#joints/links exceeds maximum?).");
+		return NULL;
+	}
     
     {
     
@@ -848,42 +922,44 @@ pybullet_getJointInfo(PyObject* self, PyObject* args)
       
       // printf("body index = %d, joint index =%d\n", bodyIndex, jointIndex);
 
-    b3SharedMemoryCommandHandle cmd_handle =
-            b3RequestActualStateCommandInit(sm, bodyIndex);
-        b3SharedMemoryStatusHandle status_handle =
-                b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
-                  
      pyListJointInfo = PyTuple_New(jointInfoSize);
 
-     b3GetJointInfo(sm, bodyIndex, jointIndex, &info);
+	 if (b3GetJointInfo(sm, bodyIndex, jointIndex, &info))
+	 {
 
-    //  printf("Joint%d %s, type %d, at q-index %d and u-index %d\n",
+		 //  printf("Joint%d %s, type %d, at q-index %d and u-index %d\n",
-    //          info.m_jointIndex,
+		 //          info.m_jointIndex,
-    //          info.m_jointName, 
+		 //          info.m_jointName, 
-    //          info.m_jointType, 
+		 //          info.m_jointType, 
-    //          info.m_qIndex,
+		 //          info.m_qIndex,
-    //          info.m_uIndex);
+		 //          info.m_uIndex);
-    //  printf("  flags=%d jointDamping=%f jointFriction=%f\n",
+		 //  printf("  flags=%d jointDamping=%f jointFriction=%f\n",
-    //          info.m_flags,
+		 //          info.m_flags,
-    //          info.m_jointDamping,
+		 //          info.m_jointDamping,
-    //          info.m_jointFriction);
+		 //          info.m_jointFriction);
-      PyTuple_SetItem(pyListJointInfo, 0, 
+		 PyTuple_SetItem(pyListJointInfo, 0,
-      PyInt_FromLong(info.m_jointIndex));
+			 PyInt_FromLong(info.m_jointIndex));
-      PyTuple_SetItem(pyListJointInfo, 1, 
+		 PyTuple_SetItem(pyListJointInfo, 1,
-        PyString_FromString(info.m_jointName));
+			 PyString_FromString(info.m_jointName));
-      PyTuple_SetItem(pyListJointInfo, 2, 
+		 PyTuple_SetItem(pyListJointInfo, 2,
-        PyInt_FromLong(info.m_jointType));    
+			 PyInt_FromLong(info.m_jointType));
-      PyTuple_SetItem(pyListJointInfo, 3, 
+		 PyTuple_SetItem(pyListJointInfo, 3,
-        PyInt_FromLong(info.m_qIndex));
+			 PyInt_FromLong(info.m_qIndex));
-      PyTuple_SetItem(pyListJointInfo, 4, 
+		 PyTuple_SetItem(pyListJointInfo, 4,
-        PyInt_FromLong(info.m_uIndex));
+			 PyInt_FromLong(info.m_uIndex));
-      PyTuple_SetItem(pyListJointInfo, 5, 
+		 PyTuple_SetItem(pyListJointInfo, 5,
-        PyInt_FromLong(info.m_flags));   
+			 PyInt_FromLong(info.m_flags));
-      PyTuple_SetItem(pyListJointInfo, 6, 
+		 PyTuple_SetItem(pyListJointInfo, 6,
-        PyFloat_FromDouble(info.m_jointDamping));
+			 PyFloat_FromDouble(info.m_jointDamping));
-      PyTuple_SetItem(pyListJointInfo, 7, 
+		 PyTuple_SetItem(pyListJointInfo, 7,
-        PyFloat_FromDouble(info.m_jointFriction));    
+			 PyFloat_FromDouble(info.m_jointFriction));
-      return pyListJointInfo;
+		 return pyListJointInfo;
+	 }
+	 else
+	 {
+		 PyErr_SetString(SpamError, "GetJointInfo failed.");
+		 return NULL;
+	 }
     }
   }
   
@@ -934,12 +1010,19 @@ pybullet_getJointState(PyObject* self, PyObject* args)
   {
    if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex))
   	{
-
+		int status_type = 0;
   b3SharedMemoryCommandHandle cmd_handle =
           b3RequestActualStateCommandInit(sm, bodyIndex);
       b3SharedMemoryStatusHandle status_handle =
               b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
               
+	  status_type = b3GetStatusType(status_handle);
+	  if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
+	  {
+		  PyErr_SetString(SpamError, "getBasePositionAndOrientation failed.");
+		  return NULL;
+	  }
+
    pyListJointState = PyTuple_New(sensorStateSize);
    pyListJointForceTorque = PyTuple_New(forceTorqueSize);
 

src/BulletDynamics/Character/btKinematicCharacterController.cpp

@@ -134,6 +134,7 @@ btVector3 btKinematicCharacterController::perpindicularComponent (const btVector
 
 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);
 	setUp(up);
@@ -142,7 +143,6 @@ btKinematicCharacterController::btKinematicCharacterController (btPairCachingGho
 	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_turnAngle = btScalar(0.0);
 	m_convexShape=convexShape;	
 	m_useWalkDirection = true;	// use walk direction by default, legacy behavior
@@ -520,7 +520,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
 		{
 			m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
 
-			if (!callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
+			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);
@@ -529,7 +529,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
 		{
 			collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
 
-			if (!callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
+			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);
@@ -537,11 +537,11 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
 		}
 	
 		btScalar downVelocity2 = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
-		bool has_hit = false;
+		bool has_hit;
 		if (bounce_fix == true)
 			has_hit = (callback.hasHit() || callback2.hasHit()) && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject);
 		else
-			has_hit = callback2.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject);
+			has_hit = callback2.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback2.m_hitCollisionObject);
 
 		btScalar stepHeight = 0.0f;
 		if (m_verticalVelocity < 0.0)
@@ -556,7 +556,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
 			m_targetPosition = orig_position;
 			downVelocity = stepHeight;
 
-			btVector3 step_drop = m_up * (m_currentStepOffset + downVelocity);
+			step_drop = m_up * (m_currentStepOffset + downVelocity);
 			m_targetPosition -= step_drop;
 			runonce = true;
 			continue; //re-run previous tests
@@ -564,7 +564,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
 		break;
 	}
 
-	if ((callback.hasHit() || runonce == true) && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
+	if (m_ghostObject->hasContactResponse() && (callback.hasHit() && needsCollision(m_ghostObject, callback.m_hitCollisionObject)) || runonce == true)
 	{
 		// we dropped a fraction of the height -> hit floor
 		btScalar fraction = (m_currentPosition.getY() - callback.m_hitPointWorld.getY()) / 2;

src/LinearMath/btIDebugDraw.h

@@ -166,9 +166,9 @@ class	btIDebugDraw
 	virtual void drawTransform(const btTransform& transform, btScalar orthoLen)
 	{
 		btVector3 start = transform.getOrigin();
-		drawLine(start, start+transform.getBasis() * btVector3(orthoLen, 0, 0), btVector3(0.7f,0,0));
+		drawLine(start, start+transform.getBasis() * btVector3(orthoLen, 0, 0), btVector3(1.f,0.3,0.3));
-		drawLine(start, start+transform.getBasis() * btVector3(0, orthoLen, 0), btVector3(0,0.7f,0));
+		drawLine(start, start+transform.getBasis() * btVector3(0, orthoLen, 0), btVector3(0.3,1.f, 0.3));
-		drawLine(start, start+transform.getBasis() * btVector3(0, 0, orthoLen), btVector3(0,0,0.7f));
+		drawLine(start, start+transform.getBasis() * btVector3(0, 0, orthoLen), btVector3(0.3, 0.3,1.f));
 	}
 
 	virtual void drawArc(const btVector3& center, const btVector3& normal, const btVector3& axis, btScalar radiusA, btScalar radiusB, btScalar minAngle, btScalar maxAngle,