diff --git a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp index 4115effd5..5f553bcbe 100644 --- a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp +++ b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp @@ -53,6 +53,7 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& meshData.m_textureImage1 = 0; meshData.m_textureHeight = 0; meshData.m_textureWidth = 0; + meshData.m_flags = 0; meshData.m_isCached = false; char relativeFileName[1024]; @@ -78,6 +79,18 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& for (int i = 0; meshData.m_textureImage1 == 0 && i < shapes.size(); i++) { const tinyobj::shape_t& shape = shapes[i]; + meshData.m_rgbaColor[0] = shape.material.diffuse[0]; + meshData.m_rgbaColor[1] = shape.material.diffuse[1]; + meshData.m_rgbaColor[2] = shape.material.diffuse[2]; + meshData.m_rgbaColor[3] = shape.material.transparency; + meshData.m_flags |= B3_IMPORT_MESH_HAS_RGBA_COLOR; + + meshData.m_specularColor[0] = shape.material.specular[0]; + meshData.m_specularColor[1] = shape.material.specular[1]; + meshData.m_specularColor[2] = shape.material.specular[2]; + meshData.m_specularColor[3] = 1; + meshData.m_flags |= B3_IMPORT_MESH_HAS_SPECULAR_COLOR; + if (shape.material.diffuse_texname.length() > 0) { int width, height, n; diff --git a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.h b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.h index 770eefd35..1cff27b15 100644 --- a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.h +++ b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.h @@ -3,6 +3,12 @@ #include +enum b3ImportMeshDataFlags +{ + B3_IMPORT_MESH_HAS_RGBA_COLOR=1, + B3_IMPORT_MESH_HAS_SPECULAR_COLOR=2, +}; + struct b3ImportMeshData { struct GLInstanceGraphicsShape* m_gfxShape; @@ -11,6 +17,20 @@ struct b3ImportMeshData bool m_isCached; int m_textureWidth; int m_textureHeight; + double m_rgbaColor[4]; + double m_specularColor[4]; + int m_flags; + + b3ImportMeshData() + :m_gfxShape(0), + m_textureImage1(0), + m_isCached(false), + m_textureWidth(0), + m_textureHeight(0), + m_flags(0) + { + } + }; class b3ImportMeshUtility diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 80f86b5e4..b554165ed 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -228,24 +228,36 @@ bool BulletURDFImporter::loadSDF(const char* fileName, bool forceFixedBase) } else { + char path[1024]; fu.extractPath(relativeFileName, path, sizeof(path)); m_data->setSourceFile(relativeFileName, path); - std::fstream xml_file(relativeFileName, std::fstream::in); - while (xml_file.good()) + //read file + int fileId = m_data->m_fileIO->fileOpen(relativeFileName,"r"); + + char destBuffer[8192]; + char* line = 0; + do { - std::string line; - std::getline(xml_file, line); - xml_string += (line + "\n"); + line = m_data->m_fileIO->readLine(fileId, destBuffer, 8192); + if (line) + { + xml_string += (std::string(destBuffer) + "\n"); + } } - xml_file.close(); + while (line); + m_data->m_fileIO->fileClose(fileId); } BulletErrorLogger loggie; //todo: quick test to see if we can re-use the URDF parser for SDF or not m_data->m_urdfParser.setParseSDF(true); - bool result = m_data->m_urdfParser.loadSDF(xml_string.c_str(), &loggie); + bool result = false; + if (xml_string.length()) + { + result = m_data->m_urdfParser.loadSDF(xml_string.c_str(), &loggie); + } return result; } @@ -861,7 +873,7 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl return shape; } -void BulletURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut, btAlignedObjectArray& texturesOut) const +void BulletURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut, btAlignedObjectArray& texturesOut, struct b3ImportMeshData& meshData) const { BT_PROFILE("convertURDFToVisualShapeInternal"); @@ -920,7 +932,7 @@ void BulletURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu { case UrdfGeometry::FILE_OBJ: { - b3ImportMeshData meshData; + if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData, m_data->m_fileIO)) { if (meshData.m_textureImage1) @@ -1150,16 +1162,49 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP btTransform childTrans = vis.m_linkLocalFrame; btHashString matName(vis.m_materialName.c_str()); UrdfMaterial* const* matPtr = model.m_materials[matName]; - if (matPtr) + b3ImportMeshData meshData; + + convertURDFToVisualShapeInternal(&vis, pathPrefix, localInertiaFrame.inverse() * childTrans, vertices, indices, textures,meshData); + + if (m_data->m_flags&CUF_USE_MATERIAL_COLORS_FROM_MTL) { - UrdfMaterial* const mat = *matPtr; - //printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]); - UrdfMaterialColor matCol; - matCol.m_rgbaColor = mat->m_matColor.m_rgbaColor; - matCol.m_specularColor = mat->m_matColor.m_specularColor; - m_data->m_linkColors.insert(linkIndex, matCol); + if ((meshData.m_flags & B3_IMPORT_MESH_HAS_RGBA_COLOR) && + (meshData.m_flags & B3_IMPORT_MESH_HAS_SPECULAR_COLOR)) + { + UrdfMaterialColor matCol; + + if (m_data->m_flags&CUF_USE_MATERIAL_TRANSPARANCY_FROM_MTL) + { + matCol.m_rgbaColor.setValue(meshData.m_rgbaColor[0], + meshData.m_rgbaColor[1], + meshData.m_rgbaColor[2], + meshData.m_rgbaColor[3]); + } else + { + matCol.m_rgbaColor.setValue(meshData.m_rgbaColor[0], + meshData.m_rgbaColor[1], + meshData.m_rgbaColor[2], + 1); + } + + matCol.m_specularColor.setValue(meshData.m_specularColor[0], + meshData.m_specularColor[1], + meshData.m_specularColor[2]); + m_data->m_linkColors.insert(linkIndex, matCol); + } + } else + { + if (matPtr) + { + UrdfMaterial* const mat = *matPtr; + //printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]); + UrdfMaterialColor matCol; + matCol.m_rgbaColor = mat->m_matColor.m_rgbaColor; + matCol.m_specularColor = mat->m_matColor.m_specularColor; + m_data->m_linkColors.insert(linkIndex, matCol); + } } - convertURDFToVisualShapeInternal(&vis, pathPrefix, localInertiaFrame.inverse() * childTrans, vertices, indices, textures); + } } if (vertices.size() && indices.size()) diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h index 8acf2cefd..2dff82727 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h @@ -88,7 +88,7 @@ public: virtual int getAllocatedTexture(int index) const; virtual void setEnableTinyRenderer(bool enable); - void convertURDFToVisualShapeInternal(const struct UrdfVisual* visual, const char* urdfPathPrefix, const class btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut, btAlignedObjectArray& texturesOut) const; + void convertURDFToVisualShapeInternal(const struct UrdfVisual* visual, const char* urdfPathPrefix, const class btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut, btAlignedObjectArray& texturesOut, struct b3ImportMeshData& meshData) const; }; #endif //BULLET_URDF_IMPORTER_H diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.h b/examples/Importers/ImportURDFDemo/URDF2Bullet.h index f503a103e..cbbd58b58 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.h +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.h @@ -32,6 +32,8 @@ enum ConvertURDFFlags CUF_INITIALIZE_SAT_FEATURES = 4096, CUF_USE_SELF_COLLISION_INCLUDE_PARENT = 8192, CUF_PARSE_SENSORS = 16384, + CUF_USE_MATERIAL_COLORS_FROM_MTL = 32768, + CUF_USE_MATERIAL_TRANSPARANCY_FROM_MTL = 64738, }; struct UrdfVisualShapeCache diff --git a/examples/Importers/ImportURDFDemo/UrdfFindMeshFile.h b/examples/Importers/ImportURDFDemo/UrdfFindMeshFile.h index 22913210c..2903e7cf2 100644 --- a/examples/Importers/ImportURDFDemo/UrdfFindMeshFile.h +++ b/examples/Importers/ImportURDFDemo/UrdfFindMeshFile.h @@ -74,16 +74,8 @@ static bool UrdfFindMeshFile( 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) { @@ -100,6 +92,16 @@ static bool UrdfFindMeshFile( break; } } + if (existing_file.empty()) + { + 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()) { diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index e7bcb6b07..8e89d4608 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -9,7 +9,7 @@ #include "../Extras/InverseDynamics/btMultiBodyTreeCreator.hpp" #include "BulletCollision/CollisionDispatch/btInternalEdgeUtility.h" - +#include "../Importers/ImportMeshUtility/b3ImportMeshUtility.h" #include "BulletDynamics/MLCPSolvers/btDantzigSolver.h" #include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h" #include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h" @@ -2016,11 +2016,13 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface const b3CreateMultiBodyArgs& m_createBodyArgs; mutable b3AlignedObjectArray m_allocatedCollisionShapes; PhysicsServerCommandProcessorInternalData* m_data; + int m_flags; - ProgrammaticUrdfInterface(const b3CreateMultiBodyArgs& bodyArgs, PhysicsServerCommandProcessorInternalData* data) + ProgrammaticUrdfInterface(const b3CreateMultiBodyArgs& bodyArgs, PhysicsServerCommandProcessorInternalData* data, int flags) : m_bodyUniqueId(-1), m_createBodyArgs(bodyArgs), - m_data(data) + m_data(data), + m_flags(flags) { } @@ -2068,19 +2070,38 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface return false; } + mutable btHashMap m_linkColors; + virtual bool getLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const { - if (m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex] >= 0) + + if (m_flags & URDF_USE_MATERIAL_COLORS_FROM_MTL) { - const InternalVisualShapeHandle* visHandle = m_data->m_userVisualShapeHandles.getHandle(m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]); - if (visHandle) + const UrdfMaterialColor* matColPtr = m_linkColors[linkIndex]; + if (matColPtr) { - for (int i = 0; i < visHandle->m_visualShapes.size(); i++) + matCol = *matColPtr; + if ((m_flags&CUF_USE_MATERIAL_TRANSPARANCY_FROM_MTL)==0) { - if (visHandle->m_visualShapes[i].m_geometry.m_hasLocalMaterial) + matCol.m_rgbaColor[3] = 1; + } + + return true; + } + } else + { + if (m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex] >= 0) + { + const InternalVisualShapeHandle* visHandle = m_data->m_userVisualShapeHandles.getHandle(m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]); + if (visHandle) + { + for (int i = 0; i < visHandle->m_visualShapes.size(); i++) { - matCol = visHandle->m_visualShapes[i].m_geometry.m_localMaterial.m_matColor; - return true; + if (visHandle->m_visualShapes[i].m_geometry.m_hasLocalMaterial) + { + matCol = visHandle->m_visualShapes[i].m_geometry.m_localMaterial.m_matColor; + return true; + } } } } @@ -2274,7 +2295,21 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface { for (int v = 0; v < visHandle->m_visualShapes.size(); v++) { - u2b.convertURDFToVisualShapeInternal(&visHandle->m_visualShapes[v], pathPrefix, localInertiaFrame.inverse() * visHandle->m_visualShapes[v].m_linkLocalFrame, vertices, indices, textures); + b3ImportMeshData meshData; + u2b.convertURDFToVisualShapeInternal(&visHandle->m_visualShapes[v], pathPrefix, localInertiaFrame.inverse() * visHandle->m_visualShapes[v].m_linkLocalFrame, vertices, indices, textures, meshData); + if ((meshData.m_flags & B3_IMPORT_MESH_HAS_RGBA_COLOR) && + (meshData.m_flags & B3_IMPORT_MESH_HAS_SPECULAR_COLOR)) + { + UrdfMaterialColor matCol; + matCol.m_rgbaColor.setValue(meshData.m_rgbaColor[0], + meshData.m_rgbaColor[1], + meshData.m_rgbaColor[2], + meshData.m_rgbaColor[3]); + matCol.m_specularColor.setValue(meshData.m_specularColor[0], + meshData.m_specularColor[1], + meshData.m_specularColor[2]); + m_linkColors.insert(linkIndex, matCol); + } } if (vertices.size() && indices.size()) @@ -4462,6 +4497,7 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct } else { + visualShape.m_geometry.m_localMaterial.m_matColor.m_rgbaColor.setValue(1,1,1,1); } if (hasSpecular) { @@ -6641,7 +6677,14 @@ bool PhysicsServerCommandProcessor::processCreateMultiBodyCommand(const struct S { m_data->m_sdfRecentLoadedBodies.clear(); - ProgrammaticUrdfInterface u2b(clientCmd.m_createMultiBodyArgs, m_data); + int flags = 0; + + if (clientCmd.m_updateFlags & MULT_BODY_HAS_FLAGS) + { + flags = clientCmd.m_createMultiBodyArgs.m_flags; + } + + ProgrammaticUrdfInterface u2b(clientCmd.m_createMultiBodyArgs, m_data, flags); bool useMultiBody = true; if (clientCmd.m_updateFlags & MULT_BODY_USE_MAXIMAL_COORDINATES) @@ -6649,12 +6692,7 @@ bool PhysicsServerCommandProcessor::processCreateMultiBodyCommand(const struct S useMultiBody = false; } - int flags = 0; - - if (clientCmd.m_updateFlags & MULT_BODY_HAS_FLAGS) - { - flags = clientCmd.m_createMultiBodyArgs.m_flags; - } + bool ok = processImportedObjects("memory", bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b); diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 1cab59bf8..113d8d26c 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -819,6 +819,8 @@ enum eURDF_Flags URDF_INITIALIZE_SAT_FEATURES = 4096, URDF_USE_SELF_COLLISION_INCLUDE_PARENT = 8192, URDF_PARSE_SENSORS = 16384, + URDF_USE_MATERIAL_COLORS_FROM_MTL = 32768, + URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL = 64738, }; enum eUrdfGeomTypes //sync with UrdfParser UrdfGeomTypes diff --git a/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp b/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp index 84e522cf0..5f4837c45 100644 --- a/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp +++ b/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp @@ -6,7 +6,7 @@ #include #include "../../../CommonInterfaces/CommonFileIOInterface.h" #include "../../../Utils/b3ResourcePath.h" - +#include "Bullet3Common/b3HashMap.h" #ifndef B3_EXCLUDE_DEFAULT_FILEIO #include "../../../Utils/b3BulletDefaultFileIO.h" @@ -30,13 +30,258 @@ struct WrapperFileHandle int m_childFileHandle; }; +struct InMemoryFile +{ + char* m_buffer; + int m_fileSize; +}; + +struct InMemoryFileAccessor +{ + InMemoryFile* m_file; + int m_curPos; +}; + +struct InMemoryFileIO : public CommonFileIOInterface +{ + b3HashMap m_fileCache; + InMemoryFileAccessor m_fileHandles[B3_MAX_FILEIO_INTERFACES]; + + InMemoryFileIO() + { + for (int i=0;im_buffer); + delete (mem); + } + } + } + + char* allocateBuffer(int len) + { + char* buffer = 0; + if (len) + { + buffer = new char[len]; + } + return buffer; + } + + void freeBuffer(char* buffer) + { + delete[] buffer; + } + + virtual int registerFile(const char* fileName, char* buffer, int len) + { + InMemoryFile* f = new InMemoryFile(); + f->m_buffer = buffer; + f->m_fileSize = len; + b3HashString key(fileName); + m_fileCache.insert(key,f); + return 0; + } + + void removeFileFromCache(const char* fileName) + { + InMemoryFile* f = getInMemoryFile(fileName); + if (f) + { + m_fileCache.remove(fileName); + freeBuffer(f->m_buffer); + delete (f); + } + } + + InMemoryFile* getInMemoryFile(const char* fileName) + { + InMemoryFile** fPtr = m_fileCache[fileName]; + if (fPtr && *fPtr) + { + return *fPtr; + } + return 0; + } + + virtual int fileOpen(const char* fileName, const char* mode) + { + //search a free slot + int slot = -1; + for (int i=0;i=0) + { + InMemoryFile* f = getInMemoryFile(fileName); + if (f) + { + m_fileHandles[slot].m_curPos = 0; + m_fileHandles[slot].m_file = f; + } else + { + slot=-1; + } + } + //printf("InMemoryFileIO fileOpen %s, %d\n", fileName, slot); + return slot; + } + virtual int fileRead(int fileHandle, char* destBuffer, int numBytes) + { + if (fileHandle>=0 && fileHandle < B3_FILEIO_MAX_FILES) + { + InMemoryFileAccessor& f = m_fileHandles[fileHandle]; + if (f.m_file) + { + //if (numBytes>1) + // printf("curPos = %d\n", f.m_curPos); + if (f.m_curPos+numBytes <= f.m_file->m_fileSize) + { + memcpy(destBuffer,f.m_file->m_buffer+f.m_curPos,numBytes); + f.m_curPos+=numBytes; + //if (numBytes>1) + // printf("read %d bytes, now curPos = %d\n", numBytes, f.m_curPos); + return numBytes; + } else + { + if (numBytes!=1) + { + printf("InMemoryFileIO::fileRead Attempt to read beyond end of file\n"); + } + } + + } + } + return 0; + } + + virtual int fileWrite(int fileHandle,const char* sourceBuffer, int numBytes) + { + return 0; + } + virtual void fileClose(int fileHandle) + { + if (fileHandle>=0 && fileHandle < B3_FILEIO_MAX_FILES) + { + InMemoryFileAccessor& f = m_fileHandles[fileHandle]; + if (f.m_file) + { + m_fileHandles[fileHandle].m_file = 0; + m_fileHandles[fileHandle].m_curPos = 0; + //printf("InMemoryFileIO fileClose %d\n", fileHandle); + } + } + } + virtual bool findResourcePath(const char* fileName, char* resourcePathOut, int resourcePathMaxNumBytes) + { + InMemoryFile* f = getInMemoryFile(fileName); + int fileNameLen = strlen(fileName); + if (f && fileNameLen<(resourcePathMaxNumBytes-1)) + { + memcpy(resourcePathOut, fileName, fileNameLen); + resourcePathOut[fileNameLen]=0; + return true; + } + return false; + } + virtual char* readLine(int fileHandle, char* destBuffer, int numBytes) + { + int numRead = 0; + int endOfFile = 0; + if (fileHandle>=0 && fileHandle < B3_FILEIO_MAX_FILES ) + { + InMemoryFileAccessor& f = m_fileHandles[fileHandle]; + if (f.m_file) + { + //return ::fgets(destBuffer, numBytes, m_fileHandles[fileHandle]); + char c = 0; + do + { + int bytesRead = fileRead(fileHandle,&c,1); + if (bytesRead != 1) + { + endOfFile = 1; + c=0; + } + if (c && c!='\n') + { + char a='\r'; + if (c!=13) + { + destBuffer[numRead++]=c; + } else + { + destBuffer[numRead++]=0; + } + } + } while (c != 0 && c != '\n' && numRead<(numBytes-1)); + } + } + if (numRead==0 && endOfFile) + { + return 0; + } + + if (numRead=0) + { + destBuffer[numRead]=0; + } + return &destBuffer[0]; + } else + { + if (endOfFile==0) + { + printf("InMemoryFileIO::readLine readLine warning: numRead=%d, numBytes=%d\n", numRead, numBytes); + } + } + return 0; + } + virtual int getFileSize(int fileHandle) + { + if (fileHandle>=0 && fileHandle < B3_FILEIO_MAX_FILES ) + { + + InMemoryFileAccessor& f = m_fileHandles[fileHandle]; + if (f.m_file) + { + return f.m_file->m_fileSize; + } + } + return 0; + } +}; + struct WrapperFileIO : public CommonFileIOInterface { CommonFileIOInterface* m_availableFileIOInterfaces[B3_MAX_FILEIO_INTERFACES]; int m_numWrapperInterfaces; WrapperFileHandle m_wrapperFileHandles[B3_MAX_FILEIO_INTERFACES]; - + InMemoryFileIO m_cachedFiles; WrapperFileIO() :m_numWrapperInterfaces(0) @@ -47,6 +292,7 @@ struct WrapperFileIO : public CommonFileIOInterface m_wrapperFileHandles[i].childFileIO=0; m_wrapperFileHandles[i].m_childFileHandle=0; } + //addFileIOInterface(&m_cachedFiles); } virtual ~WrapperFileIO() @@ -86,6 +332,7 @@ struct WrapperFileIO : public CommonFileIOInterface virtual int fileOpen(const char* fileName, const char* mode) { + //find an available wrapperFileHandle slot int wrapperFileHandle=-1; int slot = -1; @@ -99,20 +346,76 @@ struct WrapperFileIO : public CommonFileIOInterface } if (slot>=0) { - //figure out what wrapper interface to use - //use the first one that can open the file - for (int i=0;ifileOpen(fileName, mode); - if (childHandle>=0) + CommonFileIOInterface* childFileIO=m_availableFileIOInterfaces[i]; + if (childFileIO) { - wrapperFileHandle = slot; - m_wrapperFileHandles[slot].childFileIO = childFileIO; - m_wrapperFileHandles[slot].m_childFileHandle = childHandle; - break; + int childHandle = childFileIO->fileOpen(fileName, mode); + if (childHandle>=0) + { + int fileSize = childFileIO->getFileSize(childHandle); + char* buffer = 0; + if (fileSize) + { + buffer = m_cachedFiles.allocateBuffer(fileSize); + if (buffer) + { + int readBytes = childFileIO->fileRead(childHandle, buffer, fileSize); + if (readBytes!=fileSize) + { + if (readBytesfileClose(childHandle); + break; + } + } + } + } + + { + int childHandle = m_cachedFiles.fileOpen(fileName, mode); + if (childHandle>=0) + { + wrapperFileHandle = slot; + m_wrapperFileHandles[slot].childFileIO = &m_cachedFiles; + m_wrapperFileHandles[slot].m_childFileHandle = childHandle; + } else + { + //figure out what wrapper interface to use + //use the first one that can open the file + for (int i=0;ifileOpen(fileName, mode); + if (childHandle>=0) + { + wrapperFileHandle = slot; + m_wrapperFileHandles[slot].childFileIO = childFileIO; + m_wrapperFileHandles[slot].m_childFileHandle = childHandle; + break; + } + } } } } @@ -155,6 +458,9 @@ struct WrapperFileIO : public CommonFileIOInterface } virtual bool findResourcePath(const char* fileName, char* resourcePathOut, int resourcePathMaxNumBytes) { + if (m_cachedFiles.findResourcePath(fileName, resourcePathOut, resourcePathMaxNumBytes)) + return true; + bool found = false; for (int i=0;im_text) { - obj->m_fileIO.addFileIOInterface(new ZipFileIO(arguments->m_text)); + obj->m_fileIO.addFileIOInterface(new ZipFileIO(arguments->m_text, &obj->m_fileIO)); } #else printf("eZipFileIO is not enabled in this build.\n"); diff --git a/examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h b/examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h index 0df577052..7366f1e24 100644 --- a/examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h +++ b/examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h @@ -10,7 +10,7 @@ struct ZipFileIO : public CommonFileIOInterface unzFile m_fileHandles[B3_ZIP_FILEIO_MAX_FILES ]; int m_numFileHandles; - ZipFileIO(const char* zipfileName) + ZipFileIO(const char* zipfileName, CommonFileIOInterface* wrapperFileIO) :m_zipfileName(zipfileName), m_numFileHandles(0) { @@ -37,6 +37,7 @@ struct ZipFileIO : public CommonFileIOInterface virtual int fileOpen(const char* fileName, const char* mode) { + //search a free slot int slot = -1; for (int i=0;i=0) @@ -82,20 +81,28 @@ struct ZipFileIO : public CommonFileIOInterface { printf("unzGetCurrentFileInfo() != UNZ_OK (%d)\n", result); slot=-1; + unzClose(zipfile); + zipfile = 0; } else { result = unzOpenCurrentFile(zipfile); if (result == UNZ_OK) { + printf("zipFile::fileOpen %s in mode %s in fileHandle %d\n", fileName, mode, slot); + m_fileHandles[slot] = zipfile; } else { slot=-1; + unzClose(zipfile); + zipfile = 0; } } } else { slot=-1; + unzClose(zipfile); + zipfile = 0; } } } @@ -137,6 +144,7 @@ struct ZipFileIO : public CommonFileIOInterface unzFile f = m_fileHandles[fileHandle]; if (f) { + printf("zipFile::fileClose slot %d\n", fileHandle); unzClose(f); m_fileHandles[fileHandle]=0; } diff --git a/examples/Utils/b3BulletDefaultFileIO.h b/examples/Utils/b3BulletDefaultFileIO.h index 29fe79d62..6d681bbb0 100644 --- a/examples/Utils/b3BulletDefaultFileIO.h +++ b/examples/Utils/b3BulletDefaultFileIO.h @@ -149,7 +149,14 @@ struct b3BulletDefaultFileIO : public CommonFileIOInterface if (f) { char* txt = ::fgets(destBuffer, numBytes, m_fileHandles[fileHandle]); - destBuffer[strcspn(destBuffer, "\r\n")] = 0; + for (int i=0;i