PyBullet fileIOPlugin: don't add equal fileIO interface twice (based on identical fileIOType and pathPrefix)
loadBullet goes through fileIOPlugin
This commit is contained in:
@@ -3,6 +3,15 @@
|
|||||||
|
|
||||||
struct CommonFileIOInterface
|
struct CommonFileIOInterface
|
||||||
{
|
{
|
||||||
|
int m_fileIOType;
|
||||||
|
const char* m_pathPrefix;
|
||||||
|
|
||||||
|
CommonFileIOInterface(int fileIOType, const char* pathPrefix)
|
||||||
|
:m_fileIOType(fileIOType),
|
||||||
|
m_pathPrefix(pathPrefix)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
virtual ~CommonFileIOInterface()
|
virtual ~CommonFileIOInterface()
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -143,7 +143,7 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase)
|
|||||||
b3FileUtils fu;
|
b3FileUtils fu;
|
||||||
|
|
||||||
//bool fileFound = fu.findFile(fileName, relativeFileName, 1024);
|
//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;
|
std::string xml_string;
|
||||||
|
|
||||||
@@ -217,7 +217,7 @@ bool BulletURDFImporter::loadSDF(const char* fileName, bool forceFixedBase)
|
|||||||
b3FileUtils fu;
|
b3FileUtils fu;
|
||||||
|
|
||||||
//bool fileFound = fu.findFile(fileName, relativeFileName, 1024);
|
//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;
|
std::string xml_string;
|
||||||
|
|
||||||
|
|||||||
@@ -10198,30 +10198,43 @@ bool PhysicsServerCommandProcessor::processLoadBulletCommand(const struct Shared
|
|||||||
//btBulletWorldImporter* importer = new btBulletWorldImporter(m_data->m_dynamicsWorld);
|
//btBulletWorldImporter* importer = new btBulletWorldImporter(m_data->m_dynamicsWorld);
|
||||||
btMultiBodyWorldImporter* importer = new btMultiBodyWorldImporter(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;
|
bool found = false;
|
||||||
|
|
||||||
for (int i = 0; !f && i < numPrefixes; i++)
|
CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface();
|
||||||
|
b3AlignedObjectArray<char> buffer;
|
||||||
|
buffer.reserve(1024);
|
||||||
|
if (fileIO)
|
||||||
{
|
{
|
||||||
sprintf(relativeFileName, "%s%s", prefix[i], clientCmd.m_fileArguments.m_fileName);
|
char fileName[1024];
|
||||||
f = fopen(relativeFileName, "rb");
|
int fileId = -1;
|
||||||
if (f)
|
found = fileIO->findResourcePath(clientCmd.m_fileArguments.m_fileName, fileName, 1024);
|
||||||
|
if (found)
|
||||||
{
|
{
|
||||||
found = true;
|
fileId = fileIO->fileOpen(fileName,"rb");
|
||||||
break;
|
}
|
||||||
|
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)
|
if (ok)
|
||||||
{
|
{
|
||||||
int numRb = importer->getNumRigidBodies();
|
int numRb = importer->getNumRigidBodies();
|
||||||
|
|||||||
@@ -924,6 +924,7 @@ enum eFileIOTypes
|
|||||||
ePosixFileIO = 1,
|
ePosixFileIO = 1,
|
||||||
eZipFileIO,
|
eZipFileIO,
|
||||||
eCNSFileIO,
|
eCNSFileIO,
|
||||||
|
eInMemoryFileIO,
|
||||||
};
|
};
|
||||||
|
|
||||||
//limits for vertices/indices in PyBullet::createCollisionShape
|
//limits for vertices/indices in PyBullet::createCollisionShape
|
||||||
|
|||||||
@@ -48,6 +48,7 @@ struct InMemoryFileIO : public CommonFileIOInterface
|
|||||||
int m_numFrees;
|
int m_numFrees;
|
||||||
|
|
||||||
InMemoryFileIO()
|
InMemoryFileIO()
|
||||||
|
:CommonFileIOInterface(eInMemoryFileIO,0)
|
||||||
{
|
{
|
||||||
m_numAllocs=0;
|
m_numAllocs=0;
|
||||||
m_numFrees=0;
|
m_numFrees=0;
|
||||||
@@ -293,7 +294,8 @@ struct WrapperFileIO : public CommonFileIOInterface
|
|||||||
InMemoryFileIO m_cachedFiles;
|
InMemoryFileIO m_cachedFiles;
|
||||||
|
|
||||||
WrapperFileIO()
|
WrapperFileIO()
|
||||||
:m_numWrapperInterfaces(0)
|
:CommonFileIOInterface(0,0),
|
||||||
|
m_numWrapperInterfaces(0)
|
||||||
{
|
{
|
||||||
for (int i=0;i<B3_MAX_FILEIO_INTERFACES;i++)
|
for (int i=0;i<B3_MAX_FILEIO_INTERFACES;i++)
|
||||||
{
|
{
|
||||||
@@ -327,6 +329,14 @@ struct WrapperFileIO : public CommonFileIOInterface
|
|||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
CommonFileIOInterface* getFileIOInterface(int fileIOIndex)
|
||||||
|
{
|
||||||
|
if (fileIOIndex>=0 && fileIOIndex<B3_MAX_FILEIO_INTERFACES)
|
||||||
|
{
|
||||||
|
return m_availableFileIOInterfaces[fileIOIndex];
|
||||||
|
}
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
void removeFileIOInterface(int fileIOIndex)
|
void removeFileIOInterface(int fileIOIndex)
|
||||||
{
|
{
|
||||||
if (fileIOIndex>=0 && fileIOIndex<B3_MAX_FILEIO_INTERFACES)
|
if (fileIOIndex>=0 && fileIOIndex<B3_MAX_FILEIO_INTERFACES)
|
||||||
@@ -571,51 +581,68 @@ B3_SHARED_API int executePluginCommand_fileIOPlugin(struct b3PluginContext* cont
|
|||||||
{
|
{
|
||||||
case eAddFileIOAction:
|
case eAddFileIOAction:
|
||||||
{
|
{
|
||||||
//create new fileIO interface
|
//if the fileIO already exists, skip this action
|
||||||
int fileIOType = arguments->m_ints[1];
|
int fileIOType = arguments->m_ints[1];
|
||||||
switch (fileIOType)
|
bool alreadyExists = false;
|
||||||
|
|
||||||
|
for (int i=0;i<B3_MAX_FILEIO_INTERFACES;i++)
|
||||||
{
|
{
|
||||||
case ePosixFileIO:
|
CommonFileIOInterface* fileIO = obj->m_fileIO.getFileIOInterface(i);
|
||||||
|
if (fileIO)
|
||||||
{
|
{
|
||||||
#ifdef B3_EXCLUDE_DEFAULT_FILEIO
|
if (fileIO->m_fileIOType == fileIOType)
|
||||||
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)
|
|
||||||
{
|
{
|
||||||
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;
|
break;
|
||||||
}
|
}
|
||||||
case eRemoveFileIOAction:
|
case eRemoveFileIOAction:
|
||||||
@@ -624,6 +651,7 @@ B3_SHARED_API int executePluginCommand_fileIOPlugin(struct b3PluginContext* cont
|
|||||||
//remove fileIO interface
|
//remove fileIO interface
|
||||||
int fileIOIndex = arguments->m_ints[1];
|
int fileIOIndex = arguments->m_ints[1];
|
||||||
obj->m_fileIO.removeFileIOInterface(fileIOIndex);
|
obj->m_fileIO.removeFileIOInterface(fileIOIndex);
|
||||||
|
result = fileIOIndex;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
default:
|
default:
|
||||||
|
|||||||
@@ -10,10 +10,12 @@ struct ZipFileIO : public CommonFileIOInterface
|
|||||||
unzFile m_fileHandles[B3_ZIP_FILEIO_MAX_FILES ];
|
unzFile m_fileHandles[B3_ZIP_FILEIO_MAX_FILES ];
|
||||||
int m_numFileHandles;
|
int m_numFileHandles;
|
||||||
|
|
||||||
ZipFileIO(const char* zipfileName, CommonFileIOInterface* wrapperFileIO)
|
ZipFileIO(int fileIOType, const char* zipfileName, CommonFileIOInterface* wrapperFileIO)
|
||||||
:m_zipfileName(zipfileName),
|
:CommonFileIOInterface(fileIOType,0),
|
||||||
|
m_zipfileName(zipfileName),
|
||||||
m_numFileHandles(0)
|
m_numFileHandles(0)
|
||||||
{
|
{
|
||||||
|
m_rootPath = m_zipfileName.c_str();
|
||||||
for (int i=0;i<B3_ZIP_FILEIO_MAX_FILES ;i++)
|
for (int i=0;i<B3_ZIP_FILEIO_MAX_FILES ;i++)
|
||||||
{
|
{
|
||||||
m_fileHandles[i]=0;
|
m_fileHandles[i]=0;
|
||||||
|
|||||||
@@ -16,12 +16,15 @@ struct b3BulletDefaultFileIO : public CommonFileIOInterface
|
|||||||
return fileIo->findFile(orgFileName, relativeFileName, maxRelativeFileNameMaxLen);
|
return fileIo->findFile(orgFileName, relativeFileName, maxRelativeFileNameMaxLen);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
char m_prefix[1024];
|
||||||
FILE* m_fileHandles[B3_FILEIO_MAX_FILES];
|
FILE* m_fileHandles[B3_FILEIO_MAX_FILES];
|
||||||
int m_numFileHandles;
|
int m_numFileHandles;
|
||||||
|
|
||||||
b3BulletDefaultFileIO()
|
b3BulletDefaultFileIO(int fileIOType=0, const char* pathPrefix=0)
|
||||||
:m_numFileHandles(0)
|
:CommonFileIOInterface(fileIOType, m_prefix),
|
||||||
|
m_numFileHandles(0)
|
||||||
{
|
{
|
||||||
|
sprintf(m_prefix,"%s", pathPrefix);
|
||||||
for (int i=0;i<B3_FILEIO_MAX_FILES;i++)
|
for (int i=0;i<B3_FILEIO_MAX_FILES;i++)
|
||||||
{
|
{
|
||||||
m_fileHandles[i]=0;
|
m_fileHandles[i]=0;
|
||||||
@@ -97,7 +100,7 @@ struct b3BulletDefaultFileIO : public CommonFileIOInterface
|
|||||||
|
|
||||||
virtual bool findResourcePath(const char* fileName, char* relativeFileName, int relativeFileNameSizeInBytes)
|
virtual bool findResourcePath(const char* fileName, char* relativeFileName, int relativeFileNameSizeInBytes)
|
||||||
{
|
{
|
||||||
return b3ResourcePath::findResourcePath(fileName, relativeFileName, relativeFileNameSizeInBytes, b3BulletDefaultFileIO::FileIOPluginFindFile, this);
|
return b3ResourcePath::findResourcePath(fileName, relativeFileName, relativeFileNameSizeInBytes, b3BulletDefaultFileIO::FileIOPluginFindFile, this)>0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@@ -114,7 +117,7 @@ struct b3BulletDefaultFileIO : public CommonFileIOInterface
|
|||||||
}
|
}
|
||||||
|
|
||||||
//printf("Trying various directories, relative to current working directory\n");
|
//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*);
|
int numPrefixes = sizeof(prefix) / sizeof(const char*);
|
||||||
|
|
||||||
f = 0;
|
f = 0;
|
||||||
|
|||||||
Reference in New Issue
Block a user