diff --git a/Extras/InverseDynamics/btMultiBodyFromURDF.hpp b/Extras/InverseDynamics/btMultiBodyFromURDF.hpp index 5eab2b4f9..6b46649ec 100644 --- a/Extras/InverseDynamics/btMultiBodyFromURDF.hpp +++ b/Extras/InverseDynamics/btMultiBodyFromURDF.hpp @@ -11,7 +11,7 @@ #include "../../examples/Importers/ImportURDFDemo/URDF2Bullet.h" #include "../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h" #include "../../examples/Importers/ImportURDFDemo/URDF2Bullet.h" - +#include "../../examples/Utils/b3BulletDefaultFileIO.h" /// Create a btMultiBody model from URDF. /// This is adapted from Bullet URDF loader example class MyBtMultiBodyFromURDF @@ -48,7 +48,8 @@ public: { this->createEmptyDynamicsWorld(); m_dynamicsWorld->setGravity(m_gravity); - BulletURDFImporter urdf_importer(&m_nogfx, 0, 1, 0); + b3BulletDefaultFileIO fileIO; + BulletURDFImporter urdf_importer(&m_nogfx, 0, &fileIO, 1, 0); URDFImporterInterface &u2b(urdf_importer); bool loadOk = u2b.loadURDF(m_filename.c_str(), m_base_fixed); diff --git a/Extras/obj2sdf/obj2sdf.cpp b/Extras/obj2sdf/obj2sdf.cpp index 240f065a8..b6a638c7f 100644 --- a/Extras/obj2sdf/obj2sdf.cpp +++ b/Extras/obj2sdf/obj2sdf.cpp @@ -14,6 +14,7 @@ #include "../Utils/b3ResourcePath.h" #include "Bullet3Common/b3CommandLineArgs.h" #include "Bullet3Common/b3HashMap.h" +#include "../Utils/b3BulletDefaultFileIO.h" struct ShapeContainer { @@ -71,12 +72,14 @@ int main(int argc, char* argv[]) bool mergeMaterials = args.CheckCmdLineFlag("mergeMaterials"); char fileNameWithPath[MAX_PATH_LEN]; - bool fileFound = (b3ResourcePath::findResourcePath(fileName, fileNameWithPath, MAX_PATH_LEN)) > 0; + bool fileFound = (b3ResourcePath::findResourcePath(fileName, fileNameWithPath, MAX_PATH_LEN,0)) > 0; char materialPrefixPath[MAX_PATH_LEN]; b3FileUtils::extractPath(fileNameWithPath, materialPrefixPath, MAX_PATH_LEN); std::vector shapes; - std::string err = tinyobj::LoadObj(shapes, fileNameWithPath, materialPrefixPath); + + b3BulletDefaultFileIO fileIO; + std::string err = tinyobj::LoadObj(shapes, fileNameWithPath, materialPrefixPath,&fileIO); char sdfFileName[MAX_PATH_LEN]; sprintf(sdfFileName, "%s%s.sdf", materialPrefixPath, "newsdf"); diff --git a/examples/CommonInterfaces/CommonFileIOInterface.h b/examples/CommonInterfaces/CommonFileIOInterface.h new file mode 100644 index 000000000..3e2e78f6d --- /dev/null +++ b/examples/CommonInterfaces/CommonFileIOInterface.h @@ -0,0 +1,19 @@ +#ifndef COMMON_FILE_IO_INTERFACE_H +#define COMMON_FILE_IO_INTERFACE_H + +struct CommonFileIOInterface +{ + virtual ~CommonFileIOInterface() + { + } + virtual int fileOpen(const char* fileName, const char* mode)=0; + virtual int fileRead(int fileHandle, char* destBuffer, int numBytes)=0; + virtual int fileWrite(int fileHandle,const char* sourceBuffer, int numBytes)=0; + virtual void fileClose(int fileHandle)=0; + virtual bool findResourcePath(const char* fileName, char* resourcePathOut, int resourcePathMaxNumBytes)=0; + virtual char* readLine(int fileHandle, char* destBuffer, int numBytes)=0; + virtual int getFileSize(int fileHandle)=0; + +}; + +#endif //COMMON_FILE_IO_INTERFACE_H \ No newline at end of file diff --git a/examples/ExtendedTutorials/RigidBodyFromObj.cpp b/examples/ExtendedTutorials/RigidBodyFromObj.cpp index f0c6699ef..2adf96a38 100644 --- a/examples/ExtendedTutorials/RigidBodyFromObj.cpp +++ b/examples/ExtendedTutorials/RigidBodyFromObj.cpp @@ -24,6 +24,7 @@ subject to the following restrictions: #include "Bullet3Common/b3FileUtils.h" #include "../Importers/ImportObjDemo/LoadMeshFromObj.h" #include "../OpenGLWindow/GLInstanceGraphicsShape.h" +#include "../Utils/b3BulletDefaultFileIO.h" struct RigidBodyFromObjExample : public CommonRigidBodyBase { @@ -73,13 +74,14 @@ void RigidBodyFromObjExample::initPhysics() //load our obj mesh const char* fileName = "teddy.obj"; //sphere8.obj";//sponza_closed.obj";//sphere8.obj"; char relativeFileName[1024]; - if (b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024)) + if (b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024,0)) { char pathPrefix[1024]; b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); } - GLInstanceGraphicsShape* glmesh = LoadMeshFromObj(relativeFileName, ""); + b3BulletDefaultFileIO fileIO; + GLInstanceGraphicsShape* glmesh = LoadMeshFromObj(relativeFileName, "",&fileIO); printf("[INFO] Obj loaded: Extracted %d verticed from obj file [%s]\n", glmesh->m_numvertices, fileName); const GLInstanceVertex& v = glmesh->m_vertices->at(0); diff --git a/examples/Importers/ImportColladaDemo/ImportColladaSetup.cpp b/examples/Importers/ImportColladaDemo/ImportColladaSetup.cpp index 66fa901b2..eaada4268 100644 --- a/examples/Importers/ImportColladaDemo/ImportColladaSetup.cpp +++ b/examples/Importers/ImportColladaDemo/ImportColladaSetup.cpp @@ -24,7 +24,7 @@ subject to the following restrictions: #include "LoadMeshFromCollada.h" #include "Bullet3Common/b3FileUtils.h" #include "../../Utils/b3ResourcePath.h" - +#include "../../Utils/b3BulletDefaultFileIO.h" #include "../CommonInterfaces/CommonRigidBodyBase.h" class ImportColladaSetup : public CommonRigidBodyBase @@ -79,7 +79,7 @@ void ImportColladaSetup::initPhysics() char relativeFileName[1024]; - if (!b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024)) + if (!b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024,0)) return; btVector3 shift(0, 0, 0); @@ -94,7 +94,7 @@ void ImportColladaSetup::initPhysics() btTransform upAxisTrans; upAxisTrans.setIdentity(); - btVector3 color(0, 0, 1); + btVector4 color(0, 0, 1,1); #ifdef COMPARE_WITH_ASSIMP static int useAssimp = 0; @@ -119,7 +119,8 @@ void ImportColladaSetup::initPhysics() { fileIndex = 0; } - LoadMeshFromCollada(relativeFileName, visualShapes, visualShapeInstances, upAxisTrans, unitMeterScaling, upAxis); + b3BulletDefaultFileIO fileIO; + LoadMeshFromCollada(relativeFileName, visualShapes, visualShapeInstances, upAxisTrans, unitMeterScaling, upAxis,&fileIO); #endif // COMPARE_WITH_ASSIMP //at the moment our graphics engine requires instances that share the same visual shape to be added right after registering the shape diff --git a/examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp b/examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp index bd0949585..bf5a39002 100644 --- a/examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp +++ b/examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp @@ -22,10 +22,11 @@ subject to the following restrictions: #include "../../ThirdPartyLibs/tinyxml2/tinyxml2.h" using namespace tinyxml2; -#include "Bullet3Common/b3FileUtils.h" + #include "LinearMath/btHashMap.h" #include #include "btMatrix4x4.h" +#include "../../CommonInterfaces/CommonFileIOInterface.h" #define MAX_VISUAL_SHAPES 512 @@ -561,7 +562,7 @@ void getUnitMeterScalingAndUpAxisTransform(XMLDocument& doc, btTransform& tr, fl } } -void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray& visualShapes, btAlignedObjectArray& visualShapeInstances, btTransform& upAxisTransform, float& unitMeterScaling, int clientUpAxis) +void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray& visualShapes, btAlignedObjectArray& visualShapeInstances, btTransform& upAxisTransform, float& unitMeterScaling, int clientUpAxis, struct CommonFileIOInterface* fileIO) { // GLInstanceGraphicsShape* instance = 0; @@ -570,16 +571,32 @@ void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray name2ShapeIndex; - b3FileUtils f; + char filename[1024]; - if (!f.findFile(relativeFileName, filename, 1024)) + if (!fileIO->findResourcePath(relativeFileName, filename, 1024)) { b3Warning("File not found: %s\n", filename); return; } XMLDocument doc; - if (doc.LoadFile(filename) != XML_SUCCESS) + //doc.Parse((const char*)filedata, 0, TIXML_ENCODING_UTF8); + b3AlignedObjectArray xmlString; + int fileHandle = fileIO->fileOpen(filename,"r"); + if (fileHandle>=0) + { + int size = fileIO->getFileSize(fileHandle); + xmlString.resize(size); + int actual = fileIO->fileRead(fileHandle, &xmlString[0],size); + if (actual==size) + { + } + } + if (xmlString.size()==0) + return; + + if (doc.Parse(&xmlString[0], xmlString.size()) != XML_SUCCESS) + //if (doc.LoadFile(filename) != XML_SUCCESS) return; //We need units to be in meter, so apply a scaling using the asset/units meter diff --git a/examples/Importers/ImportColladaDemo/LoadMeshFromCollada.h b/examples/Importers/ImportColladaDemo/LoadMeshFromCollada.h index 14e135b5d..16911ae6e 100644 --- a/examples/Importers/ImportColladaDemo/LoadMeshFromCollada.h +++ b/examples/Importers/ImportColladaDemo/LoadMeshFromCollada.h @@ -28,7 +28,8 @@ void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray& visualShapeInstances, btTransform& upAxisTrans, float& unitMeterScaling, - int clientUpAxis); + int clientUpAxis, + struct CommonFileIOInterface* fileIO); //#define COMPARE_WITH_ASSIMP #ifdef COMPARE_WITH_ASSIMP diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp index 88f657a76..eac4a2dae 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp @@ -6,6 +6,8 @@ #include "BulletCollision/CollisionShapes/btShapeHull.h" #include "../../CommonInterfaces/CommonRenderInterface.h" #include "../../CommonInterfaces/CommonGUIHelperInterface.h" +#include "../../CommonInterfaces/CommonFileIOInterface.h" +#include "../ImportURDFDemo/UrdfFindMeshFile.h" #include #include "../../Utils/b3ResourcePath.h" #include @@ -32,6 +34,7 @@ #include "BulletCollision/CollisionShapes/btConvexHullShape.h" #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" #include "BulletCollision/CollisionShapes/btTriangleMesh.h" + using namespace tinyxml2; #define mjcf_sphere_indiced textured_detailed_sphere_indices @@ -203,13 +206,16 @@ struct BulletMJCFImporterInternalData int m_flags; int m_textureId; + + CommonFileIOInterface* m_fileIO; BulletMJCFImporterInternalData() : m_inertiaFromGeom(true), m_activeModel(-1), m_activeBodyUniqueId(-1), m_flags(0), - m_textureId(-1) + m_textureId(-1), + m_fileIO(0) { m_pathPrefix[0] = 0; } @@ -887,7 +893,7 @@ struct BulletMJCFImporterInternalData { geom.m_type = URDF_GEOM_MESH; geom.m_meshFileName = assetPtr->m_fileName; - bool exists = findExistingMeshFile( + bool exists = UrdfFindMeshFile(m_fileIO, m_sourceFileName, assetPtr->m_fileName, sourceFileLocation(link_xml), &geom.m_meshFileName, &geom.m_meshFileType); @@ -1408,13 +1414,14 @@ struct BulletMJCFImporterInternalData } }; -BulletMJCFImporter::BulletMJCFImporter(struct GUIHelperInterface* helper, UrdfRenderingInterface* customConverter, int flags) +BulletMJCFImporter::BulletMJCFImporter(struct GUIHelperInterface* helper, UrdfRenderingInterface* customConverter, CommonFileIOInterface* fileIO, int flags) { m_data = new BulletMJCFImporterInternalData(); m_data->m_guiHelper = helper; m_data->m_customVisualShapesConverter = customConverter; m_data->m_flags = flags; m_data->m_textureId = -1; + m_data->m_fileIO = fileIO; } BulletMJCFImporter::~BulletMJCFImporter() @@ -1433,7 +1440,7 @@ bool BulletMJCFImporter::loadMJCF(const char* fileName, MJCFErrorLogger* logger, b3FileUtils fu; //bool fileFound = fu.findFile(fileName, relativeFileName, 1024); - bool fileFound = (b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024) > 0); + bool fileFound = (m_data->m_fileIO->findResourcePath(fileName, relativeFileName, 1024) > 0); m_data->m_sourceFileName = relativeFileName; std::string xml_string; @@ -1962,7 +1969,7 @@ void BulletMJCFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu case UrdfGeometry::FILE_OBJ: { b3ImportMeshData meshData; - if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData)) + if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData, m_data->m_fileIO)) { if (meshData.m_textureImage1) { @@ -1980,7 +1987,7 @@ void BulletMJCFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu case UrdfGeometry::FILE_STL: { - glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str()); + glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str(), m_data->m_fileIO); break; } @@ -1998,7 +2005,8 @@ void BulletMJCFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu visualShapeInstances, upAxisTrans, unitMeterScaling, - upAxis); + upAxis, + m_data->m_fileIO); glmesh = new GLInstanceGraphicsShape; // int index = 0; @@ -2243,7 +2251,7 @@ void BulletMJCFImporter::convertLinkVisualShapes2(int linkIndex, int urdfIndex, if (m_data->m_customVisualShapesConverter) { const UrdfLink* link = m_data->getLink(m_data->m_activeModel, urdfIndex); - m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex, pathPrefix, inertialFrame, link, 0, colObj->getBroadphaseHandle()->getUid(), objectIndex); + m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex, pathPrefix, inertialFrame, link, 0, colObj->getBroadphaseHandle()->getUid(), objectIndex, m_data->m_fileIO); } } @@ -2379,12 +2387,12 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn { if (col->m_flags & URDF_FORCE_CONCAVE_TRIMESH) { - glmesh = LoadMeshFromObj(col->m_geometry.m_meshFileName.c_str(), 0); + glmesh = LoadMeshFromObj(col->m_geometry.m_meshFileName.c_str(), 0,m_data->m_fileIO); } else { std::vector shapes; - std::string err = tinyobj::LoadObj(shapes, col->m_geometry.m_meshFileName.c_str()); + std::string err = tinyobj::LoadObj(shapes, col->m_geometry.m_meshFileName.c_str(),"",m_data->m_fileIO); //create a convex hull for each shape, and store it in a btCompoundShape childShape = MjcfCreateConvexHullFromShapes(shapes, col->m_geometry.m_meshScale, m_data->m_globalDefaults.m_defaultCollisionMargin); @@ -2393,7 +2401,7 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn } case UrdfGeometry::FILE_STL: { - glmesh = LoadMeshFromSTL(col->m_geometry.m_meshFileName.c_str()); + glmesh = LoadMeshFromSTL(col->m_geometry.m_meshFileName.c_str(), m_data->m_fileIO); break; } default: diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h index 2c89fdbc9..d4de6b282 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h @@ -27,7 +27,7 @@ class BulletMJCFImporter : public URDFImporterInterface void convertURDFToVisualShapeInternal(const struct UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut, btAlignedObjectArray& texturesOut) const; public: - BulletMJCFImporter(struct GUIHelperInterface* helper, UrdfRenderingInterface* customConverter, int flags); + BulletMJCFImporter(struct GUIHelperInterface* helper, UrdfRenderingInterface* customConverter, struct CommonFileIOInterface* fileIO, int flags); virtual ~BulletMJCFImporter(); virtual bool parseMJCFString(const char* xmlString, MJCFErrorLogger* logger); diff --git a/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp b/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp index d0d57a1ca..b4853fab8 100644 --- a/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp +++ b/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp @@ -10,7 +10,7 @@ #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" #include "../CommonInterfaces/CommonParameterInterface.h" #include "../../Utils/b3ResourcePath.h" - +#include "../../Utils/b3BulletDefaultFileIO.h" #include "../CommonInterfaces/CommonMultiBodyBase.h" #include "../ImportURDFDemo/MyMultiBodyCreator.h" @@ -200,7 +200,8 @@ void ImportMJCFSetup::initPhysics() } int flags = 0; - BulletMJCFImporter importer(m_guiHelper, 0, flags); + b3BulletDefaultFileIO fileIO; + BulletMJCFImporter importer(m_guiHelper, 0, &fileIO, flags); MyMJCFLogger logger; bool result = importer.loadMJCF(m_fileName, &logger); if (result) diff --git a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp index a0a06ca59..4115effd5 100644 --- a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp +++ b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp @@ -9,6 +9,7 @@ #include "stb_image/stb_image.h" #include "../ImportObjDemo/LoadMeshFromObj.h" #include "Bullet3Common/b3HashMap.h" +#include "../../CommonInterfaces/CommonFileIOInterface.h" struct CachedTextureResult { @@ -45,7 +46,7 @@ struct CachedTextureManager }; static CachedTextureManager sTexCacheMgr; -bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& fileName, b3ImportMeshData& meshData) +bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& fileName, b3ImportMeshData& meshData, struct CommonFileIOInterface* fileIO) { B3_PROFILE("loadAndRegisterMeshFromFileInternal"); meshData.m_gfxShape = 0; @@ -55,7 +56,7 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& meshData.m_isCached = false; char relativeFileName[1024]; - if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024)) + if (fileIO->findResourcePath(fileName.c_str(), relativeFileName, 1024)) { char pathPrefix[1024]; @@ -65,7 +66,7 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& std::vector shapes; { B3_PROFILE("tinyobj::LoadObj"); - std::string err = LoadFromCachedOrFromObj(shapes, relativeFileName, pathPrefix); + std::string err = LoadFromCachedOrFromObj(shapes, relativeFileName, pathPrefix,fileIO); //std::string err = tinyobj::LoadObj(shapes, relativeFileName, pathPrefix); } @@ -91,7 +92,7 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& char relativeFileName[1024]; sprintf(relativeFileName, "%s%s", prefix[i], filename); char relativeFileName2[1024]; - if (b3ResourcePath::findResourcePath(relativeFileName, relativeFileName2, 1024)) + if (fileIO->findResourcePath(relativeFileName, relativeFileName2, 1024)) { if (b3IsFileCachingEnabled()) { @@ -110,7 +111,30 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& if (image == 0) { - image = stbi_load(relativeFileName, &width, &height, &n, 3); + + b3AlignedObjectArray buffer; + buffer.reserve(1024); + int fileId = fileIO->fileOpen(relativeFileName,"rb"); + if (fileId>=0) + { + int size = fileIO->getFileSize(fileId); + if (size>0) + { + buffer.resize(size); + int actual = fileIO->fileRead(fileId,&buffer[0],size); + if (actual != size) + { + b3Warning("STL filesize mismatch!\n"); + buffer.resize(0); + } + } + } + + if (buffer.size()) + { + image = stbi_load_from_memory((const unsigned char*)&buffer[0], buffer.size(), &width, &height, &n, 3); + } + //image = stbi_load(relativeFileName, &width, &height, &n, 3); meshData.m_textureImage1 = image; diff --git a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.h b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.h index e69506a60..770eefd35 100644 --- a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.h +++ b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.h @@ -16,7 +16,7 @@ struct b3ImportMeshData class b3ImportMeshUtility { public: - static bool loadAndRegisterMeshFromFileInternal(const std::string& fileName, b3ImportMeshData& meshData); + static bool loadAndRegisterMeshFromFileInternal(const std::string& fileName, b3ImportMeshData& meshData, struct CommonFileIOInterface* fileIO); }; #endif //B3_IMPORT_MESH_UTILITY_H diff --git a/examples/Importers/ImportObjDemo/ImportObjExample.cpp b/examples/Importers/ImportObjDemo/ImportObjExample.cpp index 5caef0bd3..081e63c08 100644 --- a/examples/Importers/ImportObjDemo/ImportObjExample.cpp +++ b/examples/Importers/ImportObjDemo/ImportObjExample.cpp @@ -7,6 +7,7 @@ #include "../OpenGLWindow/SimpleOpenGL3App.h" #include "Wavefront2GLInstanceGraphicsShape.h" #include "../../Utils/b3ResourcePath.h" +#include "../../Utils/b3BulletDefaultFileIO.h" #include "Bullet3Common/b3FileUtils.h" #include "stb_image/stb_image.h" @@ -56,7 +57,8 @@ int loadAndRegisterMeshFromFile2(const std::string& fileName, CommonRenderInterf int shapeId = -1; b3ImportMeshData meshData; - if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fileName, meshData)) + b3BulletDefaultFileIO fileIO; + if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fileName, meshData,&fileIO)) { int textureIndex = -1; @@ -94,7 +96,7 @@ void ImportObjSetup::initPhysics() btQuaternion orn = trans.getRotation(); btVector3 scaling(1, 1, 1); - btVector3 color(1, 1, 1); + btVector4 color(1, 1, 1,1); int shapeId = loadAndRegisterMeshFromFile2(m_fileName, m_guiHelper->getRenderInterface()); if (shapeId >= 0) diff --git a/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp b/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp index 01147e817..566682d0a 100644 --- a/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp +++ b/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp @@ -33,7 +33,9 @@ void b3EnableFileCaching(int enable) std::string LoadFromCachedOrFromObj( std::vector& shapes, // [output] const char* filename, - const char* mtl_basepath) + const char* mtl_basepath, + struct CommonFileIOInterface* fileIO + ) { CachedObjResult* resultPtr = gCachedObjResults[filename]; if (resultPtr) @@ -43,7 +45,7 @@ std::string LoadFromCachedOrFromObj( return result.m_msg; } - std::string err = tinyobj::LoadObj(shapes, filename, mtl_basepath); + std::string err = tinyobj::LoadObj(shapes, filename, mtl_basepath,fileIO); CachedObjResult result; result.m_msg = err; result.m_shapes = shapes; @@ -54,13 +56,13 @@ std::string LoadFromCachedOrFromObj( return err; } -GLInstanceGraphicsShape* LoadMeshFromObj(const char* relativeFileName, const char* materialPrefixPath) +GLInstanceGraphicsShape* LoadMeshFromObj(const char* relativeFileName, const char* materialPrefixPath, struct CommonFileIOInterface* fileIO) { B3_PROFILE("LoadMeshFromObj"); std::vector shapes; { B3_PROFILE("tinyobj::LoadObj2"); - std::string err = LoadFromCachedOrFromObj(shapes, relativeFileName, materialPrefixPath); + std::string err = LoadFromCachedOrFromObj(shapes, relativeFileName, materialPrefixPath,fileIO); } { diff --git a/examples/Importers/ImportObjDemo/LoadMeshFromObj.h b/examples/Importers/ImportObjDemo/LoadMeshFromObj.h index aa35354bf..9b62074c1 100644 --- a/examples/Importers/ImportObjDemo/LoadMeshFromObj.h +++ b/examples/Importers/ImportObjDemo/LoadMeshFromObj.h @@ -8,11 +8,13 @@ struct GLInstanceGraphicsShape; int b3IsFileCachingEnabled(); void b3EnableFileCaching(int enable); + std::string LoadFromCachedOrFromObj( std::vector& shapes, // [output] const char* filename, - const char* mtl_basepath); + const char* mtl_basepath, + struct CommonFileIOInterface* fileIO); -GLInstanceGraphicsShape* LoadMeshFromObj(const char* relativeFileName, const char* materialPrefixPath); +GLInstanceGraphicsShape* LoadMeshFromObj(const char* relativeFileName, const char* materialPrefixPath,struct CommonFileIOInterface* fileIO); #endif //LOAD_MESH_FROM_OBJ_H diff --git a/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp b/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp index 926bb52b7..9744e3d44 100644 --- a/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp +++ b/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp @@ -162,7 +162,7 @@ void ImportSDFSetup::initPhysics() m_dynamicsWorld->setGravity(gravity); - BulletURDFImporter u2b(m_guiHelper, 0, 1, 0); + BulletURDFImporter u2b(m_guiHelper, 0, 0, 1, 0); bool loadOk = u2b.loadSDF(m_fileName); diff --git a/examples/Importers/ImportSTLDemo/ImportSTLSetup.cpp b/examples/Importers/ImportSTLDemo/ImportSTLSetup.cpp index 73bfd4fcf..350c78055 100644 --- a/examples/Importers/ImportSTLDemo/ImportSTLSetup.cpp +++ b/examples/Importers/ImportSTLDemo/ImportSTLSetup.cpp @@ -7,6 +7,7 @@ #include "LoadMeshFromSTL.h" #include "../CommonInterfaces/CommonRigidBodyBase.h" #include "../../Utils/b3ResourcePath.h" +#include "../../Utils/b3BulletDefaultFileIO.h" class ImportSTLSetup : public CommonRigidBodyBase { @@ -55,7 +56,7 @@ void ImportSTLSetup::initPhysics() m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe); char relativeFileName[1024]; - if (!b3ResourcePath::findResourcePath(m_fileName, relativeFileName, 1024)) + if (!b3ResourcePath::findResourcePath(m_fileName, relativeFileName, 1024,0)) { b3Warning("Cannot find file %s\n", m_fileName); return; @@ -65,7 +66,8 @@ void ImportSTLSetup::initPhysics() // int index=10; { - GLInstanceGraphicsShape* gfxShape = LoadMeshFromSTL(relativeFileName); + b3BulletDefaultFileIO fileIO; + GLInstanceGraphicsShape* gfxShape = LoadMeshFromSTL(relativeFileName,&fileIO); btTransform trans; trans.setIdentity(); @@ -74,7 +76,7 @@ void ImportSTLSetup::initPhysics() btVector3 position = trans.getOrigin(); btQuaternion orn = trans.getRotation(); - btVector3 color(0, 0, 1); + btVector4 color(0, 0, 1,1); int shapeId = m_guiHelper->getRenderInterface()->registerShape(&gfxShape->m_vertices->at(0).xyzw[0], gfxShape->m_numvertices, &gfxShape->m_indices->at(0), gfxShape->m_numIndices); diff --git a/examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h b/examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h index 8f7dbc533..b9e3bed5e 100644 --- a/examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h +++ b/examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h @@ -5,6 +5,7 @@ #include "../../OpenGLWindow/GLInstanceGraphicsShape.h" #include //fopen #include "Bullet3Common/b3AlignedObjectArray.h" +#include "../../CommonInterfaces/CommonFileIOInterface.h" struct MySTLTriangle { @@ -14,25 +15,21 @@ struct MySTLTriangle float vertex2[3]; }; -static GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName) +static GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName, struct CommonFileIOInterface* fileIO) { GLInstanceGraphicsShape* shape = 0; - FILE* file = fopen(relativeFileName, "rb"); - if (file) + int fileHandle = fileIO->fileOpen(relativeFileName, "rb"); + if (fileHandle>=0) { int size = 0; - if (fseek(file, 0, SEEK_END) || (size = ftell(file)) == EOF || fseek(file, 0, SEEK_SET)) + size = fileIO->getFileSize(fileHandle); { - b3Warning("Error: Cannot access file to determine size of %s\n", relativeFileName); - } - else - { - if (size) + if (size>=0) { //b3Warning("Open STL file of %d bytes\n",size); char* memoryBuffer = new char[size + 1]; - int actualBytesRead = fread(memoryBuffer, 1, size, file); + int actualBytesRead = fileIO->fileRead(fileHandle, memoryBuffer, size); if (actualBytesRead != size) { b3Warning("Error reading from file %s", relativeFileName); @@ -97,7 +94,7 @@ static GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName) delete[] memoryBuffer; } } - fclose(file); + fileIO->fileClose(fileHandle); } if (shape) { diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index b2d47c18e..dd44c5991 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -22,9 +22,14 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btShapeHull.h" //to create a tesselation of a generic btConvexShape #include "BulletCollision/CollisionShapes/btSdfCollisionShape.h" #include "../../CommonInterfaces/CommonGUIHelperInterface.h" +#include "../../CommonInterfaces/CommonFileIOInterface.h" #include "Bullet3Common/b3FileUtils.h" #include #include "../../Utils/b3ResourcePath.h" +#include "../../Utils/b3BulletDefaultFileIO.h" + + + #include "URDF2Bullet.h" //for flags #include "../ImportMeshUtility/b3ImportMeshUtility.h" @@ -35,13 +40,16 @@ static btScalar gUrdfDefaultCollisionMargin = 0.001; #include #include "UrdfParser.h" + + ATTRIBUTE_ALIGNED16(struct) BulletURDFInternalData { BT_DECLARE_ALIGNED_ALLOCATOR(); - + b3BulletDefaultFileIO m_defaultFileIO; UrdfParser m_urdfParser; struct GUIHelperInterface* m_guiHelper; + struct CommonFileIOInterface* m_fileIO; std::string m_sourceFile; char m_pathPrefix[1024]; int m_bodyId; @@ -63,7 +71,9 @@ BulletURDFInternalData m_pathPrefix[sizeof(m_pathPrefix) - 1] = 0; // required, strncpy doesn't write zero on overflow } - BulletURDFInternalData() + BulletURDFInternalData(CommonFileIOInterface* fileIO) + :m_urdfParser(fileIO? fileIO : &m_defaultFileIO), + m_fileIO(fileIO? fileIO : &m_defaultFileIO) { m_enableTinyRenderer = true; m_pathPrefix[0] = 0; @@ -74,6 +84,8 @@ BulletURDFInternalData { m_urdfParser.setGlobalScaling(scaling); } + + }; void BulletURDFImporter::printTree() @@ -81,9 +93,12 @@ void BulletURDFImporter::printTree() // btAssert(0); } -BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper, UrdfRenderingInterface* customConverter, double globalScaling, int flags) + + + +BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper, UrdfRenderingInterface* customConverter, struct CommonFileIOInterface* fileIO,double globalScaling, int flags) { - m_data = new BulletURDFInternalData; + m_data = new BulletURDFInternalData(fileIO); m_data->setGlobalScaling(globalScaling); m_data->m_flags = flags; m_data->m_guiHelper = helper; @@ -128,7 +143,7 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase) b3FileUtils fu; //bool fileFound = fu.findFile(fileName, relativeFileName, 1024); - bool fileFound = (b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024)) > 0; + bool fileFound = m_data->m_fileIO->findResourcePath(fileName, relativeFileName, 1024) > 0; std::string xml_string; @@ -143,6 +158,23 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase) fu.extractPath(relativeFileName, path, sizeof(path)); m_data->setSourceFile(relativeFileName, path); + //read file + int fileId = m_data->m_fileIO->fileOpen(relativeFileName,"r"); + + + char destBuffer[8192]; + char* line = 0; + do + { + line = m_data->m_fileIO->readLine(fileId, destBuffer, 8192); + if (line) + { + xml_string += (std::string(destBuffer) + "\n"); + } + } + while (line); + +#if 0 std::fstream xml_file(relativeFileName, std::fstream::in); while (xml_file.good()) { @@ -151,6 +183,8 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase) xml_string += (line + "\n"); } xml_file.close(); +#endif + } BulletErrorLogger loggie; @@ -178,7 +212,7 @@ bool BulletURDFImporter::loadSDF(const char* fileName, bool forceFixedBase) b3FileUtils fu; //bool fileFound = fu.findFile(fileName, relativeFileName, 1024); - bool fileFound = (b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024)) > 0; + bool fileFound = (m_data->m_fileIO->findResourcePath(fileName, relativeFileName, 1024)) > 0; std::string xml_string; @@ -507,107 +541,7 @@ static btCollisionShape* createConvexHullFromShapes(std::vector shorter; - shorter.push_back("../.."); - shorter.push_back(".."); - shorter.push_back("."); - int cnt = urdf_path.size(); - for (int i = 0; i < cnt; ++i) - { - if (urdf_path[i] == '/' || urdf_path[i] == '\\') - { - shorter.push_back(urdf_path.substr(0, i)); - } - } - shorter.reverse(); - - std::string existing_file; - - { - std::string attempt = fn; - FILE* f = fopen(attempt.c_str(), "rb"); - if (f) - { - existing_file = attempt; - fclose(f); - } - } - if (existing_file.empty()) - { - for (std::list::iterator x = shorter.begin(); x != shorter.end(); ++x) - { - std::string attempt = *x + "/" + fn; - FILE* f = fopen(attempt.c_str(), "rb"); - if (!f) - { - //b3Printf("%s: tried '%s'", error_message_prefix.c_str(), attempt.c_str()); - continue; - } - fclose(f); - existing_file = attempt; - //b3Printf("%s: found '%s'", error_message_prefix.c_str(), attempt.c_str()); - break; - } - } - - if (existing_file.empty()) - { - b3Warning("%s: cannot find '%s' in any directory in urdf path\n", error_message_prefix.c_str(), fn.c_str()); - return false; - } - else - { - *out_found_filename = existing_file; - return true; - } -} int BulletURDFImporter::getUrdfFromCollisionShape(const btCollisionShape* collisionShape, UrdfCollision& collision) const { @@ -708,7 +642,7 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl char relativeFileName[1024]; char pathPrefix[1024]; pathPrefix[0] = 0; - if (b3ResourcePath::findResourcePath(collision->m_geometry.m_meshFileName.c_str(), relativeFileName, 1024)) + if (m_data->m_fileIO->findResourcePath(collision->m_geometry.m_meshFileName.c_str(), relativeFileName, 1024)) { b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); @@ -758,16 +692,16 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl char relativeFileName[1024]; char pathPrefix[1024]; pathPrefix[0] = 0; - if (b3ResourcePath::findResourcePath(collision->m_geometry.m_meshFileName.c_str(), relativeFileName, 1024)) + if (m_data->m_fileIO->findResourcePath(collision->m_geometry.m_meshFileName.c_str(), relativeFileName, 1024)) { b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); } - glmesh = LoadMeshFromObj(collision->m_geometry.m_meshFileName.c_str(), pathPrefix); + glmesh = LoadMeshFromObj(collision->m_geometry.m_meshFileName.c_str(), pathPrefix,m_data->m_fileIO); } else { std::vector shapes; - std::string err = tinyobj::LoadObj(shapes, collision->m_geometry.m_meshFileName.c_str()); + std::string err = tinyobj::LoadObj(shapes, collision->m_geometry.m_meshFileName.c_str(),"",m_data->m_fileIO); //create a convex hull for each shape, and store it in a btCompoundShape shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale, m_data->m_flags); @@ -777,7 +711,7 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl break; case UrdfGeometry::FILE_STL: - glmesh = LoadMeshFromSTL(collision->m_geometry.m_meshFileName.c_str()); + glmesh = LoadMeshFromSTL(collision->m_geometry.m_meshFileName.c_str(), m_data->m_fileIO); break; case UrdfGeometry::FILE_COLLADA: @@ -787,7 +721,7 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl btTransform upAxisTrans; upAxisTrans.setIdentity(); float unitMeterScaling = 1; - LoadMeshFromCollada(collision->m_geometry.m_meshFileName.c_str(), visualShapes, visualShapeInstances, upAxisTrans, unitMeterScaling, 2); + LoadMeshFromCollada(collision->m_geometry.m_meshFileName.c_str(), visualShapes, visualShapeInstances, upAxisTrans, unitMeterScaling, 2, m_data->m_fileIO); glmesh = new GLInstanceGraphicsShape; glmesh->m_indices = new b3AlignedObjectArray(); @@ -982,7 +916,7 @@ void BulletURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu case UrdfGeometry::FILE_OBJ: { b3ImportMeshData meshData; - if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData)) + if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData, m_data->m_fileIO)) { if (meshData.m_textureImage1) { @@ -1000,7 +934,7 @@ void BulletURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu case UrdfGeometry::FILE_STL: { - glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str()); + glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str(),m_data->m_fileIO); break; } @@ -1018,7 +952,8 @@ void BulletURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu visualShapeInstances, upAxisTrans, unitMeterScaling, - upAxis); + upAxis, + m_data->m_fileIO); glmesh = new GLInstanceGraphicsShape; // int index = 0; @@ -1326,7 +1261,7 @@ void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, int urdfIndex, UrdfLink* const* linkPtr = model.m_links.getAtIndex(urdfIndex); if (linkPtr) { - m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex, pathPrefix, localInertiaFrame, *linkPtr, &model, colObj->getBroadphaseHandle()->getUid(), bodyUniqueId); + m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex, pathPrefix, localInertiaFrame, *linkPtr, &model, colObj->getBroadphaseHandle()->getUid(), bodyUniqueId, m_data->m_fileIO); } } } @@ -1403,18 +1338,18 @@ class btCompoundShape* BulletURDFImporter::convertLinkCollisionShapes(int linkIn { const UrdfCollision& col = link->m_collisionArray[v]; btCollisionShape* childShape = convertURDFToCollisionShape(&col, pathPrefix); - m_data->m_allocatedCollisionShapes.push_back(childShape); - if (childShape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE) - { - btCompoundShape* compound = (btCompoundShape*)childShape; - for (int i = 0; i < compound->getNumChildShapes(); i++) - { - m_data->m_allocatedCollisionShapes.push_back(compound->getChildShape(i)); - } - } - if (childShape) { + m_data->m_allocatedCollisionShapes.push_back(childShape); + if (childShape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE) + { + btCompoundShape* compound = (btCompoundShape*)childShape; + for (int i = 0; i < compound->getNumChildShapes(); i++) + { + m_data->m_allocatedCollisionShapes.push_back(compound->getChildShape(i)); + } + } + btTransform childTrans = col.m_linkLocalFrame; compoundShape->addChildShape(localInertiaFrame.inverse() * childTrans, childShape); diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h index aa57abdae..34a28f15d 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h @@ -19,7 +19,7 @@ class BulletURDFImporter : public URDFImporterInterface struct BulletURDFInternalData* m_data; public: - BulletURDFImporter(struct GUIHelperInterface* helper, UrdfRenderingInterface* customConverter, double globalScaling = 1, int flags = 0); + BulletURDFImporter(struct GUIHelperInterface* helper, UrdfRenderingInterface* customConverter, struct CommonFileIOInterface* fileIO,double globalScaling, int flags); virtual ~BulletURDFImporter(); diff --git a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp index 661a8162f..45a4c630b 100644 --- a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp +++ b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp @@ -171,7 +171,8 @@ void ImportUrdfSetup::initPhysics() int flags = 0; double globalScaling = 1; - BulletURDFImporter u2b(m_guiHelper, 0, globalScaling, flags); + + BulletURDFImporter u2b(m_guiHelper, 0, 0, globalScaling, flags); bool loadOk = u2b.loadURDF(m_fileName); diff --git a/examples/Importers/ImportURDFDemo/UrdfFindMeshFile.h b/examples/Importers/ImportURDFDemo/UrdfFindMeshFile.h new file mode 100644 index 000000000..22913210c --- /dev/null +++ b/examples/Importers/ImportURDFDemo/UrdfFindMeshFile.h @@ -0,0 +1,116 @@ + +#ifndef URDF_FIND_MESH_FILE_H +#define URDF_FIND_MESH_FILE_H + +#include "../../CommonInterfaces/CommonFileIOInterface.h" +#include "Bullet3Common/b3Logging.h" + +#include +#include + +#include "UrdfParser.h" + +static bool UrdfFindMeshFile( + CommonFileIOInterface* fileIO, + const std::string& urdf_path, std::string fn, + const std::string& error_message_prefix, + std::string* out_found_filename, int* out_type) +{ + if (fn.size() <= 4) + { + b3Warning("%s: invalid mesh filename '%s'\n", error_message_prefix.c_str(), fn.c_str()); + return false; + } + + std::string ext; + std::string ext_ = fn.substr(fn.size() - 4); + for (std::string::iterator i = ext_.begin(); i != ext_.end(); ++i) + { + ext += char(tolower(*i)); + } + + if (ext == ".dae") + { + *out_type = UrdfGeometry::FILE_COLLADA; + } + else if (ext == ".stl") + { + *out_type = UrdfGeometry::FILE_STL; + } + else if (ext == ".obj") + { + *out_type = UrdfGeometry::FILE_OBJ; + } + else if (ext == ".cdf") + { + *out_type = UrdfGeometry::FILE_CDF; + } + else + { + b3Warning("%s: invalid mesh filename extension '%s'\n", error_message_prefix.c_str(), ext.c_str()); + return false; + } + + std::string drop_it_pack = "package://"; + std::string drop_it_model = "model://"; + if (fn.substr(0, drop_it_pack.length()) == drop_it_pack) + fn = fn.substr(drop_it_pack.length()); + else if (fn.substr(0, drop_it_model.length()) == drop_it_model) + fn = fn.substr(drop_it_model.length()); + + std::list shorter; + shorter.push_back("../.."); + shorter.push_back(".."); + shorter.push_back("."); + int cnt = urdf_path.size(); + for (int i = 0; i < cnt; ++i) + { + if (urdf_path[i] == '/' || urdf_path[i] == '\\') + { + shorter.push_back(urdf_path.substr(0, i)); + } + } + shorter.reverse(); + + std::string existing_file; + + { + std::string attempt = fn; + int f = fileIO->fileOpen(attempt.c_str(), "rb"); + if (f>=0) + { + existing_file = attempt; + fileIO->fileClose(f); + } + } + if (existing_file.empty()) + { + for (std::list::iterator x = shorter.begin(); x != shorter.end(); ++x) + { + std::string attempt = *x + "/" + fn; + int f = fileIO->fileOpen(attempt.c_str(), "rb"); + if (f<0) + { + //b3Printf("%s: tried '%s'", error_message_prefix.c_str(), attempt.c_str()); + continue; + } + fileIO->fileClose(f); + existing_file = attempt; + //b3Printf("%s: found '%s'", error_message_prefix.c_str(), attempt.c_str()); + break; + } + } + + if (existing_file.empty()) + { + b3Warning("%s: cannot find '%s' in any directory in urdf path\n", error_message_prefix.c_str(), fn.c_str()); + return false; + } + else + { + *out_found_filename = existing_file; + return true; + } +} + +#endif //URDF_FIND_MESH_FILE_H \ No newline at end of file diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index 4c47e30a1..3dd782bf7 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -2,12 +2,15 @@ #include "../../ThirdPartyLibs/tinyxml2/tinyxml2.h" #include "urdfStringSplit.h" #include "urdfLexicalCast.h" +#include "UrdfFindMeshFile.h" + using namespace tinyxml2; -UrdfParser::UrdfParser() +UrdfParser::UrdfParser(CommonFileIOInterface* fileIO) : m_parseSDF(false), m_activeSdfModel(-1), - m_urdfScaling(1) + m_urdfScaling(1), + m_fileIO(fileIO) { m_urdf2Model.m_sourceFile = "IN_MEMORY_STRING"; // if loadUrdf() called later, source file name will be replaced with real } @@ -495,7 +498,7 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, XMLElement* g, ErrorLogger* l } geom.m_meshFileName = fn; - bool success = findExistingMeshFile( + bool success = UrdfFindMeshFile(m_fileIO, m_urdf2Model.m_sourceFile, fn, sourceFileLocation(shape), &geom.m_meshFileName, &geom.m_meshFileType); if (!success) diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index eff72d593..d2550ac82 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -106,9 +106,6 @@ struct UrdfGeometry } }; -bool findExistingMeshFile(const std::string& urdf_path, std::string fn, - const std::string& error_message_prefix, - std::string* out_found_filename, int* out_type); // intended to fill UrdfGeometry::m_meshFileName and Type, but can be used elsewhere struct UrdfShape { @@ -256,6 +253,9 @@ protected: int m_activeSdfModel; btScalar m_urdfScaling; + + struct CommonFileIOInterface* m_fileIO; + bool parseTransform(btTransform& tr, tinyxml2::XMLElement* xml, ErrorLogger* logger, bool parseSDF = false); bool parseInertia(UrdfInertia& inertia, tinyxml2::XMLElement* config, ErrorLogger* logger); bool parseGeometry(UrdfGeometry& geom, tinyxml2::XMLElement* g, ErrorLogger* logger); @@ -270,7 +270,7 @@ protected: bool parseSensor(UrdfModel& model, UrdfLink& link, UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger); public: - UrdfParser(); + UrdfParser(struct CommonFileIOInterface* fileIO); virtual ~UrdfParser(); void setParseSDF(bool useSDF) diff --git a/examples/Importers/ImportURDFDemo/UrdfRenderingInterface.h b/examples/Importers/ImportURDFDemo/UrdfRenderingInterface.h index de2ddbe51..a9c1ba57d 100644 --- a/examples/Importers/ImportURDFDemo/UrdfRenderingInterface.h +++ b/examples/Importers/ImportURDFDemo/UrdfRenderingInterface.h @@ -15,7 +15,7 @@ struct UrdfRenderingInterface virtual ~UrdfRenderingInterface() {} ///given a URDF link, convert all visual shapes into internal renderer (loading graphics meshes, textures etc) ///use the collisionObjectUid as a unique identifier to synchronize the world transform and to remove the visual shape. - virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUid, int bodyUniqueId) = 0; + virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO) = 0; ///remove a visual shapes, based on the shape unique id (shapeUid) virtual void removeVisualShape(int collisionObjectUid) = 0; diff --git a/examples/InverseDynamics/InverseDynamicsExample.cpp b/examples/InverseDynamics/InverseDynamicsExample.cpp index d424b48d4..fc112c189 100644 --- a/examples/InverseDynamics/InverseDynamicsExample.cpp +++ b/examples/InverseDynamics/InverseDynamicsExample.cpp @@ -152,7 +152,7 @@ void InverseDynamicsExample::initPhysics() { case BT_ID_LOAD_URDF: { - BulletURDFImporter u2b(m_guiHelper, 0, 1, 0); + BulletURDFImporter u2b(m_guiHelper, 0, 0, 1, 0); bool loadOk = u2b.loadURDF("kuka_iiwa/model.urdf"); // lwr / kuka.urdf"); if (loadOk) { diff --git a/examples/MultiBody/TestJointTorqueSetup.cpp b/examples/MultiBody/TestJointTorqueSetup.cpp index cfa4ccf76..27122ba3c 100644 --- a/examples/MultiBody/TestJointTorqueSetup.cpp +++ b/examples/MultiBody/TestJointTorqueSetup.cpp @@ -346,7 +346,7 @@ void TestJointTorqueSetup::initPhysics() btSerializer* s = new btDefaultSerializer; m_dynamicsWorld->serialize(s); char resourcePath[1024]; - if (b3ResourcePath::findResourcePath("multibody.bullet", resourcePath, 1024)) + if (b3ResourcePath::findResourcePath("multibody.bullet", resourcePath, 1024,0)) { FILE* f = fopen(resourcePath, "wb"); fwrite(s->getBufferPointer(), s->getCurrentBufferSize(), 1, f); diff --git a/examples/RenderingExamples/TinyRendererSetup.cpp b/examples/RenderingExamples/TinyRendererSetup.cpp index e54dfa37b..2c81995ee 100644 --- a/examples/RenderingExamples/TinyRendererSetup.cpp +++ b/examples/RenderingExamples/TinyRendererSetup.cpp @@ -16,7 +16,7 @@ #include "../Importers/ImportMeshUtility/b3ImportMeshUtility.h" #include "../OpenGLWindow/GLInstanceGraphicsShape.h" #include "../CommonInterfaces/CommonParameterInterface.h" - +#include "../Utils/b3BulletDefaultFileIO.h" struct TinyRendererSetupInternalData { TGAImage m_rgbColorBuffer; @@ -149,7 +149,8 @@ TinyRendererSetup::TinyRendererSetup(struct GUIHelperInterface* gui) int shapeId = -1; b3ImportMeshData meshData; - if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fileName, meshData)) + b3BulletDefaultFileIO fileIO; + if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fileName, meshData,&fileIO)) { int textureIndex = -1; diff --git a/examples/RenderingExamples/TinyVRGui.cpp b/examples/RenderingExamples/TinyVRGui.cpp index aeacea5f1..8d0170225 100644 --- a/examples/RenderingExamples/TinyVRGui.cpp +++ b/examples/RenderingExamples/TinyVRGui.cpp @@ -7,6 +7,7 @@ #include "../RenderingExamples/TimeSeriesFontData.h" #include "../Importers/ImportMeshUtility/b3ImportMeshUtility.h" #include "../OpenGLWindow/GLInstanceGraphicsShape.h" +#include "../Utils/b3BulletDefaultFileIO.h" #include "../CommonInterfaces/CommonRenderInterface.h" #include "../CommonInterfaces/CommonParameterInterface.h" @@ -129,7 +130,8 @@ bool TinyVRGui::init() int shapeId = -1; b3ImportMeshData meshData; - if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fileName, meshData)) + b3BulletDefaultFileIO fileIO; + if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fileName, meshData,&fileIO)) { shapeId = m_data->m_renderer->registerShape(&meshData.m_gfxShape->m_vertices->at(0).xyzw[0], meshData.m_gfxShape->m_numvertices, diff --git a/examples/RollingFrictionDemo/RollingFrictionDemo.cpp b/examples/RollingFrictionDemo/RollingFrictionDemo.cpp index 23e192011..4fab5104b 100644 --- a/examples/RollingFrictionDemo/RollingFrictionDemo.cpp +++ b/examples/RollingFrictionDemo/RollingFrictionDemo.cpp @@ -218,7 +218,7 @@ void RollingFrictionDemo::initPhysics() btSerializer* s = new btDefaultSerializer; m_dynamicsWorld->serialize(s); char resourcePath[1024]; - if (b3ResourcePath::findResourcePath("slope.bullet", resourcePath, 1024)) + if (b3ResourcePath::findResourcePath("slope.bullet", resourcePath, 1024,0)) { FILE* f = fopen(resourcePath, "wb"); fwrite(s->getBufferPointer(), s->getCurrentBufferSize(), 1, f); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 9261eb807..eb2b4e747 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -4,6 +4,8 @@ #include "../Importers/ImportURDFDemo/BulletUrdfImporter.h" #include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h" #include "../Importers/ImportURDFDemo/URDF2Bullet.h" +#include "../Importers/ImportURDFDemo/UrdfFindMeshFile.h" + #include "../Extras/InverseDynamics/btMultiBodyTreeCreator.hpp" #include "BulletCollision/CollisionDispatch/btInternalEdgeUtility.h" @@ -54,6 +56,7 @@ #include "plugins/collisionFilterPlugin/collisionFilterPlugin.h" #endif + #ifdef ENABLE_STATIC_GRPC_PLUGIN #include "plugins/grpcPlugin/grpcPlugin.h" #endif //ENABLE_STATIC_GRPC_PLUGIN @@ -74,6 +77,10 @@ #include "plugins/tinyRendererPlugin/tinyRendererPlugin.h" #endif +#ifndef B3_ENABLE_FILEIO_PLUGIN +#include "plugins/fileIOPlugin/fileIOPlugin.h" +#endif//B3_DISABLE_FILEIO_PLUGIN + #ifdef B3_ENABLE_TINY_AUDIO #include "../TinyAudio/b3SoundEngine.h" #endif @@ -104,6 +111,11 @@ btQuaternion gVRTeleportOrn(0, 0, 0, 1); btScalar simTimeScalingFactor = 1; btScalar gRhsClamp = 1.f; +#include "../CommonInterfaces/CommonFileIOInterface.h" + + + + struct UrdfLinkNameMapUtil { btMultiBody* m_mb; @@ -1639,6 +1651,7 @@ struct PhysicsServerCommandProcessorInternalData std::string m_profileTimingFileName; struct GUIHelperInterface* m_guiHelper; + int m_sharedMemoryKey; bool m_enableTinyRenderer; @@ -1704,43 +1717,66 @@ struct PhysicsServerCommandProcessorInternalData { //register static plugins: #ifdef STATIC_LINK_VR_PLUGIN - m_pluginManager.registerStaticLinkedPlugin("vrSyncPlugin", initPlugin_vrSyncPlugin, exitPlugin_vrSyncPlugin, executePluginCommand_vrSyncPlugin, preTickPluginCallback_vrSyncPlugin, 0, 0, 0, 0); + b3PluginFunctions funcs(initPlugin_vrSyncPlugin,exitPlugin_vrSyncPlugin, executePluginCommand_vrSyncPlugin); + funcs.m_preTickFunc = preTickPluginCallback_vrSyncPlugin; + m_pluginManager.registerStaticLinkedPlugin("vrSyncPlugin", funcs); #endif //STATIC_LINK_VR_PLUGIN - + } #ifndef SKIP_STATIC_PD_CONTROL_PLUGIN { - m_pdControlPlugin = m_pluginManager.registerStaticLinkedPlugin("pdControlPlugin", initPlugin_pdControlPlugin, exitPlugin_pdControlPlugin, executePluginCommand_pdControlPlugin, preTickPluginCallback_pdControlPlugin, 0, 0, 0, 0); + //int b3PluginManager::registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT initFunc, PFN_EXIT exitFunc, PFN_EXECUTE executeCommandFunc, PFN_TICK preTickFunc, PFN_TICK postTickFunc, PFN_GET_RENDER_INTERFACE getRendererFunc, PFN_TICK processClientCommandsFunc, PFN_GET_COLLISION_INTERFACE getCollisionFunc, bool initPlugin) + b3PluginFunctions funcs(initPlugin_pdControlPlugin,exitPlugin_pdControlPlugin,executePluginCommand_pdControlPlugin); + funcs.m_preTickFunc = preTickPluginCallback_pdControlPlugin; + m_pdControlPlugin = m_pluginManager.registerStaticLinkedPlugin("pdControlPlugin", funcs); } #endif //SKIP_STATIC_PD_CONTROL_PLUGIN #ifndef SKIP_COLLISION_FILTER_PLUGIN { - m_collisionFilterPlugin = m_pluginManager.registerStaticLinkedPlugin("collisionFilterPlugin", initPlugin_collisionFilterPlugin, exitPlugin_collisionFilterPlugin, executePluginCommand_collisionFilterPlugin, 0, 0, 0, 0, getCollisionInterface_collisionFilterPlugin); + b3PluginFunctions funcs(initPlugin_collisionFilterPlugin,exitPlugin_collisionFilterPlugin, executePluginCommand_collisionFilterPlugin); + funcs.m_getCollisionFunc = getCollisionInterface_collisionFilterPlugin; + m_collisionFilterPlugin = m_pluginManager.registerStaticLinkedPlugin("collisionFilterPlugin", funcs ); m_pluginManager.selectCollisionPlugin(m_collisionFilterPlugin); } #endif #ifdef ENABLE_STATIC_GRPC_PLUGIN { - m_grpcPlugin = m_pluginManager.registerStaticLinkedPlugin("grpcPlugin", initPlugin_grpcPlugin, exitPlugin_grpcPlugin, executePluginCommand_grpcPlugin, 0, 0, 0, processClientCommands_grpcPlugin, 0); + b3PluginFunctions funcs(initPlugin_grpcPlugin, exitPlugin_grpcPlugin, executePluginCommand_grpcPlugin); + funcs.m_processClientCommandsFunc = processClientCommands_grpcPlugin; + m_grpcPlugin = m_pluginManager.registerStaticLinkedPlugin("grpcPlugin", funcs); } #endif //ENABLE_STATIC_GRPC_PLUGIN #ifdef STATIC_EGLRENDERER_PLUGIN { bool initPlugin = false; - int renderPluginId = m_pluginManager.registerStaticLinkedPlugin("eglRendererPlugin", initPlugin_eglRendererPlugin, exitPlugin_eglRendererPlugin, executePluginCommand_eglRendererPlugin, 0, 0, getRenderInterface_eglRendererPlugin, 0, 0, initPlugin); + b3PluginFunctions funcs(initPlugin_eglRendererPlugin, exitPlugin_eglRendererPlugin, executePluginCommand_eglRendererPlugin); + funcs.m_getRendererFunc = getRenderInterface_eglRendererPlugin; + int renderPluginId = m_pluginManager.registerStaticLinkedPlugin("eglRendererPlugin", funcs, initPlugin); m_pluginManager.selectPluginRenderer(renderPluginId); } #endif //STATIC_EGLRENDERER_PLUGIN #ifndef SKIP_STATIC_TINYRENDERER_PLUGIN { - int renderPluginId = m_pluginManager.registerStaticLinkedPlugin("tinyRendererPlugin", initPlugin_tinyRendererPlugin, exitPlugin_tinyRendererPlugin, executePluginCommand_tinyRendererPlugin, 0, 0, getRenderInterface_tinyRendererPlugin, 0, 0); + b3PluginFunctions funcs(initPlugin_tinyRendererPlugin, exitPlugin_tinyRendererPlugin, executePluginCommand_tinyRendererPlugin); + funcs.m_getRendererFunc=getRenderInterface_tinyRendererPlugin; + int renderPluginId = m_pluginManager.registerStaticLinkedPlugin("tinyRendererPlugin", funcs); m_pluginManager.selectPluginRenderer(renderPluginId); } #endif - } + +#ifdef B3_ENABLE_FILEIO_PLUGIN + { + b3PluginFunctions funcs(initPlugin_fileIOPlugin, exitPlugin_fileIOPlugin, executePluginCommand_fileIOPlugin); + funcs.m_fileIoFunc = getFileIOFunc_fileIOPlugin; + int renderPluginId = m_pluginManager.registerStaticLinkedPlugin("fileIOPlugin", funcs); + m_pluginManager.selectFileIOPlugin(renderPluginId); + } +#endif + + m_vrControllerEvents.init(); @@ -2212,7 +2248,9 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface int graphicsIndex = -1; double globalScaling = 1.f; //todo! int flags = 0; - BulletURDFImporter u2b(m_data->m_guiHelper, m_data->m_pluginManager.getRenderInterface(), globalScaling, flags); + CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface(); + + BulletURDFImporter u2b(m_data->m_guiHelper, m_data->m_pluginManager.getRenderInterface(),fileIO, globalScaling, flags); u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer); btAlignedObjectArray vertices; @@ -2300,7 +2338,8 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface //UrdfLink*const* linkPtr = model.m_links.getAtIndex(urdfIndex); if (m_data->m_pluginManager.getRenderInterface()) { - m_data->m_pluginManager.getRenderInterface()->convertVisualShapes(linkIndex, pathPrefix, localInertiaFrame, &link, &model, colObj->getBroadphaseHandle()->getUid(), bodyUniqueId); + CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface(); + m_data->m_pluginManager.getRenderInterface()->convertVisualShapes(linkIndex, pathPrefix, localInertiaFrame, &link, &model, colObj->getBroadphaseHandle()->getUid(), bodyUniqueId, fileIO); } } virtual void setBodyUniqueId(int bodyId) @@ -3015,7 +3054,8 @@ bool PhysicsServerCommandProcessor::loadMjcf(const char* fileName, char* bufferS m_data->m_sdfRecentLoadedBodies.clear(); - BulletMJCFImporter u2b(m_data->m_guiHelper, m_data->m_pluginManager.getRenderInterface(), flags); + CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface(); + BulletMJCFImporter u2b(m_data->m_guiHelper, m_data->m_pluginManager.getRenderInterface(), fileIO, flags); bool useFixedBase = false; MyMJCFLogger2 logger; @@ -3037,8 +3077,8 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe } m_data->m_sdfRecentLoadedBodies.clear(); - - BulletURDFImporter u2b(m_data->m_guiHelper, m_data->m_pluginManager.getRenderInterface(), globalScaling, flags); + CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface(); + BulletURDFImporter u2b(m_data->m_guiHelper, m_data->m_pluginManager.getRenderInterface(), fileIO, globalScaling, flags); u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer); bool forceFixedBase = false; @@ -3067,7 +3107,8 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto return false; } - BulletURDFImporter u2b(m_data->m_guiHelper, m_data->m_pluginManager.getRenderInterface(), globalScaling, flags); + CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface(); + BulletURDFImporter u2b(m_data->m_guiHelper, m_data->m_pluginManager.getRenderInterface(), fileIO, globalScaling, flags); u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer); bool loadOk = u2b.loadURDF(fileName, useFixedBase); @@ -4101,9 +4142,9 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str urdfColObj.m_geometry.m_meshFileName = fileName; urdfColObj.m_geometry.m_meshScale = meshScale; - + CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface(); pathPrefix[0] = 0; - if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024)) + if (fileIO->findResourcePath(fileName.c_str(), relativeFileName, 1024)) { b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); } @@ -4112,14 +4153,16 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str std::string out_found_filename; int out_type; - bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type); + + bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type); if (foundFile) { urdfColObj.m_geometry.m_meshFileType = out_type; if (out_type == UrdfGeometry::FILE_STL) { - glmesh = LoadMeshFromSTL(relativeFileName); + CommonFileIOInterface* fileIO(m_data->m_pluginManager.getFileIOInterface()); + glmesh = LoadMeshFromSTL(relativeFileName,fileIO); } if (out_type == UrdfGeometry::FILE_OBJ) { @@ -4127,12 +4170,13 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags & GEOM_FORCE_CONCAVE_TRIMESH) { - glmesh = LoadMeshFromObj(relativeFileName, pathPrefix); + CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface(); + glmesh = LoadMeshFromObj(relativeFileName, pathPrefix,fileIO); } else { std::vector shapes; - std::string err = tinyobj::LoadObj(shapes, out_found_filename.c_str()); + std::string err = tinyobj::LoadObj(shapes, out_found_filename.c_str(),"",fileIO); //shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale); //static btCollisionShape* createConvexHullFromShapes(std::vector& shapes, const btVector3& geomScale) @@ -4309,7 +4353,8 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct serverStatusOut.m_type = CMD_CREATE_VISUAL_SHAPE_FAILED; double globalScaling = 1.f; int flags = 0; - BulletURDFImporter u2b(m_data->m_guiHelper, m_data->m_pluginManager.getRenderInterface(), globalScaling, flags); + CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface(); + BulletURDFImporter u2b(m_data->m_guiHelper, m_data->m_pluginManager.getRenderInterface(), fileIO, globalScaling, flags); u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer); btTransform localInertiaFrame; localInertiaFrame.setIdentity(); @@ -4374,12 +4419,12 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct const std::string& error_message_prefix = ""; std::string out_found_filename; int out_type; - if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024)) + if (fileIO->findResourcePath(fileName.c_str(), relativeFileName, 1024)) { b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); } - bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type); + bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type); visualShape.m_geometry.m_meshFileType = out_type; visualShape.m_geometry.m_meshFileName = fileName; @@ -6692,19 +6737,21 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar } { + CommonFileIOInterface* fileIO(m_data->m_pluginManager.getFileIOInterface()); char relativeFileName[1024]; char pathPrefix[1024]; pathPrefix[0] = 0; - if (b3ResourcePath::findResourcePath(loadSoftBodyArgs.m_fileName, relativeFileName, 1024)) + if (fileIO->findResourcePath(loadSoftBodyArgs.m_fileName, relativeFileName, 1024)) { b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); } const std::string& error_message_prefix = ""; std::string out_found_filename; int out_type; - bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type); + + bool foundFile = UrdfFindMeshFile(fileIO,pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type); std::vector shapes; - std::string err = tinyobj::LoadObj(shapes, out_found_filename.c_str()); + std::string err = tinyobj::LoadObj(shapes, out_found_filename.c_str(),"",fileIO); if (shapes.size() > 0) { const tinyobj::shape_t& shape = shapes[0]; @@ -9802,7 +9849,8 @@ bool PhysicsServerCommandProcessor::processLoadTextureCommand(const struct Share char relativeFileName[1024]; char pathPrefix[1024]; - if (b3ResourcePath::findResourcePath(clientCmd.m_loadTextureArguments.m_textureFileName, relativeFileName, 1024)) + CommonFileIOInterface* fileIO(m_data->m_pluginManager.getFileIOInterface()); + if (fileIO->findResourcePath(clientCmd.m_loadTextureArguments.m_textureFileName, relativeFileName, 1024)) { b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index b2d790c25..d12475707 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -1520,48 +1520,51 @@ public: if (gEnableDefaultKeyboardShortcuts) { - if (key == 'w' && state) + if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera()) { - VRTeleportPos[0] += shift; - m_physicsServer.setVRTeleportPosition(VRTeleportPos); - saveCurrentSettingsVR(VRTeleportPos); - } - if (key == 's' && state) - { - VRTeleportPos[0] -= shift; - m_physicsServer.setVRTeleportPosition(VRTeleportPos); - saveCurrentSettingsVR(VRTeleportPos); - } - if (key == 'a' && state) - { - VRTeleportPos[1] -= shift; - m_physicsServer.setVRTeleportPosition(VRTeleportPos); - saveCurrentSettingsVR(VRTeleportPos); - } - if (key == 'd' && state) - { - VRTeleportPos[1] += shift; - m_physicsServer.setVRTeleportPosition(VRTeleportPos); - saveCurrentSettingsVR(VRTeleportPos); - } - if (key == 'q' && state) - { - VRTeleportPos[2] += shift; - m_physicsServer.setVRTeleportPosition(VRTeleportPos); - saveCurrentSettingsVR(VRTeleportPos); - } - if (key == 'e' && state) - { - VRTeleportPos[2] -= shift; - m_physicsServer.setVRTeleportPosition(VRTeleportPos); - saveCurrentSettingsVR(VRTeleportPos); - } - if (key == 'z' && state) - { - gVRTeleportRotZ += shift; - btQuaternion VRTeleportOrn = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ); - m_physicsServer.setVRTeleportOrientation(VRTeleportOrn); - saveCurrentSettingsVR(VRTeleportPos); + if (key == 'w' && state) + { + VRTeleportPos[0] += shift; + m_physicsServer.setVRTeleportPosition(VRTeleportPos); + saveCurrentSettingsVR(VRTeleportPos); + } + if (key == 's' && state) + { + VRTeleportPos[0] -= shift; + m_physicsServer.setVRTeleportPosition(VRTeleportPos); + saveCurrentSettingsVR(VRTeleportPos); + } + if (key == 'a' && state) + { + VRTeleportPos[1] -= shift; + m_physicsServer.setVRTeleportPosition(VRTeleportPos); + saveCurrentSettingsVR(VRTeleportPos); + } + if (key == 'd' && state) + { + VRTeleportPos[1] += shift; + m_physicsServer.setVRTeleportPosition(VRTeleportPos); + saveCurrentSettingsVR(VRTeleportPos); + } + if (key == 'q' && state) + { + VRTeleportPos[2] += shift; + m_physicsServer.setVRTeleportPosition(VRTeleportPos); + saveCurrentSettingsVR(VRTeleportPos); + } + if (key == 'e' && state) + { + VRTeleportPos[2] -= shift; + m_physicsServer.setVRTeleportPosition(VRTeleportPos); + saveCurrentSettingsVR(VRTeleportPos); + } + if (key == 'z' && state) + { + gVRTeleportRotZ += shift; + btQuaternion VRTeleportOrn = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ); + m_physicsServer.setVRTeleportOrientation(VRTeleportOrn); + saveCurrentSettingsVR(VRTeleportPos); + } } } diff --git a/examples/SharedMemory/b3PluginManager.cpp b/examples/SharedMemory/b3PluginManager.cpp index 531275060..fa06cd52b 100644 --- a/examples/SharedMemory/b3PluginManager.cpp +++ b/examples/SharedMemory/b3PluginManager.cpp @@ -5,7 +5,7 @@ #include "PhysicsClientC_API.h" #include "PhysicsDirect.h" #include "plugins/b3PluginContext.h" - +#include "../Utils/b3BulletDefaultFileIO.h" #ifdef _WIN32 #define WIN32_LEAN_AND_MEAN #define VC_EXTRALEAN @@ -48,6 +48,7 @@ struct b3Plugin PFN_GET_RENDER_INTERFACE m_getRendererFunc; PFN_GET_COLLISION_INTERFACE m_getCollisionFunc; + PFN_GET_FILEIO_INTERFACE m_getFileIOFunc; void* m_userPointer; @@ -65,6 +66,7 @@ struct b3Plugin m_processClientCommandsFunc(0), m_getRendererFunc(0), m_getCollisionFunc(0), + m_getFileIOFunc(0), m_userPointer(0) { } @@ -84,6 +86,7 @@ struct b3Plugin m_processClientCommandsFunc = 0; m_getRendererFunc = 0; m_getCollisionFunc = 0; + m_getFileIOFunc = 0; m_userPointer = 0; m_isInitialized = false; } @@ -105,9 +108,11 @@ struct b3PluginManagerInternalData int m_activeRendererPluginUid; int m_activeCollisionPluginUid; int m_numNotificationPlugins; - + int m_activeFileIOPluginUid; + b3BulletDefaultFileIO m_defaultFileIO; + b3PluginManagerInternalData() - : m_rpcCommandProcessorInterface(0), m_activeNotificationsBufferIndex(0), m_activeRendererPluginUid(-1), m_activeCollisionPluginUid(-1), m_numNotificationPlugins(0) + : m_rpcCommandProcessorInterface(0), m_activeNotificationsBufferIndex(0), m_activeRendererPluginUid(-1), m_activeCollisionPluginUid(-1), m_numNotificationPlugins(0), m_activeFileIOPluginUid(-1) { } }; @@ -208,7 +213,8 @@ int b3PluginManager::loadPlugin(const char* pluginPath, const char* postFixStr) std::string processClientCommandsStr = std::string("processClientCommands") + postFix; std::string getRendererStr = std::string("getRenderInterface") + postFix; std::string getCollisionStr = std::string("getCollisionInterface") + postFix; - + std::string getFileIOStr = std::string("getFileIOInterface") + postFix; + plugin->m_initFunc = (PFN_INIT)B3_DYNLIB_IMPORT(pluginHandle, initStr.c_str()); plugin->m_exitFunc = (PFN_EXIT)B3_DYNLIB_IMPORT(pluginHandle, exitStr.c_str()); plugin->m_executeCommandFunc = (PFN_EXECUTE)B3_DYNLIB_IMPORT(pluginHandle, executePluginCommandStr.c_str()); @@ -224,6 +230,8 @@ int b3PluginManager::loadPlugin(const char* pluginPath, const char* postFixStr) plugin->m_getRendererFunc = (PFN_GET_RENDER_INTERFACE)B3_DYNLIB_IMPORT(pluginHandle, getRendererStr.c_str()); plugin->m_getCollisionFunc = (PFN_GET_COLLISION_INTERFACE)B3_DYNLIB_IMPORT(pluginHandle, getCollisionStr.c_str()); + plugin->m_getFileIOFunc = (PFN_GET_FILEIO_INTERFACE)B3_DYNLIB_IMPORT(pluginHandle, getFileIOStr.c_str()); + if (plugin->m_initFunc && plugin->m_exitFunc && plugin->m_executeCommandFunc) { @@ -293,6 +301,15 @@ int b3PluginManager::loadPlugin(const char* pluginPath, const char* postFixStr) selectCollisionPlugin(pluginUniqueId); } } + //for now, automatically select the loaded plugin as active fileIO plugin. + if (pluginUniqueId >= 0) + { + b3PluginHandle* plugin = m_data->m_plugins.getHandle(pluginUniqueId); + if (plugin && plugin->m_getFileIOFunc) + { + selectFileIOPlugin(pluginUniqueId); + } + } return pluginUniqueId; } @@ -438,7 +455,9 @@ int b3PluginManager::executePluginCommand(int pluginUniqueId, const b3PluginArgu return result; } -int b3PluginManager::registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT initFunc, PFN_EXIT exitFunc, PFN_EXECUTE executeCommandFunc, PFN_TICK preTickFunc, PFN_TICK postTickFunc, PFN_GET_RENDER_INTERFACE getRendererFunc, PFN_TICK processClientCommandsFunc, PFN_GET_COLLISION_INTERFACE getCollisionFunc, bool initPlugin) + + +int b3PluginManager::registerStaticLinkedPlugin(const char* pluginPath, b3PluginFunctions& functions, bool initPlugin) { b3Plugin orgPlugin; @@ -447,14 +466,15 @@ int b3PluginManager::registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT pluginHandle->m_pluginHandle = 0; pluginHandle->m_ownsPluginHandle = false; pluginHandle->m_pluginUniqueId = pluginUniqueId; - pluginHandle->m_executeCommandFunc = executeCommandFunc; - pluginHandle->m_exitFunc = exitFunc; - pluginHandle->m_initFunc = initFunc; - pluginHandle->m_preTickFunc = preTickFunc; - pluginHandle->m_postTickFunc = postTickFunc; - pluginHandle->m_getRendererFunc = getRendererFunc; - pluginHandle->m_getCollisionFunc = getCollisionFunc; - pluginHandle->m_processClientCommandsFunc = processClientCommandsFunc; + pluginHandle->m_executeCommandFunc = functions.m_executeCommandFunc; + pluginHandle->m_exitFunc = functions.m_exitFunc; + pluginHandle->m_initFunc = functions.m_initFunc; + pluginHandle->m_preTickFunc = functions.m_preTickFunc; + pluginHandle->m_postTickFunc = functions.m_postTickFunc; + pluginHandle->m_getRendererFunc = functions.m_getRendererFunc; + pluginHandle->m_getCollisionFunc = functions.m_getCollisionFunc; + pluginHandle->m_processClientCommandsFunc = functions.m_processClientCommandsFunc; + pluginHandle->m_getFileIOFunc = functions.m_fileIoFunc; pluginHandle->m_pluginHandle = 0; pluginHandle->m_pluginPath = pluginPath; pluginHandle->m_userPointer = 0; @@ -502,6 +522,32 @@ UrdfRenderingInterface* b3PluginManager::getRenderInterface() return renderer; } +void b3PluginManager::selectFileIOPlugin(int pluginUniqueId) +{ + m_data->m_activeFileIOPluginUid = pluginUniqueId; +} + +struct CommonFileIOInterface* b3PluginManager::getFileIOInterface() +{ + CommonFileIOInterface* fileIOInterface = 0; + if (m_data->m_activeFileIOPluginUid >= 0) + { + b3PluginHandle* plugin = m_data->m_plugins.getHandle(m_data->m_activeFileIOPluginUid); + if (plugin && plugin->m_getFileIOFunc) + { + b3PluginContext context = {0}; + context.m_userPointer = plugin->m_userPointer; + context.m_physClient = (b3PhysicsClientHandle)m_data->m_physicsDirect; + fileIOInterface = plugin->m_getFileIOFunc(&context); + } + } + if (fileIOInterface==0) + { + return &m_data->m_defaultFileIO; + } + return fileIOInterface; +} + void b3PluginManager::selectCollisionPlugin(int pluginUniqueId) { m_data->m_activeCollisionPluginUid = pluginUniqueId; diff --git a/examples/SharedMemory/b3PluginManager.h b/examples/SharedMemory/b3PluginManager.h index f58cd8c17..4beb05cd6 100644 --- a/examples/SharedMemory/b3PluginManager.h +++ b/examples/SharedMemory/b3PluginManager.h @@ -10,6 +10,37 @@ enum b3PluginManagerTickMode B3_PROCESS_CLIENT_COMMANDS_TICK, }; +struct b3PluginFunctions +{ + //required + PFN_INIT m_initFunc; + PFN_EXIT m_exitFunc; + PFN_EXECUTE m_executeCommandFunc; + + //optional + PFN_TICK m_preTickFunc; + PFN_TICK m_postTickFunc; + PFN_GET_RENDER_INTERFACE m_getRendererFunc; + PFN_TICK m_processClientCommandsFunc; + PFN_TICK m_processNotificationsFunc; + PFN_GET_COLLISION_INTERFACE m_getCollisionFunc; + PFN_GET_FILEIO_INTERFACE m_fileIoFunc; + + b3PluginFunctions(PFN_INIT initFunc, PFN_EXIT exitFunc, PFN_EXECUTE executeCommandFunc) + :m_initFunc(initFunc), + m_exitFunc(exitFunc), + m_executeCommandFunc(executeCommandFunc), + m_preTickFunc(0), + m_postTickFunc(0), + m_getRendererFunc(0), + m_processClientCommandsFunc(0), + m_processNotificationsFunc(0), + m_getCollisionFunc(0), + m_fileIoFunc(0) + { + } +}; + class b3PluginManager { struct b3PluginManagerInternalData* m_data; @@ -29,11 +60,14 @@ public: void tickPlugins(double timeStep, b3PluginManagerTickMode tickMode); - int registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT initFunc, PFN_EXIT exitFunc, PFN_EXECUTE executeCommandFunc, PFN_TICK preTickFunc, PFN_TICK postTickFunc, PFN_GET_RENDER_INTERFACE getRendererFunc, PFN_TICK processClientCommandsFunc, PFN_GET_COLLISION_INTERFACE getCollisionFunc, bool initPlugin = true); + int registerStaticLinkedPlugin(const char* pluginPath, b3PluginFunctions& functions, bool initPlugin = true); void selectPluginRenderer(int pluginUniqueId); struct UrdfRenderingInterface* getRenderInterface(); + void selectFileIOPlugin(int pluginUniqueId); + struct CommonFileIOInterface* getFileIOInterface(); + void selectCollisionPlugin(int pluginUniqueId); struct b3PluginCollisionInterface* getCollisionInterface(); }; diff --git a/examples/SharedMemory/plugins/b3PluginAPI.h b/examples/SharedMemory/plugins/b3PluginAPI.h index 18ea6365d..ef983abdb 100644 --- a/examples/SharedMemory/plugins/b3PluginAPI.h +++ b/examples/SharedMemory/plugins/b3PluginAPI.h @@ -31,7 +31,7 @@ extern "C" typedef B3_API_ENTRY struct UrdfRenderingInterface*(B3_API_CALL* PFN_GET_RENDER_INTERFACE)(struct b3PluginContext* context); typedef B3_API_ENTRY struct b3PluginCollisionInterface*(B3_API_CALL* PFN_GET_COLLISION_INTERFACE)(struct b3PluginContext* context); - + typedef B3_API_ENTRY struct CommonFileIOInterface*(B3_API_CALL* PFN_GET_FILEIO_INTERFACE)(struct b3PluginContext* context); #ifdef __cplusplus } #endif diff --git a/examples/SharedMemory/plugins/b3PluginFileIOInterface.h b/examples/SharedMemory/plugins/b3PluginFileIOInterface.h new file mode 100644 index 000000000..57c00cff3 --- /dev/null +++ b/examples/SharedMemory/plugins/b3PluginFileIOInterface.h @@ -0,0 +1 @@ +#error \ No newline at end of file diff --git a/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp b/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp new file mode 100644 index 000000000..80ebafb69 --- /dev/null +++ b/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp @@ -0,0 +1,333 @@ + + +#include "fileIOPlugin.h" +#include "../../SharedMemoryPublic.h" +#include "../b3PluginContext.h" +#include +#include "../../../CommonInterfaces/CommonFileIOInterface.h" +#include "../../../Utils/b3ResourcePath.h" +#include "../../../Utils/b3BulletDefaultFileIO.h" + + +//#define B3_USE_ZLIB +#ifdef B3_USE_ZLIB + +#include "minizip/unzip.h" + +struct MyFileIO : public CommonFileIOInterface +{ + std::string m_zipfileName; + + unzFile m_fileHandles[FILEIO_MAX_FILES]; + int m_numFileHandles; + + MyFileIO(const char* zipfileName) + :m_zipfileName(zipfileName), + m_numFileHandles(0) + { + for (int i=0;ifindFile(orgFileName, relativeFileName, maxRelativeFileNameMaxLen); + } + + virtual ~MyFileIO() + { + } + virtual int fileOpen(const char* fileName, const char* mode) + { + //search a free slot + int slot = -1; + for (int i=0;i=0) + { + unzFile zipfile; + unz_global_info m_global_info; + zipfile = unzOpen(m_zipfileName.c_str()); + if (zipfile == NULL) + { + printf("%s: not found\n", m_zipfileName.c_str()); + slot = -1; + } else + { + int result = 0; + result = unzGetGlobalInfo(zipfile, &m_global_info ); + if (result != UNZ_OK) + { + printf("could not read file global info from %s\n", m_zipfileName.c_str()); + unzClose(zipfile); + zipfile = 0; + slot = -1; + } else + { + m_fileHandles[slot] = zipfile; + } + } + if (slot >=0) + { + int result = unzLocateFile(zipfile, fileName, 0); + if (result == UNZ_OK) + { + unz_file_info info; + result = unzGetCurrentFileInfo(zipfile, &info, NULL, 0, NULL, 0, NULL, 0); + if (result != UNZ_OK) + { + printf("unzGetCurrentFileInfo() != UNZ_OK (%d)\n", result); + slot=-1; + } + else + { + result = unzOpenCurrentFile(zipfile); + if (result == UNZ_OK) + { + } else + { + slot=-1; + } + } + } + } + } + return slot; + } + virtual int fileRead(int fileHandle, char* destBuffer, int numBytes) + { + int result = -1; + if (fileHandle>=0 && fileHandle < FILEIO_MAX_FILES) + { + unzFile f = m_fileHandles[fileHandle]; + if (f) + { + result = unzReadCurrentFile(f, destBuffer,numBytes); + //::fread(destBuffer, 1, numBytes, f); + } + } + return result; + + } + virtual int fileWrite(int fileHandle,const char* sourceBuffer, int numBytes) + { +#if 0 + if (fileHandle>=0 && fileHandle < FILEIO_MAX_FILES) + { + FILE* f = m_fileHandles[fileHandle]; + if (f) + { + return ::fwrite(sourceBuffer, 1, numBytes,m_fileHandles[fileHandle]); + } + } +#endif + return -1; + } + virtual void fileClose(int fileHandle) + { + if (fileHandle>=0 && fileHandle < FILEIO_MAX_FILES) + { + unzFile f = m_fileHandles[fileHandle]; + if (f) + { + unzClose(f); + m_fileHandles[fileHandle]=0; + } + } + } + + virtual bool findResourcePath(const char* fileName, char* relativeFileName, int relativeFileNameSizeInBytes) + { + return b3ResourcePath::findResourcePath(fileName, relativeFileName, relativeFileNameSizeInBytes, MyFileIO::FileIOPluginFindFile, this); + } + + + virtual bool findFile(const char* orgFileName, char* relativeFileName, int maxRelativeFileNameMaxLen) + { + int fileHandle = -1; + fileHandle = fileOpen(orgFileName, "rb"); + if (fileHandle>=0) + { + //printf("original file found: [%s]\n", orgFileName); + sprintf(relativeFileName, "%s", orgFileName); + fileClose(fileHandle); + 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*); + + int f = 0; + bool fileFound = false; + + for (int i = 0; !f && i < numPrefixes; i++) + { +#ifdef _MSC_VER + sprintf_s(relativeFileName, maxRelativeFileNameMaxLen, "%s%s", prefix[i], orgFileName); +#else + sprintf(relativeFileName, "%s%s", prefix[i], orgFileName); +#endif + f = fileOpen(relativeFileName, "rb"); + if (f>=0) + { + fileFound = true; + break; + } + } + if (f>=0) + { + fileClose(f); + } + + return fileFound; + } + virtual char* readLine(int fileHandle, char* destBuffer, int numBytes) + { + int numRead = 0; + + if (fileHandle>=0 && fileHandle < FILEIO_MAX_FILES) + { + unzFile f = m_fileHandles[fileHandle]; + if (f) + { + //return ::fgets(destBuffer, numBytes, m_fileHandles[fileHandle]); + char c = 0; + do + { + fileRead(fileHandle,&c,1); + if (c && c!='\n') + { + if (c!=13) + { + destBuffer[numRead++]=c; + } else + { + destBuffer[numRead++]=0; + } + } + } while (c != 0 && c != '\n' && numRead<(numBytes-1)); + } + } + if (numRead0) + { + destBuffer[numRead]=0; + return &destBuffer[0]; + } + return 0; + } + virtual int getFileSize(int fileHandle) + { + int size=0; + + if (fileHandle>=0 && fileHandle < FILEIO_MAX_FILES) + { + unzFile f = m_fileHandles[fileHandle]; + if (f) + { + unz_file_info info; + int result = unzGetCurrentFileInfo(f, &info, NULL, 0, NULL, 0, NULL, 0); + if (result == UNZ_OK) + { + size = info.uncompressed_size; + } + } + } + return size; + } + +}; + + + +#else +typedef b3BulletDefaultFileIO MyFileIO; +#endif + +struct FileIOClass +{ + int m_testData; + + MyFileIO m_fileIO; + + FileIOClass() + : m_testData(42), + m_fileIO()//"e:/develop/bullet3/data/plane.zip") + { + } + virtual ~FileIOClass() + { + } +}; + +B3_SHARED_API int initPlugin_fileIOPlugin(struct b3PluginContext* context) +{ + FileIOClass* obj = new FileIOClass(); + context->m_userPointer = obj; + return SHARED_MEMORY_MAGIC_NUMBER; +} + + +B3_SHARED_API int executePluginCommand_fileIOPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments) +{ + printf("text argument:%s\n", arguments->m_text); + printf("int args: ["); + for (int i = 0; i < arguments->m_numInts; i++) + { + printf("%d", arguments->m_ints[i]); + if ((i + 1) < arguments->m_numInts) + { + printf(","); + } + } + printf("]\nfloat args: ["); + for (int i = 0; i < arguments->m_numFloats; i++) + { + printf("%f", arguments->m_floats[i]); + if ((i + 1) < arguments->m_numFloats) + { + printf(","); + } + } + printf("]\n"); + + FileIOClass* obj = (FileIOClass*)context->m_userPointer; + + b3SharedMemoryStatusHandle statusHandle; + int statusType = -1; + int bodyUniqueId = -1; + + b3SharedMemoryCommandHandle command = + b3LoadUrdfCommandInit(context->m_physClient, arguments->m_text); + + statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, command); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_URDF_LOADING_COMPLETED) + { + bodyUniqueId = b3GetStatusBodyIndex(statusHandle); + } + return bodyUniqueId; +} + +B3_SHARED_API struct CommonFileIOInterface* getFileIOFunc_fileIOPlugin(struct b3PluginContext* context) +{ + FileIOClass* obj = (FileIOClass*)context->m_userPointer; + return &obj->m_fileIO; +} + +B3_SHARED_API void exitPlugin_fileIOPlugin(struct b3PluginContext* context) +{ + FileIOClass* obj = (FileIOClass*)context->m_userPointer; + delete obj; + context->m_userPointer = 0; +} diff --git a/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.h b/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.h new file mode 100644 index 000000000..6a8503904 --- /dev/null +++ b/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.h @@ -0,0 +1,24 @@ +#ifndef FILE_IO_PLUGIN_H +#define FILE_IO_PLUGIN_H + +#include "../b3PluginAPI.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + + //initPlugin, exitPlugin and executePluginCommand are required, otherwise plugin won't load + B3_SHARED_API int initPlugin_fileIOPlugin(struct b3PluginContext* context); + B3_SHARED_API void exitPlugin_fileIOPlugin(struct b3PluginContext* context); + B3_SHARED_API int executePluginCommand_fileIOPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments); + + //all the APIs below are optional + B3_SHARED_API struct CommonFileIOInterface* getFileIOFunc_fileIOPlugin(struct b3PluginContext* context); + + +#ifdef __cplusplus +}; +#endif + +#endif //#define FILE_IO_PLUGIN_H diff --git a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp index 9a6c91c69..12bc80cb7 100644 --- a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp @@ -182,7 +182,7 @@ void TinyRendererVisualShapeConverter::setLightSpecularCoeff(float specularCoeff m_data->m_hasLightSpecularCoeff = true; } -void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut, btAlignedObjectArray& texturesOut, b3VisualShapeData& visualShapeOut) +static void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut, btAlignedObjectArray& texturesOut, b3VisualShapeData& visualShapeOut, struct CommonFileIOInterface* fileIO) { visualShapeOut.m_visualGeometryType = visual->m_geometry.m_type; visualShapeOut.m_dimensions[0] = 0; @@ -322,7 +322,7 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi { //glmesh = LoadMeshFromObj(fullPath,visualPathPrefix); b3ImportMeshData meshData; - if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData)) + if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData, fileIO)) { if (meshData.m_textureImage1) { @@ -338,7 +338,7 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi break; } case UrdfGeometry::FILE_STL: - glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str()); + glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str(), fileIO); break; case UrdfGeometry::FILE_COLLADA: { @@ -354,7 +354,8 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi visualShapeInstances, upAxisTrans, unitMeterScaling, - upAxis); + upAxis, + fileIO); glmesh = new GLInstanceGraphicsShape; // int index = 0; @@ -540,9 +541,10 @@ static btVector4 sColors[4] = }; void TinyRendererVisualShapeConverter::convertVisualShapes( - int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, - const UrdfLink* linkPtr, const UrdfModel* model, - int collisionObjectUniqueId, int bodyUniqueId) + int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, + const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUniqueId, + int bodyUniqueId, struct CommonFileIOInterface* fileIO) + { btAssert(linkPtr); // TODO: remove if (not doing it now, because diff will be 50+ lines) if (linkPtr) @@ -663,7 +665,7 @@ void TinyRendererVisualShapeConverter::convertVisualShapes( { B3_PROFILE("convertURDFToVisualShape"); - convertURDFToVisualShape(vis, pathPrefix, localInertiaFrame.inverse() * childTrans, vertices, indices, textures, visualShape); + convertURDFToVisualShape(vis, pathPrefix, localInertiaFrame.inverse() * childTrans, vertices, indices, textures, visualShape, fileIO); } if (vertices.size() && indices.size()) diff --git a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.h b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.h index b28727492..fcd009c3e 100644 --- a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.h +++ b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.h @@ -11,7 +11,7 @@ struct TinyRendererVisualShapeConverter : public UrdfRenderingInterface virtual ~TinyRendererVisualShapeConverter(); - virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int shapeUid, int objectIndex); + virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO); virtual int getNumVisualShapes(int bodyUniqueId); diff --git a/examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.cpp b/examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.cpp index b92c44ee5..5d15c44ed 100644 --- a/examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.cpp +++ b/examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.cpp @@ -41,8 +41,7 @@ B3_SHARED_API int initPlugin_vrSyncPlugin(struct b3PluginContext* context) { MyClass* obj = new MyClass(); context->m_userPointer = obj; - - printf("hi vrSyncPlugin!\n"); + return SHARED_MEMORY_MAGIC_NUMBER; } @@ -192,6 +191,4 @@ B3_SHARED_API void exitPlugin_vrSyncPlugin(struct b3PluginContext* context) MyClass* obj = (MyClass*)context->m_userPointer; delete obj; context->m_userPointer = 0; - - printf("bye vrSyncPlugin!\n"); } diff --git a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp index ecb33253a..f3c470707 100644 --- a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp +++ b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp @@ -22,13 +22,17 @@ #include #include #include +#ifdef USE_STREAM #include +#else +#include "../../CommonInterfaces/CommonFileIOInterface.h" +#endif #include - #include "tiny_obj_loader.h" namespace tinyobj { +#ifdef USE_STREAM //See http://stackoverflow.com/questions/6089231/getting-std-ifstream-to-handle-lf-cr-and-crlf std::istream& safeGetline(std::istream& is, std::string& t) { @@ -64,7 +68,7 @@ std::istream& safeGetline(std::istream& is, std::string& t) } } } - +#endif struct vertex_index { int v_idx, vt_idx, vn_idx, dummy; @@ -340,7 +344,8 @@ void InitMaterial(material_t& material) std::string LoadMtl( std::map& material_map, const char* filename, - const char* mtl_basepath) + const char* mtl_basepath, + CommonFileIOInterface* fileIO) { material_map.clear(); std::stringstream err; @@ -355,22 +360,44 @@ std::string LoadMtl( { filepath = std::string(filename); } - +#ifdef USE_STREAM std::ifstream ifs(filepath.c_str()); if (!ifs) { err << "Cannot open file [" << filepath << "]" << std::endl; return err.str(); } +#else + int fileHandle = fileIO->fileOpen(filepath.c_str(),"r"); + if (fileHandle<0) + { + err << "Cannot open file [" << filepath << "]" << std::endl; + return err.str(); + } +#endif material_t material; int maxchars = 8192; // Alloc enough size. std::vector buf(maxchars); // Alloc enough size. +#ifdef USE_STREAM while (ifs.peek() != -1) +#else + char* line = 0; + do +#endif { std::string linebuf; +#ifdef USE_STREAM safeGetline(ifs, linebuf); +#else + char tmpBuf[1024]; + line = fileIO->readLine(fileHandle, tmpBuf, 1024); + if (line) + { + linebuf=line; + } +#endif // Trim newline '\r\n' or '\r' if (linebuf.size() > 0) @@ -546,9 +573,16 @@ std::string LoadMtl( material.unknown_parameter.insert(std::pair(key, value)); } } +#ifndef USE_STREAM + while (line); +#endif // flush last material. material_map.insert(std::pair(material.name, material)); + if (fileHandle) + { + fileIO->fileClose(fileHandle); + } return err.str(); } @@ -556,7 +590,8 @@ std::string LoadObj( std::vector& shapes, const char* filename, - const char* mtl_basepath) + const char* mtl_basepath, + CommonFileIOInterface* fileIO) { std::string tmp = filename; if (!mtl_basepath) @@ -577,13 +612,21 @@ LoadObj( MyIndices face; std::stringstream err; - +#ifdef USE_STREAM std::ifstream ifs(filename); if (!ifs) { err << "Cannot open file [" << filename << "]" << std::endl; return err.str(); } +#else + int fileHandle = fileIO->fileOpen(filename,"r"); + if (fileHandle<0) + { + err << "Cannot open file [" << filename << "]" << std::endl; + return err.str(); + } +#endif std::vector v; v.reserve(1024 * 1024); @@ -606,10 +649,24 @@ LoadObj( std::string linebuf; linebuf.reserve(maxchars); +#ifdef USE_STREAM while (ifs.peek() != -1) +#else + char* line = 0; + do +#endif { linebuf.resize(0); +#ifdef USE_STREAM safeGetline(ifs, linebuf); +#else + char tmpBuf[1024]; + line = fileIO->readLine(fileHandle, tmpBuf, 1024); + if (line) + { + linebuf=line; + } +#endif // Trim newline '\r\n' or '\r' if (linebuf.size() > 0) @@ -720,7 +777,7 @@ LoadObj( token += 7; sscanf(token, "%s", namebuf); - std::string err_mtl = LoadMtl(material_map, namebuf, mtl_basepath); + std::string err_mtl = LoadMtl(material_map, namebuf, mtl_basepath,fileIO); if (!err_mtl.empty()) { //faceGroup.resize(0); // for safety @@ -789,6 +846,9 @@ LoadObj( // Ignore unknown command. } +#ifndef USE_STREAM + while (line); +#endif shape_t shape; bool ret = exportFaceGroupToShape(shape, v, vn, vt, faceGroup, material, name, allIndices); @@ -798,6 +858,10 @@ LoadObj( } faceGroup.resize(0); // for safety + if (fileHandle) + { + fileIO->fileClose(fileHandle); + } return err.str(); } diff --git a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h index fa8a16a0b..0319b7c0d 100644 --- a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h +++ b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h @@ -10,6 +10,8 @@ #include #include +struct CommonFileIOInterface; + namespace tinyobj { typedef struct @@ -51,10 +53,19 @@ typedef struct /// The function returns error string. /// Returns empty string when loading .obj success. /// 'mtl_basepath' is optional, and used for base path for .mtl file. +#ifdef USE_STREAM std::string LoadObj( std::vector& shapes, // [output] const char* filename, const char* mtl_basepath = NULL); +#else +std::string +LoadObj( + std::vector& shapes, + const char* filename, + const char* mtl_basepath, + CommonFileIOInterface* fileIO); +#endif }; // namespace tinyobj diff --git a/examples/TinyRenderer/TinyRenderer.cpp b/examples/TinyRenderer/TinyRenderer.cpp index 647c5eb1d..0e3a5f4e6 100644 --- a/examples/TinyRenderer/TinyRenderer.cpp +++ b/examples/TinyRenderer/TinyRenderer.cpp @@ -13,7 +13,7 @@ #include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btVector3.h" #include "Bullet3Common/b3Logging.h" - +#include "../CommonInterfaces/CommonFileIOInterface.h" struct DepthShader : public IShader { Model* m_model; @@ -264,11 +264,11 @@ TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer, b3AlignedOb m_lightSpecularCoeff = 0.05; } -void TinyRenderObjectData::loadModel(const char* fileName) +void TinyRenderObjectData::loadModel(const char* fileName, CommonFileIOInterface* fileIO) { //todo(erwincoumans) move the file loading out of here char relativeFileName[1024]; - if (!b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024)) + if (!fileIO->findResourcePath(fileName, relativeFileName, 1024)) { printf("Cannot find file %s\n", fileName); } @@ -335,7 +335,7 @@ void TinyRenderObjectData::registerMeshShape(const float* vertices, int numVerti } } -void TinyRenderObjectData::registerMesh2(btAlignedObjectArray& vertices, btAlignedObjectArray& normals, btAlignedObjectArray& indices) +void TinyRenderObjectData::registerMesh2(btAlignedObjectArray& vertices, btAlignedObjectArray& normals, btAlignedObjectArray& indices, CommonFileIOInterface* fileIO) { if (0 == m_model) { @@ -344,7 +344,7 @@ void TinyRenderObjectData::registerMesh2(btAlignedObjectArray& vertic m_model = new Model(); char relativeFileName[1024]; - if (b3ResourcePath::findResourcePath("floor_diffuse.tga", relativeFileName, 1024)) + if (fileIO->findResourcePath("floor_diffuse.tga", relativeFileName, 1024)) { m_model->loadDiffuseTexture(relativeFileName); } @@ -368,12 +368,12 @@ void TinyRenderObjectData::registerMesh2(btAlignedObjectArray& vertic } } -void TinyRenderObjectData::createCube(float halfExtentsX, float halfExtentsY, float halfExtentsZ) +void TinyRenderObjectData::createCube(float halfExtentsX, float halfExtentsY, float halfExtentsZ, CommonFileIOInterface* fileIO) { m_model = new Model(); char relativeFileName[1024]; - if (b3ResourcePath::findResourcePath("floor_diffuse.tga", relativeFileName, 1024)) + if (fileIO->findResourcePath("floor_diffuse.tga", relativeFileName, 1024)) { m_model->loadDiffuseTexture(relativeFileName); } diff --git a/examples/TinyRenderer/TinyRenderer.h b/examples/TinyRenderer/TinyRenderer.h index 9a9cea72d..414a7814d 100644 --- a/examples/TinyRenderer/TinyRenderer.h +++ b/examples/TinyRenderer/TinyRenderer.h @@ -41,12 +41,12 @@ struct TinyRenderObjectData TinyRenderObjectData(TGAImage& rgbColorBuffer, b3AlignedObjectArray& depthBuffer, b3AlignedObjectArray* shadowBuffer, b3AlignedObjectArray* segmentationMaskBuffer, int objectIndex, int linkIndex); virtual ~TinyRenderObjectData(); - void loadModel(const char* fileName); - void createCube(float HalfExtentsX, float HalfExtentsY, float HalfExtentsZ); + void loadModel(const char* fileName, struct CommonFileIOInterface* fileIO); + void createCube(float HalfExtentsX, float HalfExtentsY, float HalfExtentsZ, struct CommonFileIOInterface* fileIO); void registerMeshShape(const float* vertices, int numVertices, const int* indices, int numIndices, const float rgbaColor[4], unsigned char* textureImage = 0, int textureWidth = 0, int textureHeight = 0); - void registerMesh2(btAlignedObjectArray& vertices, btAlignedObjectArray& normals, btAlignedObjectArray& indices); + void registerMesh2(btAlignedObjectArray& vertices, btAlignedObjectArray& normals, btAlignedObjectArray& indices, struct CommonFileIOInterface* fileIO); void* m_userData; int m_userIndex; diff --git a/examples/Utils/b3BulletDefaultFileIO.h b/examples/Utils/b3BulletDefaultFileIO.h new file mode 100644 index 000000000..927675e1a --- /dev/null +++ b/examples/Utils/b3BulletDefaultFileIO.h @@ -0,0 +1,175 @@ + +#ifndef B3_BULLET_DEFAULT_FILE_IO_H +#define B3_BULLET_DEFAULT_FILE_IO_H + +#include "../CommonInterfaces/CommonFileIOInterface.h" +#include "b3ResourcePath.h" + +#include +#define B3_FILEIO_MAX_FILES 1024 + +struct b3BulletDefaultFileIO : public CommonFileIOInterface +{ + static bool FileIOPluginFindFile(void* userPtr, const char* orgFileName, char* relativeFileName, int maxRelativeFileNameMaxLen) + { + b3BulletDefaultFileIO* fileIo = (b3BulletDefaultFileIO*) userPtr; + return fileIo->findFile(orgFileName, relativeFileName, maxRelativeFileNameMaxLen); + } + + FILE* m_fileHandles[B3_FILEIO_MAX_FILES]; + int m_numFileHandles; + + b3BulletDefaultFileIO() + :m_numFileHandles(0) + { + for (int i=0;i=0) + { + FILE*f = ::fopen(fileName, mode); + if (f) + { + m_fileHandles[slot]=f; + } else + { + slot=-1; + } + } + return slot; + } + virtual int fileRead(int fileHandle, char* destBuffer, int numBytes) + { + if (fileHandle>=0 && fileHandle < B3_FILEIO_MAX_FILES) + { + FILE* f = m_fileHandles[fileHandle]; + if (f) + { + int readBytes = ::fread(destBuffer, 1, numBytes, f); + return readBytes; + } + } + return -1; + + } + virtual int fileWrite(int fileHandle,const char* sourceBuffer, int numBytes) + { + if (fileHandle>=0 && fileHandle < B3_FILEIO_MAX_FILES) + { + FILE* f = m_fileHandles[fileHandle]; + if (f) + { + return ::fwrite(sourceBuffer, 1, numBytes,m_fileHandles[fileHandle]); + } + } + return -1; + } + virtual void fileClose(int fileHandle) + { + if (fileHandle>=0 && fileHandle < B3_FILEIO_MAX_FILES) + { + FILE* f = m_fileHandles[fileHandle]; + if (f) + { + ::fclose(f); + m_fileHandles[fileHandle]=0; + } + } + } + + virtual bool findResourcePath(const char* fileName, char* relativeFileName, int relativeFileNameSizeInBytes) + { + return b3ResourcePath::findResourcePath(fileName, relativeFileName, relativeFileNameSizeInBytes, b3BulletDefaultFileIO::FileIOPluginFindFile, this); + } + + + virtual bool findFile(const char* orgFileName, char* relativeFileName, int maxRelativeFileNameMaxLen) + { + FILE* f = 0; + 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*); + + f = 0; + bool fileFound = false; + + for (int i = 0; !f && i < numPrefixes; i++) + { +#ifdef _MSC_VER + sprintf_s(relativeFileName, maxRelativeFileNameMaxLen, "%s%s", prefix[i], orgFileName); +#else + sprintf(relativeFileName, "%s%s", prefix[i], orgFileName); +#endif + f = fopen(relativeFileName, "rb"); + if (f) + { + fileFound = true; + break; + } + } + if (f) + { + fclose(f); + } + + return fileFound; + } + virtual char* readLine(int fileHandle, char* destBuffer, int numBytes) + { + if (fileHandle>=0 && fileHandle < B3_FILEIO_MAX_FILES) + { + FILE* f = m_fileHandles[fileHandle]; + if (f) + { + return ::fgets(destBuffer, numBytes, m_fileHandles[fileHandle]); + } + } + return 0; + } + virtual int getFileSize(int fileHandle) + { + int size = 0; + if (fileHandle>=0 && fileHandle < B3_FILEIO_MAX_FILES) + { + FILE* f = m_fileHandles[fileHandle]; + if (f) + { + + if (fseek(f, 0, SEEK_END) || (size = ftell(f)) == EOF || fseek(f, 0, SEEK_SET)) + { + printf("Error: Cannot access file to determine size\n"); + } + } + } + return size; + } +}; + +#endif //B3_BULLET_DEFAULT_FILE_IO_H diff --git a/examples/Utils/b3ResourcePath.cpp b/examples/Utils/b3ResourcePath.cpp index 4ff3c2ac7..070421c0a 100644 --- a/examples/Utils/b3ResourcePath.cpp +++ b/examples/Utils/b3ResourcePath.cpp @@ -87,12 +87,21 @@ void b3ResourcePath::setAdditionalSearchPath(const char* path) } } -int b3ResourcePath::findResourcePath(const char* resourceName, char* resourcePathOut, int resourcePathMaxNumBytes) +bool b3MyFindFile(void* userPointer, const char* orgFileName, char* relativeFileName, int maxRelativeFileNameMaxLen) { + return b3FileUtils::findFile(orgFileName, relativeFileName, maxRelativeFileNameMaxLen); +} + +int b3ResourcePath::findResourcePath(const char* resourceName, char* resourcePathOut, int resourcePathMaxNumBytes, PFN_FIND_FILE findFile, void* userPointer) +{ + if (findFile==0) + { + findFile=b3MyFindFile; + } //first find in a resource/ location, then in various folders within 'data' using b3FileUtils char exePath[B3_MAX_EXE_PATH_LEN]; - bool res = b3FileUtils::findFile(resourceName, resourcePathOut, resourcePathMaxNumBytes); + bool res = findFile(userPointer, resourceName, resourcePathOut, resourcePathMaxNumBytes); if (res) { return strlen(resourcePathOut); @@ -104,7 +113,7 @@ int b3ResourcePath::findResourcePath(const char* resourceName, char* resourcePat char* resourcePathIn = tmpPath.m_path; sprintf(resourcePathIn, "%s/%s", sAdditionalSearchPath, resourceName); //printf("try resource at %s\n", resourcePath); - if (b3FileUtils::findFile(resourcePathIn, resourcePathOut, resourcePathMaxNumBytes)) + if (findFile(userPointer, resourcePathIn, resourcePathOut, resourcePathMaxNumBytes)) { return strlen(resourcePathOut); } @@ -122,20 +131,20 @@ int b3ResourcePath::findResourcePath(const char* resourceName, char* resourcePat char* resourcePathIn = tmpPath.m_path; sprintf(resourcePathIn, "%s../data/%s", pathToExe, resourceName); //printf("try resource at %s\n", resourcePath); - if (b3FileUtils::findFile(resourcePathIn, resourcePathOut, resourcePathMaxNumBytes)) + if (findFile(userPointer, resourcePathIn, resourcePathOut, resourcePathMaxNumBytes)) { return strlen(resourcePathOut); } sprintf(resourcePathIn, "%s../resources/%s/%s", pathToExe, &exePath[exeNamePos], resourceName); //printf("try resource at %s\n", resourcePath); - if (b3FileUtils::findFile(resourcePathIn, resourcePathOut, resourcePathMaxNumBytes)) + if (findFile(userPointer, resourcePathIn, resourcePathOut, resourcePathMaxNumBytes)) { return strlen(resourcePathOut); } sprintf(resourcePathIn, "%s.runfiles/google3/third_party/bullet/data/%s", exePath, resourceName); //printf("try resource at %s\n", resourcePath); - if (b3FileUtils::findFile(resourcePathIn, resourcePathOut, resourcePathMaxNumBytes)) + if (findFile(userPointer, resourcePathIn, resourcePathOut, resourcePathMaxNumBytes)) { return strlen(resourcePathOut); } diff --git a/examples/Utils/b3ResourcePath.h b/examples/Utils/b3ResourcePath.h index b7b0f129b..49967b8c3 100644 --- a/examples/Utils/b3ResourcePath.h +++ b/examples/Utils/b3ResourcePath.h @@ -3,11 +3,13 @@ #include +typedef bool (* PFN_FIND_FILE)(void* userPointer, const char* orgFileName, char* relativeFileName, int maxRelativeFileNameMaxLen); + class b3ResourcePath { public: static int getExePath(char* path, int maxPathLenInBytes); - static int findResourcePath(const char* sourceName, char* resourcePath, int maxResourcePathLenInBytes); + static int findResourcePath(const char* resourceName, char* resourcePathOut, int resourcePathMaxNumBytes, PFN_FIND_FILE findFile, void* userPointer=0); static void setAdditionalSearchPath(const char* path); }; #endif diff --git a/test/InverseDynamics/test_invdyn_bullet.cpp b/test/InverseDynamics/test_invdyn_bullet.cpp index 3cec06f4f..51a7e1482 100644 --- a/test/InverseDynamics/test_invdyn_bullet.cpp +++ b/test/InverseDynamics/test_invdyn_bullet.cpp @@ -47,7 +47,7 @@ TEST(InvDynCompare, bulletUrdfR2D2) char relativeFileName[1024]; - ASSERT_TRUE(b3ResourcePath::findResourcePath(kUrdfFile, relativeFileName, 1024)); + ASSERT_TRUE(b3ResourcePath::findResourcePath(kUrdfFile, relativeFileName, 1024,0)); mb_load.setFileName(relativeFileName); mb_load.init();