From 078887c4d7ba7811615575ea70bdd16d5b10477a Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Mon, 29 Oct 2018 10:25:40 -0700 Subject: [PATCH] PyBullet fileIOPlugin: don't add equal fileIO interface twice (based on identical fileIOType and pathPrefix) loadBullet goes through fileIOPlugin --- .../CommonInterfaces/CommonFileIOInterface.h | 9 ++ .../ImportURDFDemo/BulletUrdfImporter.cpp | 4 +- .../PhysicsServerCommandProcessor.cpp | 45 +++++--- examples/SharedMemory/SharedMemoryPublic.h | 1 + .../plugins/fileIOPlugin/fileIOPlugin.cpp | 106 +++++++++++------- .../plugins/fileIOPlugin/zipFileIO.h | 6 +- examples/Utils/b3BulletDefaultFileIO.h | 11 +- 7 files changed, 119 insertions(+), 63 deletions(-) diff --git a/examples/CommonInterfaces/CommonFileIOInterface.h b/examples/CommonInterfaces/CommonFileIOInterface.h index 3e2e78f6d..e41faf78a 100644 --- a/examples/CommonInterfaces/CommonFileIOInterface.h +++ b/examples/CommonInterfaces/CommonFileIOInterface.h @@ -3,6 +3,15 @@ struct CommonFileIOInterface { + int m_fileIOType; + const char* m_pathPrefix; + + CommonFileIOInterface(int fileIOType, const char* pathPrefix) + :m_fileIOType(fileIOType), + m_pathPrefix(pathPrefix) + { + } + virtual ~CommonFileIOInterface() { } diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index e86779174..943b26ddd 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -143,7 +143,7 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase) b3FileUtils fu; //bool fileFound = fu.findFile(fileName, relativeFileName, 1024); - bool fileFound = m_data->m_fileIO->findResourcePath(fileName, relativeFileName, 1024) > 0; + bool fileFound = m_data->m_fileIO->findResourcePath(fileName, relativeFileName, 1024); std::string xml_string; @@ -217,7 +217,7 @@ bool BulletURDFImporter::loadSDF(const char* fileName, bool forceFixedBase) b3FileUtils fu; //bool fileFound = fu.findFile(fileName, relativeFileName, 1024); - bool fileFound = (m_data->m_fileIO->findResourcePath(fileName, relativeFileName, 1024)) > 0; + bool fileFound = (m_data->m_fileIO->findResourcePath(fileName, relativeFileName, 1024)); std::string xml_string; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 1f4242cb9..4085a6949 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -10198,30 +10198,43 @@ bool PhysicsServerCommandProcessor::processLoadBulletCommand(const struct Shared //btBulletWorldImporter* importer = new btBulletWorldImporter(m_data->m_dynamicsWorld); btMultiBodyWorldImporter* importer = new btMultiBodyWorldImporter(m_data->m_dynamicsWorld); - const char* prefix[] = {"", "./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/"}; - int numPrefixes = sizeof(prefix) / sizeof(const char*); - char relativeFileName[1024]; - FILE* f = 0; bool found = false; - for (int i = 0; !f && i < numPrefixes; i++) + CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface(); + b3AlignedObjectArray buffer; + buffer.reserve(1024); + if (fileIO) { - sprintf(relativeFileName, "%s%s", prefix[i], clientCmd.m_fileArguments.m_fileName); - f = fopen(relativeFileName, "rb"); - if (f) + char fileName[1024]; + int fileId = -1; + found = fileIO->findResourcePath(clientCmd.m_fileArguments.m_fileName, fileName, 1024); + if (found) { - found = true; - break; + fileId = fileIO->fileOpen(fileName,"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("image filesize mismatch!\n"); + buffer.resize(0); + } else + { + found=true; + } + } + fileIO->fileClose(fileId); } } - if (f) - { - fclose(f); - } - if (found) + if (found && buffer.size()) { - bool ok = importer->loadFile(relativeFileName); + bool ok = importer->loadFileFromMemory(&buffer[0], buffer.size()); if (ok) { int numRb = importer->getNumRigidBodies(); diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index b4f4aa6ca..166de101c 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -924,6 +924,7 @@ enum eFileIOTypes ePosixFileIO = 1, eZipFileIO, eCNSFileIO, + eInMemoryFileIO, }; //limits for vertices/indices in PyBullet::createCollisionShape diff --git a/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp b/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp index d5d063488..9e53b9f77 100644 --- a/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp +++ b/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp @@ -48,6 +48,7 @@ struct InMemoryFileIO : public CommonFileIOInterface int m_numFrees; InMemoryFileIO() + :CommonFileIOInterface(eInMemoryFileIO,0) { m_numAllocs=0; m_numFrees=0; @@ -293,7 +294,8 @@ struct WrapperFileIO : public CommonFileIOInterface InMemoryFileIO m_cachedFiles; WrapperFileIO() - :m_numWrapperInterfaces(0) + :CommonFileIOInterface(0,0), + m_numWrapperInterfaces(0) { for (int i=0;i=0 && fileIOIndex=0 && fileIOIndexm_ints[1]; - switch (fileIOType) + bool alreadyExists = false; + + for (int i=0;im_fileIO.getFileIOInterface(i); + if (fileIO) { -#ifdef B3_EXCLUDE_DEFAULT_FILEIO - printf("ePosixFileIO is not enabled in this build.\n"); -#else - obj->m_fileIO.addFileIOInterface(new b3BulletDefaultFileIO()); -#endif - break; - } - case eZipFileIO: - { -#ifdef B3_USE_ZIPFILE_FILEIO - if (arguments->m_text) + if (fileIO->m_fileIOType == fileIOType) { - obj->m_fileIO.addFileIOInterface(new ZipFileIO(arguments->m_text, &obj->m_fileIO)); + if (fileIO->m_pathPrefix && strcmp(fileIO->m_pathPrefix,arguments->m_text)==0) + { + result = i; + alreadyExists = true; + break; + } } -#else - printf("eZipFileIO is not enabled in this build.\n"); -#endif - break; - } - case eCNSFileIO: - { -#ifdef B3_USE_CNS_FILEIO - if (strlen(arguments->m_text)) - { - obj->m_fileIO.addFileIOInterface(new CNSFileIO(arguments->m_text)); - } - else - { - obj->m_fileIO.addFileIOInterface(new CNSFileIO("")); - } -#else//B3_USE_CNS_FILEIO - printf("CNSFileIO is not enabled in this build.\n"); -#endif //B3_USE_CNS_FILEIO - break; - } - default: - { } } + + + //create new fileIO interface + if (!alreadyExists) + { + switch (fileIOType) + { + case ePosixFileIO: + { + #ifdef B3_EXCLUDE_DEFAULT_FILEIO + printf("ePosixFileIO is not enabled in this build.\n"); + #else + result = obj->m_fileIO.addFileIOInterface(new b3BulletDefaultFileIO(ePosixFileIO, arguments->m_text)); + #endif + break; + } + case eZipFileIO: + { + #ifdef B3_USE_ZIPFILE_FILEIO + if (arguments->m_text) + { + result = obj->m_fileIO.addFileIOInterface(new ZipFileIO(eZipFileIO, arguments->m_text, &obj->m_fileIO)); + } + #else + printf("eZipFileIO is not enabled in this build.\n"); + #endif + break; + } + case eCNSFileIO: + { + #ifdef B3_USE_CNS_FILEIO + result = obj->m_fileIO.addFileIOInterface(new CNSFileIO(eCNSFileIO, arguments->m_text)); + #else//B3_USE_CNS_FILEIO + printf("CNSFileIO is not enabled in this build.\n"); + #endif //B3_USE_CNS_FILEIO + break; + } + default: + { + } + }//switch (fileIOType) + }//if (!alreadyExists) break; } case eRemoveFileIOAction: @@ -624,6 +651,7 @@ B3_SHARED_API int executePluginCommand_fileIOPlugin(struct b3PluginContext* cont //remove fileIO interface int fileIOIndex = arguments->m_ints[1]; obj->m_fileIO.removeFileIOInterface(fileIOIndex); + result = fileIOIndex; break; } default: diff --git a/examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h b/examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h index 7366f1e24..c3efdb0ce 100644 --- a/examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h +++ b/examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h @@ -10,10 +10,12 @@ struct ZipFileIO : public CommonFileIOInterface unzFile m_fileHandles[B3_ZIP_FILEIO_MAX_FILES ]; int m_numFileHandles; - ZipFileIO(const char* zipfileName, CommonFileIOInterface* wrapperFileIO) - :m_zipfileName(zipfileName), + ZipFileIO(int fileIOType, const char* zipfileName, CommonFileIOInterface* wrapperFileIO) + :CommonFileIOInterface(fileIOType,0), + m_zipfileName(zipfileName), m_numFileHandles(0) { + m_rootPath = m_zipfileName.c_str(); for (int i=0;ifindFile(orgFileName, relativeFileName, maxRelativeFileNameMaxLen); } + char m_prefix[1024]; FILE* m_fileHandles[B3_FILEIO_MAX_FILES]; int m_numFileHandles; - b3BulletDefaultFileIO() - :m_numFileHandles(0) + b3BulletDefaultFileIO(int fileIOType=0, const char* pathPrefix=0) + :CommonFileIOInterface(fileIOType, m_prefix), + m_numFileHandles(0) { + sprintf(m_prefix,"%s", pathPrefix); for (int i=0;i0; } @@ -114,7 +117,7 @@ struct b3BulletDefaultFileIO : public CommonFileIOInterface } //printf("Trying various directories, relative to current working directory\n"); - const char* prefix[] = {"./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/"}; + const char* prefix[] = {m_prefix, "./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/"}; int numPrefixes = sizeof(prefix) / sizeof(const char*); f = 0;