Merge pull request #647 from erwincoumans/master

Support the <static> field under <model> in SDF, fix premake build system

data/cube.mtl

@@ -10,5 +10,5 @@ newmtl cube
   Ks 0.0000 0.0000 0.0000
   Ke 0.0000 0.0000 0.0000
   map_Ka cube.tga
-  map_Kd floor_diffuse.tga
+  map_Kd cube.png
 

data/two_cubes.sdf

@@ -86,6 +86,7 @@
       <heading_deg>0</heading_deg>
     </spherical_coordinates>
     <model name='unit_box_0'>
+ <static>1</static>
       <pose frame=''>0.512455 -1.58317 0.5 0 -0 0</pose>
       <link name='unit_box_0::link'>
         <inertial>

examples/ExampleBrowser/ExampleEntries.cpp

@@ -242,7 +242,10 @@ static ExampleEntry gDefaultExamples[]=
 	ExampleEntry(1,"Physics Server (Replay Log)", "Create a physics server that replay a command log from disk.",
 			PhysicsServerCreateFunc,PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG),
 
-	ExampleEntry(1, "Physics Client", "Create a physics client that can communicate with a physics server over shared memory", PhysicsClientCreateFunc),
+	ExampleEntry(1, "Physics Client (Shared Mem)", "Create a physics client that can communicate with a physics server over shared memory.", PhysicsClientCreateFunc),
+	ExampleEntry(1, "Physics Client (Direct)", "Create a physics client that can communicate with a physics server directly in-process.", PhysicsClientCreateFunc,eCLIENTEXAMPLE_DIRECT),
+
+		
 
 
 #ifdef ENABLE_LUA

examples/ExampleBrowser/main.cpp

@@ -13,6 +13,8 @@
 #include "../Importers/ImportSTLDemo/ImportSTLSetup.h"
 #include "../Importers/ImportURDFDemo/ImportURDFSetup.h"
 #include "../Importers/ImportSDFDemo/ImportSDFSetup.h"
+#include "../Importers/ImportSTLDemo/ImportSTLSetup.h"
+
 
 
 int main(int argc, char* argv[])
@@ -29,6 +31,7 @@ int main(int argc, char* argv[])
 	exampleBrowser->registerFileImporter(".urdf",ImportURDFCreateFunc);
 	exampleBrowser->registerFileImporter(".sdf",ImportSDFCreateFunc);
 	exampleBrowser->registerFileImporter(".obj",ImportObjCreateFunc);
+	exampleBrowser->registerFileImporter(".stl",ImportSTLCreateFunc);
 	
 	clock.reset();
 	if (init)

examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp

@@ -235,6 +235,60 @@ void ImportSDFSetup::initPhysics()
 			ConvertURDF2Bullet(u2b,creation, rootTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix(),true);
 			mb = creation.getBulletMultiBody();
 			
+			
+			if (m_useMultiBody && mb )
+			{
+				std::string*   name = new std::string(u2b.getLinkName(u2b.getRootLinkIndex()));
+				m_nameMemory.push_back(name);
+#ifdef TEST_MULTIBODY_SERIALIZATION
+				s->registerNameForPointer(name->c_str(),name->c_str());
+#endif//TEST_MULTIBODY_SERIALIZATION
+				mb->setBaseName(name->c_str());
+				//create motors for each btMultiBody joint
+				int numLinks = mb->getNumLinks();
+				for (int i=0;i<numLinks;i++)
+				{
+					int mbLinkIndex = i;
+					int urdfLinkIndex = creation.m_mb2urdfLink[mbLinkIndex];
+
+					std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex));
+					std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str());
+#ifdef TEST_MULTIBODY_SERIALIZATION					
+					s->registerNameForPointer(jointName->c_str(),jointName->c_str());
+					s->registerNameForPointer(linkName->c_str(),linkName->c_str());
+#endif//TEST_MULTIBODY_SERIALIZATION
+					m_nameMemory.push_back(jointName);
+					m_nameMemory.push_back(linkName);
+
+					mb->getLink(i).m_linkName = linkName->c_str();
+					mb->getLink(i).m_jointName = jointName->c_str();
+							
+					if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute
+					    ||mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::ePrismatic
+					)
+					{
+						if (m_data->m_numMotors<MAX_NUM_MOTORS)
+						{
+							
+							char motorName[1024];
+							sprintf(motorName,"%s q'", jointName->c_str());
+							btScalar* motorVel = &m_data->m_motorTargetVelocities[m_data->m_numMotors];
+							*motorVel = 0.f;
+							SliderParams slider(motorName,motorVel);
+							slider.m_minVal=-4;
+							slider.m_maxVal=4;
+							m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+							float maxMotorImpulse = 10.1f;
+							btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,0,0,maxMotorImpulse);
+							//motor->setMaxAppliedImpulse(0);
+							m_data->m_jointMotors[m_data->m_numMotors]=motor;
+							m_dynamicsWorld->addMultiBodyConstraint(motor);
+							m_data->m_numMotors++;
+						}
+					}
+
+				}
+			} 
 		}
 
 	

examples/Importers/ImportSTLDemo/ImportSTLSetup.cpp

@@ -13,9 +13,12 @@
 
 class ImportSTLSetup : public CommonRigidBodyBase
 {
+
+	const char* m_fileName;
+	btVector3 m_scaling;
 	
 public:
-    ImportSTLSetup(struct GUIHelperInterface* helper);
+    ImportSTLSetup(struct GUIHelperInterface* helper, const char* fileName);
     virtual ~ImportSTLSetup();
     
 	virtual void initPhysics();
@@ -31,10 +34,19 @@ public:
 };
 
 
-ImportSTLSetup::ImportSTLSetup(struct GUIHelperInterface* helper)
+ImportSTLSetup::ImportSTLSetup(struct GUIHelperInterface* helper, const char* fileName)
-:CommonRigidBodyBase(helper)
+:CommonRigidBodyBase(helper),
+m_scaling(btVector3(10,10,10))
 {
-    
+	if (fileName)
+	{
+		m_fileName = fileName;
+		m_scaling = btVector3(0.01,0.01,0.01);
+	} else
+	{
+		m_fileName = "l_finger_tip.stl";
+
+	}
 }
 
 ImportSTLSetup::~ImportSTLSetup()
@@ -51,17 +63,16 @@ void ImportSTLSetup::initPhysics()
 	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
 	m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe);
 
-	const char* fileName = "l_finger_tip.stl";
+	
     char relativeFileName[1024];
-  if (!b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024))
+  if (!b3ResourcePath::findResourcePath(m_fileName, relativeFileName, 1024))
         {
-                b3Warning("Cannot find file %s\n", fileName);
+                b3Warning("Cannot find file %s\n", m_fileName);
                 return;
         }
 
 	
 	btVector3 shift(0,0,0);
-	btVector3 scaling(10,10,10);
 //	int index=10;
 	
 	{
@@ -81,12 +92,12 @@ void ImportSTLSetup::initPhysics()
 		
 		
 	
-		m_guiHelper->getRenderInterface()->registerGraphicsInstance(shapeId,position,orn,color,scaling);
+		m_guiHelper->getRenderInterface()->registerGraphicsInstance(shapeId,position,orn,color,m_scaling);
 
 	}
 }
 
 class CommonExampleInterface*    ImportSTLCreateFunc(struct CommonExampleOptions& options)
 {
-	return new ImportSTLSetup(options.m_guiHelper);
+	return new ImportSTLSetup(options.m_guiHelper, options.m_fileName);
 }

examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp

@@ -267,14 +267,25 @@ void  BulletURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVect
 	//the link->m_inertia is NOT necessarily aligned with the inertial frame
 	//so an additional transform might need to be computed
 	UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
+	
+	
 	btAssert(linkPtr);
 	if (linkPtr)
 	{
 		UrdfLink* link = *linkPtr;
-		mass = link->m_inertia.m_mass;
+		if (link->m_parentJoint==0 && m_data->m_urdfParser.getModel().m_overrideFixedBase)
+		{
+			mass = 0.f;
+			localInertiaDiagonal.setValue(0,0,0);
+		}
+		else
+		{
+			mass = link->m_inertia.m_mass;
+			localInertiaDiagonal.setValue(link->m_inertia.m_ixx,link->m_inertia.m_iyy,
+										  link->m_inertia.m_izz);
+		}
 		inertialFrame = link->m_inertia.m_linkLocalFrame;
-		localInertiaDiagonal.setValue(link->m_inertia.m_ixx,link->m_inertia.m_iyy,
+		
-									  link->m_inertia.m_izz);
 	}
 	else
     {

examples/Importers/ImportURDFDemo/UrdfParser.cpp

@@ -529,6 +529,8 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
 	link.m_name = linkName;
     
     if (m_parseSDF) {
+
+
         TiXmlElement* pose = config->FirstChildElement("pose");
         if (0==pose)
         {
@@ -572,7 +574,7 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
           logger->reportWarning(link.m_name.c_str());
       }
   }
-		
+
   // Multiple Visuals (optional)
   for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual"))
   {
@@ -1240,6 +1242,16 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
         UrdfModel* localModel = new UrdfModel;
         m_tmpModels.push_back(localModel);
         
+		TiXmlElement* stat = robot_xml->FirstChildElement("static");
+        if (0!=stat)
+        {
+			int val = int(atof(stat->GetText()));
+			if (val==1)
+			{
+				localModel->m_overrideFixedBase = true;
+			}
+		}
+
         
         // Get robot name
         const char *name = robot_xml->Attribute("name");

examples/Importers/ImportURDFDemo/UrdfParser.h

@@ -134,6 +134,13 @@ struct UrdfModel
 	btHashMap<btHashString, UrdfJoint*> m_joints;
 	
 	btArray<UrdfLink*> m_rootLinks;
+	bool m_overrideFixedBase;
+
+	UrdfModel()
+		:m_overrideFixedBase(false)
+	{
+		m_rootTransformInWorld.setIdentity();
+	}
 	
 };
 

examples/OpenGLWindow/MacOpenGLWindow.mm

@@ -102,11 +102,11 @@ float loop;
 }
 -(void)drawRect:(NSRect)rect
 {
+	
 	if (([self frame].size.width != m_lastWidth) || ([self frame].size.height != m_lastHeight))
 	{
 		m_lastWidth = [self frame].size.width;
 		m_lastHeight = [self frame].size.height;
-		
 		// Only needed on resize:
 		[m_context clearDrawable];
 		
@@ -114,7 +114,6 @@ float loop;
         float width = [self frame].size.width;
         float height = [self frame].size.height;
         
-        
         // Get view dimensions in pixels
      //   glViewport(0,0,10,10);
         
@@ -209,16 +208,12 @@ struct MacOpenGLWindowInternalData
         m_myview = 0;
         m_pool = 0;
         m_window = 0;
-        m_width = -1;
-        m_height = -1;
         m_exitRequested = false;
     }
     NSApplication*      m_myApp;
     TestView*             m_myview;
     NSAutoreleasePool*  m_pool;
     NSWindow*           m_window;
-    int m_width;
-    int m_height;
     bool m_exitRequested;
     
 };
@@ -294,8 +289,6 @@ void MacOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
     if (m_internalData)
         closeWindow();
 
-    int width = ci.m_width;
-	int height = ci.m_height;
 	const char* windowTitle = ci.m_title;
 	
 
@@ -303,9 +296,7 @@ void MacOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
     NSAutoreleasePool *pool = [[NSAutoreleasePool alloc] init];
     
     m_internalData = new MacOpenGLWindowInternalData;
-    m_internalData->m_width = width;
+  
-    m_internalData->m_height = height;
-    
     m_internalData->m_pool = [NSAutoreleasePool new];
 	m_internalData->m_myApp = [NSApplication sharedApplication];
 	//myApp = [MyApp sharedApplication];
@@ -373,7 +364,7 @@ void MacOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
     [newItem release];
     */
     
-	NSRect frame = NSMakeRect(0., 0., width, height);
+	NSRect frame = NSMakeRect(0., 0., ci.m_width, ci.m_height);
 	
 	m_internalData->m_window = [NSWindow alloc];
 	[m_internalData->m_window initWithContentRect:frame
@@ -423,9 +414,7 @@ void MacOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
     [m_internalData->m_window makeKeyAndOrderFront: nil];
     
     [m_internalData->m_myview MakeCurrent];
-    m_internalData->m_width = m_internalData->m_myview.GetWindowWidth;
+	
-    m_internalData->m_height = m_internalData->m_myview.GetWindowHeight;
-    
     
     [NSApp activateIgnoringOtherApps:YES];
     
@@ -1035,13 +1024,13 @@ void MacOpenGLWindow::startRendering()
     float aspect;
     //b3Vector3 extents;
     
-    if (m_internalData->m_width > m_internalData->m_height)
+    if (getWidth()  > getHeight())
     {
-        aspect = (float)m_internalData->m_width / (float)m_internalData->m_height;
+        aspect = (float)getWidth() / (float)getHeight();
         //extents.setValue(aspect * 1.0f, 1.0f,0);
     } else
     {
-        aspect = (float)m_internalData->m_height / (float)m_internalData->m_width;
+        aspect = (float)getHeight() / (float)getWidth();
         //extents.setValue(1.0f, aspect*1.f,0);
     }
     
@@ -1082,7 +1071,7 @@ int MacOpenGLWindow::fileOpenDialog(char* filename, int maxNameLength)
     NSOpenGLContext *foo = [NSOpenGLContext currentContext];
     // get the url of a .txt file
     NSOpenPanel * zOpenPanel = [NSOpenPanel openPanel];
-	NSArray * zAryOfExtensions = [NSArray arrayWithObjects:@"urdf",@"bullet",@"obj",@"sdf",nil];
+	NSArray * zAryOfExtensions = [NSArray arrayWithObjects:@"urdf",@"bullet",@"obj",@"sdf",@"stl",nil];
     [zOpenPanel setAllowedFileTypes:zAryOfExtensions];
     NSInteger zIntResult = [zOpenPanel runModal];
     
@@ -1136,6 +1125,7 @@ int   MacOpenGLWindow::getWidth() const
 {
     if (m_internalData && m_internalData->m_myview && m_internalData->m_myview.GetWindowWidth)
         return m_internalData->m_myview.GetWindowWidth;
+
     return 0;
 }
 
@@ -1152,7 +1142,7 @@ void MacOpenGLWindow::setResizeCallback(b3ResizeCallback resizeCallback)
     [m_internalData->m_myview setResizeCallback:resizeCallback];
     if (resizeCallback)
     {
-        (resizeCallback)(m_internalData->m_width,m_internalData->m_height);
+		(resizeCallback)(getWidth(), getHeight());
     }
 }
 

examples/RenderingExamples/TinyRendererSetup.cpp

@@ -147,7 +147,9 @@ TinyRendererSetup::TinyRendererSetup(struct GUIHelperInterface* gui)
 	m_internalData = new TinyRendererSetupInternalData(gui->getAppInterface()->m_window->getWidth(),gui->getAppInterface()->m_window->getHeight());
 	
 	m_app->m_renderer->enableBlend(true);
-	const char* fileName = "teddy.obj";//cube.obj";//textured_sphere_smooth.obj";//cube.obj";
+    
+	const char* fileName = "textured_sphere_smooth.obj";
+    fileName = "cube.obj";
 	
 
 	{
@@ -181,7 +183,7 @@ TinyRendererSetup::TinyRendererSetup(struct GUIHelperInterface* gui)
 				m_guiHelper->getRenderInterface()->writeTransforms();
 
 				m_internalData->m_shapePtr.push_back(0);
-				TinyRenderObjectData* ob = new TinyRenderObjectData(m_internalData->m_width,m_internalData->m_height,
+				TinyRenderObjectData* ob = new TinyRenderObjectData(
 					m_internalData->m_rgbColorBuffer,
 					m_internalData->m_depthBuffer);
 					//ob->loadModel("cube.obj");

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -5,6 +5,29 @@
 #include "SharedMemoryCommands.h"
 
 
+b3SharedMemoryCommandHandle	b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName)
+{
+	PhysicsClient* cl = (PhysicsClient* ) physClient;
+    b3Assert(cl);
+    b3Assert(cl->canSubmitCommand());
+    
+	struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+    b3Assert(command);
+	command->m_type = CMD_LOAD_SDF;
+	int len = strlen(sdfFileName);
+	if (len<MAX_SDF_FILENAME_LENGTH)
+	{
+		strcpy(command->m_sdfArguments.m_sdfFileName,sdfFileName);
+	} else
+	{
+		command->m_sdfArguments.m_sdfFileName[0] = 0;
+	}
+	command->m_updateFlags = SDF_ARGS_FILE_NAME;
+	
+	return (b3SharedMemoryCommandHandle) command;
+}
+
+
 
 b3SharedMemoryCommandHandle	b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName)
 {
@@ -146,6 +169,8 @@ int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle,
     struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
     b3Assert(command);
     command->m_sendDesiredStateCommandArgument.m_desiredStateQ[qIndex] = value;
+	command->m_updateFlags |= SIM_DESIRED_STATE_HAS_Q;
+
     return 0;
 }
 
@@ -154,6 +179,7 @@ int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex,
     struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
     b3Assert(command);
     command->m_sendDesiredStateCommandArgument.m_Kp[dofIndex] = value;
+	command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KP;
     return 0;
 }
 
@@ -162,6 +188,8 @@ int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex,
     struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
     b3Assert(command);
     command->m_sendDesiredStateCommandArgument.m_Kd[dofIndex] = value;
+	command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KD;
+
     return 0;
 }
 
@@ -170,6 +198,7 @@ int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle,
     struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
     b3Assert(command);
     command->m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex] = value;
+	command->m_updateFlags |= SIM_DESIRED_STATE_HAS_QDOT;
     return 0;
 }
 
@@ -179,6 +208,7 @@ int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int
     struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
     b3Assert(command);
     command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value;
+	command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
     return 0;
 }
 
@@ -705,6 +735,16 @@ void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle commandHa
 	command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES;
 }
 
+void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle commandHandle, int width, int height )
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command);
+	b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
+	command->m_requestPixelDataArguments.m_pixelWidth = width;
+	command->m_requestPixelDataArguments.m_pixelHeight = height;
+	command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT;	
+}
+
 void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData)
 {
 	PhysicsClient* cl = (PhysicsClient* ) physClient;

examples/SharedMemory/PhysicsClientC_API.h

@@ -68,6 +68,7 @@ void    b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* l
 ///request an image from a simulated camera, using a software renderer.
 b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient);
 void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle command, float viewMatrix[16], float projectionMatrix[16]);
+void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle command, int width, int height );
 void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData);
 
 
@@ -88,6 +89,8 @@ int	b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHand
 int	b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
 int	b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase);
 
+b3SharedMemoryCommandHandle	b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
+
 ///Set joint control variables such as desired position/angle, desired velocity,
 ///applied joint forces, dependent on the control mode (CONTROL_MODE_VELOCITY or CONTROL_MODE_TORQUE)
 b3SharedMemoryCommandHandle  b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode);

examples/SharedMemory/PhysicsClientExample.cpp

@@ -21,8 +21,8 @@ struct MyMotorInfo2
     int     m_qIndex;
 };
 
-int camVisualizerWidth = 640;//1024/3;
+int camVisualizerWidth = 320;//1024/3;
-int camVisualizerHeight = 480;//768/3;
+int camVisualizerHeight = 240;//768/3;
 
 
 #define MAX_NUM_MOTORS 128
@@ -77,10 +77,10 @@ protected:
     
 	virtual void resetCamera()
 	{
-		float dist = 5;
+		float dist = 1.1;
 		float pitch = 50;
 		float yaw = 35;
-		float targetPos[3]={0,0,0};//-3,2.8,-2.5};
+		float targetPos[3]={0,0,0.5};//-3,2.8,-2.5};
 		m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
 
 	}
@@ -156,16 +156,12 @@ protected:
 	{
         for (int i=0;i<m_numMotors;i++)
         {
-            // btScalar targetVel = m_motorTargetVelocities[i].m_velTarget;
-            // int uIndex = m_motorTargetVelocities[i].m_uIndex;
-            // b3JointControlSetDesiredVelocity(commandHandle, uIndex,targetVel);
             
             btScalar targetPos = m_motorTargetPositions[i].m_posTarget;
             int qIndex = m_motorTargetPositions[i].m_qIndex;
             int uIndex = m_motorTargetPositions[i].m_uIndex;
             b3JointControlSetDesiredPosition(commandHandle, qIndex, targetPos);
             b3JointControlSetKp(commandHandle, uIndex, 0.1);
-            b3JointControlSetKd(commandHandle, uIndex, 0.0);
             
             b3JointControlSetMaximumForce(commandHandle,uIndex,1000);
         }
@@ -252,7 +248,8 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
 			this->m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix);
 			this->m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraViewMatrix(viewMatrix);
             b3RequestCameraImageSetCameraMatrices(commandHandle, viewMatrix,projectionMatrix);
-            b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
+			b3RequestCameraImageSetPixelResolution(commandHandle, camVisualizerWidth,camVisualizerHeight);
+			b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
             break;
         }
         case CMD_CREATE_BOX_COLLISION_SHAPE:
@@ -430,6 +427,11 @@ PhysicsClientExample::~PhysicsClientExample()
 		bool deInitializeSharedMemory = true;
 		m_physicsServer.disconnectSharedMemory(deInitializeSharedMemory);
 	}
+	
+	if (m_canvas && (m_canvasIndex>=0))
+	{
+		m_canvas->destroyCanvas(m_canvasIndex);
+	}
     b3Printf("~PhysicsClientExample\n");
 }
 
@@ -527,7 +529,6 @@ void	PhysicsClientExample::initPhysics()
 	m_selectedBody = -1;
 	m_prevSelectedBody = -1;
 
-	if (m_options == eCLIENTEXAMPLE_SERVER)
 	{
 		m_canvas = m_guiHelper->get2dCanvasInterface();
 		if (m_canvas)
@@ -557,13 +558,22 @@ void	PhysicsClientExample::initPhysics()
 			
 		}
 
-		m_isOptionalServerConnected = m_physicsServer.connectSharedMemory( m_guiHelper);
 	}
 
-    m_physicsClientHandle  = b3ConnectSharedMemory(m_sharedMemoryKey);
+    if (m_options == eCLIENTEXAMPLE_SERVER)
-	//m_physicsClientHandle  = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY);
+    {
-	//m_physicsClientHandle = b3ConnectPhysicsDirect();
+        m_isOptionalServerConnected = m_physicsServer.connectSharedMemory( m_guiHelper);
-
+    }
+    
+	if (m_options == eCLIENTEXAMPLE_DIRECT)
+	{
+		m_physicsClientHandle = b3ConnectPhysicsDirect();
+	} else
+	{
+	    m_physicsClientHandle  = b3ConnectSharedMemory(m_sharedMemoryKey);
+		//m_physicsClientHandle  = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY);
+	}
+	
     if (!b3CanSubmitCommand(m_physicsClientHandle))
     {
 		b3Warning("Cannot connect to physics client");
@@ -603,25 +613,25 @@ void	PhysicsClientExample::stepSimulation(float deltaTime)
 			}
 			if (statusType ==CMD_CAMERA_IMAGE_COMPLETED)
             {
-				static int counter=0;
+			//	static int counter=0;
-				char msg[1024];
+			//	char msg[1024];
-				sprintf(msg,"Camera image %d OK\n",counter++);
+			//	sprintf(msg,"Camera image %d OK\n",counter++);
 				b3CameraImageData imageData;
 				b3GetCameraImageData(m_physicsClientHandle,&imageData);
 				if (m_canvas && m_canvasIndex >=0)
 				{
-					for (int i=0;i<imageData.m_pixelWidth;i++)
+					for (int i=0;i<camVisualizerWidth;i++)
 					{
-						for (int j=0;j<imageData.m_pixelHeight;j++)
+						for (int j=0;j<camVisualizerHeight;j++)
 						{
-                            int xIndex = int(float(i)*(float(camVisualizerWidth)/float(imageData.m_pixelWidth)));
+                            int xIndex = int(float(i)*(float(imageData.m_pixelWidth)/float(camVisualizerWidth)));
-                            int yIndex = int(float(j)*(float(camVisualizerHeight)/float(imageData.m_pixelHeight)));
+                            int yIndex = int(float(j)*(float(imageData.m_pixelHeight)/float(camVisualizerHeight)));
 							btClamp(yIndex,0,imageData.m_pixelHeight);
 							btClamp(xIndex,0,imageData.m_pixelWidth);
 							int bytesPerPixel = 4;
 							
-							int pixelIndex = (i+j*imageData.m_pixelWidth)*bytesPerPixel;
+							int pixelIndex = (xIndex+yIndex*imageData.m_pixelWidth)*bytesPerPixel;
-							m_canvas->setPixel(m_canvasIndex,xIndex,camVisualizerHeight-1-yIndex,
+							m_canvas->setPixel(m_canvasIndex,i,camVisualizerHeight-1-j,
                                                
 									imageData.m_rgbColorData[pixelIndex],
 									imageData.m_rgbColorData[pixelIndex+1],
@@ -633,11 +643,11 @@ void	PhysicsClientExample::stepSimulation(float deltaTime)
 					m_canvas->refreshImageData(m_canvasIndex);
 				}
 
-                b3Printf(msg);
+               // b3Printf(msg);
             } 
             if (statusType == CMD_CAMERA_IMAGE_FAILED)
             {
-                b3Printf("Camera image FAILED\n");
+                b3Warning("Camera image FAILED\n");
             }
        
         
@@ -707,8 +717,8 @@ void	PhysicsClientExample::stepSimulation(float deltaTime)
 						bool hasStatus = (status != 0);
 						if (hasStatus)
 						{
-							int statusType = b3GetStatusType(status);
+							//int statusType = b3GetStatusType(status);
-							b3Printf("Status after reset: %d",statusType);
+							//b3Printf("Status after reset: %d",statusType);
 						}
 					}
 				} else

examples/SharedMemory/PhysicsClientExample.h

@@ -4,7 +4,7 @@
 enum ClientExampleOptions
 {
 	eCLIENTEXAMPLE_LOOPBACK=1,
-	eCLIENTEAXMPLE_DIRECT=2,
+	eCLIENTEXAMPLE_DIRECT=2,
 	eCLIENTEXAMPLE_SERVER=3,
 };
 

examples/SharedMemory/PhysicsClientSharedMemory.cpp

@@ -448,8 +448,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
                 
 				unsigned char* rgbaPixelsReceived =
                     (unsigned char*)&m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0];
-                printf("pixel = %d\n", rgbaPixelsReceived[0]);
+              //  printf("pixel = %d\n", rgbaPixelsReceived[0]);
                 
+				float* depthBuffer = (float*)&(m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*4]);
+			
+				for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied;i++)
+				{
+					m_data->m_cachedCameraDepthBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = depthBuffer[i];
+				}
+				
 				for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*numBytesPerPixel;i++)
 				{
 					m_data->m_cachedCameraPixelsRGBA[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex*numBytesPerPixel] 
@@ -461,7 +468,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
             
             case CMD_CAMERA_IMAGE_FAILED:
             {
-                b3Printf("Camera image FAILED\n");
+                b3Warning("Camera image FAILED\n");
                 break;
             }
 

examples/SharedMemory/PhysicsDirect.cpp

@@ -33,6 +33,12 @@ struct PhysicsDirectInternalData
 	
 	char    m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE];
 
+	int m_cachedCameraPixelsWidth;
+	int m_cachedCameraPixelsHeight;
+	btAlignedObjectArray<unsigned char> m_cachedCameraPixelsRGBA;
+	btAlignedObjectArray<float> m_cachedCameraDepthBuffer;
+
+
 	PhysicsServerCommandProcessor* m_commandProcessor;
 
 	PhysicsDirectInternalData()
@@ -167,6 +173,81 @@ bool PhysicsDirect::processDebugLines(const struct SharedMemoryCommand& orgComma
 	return m_data->m_hasStatus;
 }
 
+bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand)
+{
+	SharedMemoryCommand command = orgCommand;
+
+	const SharedMemoryStatus& serverCmd = m_data->m_serverStatus;
+
+	do
+	{
+
+		bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
+		m_data->m_hasStatus = hasStatus;
+		if (hasStatus)
+		{
+			btAssert(m_data->m_serverStatus.m_type == CMD_CAMERA_IMAGE_COMPLETED);
+
+			if (m_data->m_verboseOutput) 
+			{
+				b3Printf("Camera image OK\n");
+			}
+
+			int numBytesPerPixel = 4;//RGBA
+			int numTotalPixels = serverCmd.m_sendPixelDataArguments.m_startingPixelIndex+
+				serverCmd.m_sendPixelDataArguments.m_numPixelsCopied+
+				serverCmd.m_sendPixelDataArguments.m_numRemainingPixels;
+
+			m_data->m_cachedCameraPixelsWidth = 0;
+			m_data->m_cachedCameraPixelsHeight = 0;
+
+            int numPixels = serverCmd.m_sendPixelDataArguments.m_imageWidth*serverCmd.m_sendPixelDataArguments.m_imageHeight;
+
+            m_data->m_cachedCameraPixelsRGBA.reserve(numPixels*numBytesPerPixel);
+			m_data->m_cachedCameraDepthBuffer.resize(numTotalPixels);
+			m_data->m_cachedCameraPixelsRGBA.resize(numTotalPixels*numBytesPerPixel);
+                
+                
+			unsigned char* rgbaPixelsReceived =
+                (unsigned char*)&m_data->m_bulletStreamDataServerToClient[0];
+			
+			float* depthBuffer = (float*)&(m_data->m_bulletStreamDataServerToClient[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*4]);
+			
+          //  printf("pixel = %d\n", rgbaPixelsReceived[0]);
+                
+			for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied;i++)
+			{
+				m_data->m_cachedCameraDepthBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = depthBuffer[i];
+			}
+			for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*numBytesPerPixel;i++)
+			{
+				m_data->m_cachedCameraPixelsRGBA[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex*numBytesPerPixel] 
+					= rgbaPixelsReceived[i];
+			}
+
+			if (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0)
+			{
+				
+
+				// continue requesting remaining pixels
+				command.m_type = CMD_REQUEST_CAMERA_IMAGE_DATA;
+				command.m_requestPixelDataArguments.m_startPixelIndex = 
+					serverCmd.m_sendPixelDataArguments.m_startingPixelIndex + 
+					serverCmd.m_sendPixelDataArguments.m_numPixelsCopied;
+				
+			} else
+			{
+				m_data->m_cachedCameraPixelsWidth = serverCmd.m_sendPixelDataArguments.m_imageWidth;
+				m_data->m_cachedCameraPixelsHeight = serverCmd.m_sendPixelDataArguments.m_imageHeight;
+			}	
+		}
+	}  while (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0);
+	
+	return m_data->m_hasStatus;
+
+
+}
+
 bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& command)
 {
 	if (command.m_type==CMD_REQUEST_DEBUG_LINES)
@@ -174,6 +255,11 @@ bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& comman
 		return processDebugLines(command);
 	}
 
+	if (command.m_type==CMD_REQUEST_CAMERA_IMAGE_DATA)
+	{
+		return processCamera(command);
+	}
+
 	bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
 	m_data->m_hasStatus = hasStatus;
 	if (hasStatus)
@@ -333,9 +419,9 @@ void PhysicsDirect::getCachedCameraImage(b3CameraImageData* cameraData)
 {
 	if (cameraData)
 	{
-		cameraData->m_pixelHeight = 0;
+		cameraData->m_pixelWidth = m_data->m_cachedCameraPixelsWidth;
-		cameraData->m_pixelWidth = 0;
+		cameraData->m_pixelHeight = m_data->m_cachedCameraPixelsHeight;
-		cameraData->m_depthValues = 0;
+		cameraData->m_depthValues = m_data->m_cachedCameraDepthBuffer.size() ? &m_data->m_cachedCameraDepthBuffer[0] : 0;
-		cameraData->m_rgbColorData = 0;
+		cameraData->m_rgbColorData = m_data->m_cachedCameraPixelsRGBA.size() ? &m_data->m_cachedCameraPixelsRGBA[0] : 0;
 	}
 }

examples/SharedMemory/PhysicsDirect.h

@@ -19,6 +19,8 @@ protected:
 
 	bool processDebugLines(const struct SharedMemoryCommand& orgCommand);
 
+	bool processCamera(const struct SharedMemoryCommand& orgCommand);
+
 public:
 
     PhysicsDirect();

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -972,6 +972,14 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                     int width, height;
                     int numPixelsCopied = 0;
                                       
+					if (
+						(clientCmd.m_requestPixelDataArguments.m_startPixelIndex==0) && 
+						(clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT)!=0)
+					{
+						m_data->m_visualConverter.setWidthAndHeight(clientCmd.m_requestPixelDataArguments.m_pixelWidth,
+																	clientCmd.m_requestPixelDataArguments.m_pixelHeight);
+					}		
+					
 					if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_USE_HARDWARE_OPENGL)!=0)
 					{
 						m_data->m_guiHelper->copyCameraImageData(0,0,0,0,0,&width,&height,0);
@@ -1003,8 +1011,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                             
                             if (clientCmd.m_requestPixelDataArguments.m_startPixelIndex==0)
                             {
-                                printf("-------------------------------\nRendering\n");
+                             //   printf("-------------------------------\nRendering\n");
                                 
+																	
                                 if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES)!=0)
                                 {
                                     m_data->m_visualConverter.render(
@@ -1192,12 +1201,18 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                                         mb->clearForcesAndTorques();
                                         
                                         int torqueIndex = 0;
-										btVector3 f(clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[0],
+										btVector3 f(0,0,0);
-                                                    clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[1],
+										btVector3 t(0,0,0);
-                                                    clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[2]);
+
-                                        btVector3 t(clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[3],
+										if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
-                                                    clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[4],
+										{
-                                                    clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[5]);
+											f = btVector3 (clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[0],
+														clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[1],
+														clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[2]);
+											t = btVector3 (clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[3],
+														clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[4],
+														clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[5]);
+										}
                                         torqueIndex+=6;
                                         mb->addBaseForce(f);
                                         mb->addBaseTorque(t);
@@ -1206,7 +1221,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                                             
                                             for (int dof=0;dof<mb->getLink(link).m_dofCount;dof++)
                                             {               
-                                                double torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex];
+                                                double torque = 0.f;
+												if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
+													torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex];
                                                 mb->addJointTorqueMultiDof(link,dof,torque);
                                                 torqueIndex++;
                                             }
@@ -1233,10 +1250,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 													if (motorPtr)
 													{
 														btMultiBodyJointMotor* motor = *motorPtr;
-														btScalar desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex];
+														btScalar desiredVelocity = 0.f;
+														if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_QDOT)!=0)
+															desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex];
 														motor->setVelocityTarget(desiredVelocity);
 
-														btScalar maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*m_data->m_physicsDeltaTime;
+														btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime;
+														if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
+															maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*m_data->m_physicsDeltaTime;
+														
 														motor->setMaxAppliedImpulse(maxImp);
 														numMotors++;
 
@@ -1247,6 +1269,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 										}
 										break;
 									}
+
 								case CONTROL_MODE_POSITION_VELOCITY_PD:
 									{
 										if (m_data->m_verboseOutput)
@@ -1271,11 +1294,19 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 													{
 														btMultiBodyJointMotor* motor = *motorPtr;
 													
-                                                        btScalar desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex];
+                                                        btScalar desiredVelocity = 0.f;
-                                                        btScalar desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex];
+														if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_QDOT)!=0)
-                                                        
+															desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex];
-                                                        btScalar kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex];
+														btScalar desiredPosition = 0.f;
-                                                        btScalar kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex];
+														if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_Q)!=0)
+															desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex];
+
+                                                        btScalar kp = 0.f;
+														if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_KP)!=0)
+															kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex];
+														btScalar kd = 0.f;
+														if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_KD)!=0)
+															kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex];
 
                                                         int dof1 = 0;
                                                         btScalar currentPosition = mb->getJointPosMultiDof(link)[dof1];
@@ -1288,9 +1319,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                                                         
                                                         motor->setVelocityTarget(desiredVelocity);
                                                         
-                                                        btScalar maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex]*m_data->m_physicsDeltaTime;
+                                                        btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime;
+
+														if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
+															maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex]*m_data->m_physicsDeltaTime;
                                                         
-                                                        motor->setMaxAppliedImpulse(1000);//maxImp);
+                                                        motor->setMaxAppliedImpulse(maxImp);
                                                         numMotors++;
                                                     }
 

examples/SharedMemory/SharedMemoryCommands.h

@@ -30,6 +30,7 @@
 #define MAX_DEGREE_OF_FREEDOM 64
 #define MAX_NUM_SENSORS 256
 #define MAX_URDF_FILENAME_LENGTH 1024
+#define MAX_SDF_FILENAME_LENGTH 1024
 #define MAX_FILENAME_LENGTH MAX_URDF_FILENAME_LENGTH
 #define MAX_NUM_LINKS MAX_DEGREE_OF_FREEDOM
 
@@ -54,6 +55,16 @@ TmpFloat3 CreateTmpFloat3(float x, float y, float z)
     return tmp;
 }
 
+enum EnumSdfArgsUpdateFlags
+{
+	SDF_ARGS_FILE_NAME=1,
+};
+
+struct SdfArgs
+{
+	char m_sdfFileName[MAX_URDF_FILENAME_LENGTH];
+};
+
 enum EnumUrdfArgsUpdateFlags
 {
 	URDF_ARGS_FILE_NAME=1,
@@ -118,12 +129,16 @@ struct RequestPixelDataArgs
 	float m_viewMatrix[16];
 	float m_projectionMatrix[16];
 	int m_startPixelIndex;
+	int m_pixelWidth;
+	int m_pixelHeight;
 };
 
 enum EnumRequestPixelDataUpdateFlags
 {
 	REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES=1,
 	REQUEST_PIXEL_ARGS_USE_HARDWARE_OPENGL=2,
+	REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT=4,
+	
 };
 
 
@@ -179,6 +194,15 @@ struct SendDesiredStateArgs
     
 };
 
+enum EnumSimDesiredStateUpdateFlags
+{
+	SIM_DESIRED_STATE_HAS_Q=1,
+	SIM_DESIRED_STATE_HAS_QDOT=2,
+	SIM_DESIRED_STATE_HAS_KD=4,
+	SIM_DESIRED_STATE_HAS_KP=8,
+	SIM_DESIRED_STATE_HAS_MAX_FORCE=16,
+};
+
 
 enum EnumSimParamUpdateFlags
 {
@@ -287,6 +311,7 @@ struct SharedMemoryCommand
     union
     {
         struct UrdfArgs m_urdfArguments;
+		struct SdfArgs m_sdfArguments;
 		struct InitPoseArgs m_initPoseArgs;
 		struct SendPhysicsSimulationParameters m_physSimParamArgs;
 		struct BulletDataStreamArgs	m_dataStreamArguments;

examples/SharedMemory/SharedMemoryPublic.h

@@ -5,7 +5,8 @@
 
 enum EnumSharedMemoryClientCommand
 {
-    CMD_LOAD_URDF,
+    CMD_LOAD_SDF,
+	CMD_LOAD_URDF,
         CMD_SEND_BULLET_DATA_STREAM,
         CMD_CREATE_BOX_COLLISION_SHAPE,
 //      CMD_DELETE_BOX_COLLISION_SHAPE,
@@ -35,7 +36,8 @@ enum EnumSharedMemoryServerStatus
         CMD_CLIENT_COMMAND_COMPLETED,
         //the server will skip unknown command and report a status 'CMD_UNKNOWN_COMMAND_FLUSHED'
         CMD_UNKNOWN_COMMAND_FLUSHED,
-
+		CMD_SDF_LOADING_COMPLETED,
+        CMD_SDF_LOADING_FAILED,
         CMD_URDF_LOADING_COMPLETED,
         CMD_URDF_LOADING_FAILED,
         CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED,

examples/SharedMemory/TinyRendererVisualShapeConverter.cpp

@@ -462,7 +462,7 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const
 
             if (vertices.size() && indices.size())
             {
-                TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_swWidth,m_data->m_swHeight,m_data->m_rgbColorBuffer,m_data->m_depthBuffer);
+                TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer);
                 tinyObj->registerMeshShape(&vertices[0].xyzw[0],vertices.size(),&indices[0],indices.size(),rgbaColor);
                 visuals->m_renderObjects.push_back(tinyObj);
             }
@@ -541,7 +541,7 @@ void TinyRendererVisualShapeConverter::render(const float viewMat[16], const flo
     
     lightDirWorld.normalize();
     
-    printf("num m_swRenderInstances = %d\n", m_data->m_swRenderInstances.size());
+  //  printf("num m_swRenderInstances = %d\n", m_data->m_swRenderInstances.size());
     for (int i=0;i<m_data->m_swRenderInstances.size();i++)
     {
         TinyRendererObjectArray** visualArrayPtr = m_data->m_swRenderInstances.getAtIndex(i);
@@ -590,6 +590,17 @@ void TinyRendererVisualShapeConverter::getWidthAndHeight(int& width, int& height
     height = m_data->m_swHeight;
 }
 
+
+void TinyRendererVisualShapeConverter::setWidthAndHeight(int width, int height)
+{
+	m_data->m_swWidth = width;
+	m_data->m_swHeight = height;
+
+	m_data->m_depthBuffer.resize(m_data->m_swWidth*m_data->m_swHeight);
+	m_data->m_rgbColorBuffer = TGAImage(width, height, TGAImage::RGB);
+		
+}
+
 void TinyRendererVisualShapeConverter::copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied)
 {
     int w = m_data->m_rgbColorBuffer.get_width();
@@ -612,6 +623,10 @@ void TinyRendererVisualShapeConverter::copyCameraImageData(unsigned char* pixels
     {
         for (int i=0;i<numRequestedPixels;i++)
         {
+			if (depthBuffer)
+			{
+				depthBuffer[i] = m_data->m_depthBuffer[i+startPixelIndex];
+			}
             if (pixelsRGBA)
             {
                 pixelsRGBA[i*numBytesPerPixel] =   m_data->m_rgbColorBuffer.buffer()[(i+startPixelIndex)*3+0];

examples/SharedMemory/TinyRendererVisualShapeConverter.h

@@ -24,6 +24,7 @@ struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter
 	void resetAll();
 
     void getWidthAndHeight(int& width, int& height);
+	void setWidthAndHeight(int width, int height);
     
     void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied);
     

examples/SimpleOpenGL3/main.cpp

@@ -14,8 +14,8 @@ static b3MouseButtonCallback sOldMouseButtonCB = 0;
 static b3KeyboardCallback sOldKeyboardCB = 0;
 //static b3RenderCallback sOldRenderCB = 0;
 
-float gWidth = 0 ;
+float gWidth = 1024;
-float gHeight = 0;
+float gHeight = 768;
 
 void MyWheelCallback(float deltax, float deltay)
 {
@@ -61,7 +61,7 @@ int main(int argc, char* argv[])
     b3CommandLineArgs myArgs(argc,argv);
 
 
-	SimpleOpenGL3App* app = new SimpleOpenGL3App("SimpleOpenGL3App",1024,768,true);
+	SimpleOpenGL3App* app = new SimpleOpenGL3App("SimpleOpenGL3App",gWidth,gHeight,true);
 	
 	app->m_instancingRenderer->getActiveCamera()->setCameraDistance(13);
 	app->m_instancingRenderer->getActiveCamera()->setCameraPitch(0);

examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp

@@ -110,8 +110,9 @@ public:
    	    int shapeIndex = OpenGLGuiHelper::registerGraphicsShape(vertices,numvertices,indices,numIndices);
    	    if (shapeIndex>=0)
         {
-            TinyRenderObjectData* swObj = new TinyRenderObjectData(m_swWidth,m_swHeight,m_rgbColorBuffer,m_depthBuffer);
+            TinyRenderObjectData* swObj = new TinyRenderObjectData(m_rgbColorBuffer,m_depthBuffer);
-            swObj->registerMeshShape(vertices,numvertices,indices,numIndices);
+        float rgbaColor[4] = {1,1,1,1};    
+	swObj->registerMeshShape(vertices,numvertices,indices,numIndices,rgbaColor);
 			//swObj->createCube(1,1,1);//MeshShape(vertices,numvertices,indices,numIndices);
             m_swRenderObjects.insert(shapeIndex,swObj);
         }

examples/StandaloneMain/main_tinyrenderer_single_example.cpp

@@ -253,8 +253,9 @@ struct TinyRendererGUIHelper : public GUIHelperInterface
 	{
 		int shapeIndex = m_swRenderObjects.size();
 
-		TinyRenderObjectData* swObj = new TinyRenderObjectData(m_swWidth,m_swHeight,m_rgbColorBuffer,m_depthBuffer);
+		TinyRenderObjectData* swObj = new TinyRenderObjectData(m_rgbColorBuffer,m_depthBuffer);
-        swObj->registerMeshShape(vertices,numvertices,indices,numIndices);
+        float rgbaColor[4] = {1,1,1,1};
+	swObj->registerMeshShape(vertices,numvertices,indices,numIndices,rgbaColor);
 		//swObj->createCube(1,1,1);//MeshShape(vertices,numvertices,indices,numIndices);
         m_swRenderObjects.insert(shapeIndex,swObj);
 		return shapeIndex;

examples/TinyRenderer/TinyRenderer.cpp

@@ -76,21 +76,20 @@ struct Shader : public IShader {
 		float diff = ambient+b3Min(b3Max(0.f, bn*m_light_dir_local),(1-ambient));
 		//float diff = b3Max(0.f, n*m_light_dir_local);
         color = m_model->diffuse(uv)*diff;
-        //colors are store in BGRA?
+        
-        color = TGAColor(color[0]*m_colorRGBA[2],
+        //warning: bgra color is swapped to rgba to upload texture
-                            color[1]*m_colorRGBA[1],
+        color.bgra[0] *= m_colorRGBA[0];
-                            color[2]*m_colorRGBA[0],
+        color.bgra[1] *= m_colorRGBA[1];
-                            color[3]*m_colorRGBA[3]);
+        color.bgra[2] *= m_colorRGBA[2];
-
+        color.bgra[3] *= m_colorRGBA[3];
+        
         return false;
     }
 };
 
 
-TinyRenderObjectData::TinyRenderObjectData(int width, int height,TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer)
+TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer)
-:m_width(width),
+:m_rgbColorBuffer(rgbColorBuffer),
-m_height(height),
-m_rgbColorBuffer(rgbColorBuffer),
 m_depthBuffer(depthBuffer),
 m_model(0),
 m_userData(0),
@@ -102,11 +101,6 @@ m_userIndex(-1)
     m_lightDirWorld.setValue(0,0,0);
 	m_localScaling.setValue(1,1,1);
     m_modelMatrix = Matrix::identity();
-    m_viewMatrix = lookat(eye, center, up);
-    //m_viewportMatrix = viewport(width/8, height/8, width*3/4, height*3/4);
-	//m_viewportMatrix = viewport(width/8, height/8, width*3/4, height*3/4);
-	m_viewportMatrix = viewport(0,0,width,height);
-    m_projectionMatrix = projection(-1.f/(eye-center).norm());
  
 }
 
@@ -239,6 +233,9 @@ TinyRenderObjectData::~TinyRenderObjectData()
 
 void TinyRenderer::renderObject(TinyRenderObjectData& renderData)
 {
+	int width = renderData.m_rgbColorBuffer.get_width();
+	int height = renderData.m_rgbColorBuffer.get_height();
+
 	Vec3f light_dir_local = Vec3f(renderData.m_lightDirWorld[0],renderData.m_lightDirWorld[1],renderData.m_lightDirWorld[2]);
     Model* model = renderData.m_model;
     if (0==model)
@@ -246,13 +243,8 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData)
     
 	
 
-	//renderData.m_viewMatrix = lookat(eye, center, up);
+	renderData.m_viewportMatrix = viewport(0,0,width, height);
-	int width = renderData.m_width;
+	
-	int height = renderData.m_height;
-	//renderData.m_viewportMatrix = viewport(width/8, height/8, width*3/4, height*3/4);
-	renderData.m_viewportMatrix = viewport(0,0,renderData.m_width,renderData.m_height);
-    //renderData.m_projectionMatrix = projection(-1.f/(eye-center).norm());
-
     b3AlignedObjectArray<float>& zbuffer = renderData.m_depthBuffer;
     
     TGAImage& frame = renderData.m_rgbColorBuffer;
@@ -264,7 +256,7 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData)
         Vec3f localScaling(renderData.m_localScaling[0],renderData.m_localScaling[1],renderData.m_localScaling[2]);
         Shader shader(model, light_dir_local, modelViewMatrix, renderData.m_projectionMatrix,renderData.m_modelMatrix, localScaling, model->getColorRGBA());
         		
-		printf("Render %d triangles.\n",model->nfaces());
+		//printf("Render %d triangles.\n",model->nfaces());
 		for (int i=0; i<model->nfaces(); i++) 
 		{
 			 

examples/TinyRenderer/TinyRenderer.h

@@ -25,12 +25,11 @@ struct TinyRenderObjectData
     //class IShader* m_shader; todo(erwincoumans) expose the shader, for now we use a default shader
             
     //Output
-    int m_width;
+    
-    int m_height;
     TGAImage& m_rgbColorBuffer;
     b3AlignedObjectArray<float>& m_depthBuffer;
     
-    TinyRenderObjectData(int width, int height,TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>& depthBuffer);
+    TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>& depthBuffer);
     virtual ~TinyRenderObjectData();
     
     void loadModel(const char* fileName);

examples/TinyRenderer/main.cpp

@@ -86,7 +86,7 @@ int main(int argc, char* argv[])
         b3AlignedObjectArray<float> depthBuffer;
 	depthBuffer.resize(gWidth*gHeight);
  
-	TinyRenderObjectData renderData(textureWidth, textureHeight,rgbColorBuffer,depthBuffer);//, "african_head/african_head.obj");//floor.obj");
+	TinyRenderObjectData renderData(rgbColorBuffer,depthBuffer);//, "african_head/african_head.obj");//floor.obj");
 	
 	//renderData.loadModel("african_head/african_head.obj");
 	renderData.loadModel("floor.obj");

examples/TinyRenderer/model.cpp

@@ -57,9 +57,9 @@ void Model::setDiffuseTextureFromData(unsigned char* textureImage,int textureWid
 		for (int j=0;j<textureHeight;j++)
 		{
 			TGAColor color;
-			color.bgra[0] = textureImage[(i+j*textureWidth)*3+0];
+            color.bgra[0] = textureImage[(i+j*textureWidth)*3+0];
-			color.bgra[1] = textureImage[(i+j*textureWidth)*3+1];
+            color.bgra[1] = textureImage[(i+j*textureWidth)*3+1];
-			color.bgra[2] = textureImage[(i+j*textureWidth)*3+2];
+            color.bgra[2] = textureImage[(i+j*textureWidth)*3+2];
 			color.bgra[3] = 255;
 
 			color.bytespp = 3;

examples/pybullet/CMakeLists.txt

@@ -9,6 +9,15 @@ INCLUDE_DIRECTORIES(
 SET(pybullet_SRCS
 	pybullet.c
 	../../examples/ExampleBrowser/InProcessExampleBrowser.cpp
+      ../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
+                                                                                                ../../examples/SharedMemory/TinyRendererVisualShapeConverter.h
+                                                                                                ../../examples/OpenGLWindow/SimpleCamera.cpp
+                                                                                                ../../examples/OpenGLWindow/SimpleCamera.h
+                                                                                                ../../examples/TinyRenderer/geometry.cpp
+                                                                                                ../../examples/TinyRenderer/model.cpp
+                                                                                                ../../examples/TinyRenderer/tgaimage.cpp
+                                                                                                ../../examples/TinyRenderer/our_gl.cpp
+                                                                                                ../../examples/TinyRenderer/TinyRenderer.cpp
                         ../../examples/SharedMemory/InProcessMemory.cpp
                         ../../examples/SharedMemory/PhysicsClient.cpp
                         ../../examples/SharedMemory/PhysicsClient.h
@@ -48,7 +57,6 @@ SET(pybullet_SRCS
                         ../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
                         ../../examples/Importers/ImportURDFDemo/UrdfParser.cpp
                         ../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp
-			../../examples/Importers/ImportURDFDemo/DefaultVisualShapeConverter.cpp
                         ../../examples/MultiThreading/b3PosixThreadSupport.cpp
                         ../../examples/MultiThreading/b3Win32ThreadSupport.cpp
 			../../examples/MultiThreading/b3ThreadSupportInterface.cpp

examples/pybullet/pybullet.c

@@ -134,7 +134,7 @@ pybullet_loadURDF(PyObject* self, PyObject* args)
 	float startOrnY = 0;
 	float startOrnZ = 0;
 	float startOrnW = 1;
-	printf("size=%d\n", size);
+	//printf("size=%d\n", size);
 	if (0==sm)
     {
         PyErr_SetString(SpamError, "Not connected to physics server.");
@@ -164,7 +164,7 @@ pybullet_loadURDF(PyObject* self, PyObject* args)
         b3SharedMemoryStatusHandle statusHandle;
         int statusType;
         b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(sm, urdfFileName);
-		printf("urdf filename = %s\n", urdfFileName);
+		//printf("urdf filename = %s\n", urdfFileName);
         //setting the initial position, orientation and other arguments are optional
         b3LoadUrdfCommandSetStartPosition(command, startPosX,startPosY,startPosZ);
         statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
@@ -359,6 +359,176 @@ pybullet_getNumJoints(PyObject* self, PyObject* args)
 	}
 }
 
+static PyObject*
+pybullet_setJointPositions(PyObject* self, PyObject* args)
+{
+	if (0==sm)
+	{
+		PyErr_SetString(SpamError, "Not connected to physics server.");
+		return NULL;
+	}
+	
+	Py_INCREF(Py_None);
+        return Py_None;
+}
+
+       // const unsigned char* m_rgbColorData;//3*m_pixelWidth*m_pixelHeight bytes
+       // const float* m_depthValues;//m_pixelWidth*m_pixelHeight floats
+
+static PyObject* pybullet_renderImage(PyObject* self, PyObject* args)
+{
+	if (0==sm)
+	{
+		PyErr_SetString(SpamError, "Not connected to physics server.");
+		return NULL;
+	}
+	
+	///request an image from a simulated camera, using a software renderer.
+	struct b3CameraImageData imageData;
+	PyObject* objViewMat,* objProjMat;
+	int width,  height;
+	
+	if (PyArg_ParseTuple(args, "iiOO", &width, &height, &objViewMat, &objProjMat))
+	{
+	
+		PyObject* seq;
+		int i, len;
+		PyObject* item;
+		float viewMatrix[16];
+		float projectionMatrix[16];
+		int valid = 1;
+		{
+    		seq = PySequence_Fast(objViewMat, "expected a sequence");
+    		len = PySequence_Size(objViewMat);
+		//printf("objViewMat size = %d\n", len);
+			if (len==16)
+			{
+				
+				if (PyList_Check(seq))
+				{
+					for (i = 0; i < len; i++) 
+					{
+            			item = PyList_GET_ITEM(seq, i);
+						viewMatrix[i] = PyFloat_AsDouble(item);
+						float v = viewMatrix[i]; 
+	 			//printf("view %d to %f\n", i,v);
+	
+					}
+				}
+				else
+				{
+					for (i = 0; i < len; i++)
+					{
+        			item = PyTuple_GET_ITEM(seq,i);
+						viewMatrix[i] = PyFloat_AsDouble(item);
+					}
+				}
+			} else
+			{
+				valid = 0;
+			}
+		}
+		
+		
+		{
+			seq = PySequence_Fast(objProjMat, "expected a sequence");
+			len = PySequence_Size(objProjMat);
+	//printf("projMat len = %d\n", len);
+			if (len==16)
+			{
+				if (PyList_Check(seq))
+				{
+					for (i = 0; i < len; i++)
+					{
+							item = PyList_GET_ITEM(seq, i);
+							projectionMatrix[i] = PyFloat_AsDouble(item);
+					}
+				}
+				else
+				{
+					for (i = 0; i < len; i++)
+					{
+							item = PyTuple_GET_ITEM(seq,i);
+							projectionMatrix[i] = PyFloat_AsDouble(item);
+					}
+				}
+			} else
+			{
+					valid = 0;
+			}
+		}
+
+    		Py_DECREF(seq);	
+		if (valid)
+		{
+			b3SharedMemoryCommandHandle command;
+			
+			command = b3InitRequestCameraImage(sm);
+
+			//printf("set b3RequestCameraImageSetCameraMatrices\n");
+			b3RequestCameraImageSetCameraMatrices(command, viewMatrix, projectionMatrix);
+			
+			b3RequestCameraImageSetPixelResolution(command,width,height);
+		
+			if (b3CanSubmitCommand(sm))
+			{
+				b3SharedMemoryStatusHandle statusHandle;
+				int statusType;
+				statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+				statusType = b3GetStatusType(statusHandle);
+				if (statusType==CMD_CAMERA_IMAGE_COMPLETED)
+				{
+					PyObject *item2;
+					PyObject* pyResultList;//store 4 elements in this result: width, height, rgbData, depth
+
+					b3GetCameraImageData(sm, &imageData);
+					//todo: error handling if image size is 0
+					pyResultList =  PyTuple_New(4);
+					PyTuple_SetItem(pyResultList, 0, PyInt_FromLong(imageData.m_pixelWidth));
+					PyTuple_SetItem(pyResultList, 1, PyInt_FromLong(imageData.m_pixelHeight));	
+			
+					PyObject *pylistPos;
+					PyObject* pylistDep;
+
+						//printf("image width = %d, height = %d\n", imageData.m_pixelWidth, imageData.m_pixelHeight);
+						{
+
+							PyObject *item;
+						int bytesPerPixel = 3;//Red, Green, Blue, each 8 bit values
+							int num=bytesPerPixel*imageData.m_pixelWidth*imageData.m_pixelHeight;
+							pylistPos = PyTuple_New(num);
+						pylistDep = PyTuple_New(imageData.m_pixelWidth*imageData.m_pixelHeight);
+			
+						for (int i=0;i<imageData.m_pixelWidth;i++)
+						{
+							for (int j=0;j<imageData.m_pixelHeight;j++)
+							{
+								int depIndex = i+j*imageData.m_pixelWidth;
+								item = PyFloat_FromDouble(imageData.m_depthValues[depIndex]);
+								PyTuple_SetItem(pylistDep, depIndex, item);
+								for (int p=0;p<bytesPerPixel;p++)
+								{
+									int pixelIndex = bytesPerPixel*(i+j*imageData.m_pixelWidth)+p;
+									item = PyInt_FromLong(imageData.m_rgbColorData[pixelIndex]);
+											PyTuple_SetItem(pylistPos, pixelIndex, item);
+								}
+							}
+						}	
+					}
+
+					PyTuple_SetItem(pyResultList, 2,pylistPos);
+					PyTuple_SetItem(pyResultList, 3,pylistDep);
+					return pyResultList;	
+				}	
+			}
+			
+		}
+
+	}
+
+	Py_INCREF(Py_None);
+	return Py_None;
+}
 
 static PyMethodDef SpamMethods[] = {
     {"loadURDF",  pybullet_loadURDF, METH_VARARGS,
@@ -378,6 +548,12 @@ static PyMethodDef SpamMethods[] = {
 
 	{"setGravity",  pybullet_setGravity, METH_VARARGS,
         "Set the gravity acceleration (x,y,z)."},
+
+	{"initializeJointPositions", pybullet_setJointPositions, METH_VARARGS,
+	"Initialize the joint positions for all joints. This method skips any physics simulation and teleports all joints to the new positions."},
+	
+	{"renderImage", pybullet_renderImage, METH_VARARGS,
+	"Render an image (given the pixel resolution width & height and camera view & projection matrices), and return the 8-8-8bit RGB pixel data and floating point depth values"},	
 	
 	{"getBasePositionAndOrientation",  pybullet_getBasePositionAndOrientation, METH_VARARGS,
         "Get the world position and orientation of the base of the object. (x,y,z) position vector and (x,y,z,w) quaternion orientation."},

src/LinearMath/btMatrix3x3.h

@@ -1329,7 +1329,9 @@ SIMD_FORCE_INLINE bool operator==(const btMatrix3x3& m1, const btMatrix3x3& m2)
     c0 = _mm_and_ps(c0, c1);
     c0 = _mm_and_ps(c0, c2);
 
-    return (0x7 == _mm_movemask_ps((__m128)c0));
+	int m = _mm_movemask_ps((__m128)c0);
+	return (0x7 == (m & 0x7));
+	
 #else 
 	return 
     (   m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&