add initial SDF importer, work-in-progress (still too incomplete to be useful)

examples/Importers/ImportURDFDemo/UrdfParser.h

@@ -42,7 +42,9 @@ enum UrdfGeomTypes
 	URDF_GEOM_SPHERE=2,
 	URDF_GEOM_BOX,
 	URDF_GEOM_CYLINDER,
-	URDF_GEOM_MESH
+	URDF_GEOM_MESH,
+    URDF_GEOM_PLANE,
+    
 };
 
 
@@ -57,6 +59,8 @@ struct UrdfGeometry
 	double m_cylinderRadius;
 	double m_cylinderLength;
 
+    btVector3 m_planeNormal;
+    
 	std::string m_meshFileName;
 	btVector3 m_meshScale;
 };
@@ -134,32 +138,88 @@ struct UrdfModel
 class UrdfParser
 {
 protected:
+    
+    UrdfModel m_urdf2Model;
+    btAlignedObjectArray<UrdfModel*> m_sdfModels;
+    btAlignedObjectArray<UrdfModel*> m_tmpModels;
+    
+    bool m_parseSDF;
+    int m_activeSdfModel;
+
+    
+    void cleanModel(UrdfModel* model);
 	bool parseInertia(UrdfInertia& inertia, class TiXmlElement* config, ErrorLogger* logger);
 	bool parseGeometry(UrdfGeometry& geom, class TiXmlElement* g, ErrorLogger* logger);
-	bool parseVisual(UrdfVisual& visual, class TiXmlElement* config, ErrorLogger* logger);
+	bool parseVisual(UrdfModel& model, UrdfVisual& visual, class TiXmlElement* config, ErrorLogger* logger);
 	bool parseCollision(UrdfCollision& collision, class TiXmlElement* config, ErrorLogger* logger);
-	bool initTreeAndRoot(ErrorLogger* logger);
+	bool initTreeAndRoot(UrdfModel& model, ErrorLogger* logger);
 	bool parseMaterial(UrdfMaterial& material, class TiXmlElement *config, ErrorLogger* logger);
 	bool parseJointLimits(UrdfJoint& joint, TiXmlElement* config, ErrorLogger* logger);
 	bool parseJoint(UrdfJoint& link, TiXmlElement *config, ErrorLogger* logger);
-	bool parseLink(UrdfLink& link, TiXmlElement *config, ErrorLogger* logger);
-	UrdfModel m_model;
+	bool parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *config, ErrorLogger* logger);
 	
+    
 public:
 	
 	UrdfParser();
 	virtual ~UrdfParser();
 	
-	bool loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase);
+    void setParseSDF(bool useSDF)
+    {
+        m_parseSDF = useSDF;
+    }
+    bool getParseSDF() const
+    {
+        return m_parseSDF;
+    }
+    bool loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase);
+    bool loadSDF(const char* sdfText, ErrorLogger* logger);
+    
+    int getNumModels() const
+    {
+        //user should have loaded an SDF when calling this method
+        btAssert(m_parseSDF);
+        if (m_parseSDF)
+        {
+            return m_sdfModels.size();
+        }
+    }
+    
+    void activateModel(int modelIndex);
+    
+    UrdfModel& getModelByIndex(int index)
+    {
+        //user should have loaded an SDF when calling this method
+        btAssert(m_parseSDF);
 
-	const UrdfModel& getModel() const
+        return *m_sdfModels[index];
+    }
+
+    const UrdfModel& getModelByIndex(int index) const
+    {
+        //user should have loaded an SDF when calling this method
+        btAssert(m_parseSDF);
+
+        return *m_sdfModels[index];
+    }
+    
+    const UrdfModel& getModel() const
 	{
-		return m_model;
+        if (m_parseSDF)
+        {
+            return *m_sdfModels[m_activeSdfModel];
+        }
+
+		return m_urdf2Model;
 	}
 	
 	UrdfModel& getModel()
  	{
-		return m_model;
+        if (m_parseSDF)
+        {
+            return *m_sdfModels[m_activeSdfModel];
+        }
+		return m_urdf2Model;
 	}
 };