add fileOpenDialog and enable loading of urdf from GUI

(will add .bullet file support soon)
Uses native Windows (getFileOpenFileName) and Mac OSX NSOpenPanel,
on Linux using pipe popen to zenity)

Demos3/AllBullet2Demos/main.cpp

@@ -181,6 +181,32 @@ static void MyMouseButtonCallback(int button, int state, float x, float y)
 
 #include <string.h>
 
+void openURDFDemo(const char* filename)
+{
+   
+    if (sCurrentDemo)
+    {
+        sCurrentDemo->exitPhysics();
+        app->m_instancingRenderer->removeAllInstances();
+        delete sCurrentDemo;
+        sCurrentDemo=0;
+    }
+    
+    app->m_parameterInterface->removeAllParameters();
+   
+    ImportUrdfDemo* physicsSetup = new ImportUrdfDemo();
+    physicsSetup->setFileName(filename);
+    
+    sCurrentDemo = new BasicDemo(app, physicsSetup);
+    
+    if (sCurrentDemo)
+    {
+        sCurrentDemo->initPhysics();
+    }
+
+    
+}
+
 void selectDemo(int demoIndex)
 {
 	sCurrentDemoIndex = demoIndex;
@@ -329,6 +355,18 @@ struct GL3TexLoader : public MyTextureLoader
 	}
 };
 
+void fileOpenCallback()
+{
+
+ char filename[1024];
+ int len = app->m_window->fileOpenDialog(filename,1024);
+ if (len)
+ {
+     //todo(erwincoumans) check if it is actually URDF
+     //printf("file open:%s\n", filename);
+     openURDFDemo(filename);
+ }
+}
 
 extern float shadowMapWorldSize;
 int main(int argc, char* argv[])
@@ -476,7 +514,8 @@ int main(int argc, char* argv[])
 	*/
 	unsigned long int	prevTimeInMicroseconds = clock.getTimeMicroseconds();
 
-
+    gui->registerFileOpenCallback(fileOpenCallback);
+    
 	do
 	{
 

Demos3/GpuDemos/gwenInternalData.h

@@ -40,6 +40,7 @@ struct GwenInternalData
 	Gwen::Controls::TabButton*		m_explorerPage;
 	Gwen::Controls::TreeControl*	m_explorerTreeCtrl;
 	Gwen::Controls::MenuItem*		m_viewMenu;
+    class MyMenuItems*                    m_menuItems;
 
 	int		m_curYposition;
 

Demos3/GpuDemos/gwenUserInterface.cpp

@@ -44,13 +44,23 @@ class MyMenuItems :  public Gwen::Controls::Base
 {
 public:
 
-    MyMenuItems() :Gwen::Controls::Base(0)
+    b3FileOpenCallback m_fileOpenCallback;
+    
+    MyMenuItems() :Gwen::Controls::Base(0),m_fileOpenCallback(0)
     {
     }
     void myQuitApp( Gwen::Controls::Base* pControl )
     {
         exit(0);
     }
+    void fileOpen( Gwen::Controls::Base* pControl )
+    {
+        if (m_fileOpenCallback)
+        {
+            (*m_fileOpenCallback)();
+        }
+    }
+    
 };
 
 struct MyTestMenuBar : public Gwen::Controls::MenuStrip
@@ -58,17 +68,20 @@ struct MyTestMenuBar : public Gwen::Controls::MenuStrip
 
 	Gwen::Controls::MenuItem* m_fileMenu;
 	Gwen::Controls::MenuItem* m_viewMenu;
+    MyMenuItems*                m_menuItems;
 
 	MyTestMenuBar(Gwen::Controls::Base* pParent)
 		:Gwen::Controls::MenuStrip(pParent)
 	{
 //		Gwen::Controls::MenuStrip* menu = new Gwen::Controls::MenuStrip( pParent );
 		{
-			MyMenuItems* menuItems = new MyMenuItems;
+			m_menuItems = new MyMenuItems;
 
 			m_fileMenu = AddItem( L"File" );
-			m_fileMenu->GetMenu()->AddItem(L"Quit",menuItems,(Gwen::Event::Handler::Function)&MyMenuItems::myQuitApp);
+			
-			m_viewMenu = AddItem( L"View" );
+            m_fileMenu->GetMenu()->AddItem(L"Open",m_menuItems,(Gwen::Event::Handler::Function)&MyMenuItems::fileOpen);
+            m_fileMenu->GetMenu()->AddItem(L"Quit",m_menuItems,(Gwen::Event::Handler::Function)&MyMenuItems::myQuitApp);
+            m_viewMenu = AddItem( L"View" );
 			
 		}
 	}
@@ -142,6 +155,11 @@ void	GwenUserInterface::setStatusBarMessage(const char* message, bool isLeft)
 	}
 }
 
+void GwenUserInterface::registerFileOpenCallback(b3FileOpenCallback callback)
+{
+    m_data->m_menuItems->m_fileOpenCallback = callback;
+}
+
 void	GwenUserInterface::init(int width, int height,struct sth_stash* stash,float retinaScale)
 {
 	m_data->m_curYposition = 20;
@@ -157,6 +175,10 @@ void	GwenUserInterface::init(int width, int height,struct sth_stash* stash,float
 
 	MyTestMenuBar* menubar = new MyTestMenuBar(m_data->pCanvas);
 	m_data->m_viewMenu = menubar->m_viewMenu;
+    m_data->m_menuItems = menubar->m_menuItems;
+    
+    
+    
 	Gwen::Controls::StatusBar* bar = new Gwen::Controls::StatusBar(m_data->pCanvas);
 	m_data->m_rightStatusBar = new Gwen::Controls::Label( bar );
 	m_data->m_rightStatusBar->SetWidth(width/2);

Demos3/GpuDemos/gwenUserInterface.h

@@ -5,7 +5,7 @@ struct GwenInternalData;
 
 typedef void (*b3ComboBoxCallback) (int combobox, const char* item);
 typedef void (*b3ToggleButtonCallback)(int button, int state);
-
+typedef void (*b3FileOpenCallback)();
 
 
 class GwenUserInterface
@@ -40,6 +40,8 @@ class GwenUserInterface
 		
 		void	setStatusBarMessage(const char* message, bool isLeft=true);
 
+        void    registerFileOpenCallback(b3FileOpenCallback callback);
+    
 		GwenInternalData* getInternalData()
 		{
 			return m_data;

Demos3/ImportURDFDemo/ImportURDFSetup.cpp

@@ -12,7 +12,7 @@ static bool enableConstraints = true;//false;
 
 ImportUrdfDemo::ImportUrdfDemo()
 {
-
+    sprintf(m_fileName,"r2d2.urdf");
 }
 
 ImportUrdfDemo::~ImportUrdfDemo()
@@ -21,7 +21,10 @@ ImportUrdfDemo::~ImportUrdfDemo()
 }
 
 
-
+void ImportUrdfDemo::setFileName(const char* urdfFileName)
+{
+    memcpy(m_fileName,urdfFileName,strlen(urdfFileName)+1);
+}
 
 #include "urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h"
 
@@ -71,7 +74,7 @@ struct URDF_LinkInformation
     btRigidBody* m_bulletRigidBody;
 	virtual ~URDF_LinkInformation()
 	{
-printf("~\n");
+        printf("~\n");
 	}
 };
 
@@ -239,12 +242,23 @@ btMultiBody* URDF2BulletMultiBody(my_shared_ptr<const Link> link, GraphicsPhysic
 			btMatrix3x3 inertia2PrincipalAxis;
 			inertiaMat.diagonalize(inertia2PrincipalAxis,threshold,maxSteps);
 			localInertiaDiagonal.setValue(inertiaMat[0][0],inertiaMat[1][1],inertiaMat[2][2]);
-
+            
-			btVector3 inertiaLocalCOM((*link).inertial->origin.position.x,(*link).inertial->origin.position.y,(*link).inertial->origin.position.z);
+            btVector3 inertiaLocalCOM((*link).inertial->origin.position.x,(*link).inertial->origin.position.y,(*link).inertial->origin.position.z);
-			localInertialTransform.setOrigin(inertiaLocalCOM);
+            localInertialTransform.setOrigin(inertiaLocalCOM);
-			btQuaternion inertiaOrn((*link).inertial->origin.rotation.x,(*link).inertial->origin.rotation.y,(*link).inertial->origin.rotation.z,(*link).inertial->origin.rotation.w);
+            btQuaternion inertiaOrn((*link).inertial->origin.rotation.x,(*link).inertial->origin.rotation.y,(*link).inertial->origin.rotation.z,(*link).inertial->origin.rotation.w);
-			btMatrix3x3 inertiaOrnMat(inertiaOrn);
+            btMatrix3x3 inertiaOrnMat(inertiaOrn);
-			localInertialTransform.setBasis(inertiaOrnMat*inertia2PrincipalAxis);
+            
+            if (mass > 0 && (localInertiaDiagonal[0]==0.f || localInertiaDiagonal[1] == 0.f
+				|| localInertiaDiagonal[2] == 0.f))
+            {
+				b3Warning("Error: inertia should not be zero if mass is positive\n");
+                localInertiaDiagonal.setMax(btVector3(0.1,0.1,0.1));
+                localInertialTransform.setIdentity();//.setBasis(inertiaOrnMat);
+            }
+            else
+            {
+                localInertialTransform.setBasis(inertiaOrnMat*inertia2PrincipalAxis);
+            }
 		}
 	}
 	btTransform linkTransformInWorldSpace;
@@ -403,30 +417,30 @@ btMultiBody* URDF2BulletMultiBody(my_shared_ptr<const Link> link, GraphicsPhysic
 	
 	if (shape)//compoundShape->getNumChildShapes()>0)
 	{
-		btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(mb, linkIndex-1);
+		btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(mb, linkIndex-1);
-        col->setCollisionShape(shape);
+        col->setCollisionShape(shape);
-
+
-        btTransform tr;
+        btTransform tr;
-        tr.setIdentity();
+        tr.setIdentity();
-		tr = linkTransformInWorldSpace;
+		tr = linkTransformInWorldSpace;
-		//if we don't set the initial pose of the btCollisionObject, the simulator will do this 
+		//if we don't set the initial pose of the btCollisionObject, the simulator will do this 
-		//when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider
+		//when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider
-                
+                
-		//tr.setOrigin(local_origin[0]);
+		//tr.setOrigin(local_origin[0]);
-        //tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
+        //tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
-        col->setWorldTransform(tr);
+        col->setWorldTransform(tr);
-
+
 		bool isDynamic = true;
 		short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
 		short collisionFilterMask = isDynamic? 	short(btBroadphaseProxy::AllFilter) : 	short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
-		
+		
-        world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);
+        world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);
-
+
-        btVector3 color(0.0,0.0,0.5);
+        btVector3 color(0.0,0.0,0.5);
-        gfxBridge.createCollisionObjectGraphicsObject(col,color);
+        gfxBridge.createCollisionObjectGraphicsObject(col,color);
-		btScalar friction = 0.5f;
+		btScalar friction = 0.5f;
-
+
-        col->setFriction(friction);
+        col->setFriction(friction);
         
 		if (parentIndex>=0)
 		{
@@ -554,6 +568,8 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
                 //create a joint if necessary
                 if ((*link).parent_joint)
                 {
+					btAssert(pp);
+
                     btRigidBody* parentBody =pp->m_bulletRigidBody;
 
                     const Joint* pj = (*link).parent_joint.get();
@@ -667,11 +683,11 @@ void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
 
 	m_dynamicsWorld->setGravity(gravity);
     //int argc=0;
-	const char* someFileName="r2d2.urdf";
 	char relativeFileName[1024];
 	
 	b3FileUtils fu;
-	bool fileFound = fu.findFile(someFileName, relativeFileName, 1024);
+	printf("m_fileName=%s\n", m_fileName);
+	bool fileFound = fu.findFile(m_fileName, relativeFileName, 1024);
 
 
 
@@ -773,6 +789,8 @@ void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
         groundOrigin[upAxis]=-2.5;
         start.setOrigin(groundOrigin);
         btRigidBody* body =  createRigidBody(0,start,box);
+        //m_dynamicsWorld->removeRigidBody(body);
+       // m_dynamicsWorld->addRigidBody(body,2,1);
         btVector3 color(0.5,0.5,0.5);
         gfxBridge.createRigidBodyGraphicsObject(body,color);
     }
@@ -787,4 +805,4 @@ void ImportUrdfDemo::stepSimulation(float deltaTime)
 		//the maximal coordinates/iterative MLCP solver requires a smallish timestep to converge
 		m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.);
 	}
-}
+}

Demos3/ImportURDFDemo/ImportURDFSetup.h

@@ -6,12 +6,16 @@
 
 class ImportUrdfDemo : public CommonMultiBodySetup
 {
+    char m_fileName[1024];
+    
 public:
     ImportUrdfDemo();
     virtual ~ImportUrdfDemo();
 
 	virtual void initPhysics(GraphicsPhysicsBridge& gfxBridge);
 	virtual void stepSimulation(float deltaTime);
+    
+    void setFileName(const char* urdfFileName);
 };
 
 #endif //IMPORT_URDF_SETUP_H

btgui/OpenGLWindow/MacOpenGLWindow.h

@@ -81,7 +81,7 @@ public:
 	
 	virtual void setWindowTitle(const char* title);
 
-	
+    int fileOpenDialog(char* filename, int maxNameLength);
 
 };
 

btgui/OpenGLWindow/MacOpenGLWindow.mm

@@ -989,8 +989,48 @@ void MacOpenGLWindow::setRequestExit()
 	m_internalData->m_exitRequested = true;
 }
 
+#include <string.h>
+int MacOpenGLWindow::fileOpenDialog(char* filename, int maxNameLength)
+{
+    //save/restore the OpenGL context, NSOpenPanel can mess it up
+    //http://stackoverflow.com/questions/13987148/nsopenpanel-breaks-my-sdl-opengl-app
+    
+    NSOpenGLContext *foo = [NSOpenGLContext currentContext];
+    // get the url of a .txt file
+    NSOpenPanel * zOpenPanel = [NSOpenPanel openPanel];
+    NSArray * zAryOfExtensions = [NSArray arrayWithObject:@"urdf"];
+    [zOpenPanel setAllowedFileTypes:zAryOfExtensions];
+    NSInteger zIntResult = [zOpenPanel runModal];
+    
+    [foo makeCurrentContext];
+    
+    if (zIntResult == NSFileHandlingPanelCancelButton) {
+        NSLog(@"readUsingOpenPanel cancelled");
+        return 0;
+    }
+    NSURL *zUrl = [zOpenPanel URL];
+   if (zUrl)
+   {
+       //without the file://
+       NSString *myString = [zUrl absoluteString];
+       int slen = [myString length];
+       if (slen < maxNameLength)
+       {
+           const char *cfilename=[myString UTF8String];
+           //expect file:// at start of URL
+           const char* p = strstr(cfilename, "file://");
+            if (p==cfilename)
+            {
+                int actualLen = strlen(cfilename)-7;
+                memcpy(filename, cfilename+7,actualLen);
+                filename[actualLen]=0;
+                return actualLen;
+            }
+       }
+   }
 
-
+    return 0;
+}
 
 
 

btgui/OpenGLWindow/Win32OpenGLWindow.cpp

@@ -137,6 +137,40 @@ void	Win32OpenGLWindow::endRendering()
 
 }
 
+int Win32OpenGLWindow::fileOpenDialog(char* fileName, int maxFileNameLength)
+{
+	//wchar_t wideChars[1024];
 
+		OPENFILENAME ofn ;
+	ZeroMemory( &ofn , sizeof( ofn));
+	ofn.lStructSize = sizeof ( ofn );
+	ofn.hwndOwner = NULL  ;
+
+#ifdef UNICODE
+	WCHAR bla[1024];
+	ofn.lpstrFile = bla;
+	ofn.lpstrFile[0] = '\0';
+	ofn.nMaxFile = 1023;
+	ofn.lpstrFilter = L"URDF\0*.urdf\0";
+#else
+	ofn.lpstrFile = fileName;
+	ofn.lpstrFile[0] = '\0';
+	ofn.nMaxFile = 1023;
+	//ofn.lpstrFilter = "All\0*.*\0Text\0*.TXT\0";
+	ofn.lpstrFilter = "URDF\0*.urdf\0";
+	
+#endif
+
+	ofn.nFilterIndex =1;
+	ofn.lpstrFileTitle = NULL ;
+	ofn.nMaxFileTitle = 0 ;
+	ofn.lpstrInitialDir=NULL ;
+	ofn.Flags = OFN_PATHMUSTEXIST|OFN_FILEMUSTEXIST ;
+	GetOpenFileName( &ofn );
+	return strlen(fileName);
+
+
+	//return 0;
+}
 
 	

btgui/OpenGLWindow/Win32OpenGLWindow.h

@@ -51,6 +51,8 @@ public:
 	virtual	void	endRendering();
 
 	virtual float getRetinaScale() const {return 1.f;}
+
+	virtual int fileOpenDialog(char* fileName, int maxFileNameLength);
 };
 
 

btgui/OpenGLWindow/X11OpenGLWindow.cpp

@@ -959,4 +959,29 @@ b3KeyboardCallback      X11OpenGLWindow::getKeyboardCallback()
 {
 	return m_data->m_keyboardCallback;
 }
+#include <stdio.h>
 
+int X11OpenGLWindow::fileOpenDialog(char* filename, int maxNameLength)
+{
+	int len = 0;
+	FILE * output = popen("zenity --file-selection --file-filter=\"*.urdf\" --file-filter=\"*.*\"","r");
+	if (output)
+	{
+		while( fgets(filename, maxNameLength-1, output) != NULL )
+		{
+			len=strlen(filename);
+			if (len>0)
+			{
+				filename[len-1]=0;
+				printf("file open (length=%d) = %s\n", len,filename);
+			}	
+		}
+		pclose(output);
+	} else
+	{
+		printf("Error: fileOpenDialog no popen output, perhaps install zenity?\n");
+	}
+	XRaiseWindow(m_data->m_dpy, m_data->m_win);
+	return len;
+	
+}

btgui/OpenGLWindow/X11OpenGLWindow.h

@@ -59,6 +59,7 @@ public:
 
         virtual void setWindowTitle(const char* title);
 
+	int fileOpenDialog(char* filename, int maxNameLength);
 };
 
 

btgui/OpenGLWindow/b3gWindowInterface.h

@@ -110,6 +110,8 @@ class b3gWindowInterface
 
 		virtual	float	getRetinaScale() const =0;
 
+        virtual int fileOpenDialog(char* fileName, int maxFileNameLength) = 0;
+    
 };
 
 #endif //B3G_WINDOW_INTERFACE_H

src/Bullet3Common/b3FileUtils.h

@@ -16,11 +16,21 @@ struct b3FileUtils
 
 	bool findFile(const char* orgFileName, char* relativeFileName, int maxRelativeFileNameMaxLen)
 	{
-		
+		FILE* f=0;
-			const char* prefix[]={"","./","./data/","../data/","../../data/","../../../data/","../../../../data/"};
+		f = fopen(orgFileName,"rb");
+                if (f)
+                {
+			//printf("original file found: [%s]\n", orgFileName);
+			sprintf(relativeFileName,"%s", orgFileName);
+			fclose(f);
+			return true;
+		}
+
+		//printf("Trying various directories, relative to current working directory\n");	
+			const char* prefix[]={"./","./data/","../data/","../../data/","../../../data/","../../../../data/"};
 			int numPrefixes = sizeof(prefix)/sizeof(const char*);
 	
-			FILE* f=0;
+			f=0;
 			bool fileFound = false;
 			int result = 0;
 
@@ -79,11 +89,11 @@ struct b3FileUtils
 			path[len]=0;
 		} else
 		{
-#ifdef _WIN32
+			b3Assert(maxPathLength>0);
-		sprintf_s(path, maxPathLength,"");
+			if (maxPathLength>0)
-#else
+			{
-			sprintf(path, "");
+				path[0] = 0;
-#endif
+			}
 		}
 	}
 
@@ -103,4 +113,4 @@ struct b3FileUtils
 	*/
 
 };
-#endif //B3_FILE_UTILS_H
+#endif //B3_FILE_UTILS_H