First pass of load files through an interface (to allow loading from memory, zip file etc). So instead of posix fopen/fread, using CommonFileIOInterface.
A fileIO plugin can override custom file IO operations. As a small test, load files from a zipfile in memory. Default fileIO implementation is in examples/Utils/b3BulletDefaultFileIO.h Affects URDF, SDF, MJCF, Wavefront OBJ, STL, DAE, images.
This commit is contained in:
@@ -11,7 +11,7 @@
|
|||||||
#include "../../examples/Importers/ImportURDFDemo/URDF2Bullet.h"
|
#include "../../examples/Importers/ImportURDFDemo/URDF2Bullet.h"
|
||||||
#include "../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h"
|
#include "../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h"
|
||||||
#include "../../examples/Importers/ImportURDFDemo/URDF2Bullet.h"
|
#include "../../examples/Importers/ImportURDFDemo/URDF2Bullet.h"
|
||||||
|
#include "../../examples/Utils/b3BulletDefaultFileIO.h"
|
||||||
/// Create a btMultiBody model from URDF.
|
/// Create a btMultiBody model from URDF.
|
||||||
/// This is adapted from Bullet URDF loader example
|
/// This is adapted from Bullet URDF loader example
|
||||||
class MyBtMultiBodyFromURDF
|
class MyBtMultiBodyFromURDF
|
||||||
@@ -48,7 +48,8 @@ public:
|
|||||||
{
|
{
|
||||||
this->createEmptyDynamicsWorld();
|
this->createEmptyDynamicsWorld();
|
||||||
m_dynamicsWorld->setGravity(m_gravity);
|
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);
|
URDFImporterInterface &u2b(urdf_importer);
|
||||||
bool loadOk = u2b.loadURDF(m_filename.c_str(), m_base_fixed);
|
bool loadOk = u2b.loadURDF(m_filename.c_str(), m_base_fixed);
|
||||||
|
|
||||||
|
|||||||
@@ -14,6 +14,7 @@
|
|||||||
#include "../Utils/b3ResourcePath.h"
|
#include "../Utils/b3ResourcePath.h"
|
||||||
#include "Bullet3Common/b3CommandLineArgs.h"
|
#include "Bullet3Common/b3CommandLineArgs.h"
|
||||||
#include "Bullet3Common/b3HashMap.h"
|
#include "Bullet3Common/b3HashMap.h"
|
||||||
|
#include "../Utils/b3BulletDefaultFileIO.h"
|
||||||
|
|
||||||
struct ShapeContainer
|
struct ShapeContainer
|
||||||
{
|
{
|
||||||
@@ -71,12 +72,14 @@ int main(int argc, char* argv[])
|
|||||||
bool mergeMaterials = args.CheckCmdLineFlag("mergeMaterials");
|
bool mergeMaterials = args.CheckCmdLineFlag("mergeMaterials");
|
||||||
|
|
||||||
char fileNameWithPath[MAX_PATH_LEN];
|
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];
|
char materialPrefixPath[MAX_PATH_LEN];
|
||||||
b3FileUtils::extractPath(fileNameWithPath, materialPrefixPath, MAX_PATH_LEN);
|
b3FileUtils::extractPath(fileNameWithPath, materialPrefixPath, MAX_PATH_LEN);
|
||||||
|
|
||||||
std::vector<tinyobj::shape_t> shapes;
|
std::vector<tinyobj::shape_t> 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];
|
char sdfFileName[MAX_PATH_LEN];
|
||||||
sprintf(sdfFileName, "%s%s.sdf", materialPrefixPath, "newsdf");
|
sprintf(sdfFileName, "%s%s.sdf", materialPrefixPath, "newsdf");
|
||||||
|
|||||||
19
examples/CommonInterfaces/CommonFileIOInterface.h
Normal file
19
examples/CommonInterfaces/CommonFileIOInterface.h
Normal file
@@ -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
|
||||||
@@ -24,6 +24,7 @@ subject to the following restrictions:
|
|||||||
#include "Bullet3Common/b3FileUtils.h"
|
#include "Bullet3Common/b3FileUtils.h"
|
||||||
#include "../Importers/ImportObjDemo/LoadMeshFromObj.h"
|
#include "../Importers/ImportObjDemo/LoadMeshFromObj.h"
|
||||||
#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
|
#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
|
||||||
|
#include "../Utils/b3BulletDefaultFileIO.h"
|
||||||
|
|
||||||
struct RigidBodyFromObjExample : public CommonRigidBodyBase
|
struct RigidBodyFromObjExample : public CommonRigidBodyBase
|
||||||
{
|
{
|
||||||
@@ -73,13 +74,14 @@ void RigidBodyFromObjExample::initPhysics()
|
|||||||
//load our obj mesh
|
//load our obj mesh
|
||||||
const char* fileName = "teddy.obj"; //sphere8.obj";//sponza_closed.obj";//sphere8.obj";
|
const char* fileName = "teddy.obj"; //sphere8.obj";//sponza_closed.obj";//sphere8.obj";
|
||||||
char relativeFileName[1024];
|
char relativeFileName[1024];
|
||||||
if (b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024))
|
if (b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024,0))
|
||||||
{
|
{
|
||||||
char pathPrefix[1024];
|
char pathPrefix[1024];
|
||||||
b3FileUtils::extractPath(relativeFileName, 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);
|
printf("[INFO] Obj loaded: Extracted %d verticed from obj file [%s]\n", glmesh->m_numvertices, fileName);
|
||||||
|
|
||||||
const GLInstanceVertex& v = glmesh->m_vertices->at(0);
|
const GLInstanceVertex& v = glmesh->m_vertices->at(0);
|
||||||
|
|||||||
@@ -24,7 +24,7 @@ subject to the following restrictions:
|
|||||||
#include "LoadMeshFromCollada.h"
|
#include "LoadMeshFromCollada.h"
|
||||||
#include "Bullet3Common/b3FileUtils.h"
|
#include "Bullet3Common/b3FileUtils.h"
|
||||||
#include "../../Utils/b3ResourcePath.h"
|
#include "../../Utils/b3ResourcePath.h"
|
||||||
|
#include "../../Utils/b3BulletDefaultFileIO.h"
|
||||||
#include "../CommonInterfaces/CommonRigidBodyBase.h"
|
#include "../CommonInterfaces/CommonRigidBodyBase.h"
|
||||||
|
|
||||||
class ImportColladaSetup : public CommonRigidBodyBase
|
class ImportColladaSetup : public CommonRigidBodyBase
|
||||||
@@ -79,7 +79,7 @@ void ImportColladaSetup::initPhysics()
|
|||||||
|
|
||||||
char relativeFileName[1024];
|
char relativeFileName[1024];
|
||||||
|
|
||||||
if (!b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024))
|
if (!b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024,0))
|
||||||
return;
|
return;
|
||||||
|
|
||||||
btVector3 shift(0, 0, 0);
|
btVector3 shift(0, 0, 0);
|
||||||
@@ -94,7 +94,7 @@ void ImportColladaSetup::initPhysics()
|
|||||||
btTransform upAxisTrans;
|
btTransform upAxisTrans;
|
||||||
upAxisTrans.setIdentity();
|
upAxisTrans.setIdentity();
|
||||||
|
|
||||||
btVector3 color(0, 0, 1);
|
btVector4 color(0, 0, 1,1);
|
||||||
|
|
||||||
#ifdef COMPARE_WITH_ASSIMP
|
#ifdef COMPARE_WITH_ASSIMP
|
||||||
static int useAssimp = 0;
|
static int useAssimp = 0;
|
||||||
@@ -119,7 +119,8 @@ void ImportColladaSetup::initPhysics()
|
|||||||
{
|
{
|
||||||
fileIndex = 0;
|
fileIndex = 0;
|
||||||
}
|
}
|
||||||
LoadMeshFromCollada(relativeFileName, visualShapes, visualShapeInstances, upAxisTrans, unitMeterScaling, upAxis);
|
b3BulletDefaultFileIO fileIO;
|
||||||
|
LoadMeshFromCollada(relativeFileName, visualShapes, visualShapeInstances, upAxisTrans, unitMeterScaling, upAxis,&fileIO);
|
||||||
#endif // COMPARE_WITH_ASSIMP
|
#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
|
//at the moment our graphics engine requires instances that share the same visual shape to be added right after registering the shape
|
||||||
|
|||||||
@@ -22,10 +22,11 @@ subject to the following restrictions:
|
|||||||
#include "../../ThirdPartyLibs/tinyxml2/tinyxml2.h"
|
#include "../../ThirdPartyLibs/tinyxml2/tinyxml2.h"
|
||||||
using namespace tinyxml2;
|
using namespace tinyxml2;
|
||||||
|
|
||||||
#include "Bullet3Common/b3FileUtils.h"
|
|
||||||
#include "LinearMath/btHashMap.h"
|
#include "LinearMath/btHashMap.h"
|
||||||
#include <assert.h>
|
#include <assert.h>
|
||||||
#include "btMatrix4x4.h"
|
#include "btMatrix4x4.h"
|
||||||
|
#include "../../CommonInterfaces/CommonFileIOInterface.h"
|
||||||
|
|
||||||
#define MAX_VISUAL_SHAPES 512
|
#define MAX_VISUAL_SHAPES 512
|
||||||
|
|
||||||
@@ -561,7 +562,7 @@ void getUnitMeterScalingAndUpAxisTransform(XMLDocument& doc, btTransform& tr, fl
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances, btTransform& upAxisTransform, float& unitMeterScaling, int clientUpAxis)
|
void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances, btTransform& upAxisTransform, float& unitMeterScaling, int clientUpAxis, struct CommonFileIOInterface* fileIO)
|
||||||
{
|
{
|
||||||
// GLInstanceGraphicsShape* instance = 0;
|
// GLInstanceGraphicsShape* instance = 0;
|
||||||
|
|
||||||
@@ -570,16 +571,32 @@ void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray<GLIn
|
|||||||
|
|
||||||
float extraScaling = 1; //0.01;
|
float extraScaling = 1; //0.01;
|
||||||
btHashMap<btHashString, int> name2ShapeIndex;
|
btHashMap<btHashString, int> name2ShapeIndex;
|
||||||
b3FileUtils f;
|
|
||||||
char filename[1024];
|
char filename[1024];
|
||||||
if (!f.findFile(relativeFileName, filename, 1024))
|
if (!fileIO->findResourcePath(relativeFileName, filename, 1024))
|
||||||
{
|
{
|
||||||
b3Warning("File not found: %s\n", filename);
|
b3Warning("File not found: %s\n", filename);
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
XMLDocument doc;
|
XMLDocument doc;
|
||||||
if (doc.LoadFile(filename) != XML_SUCCESS)
|
//doc.Parse((const char*)filedata, 0, TIXML_ENCODING_UTF8);
|
||||||
|
b3AlignedObjectArray<char> 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;
|
return;
|
||||||
|
|
||||||
//We need units to be in meter, so apply a scaling using the asset/units meter
|
//We need units to be in meter, so apply a scaling using the asset/units meter
|
||||||
|
|||||||
@@ -28,7 +28,8 @@ void LoadMeshFromCollada(const char* relativeFileName,
|
|||||||
btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances,
|
btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances,
|
||||||
btTransform& upAxisTrans,
|
btTransform& upAxisTrans,
|
||||||
float& unitMeterScaling,
|
float& unitMeterScaling,
|
||||||
int clientUpAxis);
|
int clientUpAxis,
|
||||||
|
struct CommonFileIOInterface* fileIO);
|
||||||
|
|
||||||
//#define COMPARE_WITH_ASSIMP
|
//#define COMPARE_WITH_ASSIMP
|
||||||
#ifdef COMPARE_WITH_ASSIMP
|
#ifdef COMPARE_WITH_ASSIMP
|
||||||
|
|||||||
@@ -6,6 +6,8 @@
|
|||||||
#include "BulletCollision/CollisionShapes/btShapeHull.h"
|
#include "BulletCollision/CollisionShapes/btShapeHull.h"
|
||||||
#include "../../CommonInterfaces/CommonRenderInterface.h"
|
#include "../../CommonInterfaces/CommonRenderInterface.h"
|
||||||
#include "../../CommonInterfaces/CommonGUIHelperInterface.h"
|
#include "../../CommonInterfaces/CommonGUIHelperInterface.h"
|
||||||
|
#include "../../CommonInterfaces/CommonFileIOInterface.h"
|
||||||
|
#include "../ImportURDFDemo/UrdfFindMeshFile.h"
|
||||||
#include <string>
|
#include <string>
|
||||||
#include "../../Utils/b3ResourcePath.h"
|
#include "../../Utils/b3ResourcePath.h"
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
@@ -32,6 +34,7 @@
|
|||||||
#include "BulletCollision/CollisionShapes/btConvexHullShape.h"
|
#include "BulletCollision/CollisionShapes/btConvexHullShape.h"
|
||||||
#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
|
#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
|
||||||
#include "BulletCollision/CollisionShapes/btTriangleMesh.h"
|
#include "BulletCollision/CollisionShapes/btTriangleMesh.h"
|
||||||
|
|
||||||
using namespace tinyxml2;
|
using namespace tinyxml2;
|
||||||
|
|
||||||
#define mjcf_sphere_indiced textured_detailed_sphere_indices
|
#define mjcf_sphere_indiced textured_detailed_sphere_indices
|
||||||
@@ -204,12 +207,15 @@ struct BulletMJCFImporterInternalData
|
|||||||
int m_flags;
|
int m_flags;
|
||||||
int m_textureId;
|
int m_textureId;
|
||||||
|
|
||||||
|
CommonFileIOInterface* m_fileIO;
|
||||||
|
|
||||||
BulletMJCFImporterInternalData()
|
BulletMJCFImporterInternalData()
|
||||||
: m_inertiaFromGeom(true),
|
: m_inertiaFromGeom(true),
|
||||||
m_activeModel(-1),
|
m_activeModel(-1),
|
||||||
m_activeBodyUniqueId(-1),
|
m_activeBodyUniqueId(-1),
|
||||||
m_flags(0),
|
m_flags(0),
|
||||||
m_textureId(-1)
|
m_textureId(-1),
|
||||||
|
m_fileIO(0)
|
||||||
{
|
{
|
||||||
m_pathPrefix[0] = 0;
|
m_pathPrefix[0] = 0;
|
||||||
}
|
}
|
||||||
@@ -887,7 +893,7 @@ struct BulletMJCFImporterInternalData
|
|||||||
{
|
{
|
||||||
geom.m_type = URDF_GEOM_MESH;
|
geom.m_type = URDF_GEOM_MESH;
|
||||||
geom.m_meshFileName = assetPtr->m_fileName;
|
geom.m_meshFileName = assetPtr->m_fileName;
|
||||||
bool exists = findExistingMeshFile(
|
bool exists = UrdfFindMeshFile(m_fileIO,
|
||||||
m_sourceFileName, assetPtr->m_fileName, sourceFileLocation(link_xml),
|
m_sourceFileName, assetPtr->m_fileName, sourceFileLocation(link_xml),
|
||||||
&geom.m_meshFileName,
|
&geom.m_meshFileName,
|
||||||
&geom.m_meshFileType);
|
&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 = new BulletMJCFImporterInternalData();
|
||||||
m_data->m_guiHelper = helper;
|
m_data->m_guiHelper = helper;
|
||||||
m_data->m_customVisualShapesConverter = customConverter;
|
m_data->m_customVisualShapesConverter = customConverter;
|
||||||
m_data->m_flags = flags;
|
m_data->m_flags = flags;
|
||||||
m_data->m_textureId = -1;
|
m_data->m_textureId = -1;
|
||||||
|
m_data->m_fileIO = fileIO;
|
||||||
}
|
}
|
||||||
|
|
||||||
BulletMJCFImporter::~BulletMJCFImporter()
|
BulletMJCFImporter::~BulletMJCFImporter()
|
||||||
@@ -1433,7 +1440,7 @@ bool BulletMJCFImporter::loadMJCF(const char* fileName, MJCFErrorLogger* logger,
|
|||||||
b3FileUtils fu;
|
b3FileUtils fu;
|
||||||
|
|
||||||
//bool fileFound = fu.findFile(fileName, relativeFileName, 1024);
|
//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;
|
m_data->m_sourceFileName = relativeFileName;
|
||||||
|
|
||||||
std::string xml_string;
|
std::string xml_string;
|
||||||
@@ -1962,7 +1969,7 @@ void BulletMJCFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu
|
|||||||
case UrdfGeometry::FILE_OBJ:
|
case UrdfGeometry::FILE_OBJ:
|
||||||
{
|
{
|
||||||
b3ImportMeshData meshData;
|
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)
|
if (meshData.m_textureImage1)
|
||||||
{
|
{
|
||||||
@@ -1980,7 +1987,7 @@ void BulletMJCFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu
|
|||||||
|
|
||||||
case UrdfGeometry::FILE_STL:
|
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;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1998,7 +2005,8 @@ void BulletMJCFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu
|
|||||||
visualShapeInstances,
|
visualShapeInstances,
|
||||||
upAxisTrans,
|
upAxisTrans,
|
||||||
unitMeterScaling,
|
unitMeterScaling,
|
||||||
upAxis);
|
upAxis,
|
||||||
|
m_data->m_fileIO);
|
||||||
|
|
||||||
glmesh = new GLInstanceGraphicsShape;
|
glmesh = new GLInstanceGraphicsShape;
|
||||||
// int index = 0;
|
// int index = 0;
|
||||||
@@ -2243,7 +2251,7 @@ void BulletMJCFImporter::convertLinkVisualShapes2(int linkIndex, int urdfIndex,
|
|||||||
if (m_data->m_customVisualShapesConverter)
|
if (m_data->m_customVisualShapesConverter)
|
||||||
{
|
{
|
||||||
const UrdfLink* link = m_data->getLink(m_data->m_activeModel, urdfIndex);
|
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)
|
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
|
else
|
||||||
{
|
{
|
||||||
std::vector<tinyobj::shape_t> shapes;
|
std::vector<tinyobj::shape_t> 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
|
//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);
|
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:
|
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;
|
break;
|
||||||
}
|
}
|
||||||
default:
|
default:
|
||||||
|
|||||||
@@ -27,7 +27,7 @@ class BulletMJCFImporter : public URDFImporterInterface
|
|||||||
void convertURDFToVisualShapeInternal(const struct UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<struct GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MJCFURDFTexture>& texturesOut) const;
|
void convertURDFToVisualShapeInternal(const struct UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<struct GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MJCFURDFTexture>& texturesOut) const;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
BulletMJCFImporter(struct GUIHelperInterface* helper, UrdfRenderingInterface* customConverter, int flags);
|
BulletMJCFImporter(struct GUIHelperInterface* helper, UrdfRenderingInterface* customConverter, struct CommonFileIOInterface* fileIO, int flags);
|
||||||
virtual ~BulletMJCFImporter();
|
virtual ~BulletMJCFImporter();
|
||||||
|
|
||||||
virtual bool parseMJCFString(const char* xmlString, MJCFErrorLogger* logger);
|
virtual bool parseMJCFString(const char* xmlString, MJCFErrorLogger* logger);
|
||||||
|
|||||||
@@ -10,7 +10,7 @@
|
|||||||
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
|
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
|
||||||
#include "../CommonInterfaces/CommonParameterInterface.h"
|
#include "../CommonInterfaces/CommonParameterInterface.h"
|
||||||
#include "../../Utils/b3ResourcePath.h"
|
#include "../../Utils/b3ResourcePath.h"
|
||||||
|
#include "../../Utils/b3BulletDefaultFileIO.h"
|
||||||
#include "../CommonInterfaces/CommonMultiBodyBase.h"
|
#include "../CommonInterfaces/CommonMultiBodyBase.h"
|
||||||
|
|
||||||
#include "../ImportURDFDemo/MyMultiBodyCreator.h"
|
#include "../ImportURDFDemo/MyMultiBodyCreator.h"
|
||||||
@@ -200,7 +200,8 @@ void ImportMJCFSetup::initPhysics()
|
|||||||
}
|
}
|
||||||
|
|
||||||
int flags = 0;
|
int flags = 0;
|
||||||
BulletMJCFImporter importer(m_guiHelper, 0, flags);
|
b3BulletDefaultFileIO fileIO;
|
||||||
|
BulletMJCFImporter importer(m_guiHelper, 0, &fileIO, flags);
|
||||||
MyMJCFLogger logger;
|
MyMJCFLogger logger;
|
||||||
bool result = importer.loadMJCF(m_fileName, &logger);
|
bool result = importer.loadMJCF(m_fileName, &logger);
|
||||||
if (result)
|
if (result)
|
||||||
|
|||||||
@@ -9,6 +9,7 @@
|
|||||||
#include "stb_image/stb_image.h"
|
#include "stb_image/stb_image.h"
|
||||||
#include "../ImportObjDemo/LoadMeshFromObj.h"
|
#include "../ImportObjDemo/LoadMeshFromObj.h"
|
||||||
#include "Bullet3Common/b3HashMap.h"
|
#include "Bullet3Common/b3HashMap.h"
|
||||||
|
#include "../../CommonInterfaces/CommonFileIOInterface.h"
|
||||||
|
|
||||||
struct CachedTextureResult
|
struct CachedTextureResult
|
||||||
{
|
{
|
||||||
@@ -45,7 +46,7 @@ struct CachedTextureManager
|
|||||||
};
|
};
|
||||||
static CachedTextureManager sTexCacheMgr;
|
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");
|
B3_PROFILE("loadAndRegisterMeshFromFileInternal");
|
||||||
meshData.m_gfxShape = 0;
|
meshData.m_gfxShape = 0;
|
||||||
@@ -55,7 +56,7 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string&
|
|||||||
meshData.m_isCached = false;
|
meshData.m_isCached = false;
|
||||||
|
|
||||||
char relativeFileName[1024];
|
char relativeFileName[1024];
|
||||||
if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024))
|
if (fileIO->findResourcePath(fileName.c_str(), relativeFileName, 1024))
|
||||||
{
|
{
|
||||||
char pathPrefix[1024];
|
char pathPrefix[1024];
|
||||||
|
|
||||||
@@ -65,7 +66,7 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string&
|
|||||||
std::vector<tinyobj::shape_t> shapes;
|
std::vector<tinyobj::shape_t> shapes;
|
||||||
{
|
{
|
||||||
B3_PROFILE("tinyobj::LoadObj");
|
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);
|
//std::string err = tinyobj::LoadObj(shapes, relativeFileName, pathPrefix);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -91,7 +92,7 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string&
|
|||||||
char relativeFileName[1024];
|
char relativeFileName[1024];
|
||||||
sprintf(relativeFileName, "%s%s", prefix[i], filename);
|
sprintf(relativeFileName, "%s%s", prefix[i], filename);
|
||||||
char relativeFileName2[1024];
|
char relativeFileName2[1024];
|
||||||
if (b3ResourcePath::findResourcePath(relativeFileName, relativeFileName2, 1024))
|
if (fileIO->findResourcePath(relativeFileName, relativeFileName2, 1024))
|
||||||
{
|
{
|
||||||
if (b3IsFileCachingEnabled())
|
if (b3IsFileCachingEnabled())
|
||||||
{
|
{
|
||||||
@@ -110,7 +111,30 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string&
|
|||||||
|
|
||||||
if (image == 0)
|
if (image == 0)
|
||||||
{
|
{
|
||||||
image = stbi_load(relativeFileName, &width, &height, &n, 3);
|
|
||||||
|
b3AlignedObjectArray<char> 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;
|
meshData.m_textureImage1 = image;
|
||||||
|
|
||||||
|
|||||||
@@ -16,7 +16,7 @@ struct b3ImportMeshData
|
|||||||
class b3ImportMeshUtility
|
class b3ImportMeshUtility
|
||||||
{
|
{
|
||||||
public:
|
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
|
#endif //B3_IMPORT_MESH_UTILITY_H
|
||||||
|
|||||||
@@ -7,6 +7,7 @@
|
|||||||
#include "../OpenGLWindow/SimpleOpenGL3App.h"
|
#include "../OpenGLWindow/SimpleOpenGL3App.h"
|
||||||
#include "Wavefront2GLInstanceGraphicsShape.h"
|
#include "Wavefront2GLInstanceGraphicsShape.h"
|
||||||
#include "../../Utils/b3ResourcePath.h"
|
#include "../../Utils/b3ResourcePath.h"
|
||||||
|
#include "../../Utils/b3BulletDefaultFileIO.h"
|
||||||
#include "Bullet3Common/b3FileUtils.h"
|
#include "Bullet3Common/b3FileUtils.h"
|
||||||
|
|
||||||
#include "stb_image/stb_image.h"
|
#include "stb_image/stb_image.h"
|
||||||
@@ -56,7 +57,8 @@ int loadAndRegisterMeshFromFile2(const std::string& fileName, CommonRenderInterf
|
|||||||
int shapeId = -1;
|
int shapeId = -1;
|
||||||
|
|
||||||
b3ImportMeshData meshData;
|
b3ImportMeshData meshData;
|
||||||
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fileName, meshData))
|
b3BulletDefaultFileIO fileIO;
|
||||||
|
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fileName, meshData,&fileIO))
|
||||||
{
|
{
|
||||||
int textureIndex = -1;
|
int textureIndex = -1;
|
||||||
|
|
||||||
@@ -94,7 +96,7 @@ void ImportObjSetup::initPhysics()
|
|||||||
btQuaternion orn = trans.getRotation();
|
btQuaternion orn = trans.getRotation();
|
||||||
|
|
||||||
btVector3 scaling(1, 1, 1);
|
btVector3 scaling(1, 1, 1);
|
||||||
btVector3 color(1, 1, 1);
|
btVector4 color(1, 1, 1,1);
|
||||||
|
|
||||||
int shapeId = loadAndRegisterMeshFromFile2(m_fileName, m_guiHelper->getRenderInterface());
|
int shapeId = loadAndRegisterMeshFromFile2(m_fileName, m_guiHelper->getRenderInterface());
|
||||||
if (shapeId >= 0)
|
if (shapeId >= 0)
|
||||||
|
|||||||
@@ -33,7 +33,9 @@ void b3EnableFileCaching(int enable)
|
|||||||
std::string LoadFromCachedOrFromObj(
|
std::string LoadFromCachedOrFromObj(
|
||||||
std::vector<tinyobj::shape_t>& shapes, // [output]
|
std::vector<tinyobj::shape_t>& shapes, // [output]
|
||||||
const char* filename,
|
const char* filename,
|
||||||
const char* mtl_basepath)
|
const char* mtl_basepath,
|
||||||
|
struct CommonFileIOInterface* fileIO
|
||||||
|
)
|
||||||
{
|
{
|
||||||
CachedObjResult* resultPtr = gCachedObjResults[filename];
|
CachedObjResult* resultPtr = gCachedObjResults[filename];
|
||||||
if (resultPtr)
|
if (resultPtr)
|
||||||
@@ -43,7 +45,7 @@ std::string LoadFromCachedOrFromObj(
|
|||||||
return result.m_msg;
|
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;
|
CachedObjResult result;
|
||||||
result.m_msg = err;
|
result.m_msg = err;
|
||||||
result.m_shapes = shapes;
|
result.m_shapes = shapes;
|
||||||
@@ -54,13 +56,13 @@ std::string LoadFromCachedOrFromObj(
|
|||||||
return err;
|
return err;
|
||||||
}
|
}
|
||||||
|
|
||||||
GLInstanceGraphicsShape* LoadMeshFromObj(const char* relativeFileName, const char* materialPrefixPath)
|
GLInstanceGraphicsShape* LoadMeshFromObj(const char* relativeFileName, const char* materialPrefixPath, struct CommonFileIOInterface* fileIO)
|
||||||
{
|
{
|
||||||
B3_PROFILE("LoadMeshFromObj");
|
B3_PROFILE("LoadMeshFromObj");
|
||||||
std::vector<tinyobj::shape_t> shapes;
|
std::vector<tinyobj::shape_t> shapes;
|
||||||
{
|
{
|
||||||
B3_PROFILE("tinyobj::LoadObj2");
|
B3_PROFILE("tinyobj::LoadObj2");
|
||||||
std::string err = LoadFromCachedOrFromObj(shapes, relativeFileName, materialPrefixPath);
|
std::string err = LoadFromCachedOrFromObj(shapes, relativeFileName, materialPrefixPath,fileIO);
|
||||||
}
|
}
|
||||||
|
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -8,11 +8,13 @@ struct GLInstanceGraphicsShape;
|
|||||||
int b3IsFileCachingEnabled();
|
int b3IsFileCachingEnabled();
|
||||||
void b3EnableFileCaching(int enable);
|
void b3EnableFileCaching(int enable);
|
||||||
|
|
||||||
|
|
||||||
std::string LoadFromCachedOrFromObj(
|
std::string LoadFromCachedOrFromObj(
|
||||||
std::vector<tinyobj::shape_t>& shapes, // [output]
|
std::vector<tinyobj::shape_t>& shapes, // [output]
|
||||||
const char* filename,
|
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
|
#endif //LOAD_MESH_FROM_OBJ_H
|
||||||
|
|||||||
@@ -162,7 +162,7 @@ void ImportSDFSetup::initPhysics()
|
|||||||
|
|
||||||
m_dynamicsWorld->setGravity(gravity);
|
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);
|
bool loadOk = u2b.loadSDF(m_fileName);
|
||||||
|
|
||||||
|
|||||||
@@ -7,6 +7,7 @@
|
|||||||
#include "LoadMeshFromSTL.h"
|
#include "LoadMeshFromSTL.h"
|
||||||
#include "../CommonInterfaces/CommonRigidBodyBase.h"
|
#include "../CommonInterfaces/CommonRigidBodyBase.h"
|
||||||
#include "../../Utils/b3ResourcePath.h"
|
#include "../../Utils/b3ResourcePath.h"
|
||||||
|
#include "../../Utils/b3BulletDefaultFileIO.h"
|
||||||
|
|
||||||
class ImportSTLSetup : public CommonRigidBodyBase
|
class ImportSTLSetup : public CommonRigidBodyBase
|
||||||
{
|
{
|
||||||
@@ -55,7 +56,7 @@ void ImportSTLSetup::initPhysics()
|
|||||||
m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe);
|
m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe);
|
||||||
|
|
||||||
char relativeFileName[1024];
|
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);
|
b3Warning("Cannot find file %s\n", m_fileName);
|
||||||
return;
|
return;
|
||||||
@@ -65,7 +66,8 @@ void ImportSTLSetup::initPhysics()
|
|||||||
// int index=10;
|
// int index=10;
|
||||||
|
|
||||||
{
|
{
|
||||||
GLInstanceGraphicsShape* gfxShape = LoadMeshFromSTL(relativeFileName);
|
b3BulletDefaultFileIO fileIO;
|
||||||
|
GLInstanceGraphicsShape* gfxShape = LoadMeshFromSTL(relativeFileName,&fileIO);
|
||||||
|
|
||||||
btTransform trans;
|
btTransform trans;
|
||||||
trans.setIdentity();
|
trans.setIdentity();
|
||||||
@@ -74,7 +76,7 @@ void ImportSTLSetup::initPhysics()
|
|||||||
btVector3 position = trans.getOrigin();
|
btVector3 position = trans.getOrigin();
|
||||||
btQuaternion orn = trans.getRotation();
|
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);
|
int shapeId = m_guiHelper->getRenderInterface()->registerShape(&gfxShape->m_vertices->at(0).xyzw[0], gfxShape->m_numvertices, &gfxShape->m_indices->at(0), gfxShape->m_numIndices);
|
||||||
|
|
||||||
|
|||||||
@@ -5,6 +5,7 @@
|
|||||||
#include "../../OpenGLWindow/GLInstanceGraphicsShape.h"
|
#include "../../OpenGLWindow/GLInstanceGraphicsShape.h"
|
||||||
#include <stdio.h> //fopen
|
#include <stdio.h> //fopen
|
||||||
#include "Bullet3Common/b3AlignedObjectArray.h"
|
#include "Bullet3Common/b3AlignedObjectArray.h"
|
||||||
|
#include "../../CommonInterfaces/CommonFileIOInterface.h"
|
||||||
|
|
||||||
struct MySTLTriangle
|
struct MySTLTriangle
|
||||||
{
|
{
|
||||||
@@ -14,25 +15,21 @@ struct MySTLTriangle
|
|||||||
float vertex2[3];
|
float vertex2[3];
|
||||||
};
|
};
|
||||||
|
|
||||||
static GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName)
|
static GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName, struct CommonFileIOInterface* fileIO)
|
||||||
{
|
{
|
||||||
GLInstanceGraphicsShape* shape = 0;
|
GLInstanceGraphicsShape* shape = 0;
|
||||||
|
|
||||||
FILE* file = fopen(relativeFileName, "rb");
|
int fileHandle = fileIO->fileOpen(relativeFileName, "rb");
|
||||||
if (file)
|
if (fileHandle>=0)
|
||||||
{
|
{
|
||||||
int size = 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);
|
if (size>=0)
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
if (size)
|
|
||||||
{
|
{
|
||||||
//b3Warning("Open STL file of %d bytes\n",size);
|
//b3Warning("Open STL file of %d bytes\n",size);
|
||||||
char* memoryBuffer = new char[size + 1];
|
char* memoryBuffer = new char[size + 1];
|
||||||
int actualBytesRead = fread(memoryBuffer, 1, size, file);
|
int actualBytesRead = fileIO->fileRead(fileHandle, memoryBuffer, size);
|
||||||
if (actualBytesRead != size)
|
if (actualBytesRead != size)
|
||||||
{
|
{
|
||||||
b3Warning("Error reading from file %s", relativeFileName);
|
b3Warning("Error reading from file %s", relativeFileName);
|
||||||
@@ -97,7 +94,7 @@ static GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName)
|
|||||||
delete[] memoryBuffer;
|
delete[] memoryBuffer;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
fclose(file);
|
fileIO->fileClose(fileHandle);
|
||||||
}
|
}
|
||||||
if (shape)
|
if (shape)
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -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/btShapeHull.h" //to create a tesselation of a generic btConvexShape
|
||||||
#include "BulletCollision/CollisionShapes/btSdfCollisionShape.h"
|
#include "BulletCollision/CollisionShapes/btSdfCollisionShape.h"
|
||||||
#include "../../CommonInterfaces/CommonGUIHelperInterface.h"
|
#include "../../CommonInterfaces/CommonGUIHelperInterface.h"
|
||||||
|
#include "../../CommonInterfaces/CommonFileIOInterface.h"
|
||||||
#include "Bullet3Common/b3FileUtils.h"
|
#include "Bullet3Common/b3FileUtils.h"
|
||||||
#include <string>
|
#include <string>
|
||||||
#include "../../Utils/b3ResourcePath.h"
|
#include "../../Utils/b3ResourcePath.h"
|
||||||
|
#include "../../Utils/b3BulletDefaultFileIO.h"
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
#include "URDF2Bullet.h" //for flags
|
#include "URDF2Bullet.h" //for flags
|
||||||
#include "../ImportMeshUtility/b3ImportMeshUtility.h"
|
#include "../ImportMeshUtility/b3ImportMeshUtility.h"
|
||||||
|
|
||||||
@@ -35,13 +40,16 @@ static btScalar gUrdfDefaultCollisionMargin = 0.001;
|
|||||||
#include <list>
|
#include <list>
|
||||||
#include "UrdfParser.h"
|
#include "UrdfParser.h"
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
ATTRIBUTE_ALIGNED16(struct)
|
ATTRIBUTE_ALIGNED16(struct)
|
||||||
BulletURDFInternalData
|
BulletURDFInternalData
|
||||||
{
|
{
|
||||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||||
|
b3BulletDefaultFileIO m_defaultFileIO;
|
||||||
UrdfParser m_urdfParser;
|
UrdfParser m_urdfParser;
|
||||||
struct GUIHelperInterface* m_guiHelper;
|
struct GUIHelperInterface* m_guiHelper;
|
||||||
|
struct CommonFileIOInterface* m_fileIO;
|
||||||
std::string m_sourceFile;
|
std::string m_sourceFile;
|
||||||
char m_pathPrefix[1024];
|
char m_pathPrefix[1024];
|
||||||
int m_bodyId;
|
int m_bodyId;
|
||||||
@@ -63,7 +71,9 @@ BulletURDFInternalData
|
|||||||
m_pathPrefix[sizeof(m_pathPrefix) - 1] = 0; // required, strncpy doesn't write zero on overflow
|
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_enableTinyRenderer = true;
|
||||||
m_pathPrefix[0] = 0;
|
m_pathPrefix[0] = 0;
|
||||||
@@ -74,6 +84,8 @@ BulletURDFInternalData
|
|||||||
{
|
{
|
||||||
m_urdfParser.setGlobalScaling(scaling);
|
m_urdfParser.setGlobalScaling(scaling);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
void BulletURDFImporter::printTree()
|
void BulletURDFImporter::printTree()
|
||||||
@@ -81,9 +93,12 @@ void BulletURDFImporter::printTree()
|
|||||||
// btAssert(0);
|
// 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->setGlobalScaling(globalScaling);
|
||||||
m_data->m_flags = flags;
|
m_data->m_flags = flags;
|
||||||
m_data->m_guiHelper = helper;
|
m_data->m_guiHelper = helper;
|
||||||
@@ -128,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 = (b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024)) > 0;
|
bool fileFound = m_data->m_fileIO->findResourcePath(fileName, relativeFileName, 1024) > 0;
|
||||||
|
|
||||||
std::string xml_string;
|
std::string xml_string;
|
||||||
|
|
||||||
@@ -143,6 +158,23 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase)
|
|||||||
fu.extractPath(relativeFileName, path, sizeof(path));
|
fu.extractPath(relativeFileName, path, sizeof(path));
|
||||||
m_data->setSourceFile(relativeFileName, 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);
|
std::fstream xml_file(relativeFileName, std::fstream::in);
|
||||||
while (xml_file.good())
|
while (xml_file.good())
|
||||||
{
|
{
|
||||||
@@ -151,6 +183,8 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase)
|
|||||||
xml_string += (line + "\n");
|
xml_string += (line + "\n");
|
||||||
}
|
}
|
||||||
xml_file.close();
|
xml_file.close();
|
||||||
|
#endif
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
BulletErrorLogger loggie;
|
BulletErrorLogger loggie;
|
||||||
@@ -178,7 +212,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 = (b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024)) > 0;
|
bool fileFound = (m_data->m_fileIO->findResourcePath(fileName, relativeFileName, 1024)) > 0;
|
||||||
|
|
||||||
std::string xml_string;
|
std::string xml_string;
|
||||||
|
|
||||||
@@ -507,107 +541,7 @@ static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t
|
|||||||
return compound;
|
return compound;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool findExistingMeshFile(
|
|
||||||
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<std::string> 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<std::string>::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
|
int BulletURDFImporter::getUrdfFromCollisionShape(const btCollisionShape* collisionShape, UrdfCollision& collision) const
|
||||||
{
|
{
|
||||||
@@ -708,7 +642,7 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
|
|||||||
char relativeFileName[1024];
|
char relativeFileName[1024];
|
||||||
char pathPrefix[1024];
|
char pathPrefix[1024];
|
||||||
pathPrefix[0] = 0;
|
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);
|
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
|
||||||
|
|
||||||
@@ -758,16 +692,16 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
|
|||||||
char relativeFileName[1024];
|
char relativeFileName[1024];
|
||||||
char pathPrefix[1024];
|
char pathPrefix[1024];
|
||||||
pathPrefix[0] = 0;
|
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);
|
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
|
else
|
||||||
{
|
{
|
||||||
std::vector<tinyobj::shape_t> shapes;
|
std::vector<tinyobj::shape_t> 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
|
//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);
|
shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale, m_data->m_flags);
|
||||||
@@ -777,7 +711,7 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
|
|||||||
break;
|
break;
|
||||||
|
|
||||||
case UrdfGeometry::FILE_STL:
|
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;
|
break;
|
||||||
|
|
||||||
case UrdfGeometry::FILE_COLLADA:
|
case UrdfGeometry::FILE_COLLADA:
|
||||||
@@ -787,7 +721,7 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
|
|||||||
btTransform upAxisTrans;
|
btTransform upAxisTrans;
|
||||||
upAxisTrans.setIdentity();
|
upAxisTrans.setIdentity();
|
||||||
float unitMeterScaling = 1;
|
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 = new GLInstanceGraphicsShape;
|
||||||
glmesh->m_indices = new b3AlignedObjectArray<int>();
|
glmesh->m_indices = new b3AlignedObjectArray<int>();
|
||||||
@@ -982,7 +916,7 @@ void BulletURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu
|
|||||||
case UrdfGeometry::FILE_OBJ:
|
case UrdfGeometry::FILE_OBJ:
|
||||||
{
|
{
|
||||||
b3ImportMeshData meshData;
|
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)
|
if (meshData.m_textureImage1)
|
||||||
{
|
{
|
||||||
@@ -1000,7 +934,7 @@ void BulletURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu
|
|||||||
|
|
||||||
case UrdfGeometry::FILE_STL:
|
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;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1018,7 +952,8 @@ void BulletURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu
|
|||||||
visualShapeInstances,
|
visualShapeInstances,
|
||||||
upAxisTrans,
|
upAxisTrans,
|
||||||
unitMeterScaling,
|
unitMeterScaling,
|
||||||
upAxis);
|
upAxis,
|
||||||
|
m_data->m_fileIO);
|
||||||
|
|
||||||
glmesh = new GLInstanceGraphicsShape;
|
glmesh = new GLInstanceGraphicsShape;
|
||||||
// int index = 0;
|
// int index = 0;
|
||||||
@@ -1326,7 +1261,7 @@ void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, int urdfIndex,
|
|||||||
UrdfLink* const* linkPtr = model.m_links.getAtIndex(urdfIndex);
|
UrdfLink* const* linkPtr = model.m_links.getAtIndex(urdfIndex);
|
||||||
if (linkPtr)
|
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,6 +1338,8 @@ class btCompoundShape* BulletURDFImporter::convertLinkCollisionShapes(int linkIn
|
|||||||
{
|
{
|
||||||
const UrdfCollision& col = link->m_collisionArray[v];
|
const UrdfCollision& col = link->m_collisionArray[v];
|
||||||
btCollisionShape* childShape = convertURDFToCollisionShape(&col, pathPrefix);
|
btCollisionShape* childShape = convertURDFToCollisionShape(&col, pathPrefix);
|
||||||
|
if (childShape)
|
||||||
|
{
|
||||||
m_data->m_allocatedCollisionShapes.push_back(childShape);
|
m_data->m_allocatedCollisionShapes.push_back(childShape);
|
||||||
if (childShape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
|
if (childShape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
|
||||||
{
|
{
|
||||||
@@ -1413,8 +1350,6 @@ class btCompoundShape* BulletURDFImporter::convertLinkCollisionShapes(int linkIn
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if (childShape)
|
|
||||||
{
|
|
||||||
btTransform childTrans = col.m_linkLocalFrame;
|
btTransform childTrans = col.m_linkLocalFrame;
|
||||||
|
|
||||||
compoundShape->addChildShape(localInertiaFrame.inverse() * childTrans, childShape);
|
compoundShape->addChildShape(localInertiaFrame.inverse() * childTrans, childShape);
|
||||||
|
|||||||
@@ -19,7 +19,7 @@ class BulletURDFImporter : public URDFImporterInterface
|
|||||||
struct BulletURDFInternalData* m_data;
|
struct BulletURDFInternalData* m_data;
|
||||||
|
|
||||||
public:
|
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();
|
virtual ~BulletURDFImporter();
|
||||||
|
|
||||||
|
|||||||
@@ -171,7 +171,8 @@ void ImportUrdfSetup::initPhysics()
|
|||||||
|
|
||||||
int flags = 0;
|
int flags = 0;
|
||||||
double globalScaling = 1;
|
double globalScaling = 1;
|
||||||
BulletURDFImporter u2b(m_guiHelper, 0, globalScaling, flags);
|
|
||||||
|
BulletURDFImporter u2b(m_guiHelper, 0, 0, globalScaling, flags);
|
||||||
|
|
||||||
bool loadOk = u2b.loadURDF(m_fileName);
|
bool loadOk = u2b.loadURDF(m_fileName);
|
||||||
|
|
||||||
|
|||||||
116
examples/Importers/ImportURDFDemo/UrdfFindMeshFile.h
Normal file
116
examples/Importers/ImportURDFDemo/UrdfFindMeshFile.h
Normal file
@@ -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 <string>
|
||||||
|
#include <list>
|
||||||
|
|
||||||
|
#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<std::string> 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<std::string>::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
|
||||||
@@ -2,12 +2,15 @@
|
|||||||
#include "../../ThirdPartyLibs/tinyxml2/tinyxml2.h"
|
#include "../../ThirdPartyLibs/tinyxml2/tinyxml2.h"
|
||||||
#include "urdfStringSplit.h"
|
#include "urdfStringSplit.h"
|
||||||
#include "urdfLexicalCast.h"
|
#include "urdfLexicalCast.h"
|
||||||
|
#include "UrdfFindMeshFile.h"
|
||||||
|
|
||||||
using namespace tinyxml2;
|
using namespace tinyxml2;
|
||||||
|
|
||||||
UrdfParser::UrdfParser()
|
UrdfParser::UrdfParser(CommonFileIOInterface* fileIO)
|
||||||
: m_parseSDF(false),
|
: m_parseSDF(false),
|
||||||
m_activeSdfModel(-1),
|
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
|
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;
|
geom.m_meshFileName = fn;
|
||||||
bool success = findExistingMeshFile(
|
bool success = UrdfFindMeshFile(m_fileIO,
|
||||||
m_urdf2Model.m_sourceFile, fn, sourceFileLocation(shape),
|
m_urdf2Model.m_sourceFile, fn, sourceFileLocation(shape),
|
||||||
&geom.m_meshFileName, &geom.m_meshFileType);
|
&geom.m_meshFileName, &geom.m_meshFileType);
|
||||||
if (!success)
|
if (!success)
|
||||||
|
|||||||
@@ -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
|
struct UrdfShape
|
||||||
{
|
{
|
||||||
@@ -256,6 +253,9 @@ protected:
|
|||||||
int m_activeSdfModel;
|
int m_activeSdfModel;
|
||||||
|
|
||||||
btScalar m_urdfScaling;
|
btScalar m_urdfScaling;
|
||||||
|
|
||||||
|
struct CommonFileIOInterface* m_fileIO;
|
||||||
|
|
||||||
bool parseTransform(btTransform& tr, tinyxml2::XMLElement* xml, ErrorLogger* logger, bool parseSDF = false);
|
bool parseTransform(btTransform& tr, tinyxml2::XMLElement* xml, ErrorLogger* logger, bool parseSDF = false);
|
||||||
bool parseInertia(UrdfInertia& inertia, tinyxml2::XMLElement* config, ErrorLogger* logger);
|
bool parseInertia(UrdfInertia& inertia, tinyxml2::XMLElement* config, ErrorLogger* logger);
|
||||||
bool parseGeometry(UrdfGeometry& geom, tinyxml2::XMLElement* g, 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);
|
bool parseSensor(UrdfModel& model, UrdfLink& link, UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger);
|
||||||
|
|
||||||
public:
|
public:
|
||||||
UrdfParser();
|
UrdfParser(struct CommonFileIOInterface* fileIO);
|
||||||
virtual ~UrdfParser();
|
virtual ~UrdfParser();
|
||||||
|
|
||||||
void setParseSDF(bool useSDF)
|
void setParseSDF(bool useSDF)
|
||||||
|
|||||||
@@ -15,7 +15,7 @@ struct UrdfRenderingInterface
|
|||||||
virtual ~UrdfRenderingInterface() {}
|
virtual ~UrdfRenderingInterface() {}
|
||||||
///given a URDF link, convert all visual shapes into internal renderer (loading graphics meshes, textures etc)
|
///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.
|
///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)
|
///remove a visual shapes, based on the shape unique id (shapeUid)
|
||||||
virtual void removeVisualShape(int collisionObjectUid) = 0;
|
virtual void removeVisualShape(int collisionObjectUid) = 0;
|
||||||
|
|||||||
@@ -152,7 +152,7 @@ void InverseDynamicsExample::initPhysics()
|
|||||||
{
|
{
|
||||||
case BT_ID_LOAD_URDF:
|
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");
|
bool loadOk = u2b.loadURDF("kuka_iiwa/model.urdf"); // lwr / kuka.urdf");
|
||||||
if (loadOk)
|
if (loadOk)
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -346,7 +346,7 @@ void TestJointTorqueSetup::initPhysics()
|
|||||||
btSerializer* s = new btDefaultSerializer;
|
btSerializer* s = new btDefaultSerializer;
|
||||||
m_dynamicsWorld->serialize(s);
|
m_dynamicsWorld->serialize(s);
|
||||||
char resourcePath[1024];
|
char resourcePath[1024];
|
||||||
if (b3ResourcePath::findResourcePath("multibody.bullet", resourcePath, 1024))
|
if (b3ResourcePath::findResourcePath("multibody.bullet", resourcePath, 1024,0))
|
||||||
{
|
{
|
||||||
FILE* f = fopen(resourcePath, "wb");
|
FILE* f = fopen(resourcePath, "wb");
|
||||||
fwrite(s->getBufferPointer(), s->getCurrentBufferSize(), 1, f);
|
fwrite(s->getBufferPointer(), s->getCurrentBufferSize(), 1, f);
|
||||||
|
|||||||
@@ -16,7 +16,7 @@
|
|||||||
#include "../Importers/ImportMeshUtility/b3ImportMeshUtility.h"
|
#include "../Importers/ImportMeshUtility/b3ImportMeshUtility.h"
|
||||||
#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
|
#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
|
||||||
#include "../CommonInterfaces/CommonParameterInterface.h"
|
#include "../CommonInterfaces/CommonParameterInterface.h"
|
||||||
|
#include "../Utils/b3BulletDefaultFileIO.h"
|
||||||
struct TinyRendererSetupInternalData
|
struct TinyRendererSetupInternalData
|
||||||
{
|
{
|
||||||
TGAImage m_rgbColorBuffer;
|
TGAImage m_rgbColorBuffer;
|
||||||
@@ -149,7 +149,8 @@ TinyRendererSetup::TinyRendererSetup(struct GUIHelperInterface* gui)
|
|||||||
int shapeId = -1;
|
int shapeId = -1;
|
||||||
|
|
||||||
b3ImportMeshData meshData;
|
b3ImportMeshData meshData;
|
||||||
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fileName, meshData))
|
b3BulletDefaultFileIO fileIO;
|
||||||
|
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fileName, meshData,&fileIO))
|
||||||
{
|
{
|
||||||
int textureIndex = -1;
|
int textureIndex = -1;
|
||||||
|
|
||||||
|
|||||||
@@ -7,6 +7,7 @@
|
|||||||
#include "../RenderingExamples/TimeSeriesFontData.h"
|
#include "../RenderingExamples/TimeSeriesFontData.h"
|
||||||
#include "../Importers/ImportMeshUtility/b3ImportMeshUtility.h"
|
#include "../Importers/ImportMeshUtility/b3ImportMeshUtility.h"
|
||||||
#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
|
#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
|
||||||
|
#include "../Utils/b3BulletDefaultFileIO.h"
|
||||||
|
|
||||||
#include "../CommonInterfaces/CommonRenderInterface.h"
|
#include "../CommonInterfaces/CommonRenderInterface.h"
|
||||||
#include "../CommonInterfaces/CommonParameterInterface.h"
|
#include "../CommonInterfaces/CommonParameterInterface.h"
|
||||||
@@ -129,7 +130,8 @@ bool TinyVRGui::init()
|
|||||||
int shapeId = -1;
|
int shapeId = -1;
|
||||||
|
|
||||||
b3ImportMeshData meshData;
|
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],
|
shapeId = m_data->m_renderer->registerShape(&meshData.m_gfxShape->m_vertices->at(0).xyzw[0],
|
||||||
meshData.m_gfxShape->m_numvertices,
|
meshData.m_gfxShape->m_numvertices,
|
||||||
|
|||||||
@@ -218,7 +218,7 @@ void RollingFrictionDemo::initPhysics()
|
|||||||
btSerializer* s = new btDefaultSerializer;
|
btSerializer* s = new btDefaultSerializer;
|
||||||
m_dynamicsWorld->serialize(s);
|
m_dynamicsWorld->serialize(s);
|
||||||
char resourcePath[1024];
|
char resourcePath[1024];
|
||||||
if (b3ResourcePath::findResourcePath("slope.bullet", resourcePath, 1024))
|
if (b3ResourcePath::findResourcePath("slope.bullet", resourcePath, 1024,0))
|
||||||
{
|
{
|
||||||
FILE* f = fopen(resourcePath, "wb");
|
FILE* f = fopen(resourcePath, "wb");
|
||||||
fwrite(s->getBufferPointer(), s->getCurrentBufferSize(), 1, f);
|
fwrite(s->getBufferPointer(), s->getCurrentBufferSize(), 1, f);
|
||||||
|
|||||||
@@ -4,6 +4,8 @@
|
|||||||
#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h"
|
#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h"
|
||||||
#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
|
#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
|
||||||
#include "../Importers/ImportURDFDemo/URDF2Bullet.h"
|
#include "../Importers/ImportURDFDemo/URDF2Bullet.h"
|
||||||
|
#include "../Importers/ImportURDFDemo/UrdfFindMeshFile.h"
|
||||||
|
|
||||||
#include "../Extras/InverseDynamics/btMultiBodyTreeCreator.hpp"
|
#include "../Extras/InverseDynamics/btMultiBodyTreeCreator.hpp"
|
||||||
|
|
||||||
#include "BulletCollision/CollisionDispatch/btInternalEdgeUtility.h"
|
#include "BulletCollision/CollisionDispatch/btInternalEdgeUtility.h"
|
||||||
@@ -54,6 +56,7 @@
|
|||||||
#include "plugins/collisionFilterPlugin/collisionFilterPlugin.h"
|
#include "plugins/collisionFilterPlugin/collisionFilterPlugin.h"
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
||||||
#ifdef ENABLE_STATIC_GRPC_PLUGIN
|
#ifdef ENABLE_STATIC_GRPC_PLUGIN
|
||||||
#include "plugins/grpcPlugin/grpcPlugin.h"
|
#include "plugins/grpcPlugin/grpcPlugin.h"
|
||||||
#endif //ENABLE_STATIC_GRPC_PLUGIN
|
#endif //ENABLE_STATIC_GRPC_PLUGIN
|
||||||
@@ -74,6 +77,10 @@
|
|||||||
#include "plugins/tinyRendererPlugin/tinyRendererPlugin.h"
|
#include "plugins/tinyRendererPlugin/tinyRendererPlugin.h"
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#ifndef B3_ENABLE_FILEIO_PLUGIN
|
||||||
|
#include "plugins/fileIOPlugin/fileIOPlugin.h"
|
||||||
|
#endif//B3_DISABLE_FILEIO_PLUGIN
|
||||||
|
|
||||||
#ifdef B3_ENABLE_TINY_AUDIO
|
#ifdef B3_ENABLE_TINY_AUDIO
|
||||||
#include "../TinyAudio/b3SoundEngine.h"
|
#include "../TinyAudio/b3SoundEngine.h"
|
||||||
#endif
|
#endif
|
||||||
@@ -104,6 +111,11 @@ btQuaternion gVRTeleportOrn(0, 0, 0, 1);
|
|||||||
btScalar simTimeScalingFactor = 1;
|
btScalar simTimeScalingFactor = 1;
|
||||||
btScalar gRhsClamp = 1.f;
|
btScalar gRhsClamp = 1.f;
|
||||||
|
|
||||||
|
#include "../CommonInterfaces/CommonFileIOInterface.h"
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
struct UrdfLinkNameMapUtil
|
struct UrdfLinkNameMapUtil
|
||||||
{
|
{
|
||||||
btMultiBody* m_mb;
|
btMultiBody* m_mb;
|
||||||
@@ -1639,6 +1651,7 @@ struct PhysicsServerCommandProcessorInternalData
|
|||||||
std::string m_profileTimingFileName;
|
std::string m_profileTimingFileName;
|
||||||
|
|
||||||
struct GUIHelperInterface* m_guiHelper;
|
struct GUIHelperInterface* m_guiHelper;
|
||||||
|
|
||||||
int m_sharedMemoryKey;
|
int m_sharedMemoryKey;
|
||||||
bool m_enableTinyRenderer;
|
bool m_enableTinyRenderer;
|
||||||
|
|
||||||
@@ -1704,43 +1717,66 @@ struct PhysicsServerCommandProcessorInternalData
|
|||||||
{
|
{
|
||||||
//register static plugins:
|
//register static plugins:
|
||||||
#ifdef STATIC_LINK_VR_PLUGIN
|
#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
|
#endif //STATIC_LINK_VR_PLUGIN
|
||||||
|
}
|
||||||
#ifndef SKIP_STATIC_PD_CONTROL_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
|
#endif //SKIP_STATIC_PD_CONTROL_PLUGIN
|
||||||
|
|
||||||
#ifndef SKIP_COLLISION_FILTER_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);
|
m_pluginManager.selectCollisionPlugin(m_collisionFilterPlugin);
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef ENABLE_STATIC_GRPC_PLUGIN
|
#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
|
#endif //ENABLE_STATIC_GRPC_PLUGIN
|
||||||
|
|
||||||
#ifdef STATIC_EGLRENDERER_PLUGIN
|
#ifdef STATIC_EGLRENDERER_PLUGIN
|
||||||
{
|
{
|
||||||
bool initPlugin = false;
|
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);
|
m_pluginManager.selectPluginRenderer(renderPluginId);
|
||||||
}
|
}
|
||||||
#endif //STATIC_EGLRENDERER_PLUGIN
|
#endif //STATIC_EGLRENDERER_PLUGIN
|
||||||
|
|
||||||
#ifndef SKIP_STATIC_TINYRENDERER_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);
|
m_pluginManager.selectPluginRenderer(renderPluginId);
|
||||||
}
|
}
|
||||||
#endif
|
#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();
|
m_vrControllerEvents.init();
|
||||||
|
|
||||||
@@ -2212,7 +2248,9 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
|
|||||||
int graphicsIndex = -1;
|
int graphicsIndex = -1;
|
||||||
double globalScaling = 1.f; //todo!
|
double globalScaling = 1.f; //todo!
|
||||||
int flags = 0;
|
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);
|
u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer);
|
||||||
|
|
||||||
btAlignedObjectArray<GLInstanceVertex> vertices;
|
btAlignedObjectArray<GLInstanceVertex> vertices;
|
||||||
@@ -2300,7 +2338,8 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
|
|||||||
//UrdfLink*const* linkPtr = model.m_links.getAtIndex(urdfIndex);
|
//UrdfLink*const* linkPtr = model.m_links.getAtIndex(urdfIndex);
|
||||||
if (m_data->m_pluginManager.getRenderInterface())
|
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)
|
virtual void setBodyUniqueId(int bodyId)
|
||||||
@@ -3015,7 +3054,8 @@ bool PhysicsServerCommandProcessor::loadMjcf(const char* fileName, char* bufferS
|
|||||||
|
|
||||||
m_data->m_sdfRecentLoadedBodies.clear();
|
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;
|
bool useFixedBase = false;
|
||||||
MyMJCFLogger2 logger;
|
MyMJCFLogger2 logger;
|
||||||
@@ -3037,8 +3077,8 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
|
|||||||
}
|
}
|
||||||
|
|
||||||
m_data->m_sdfRecentLoadedBodies.clear();
|
m_data->m_sdfRecentLoadedBodies.clear();
|
||||||
|
CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface();
|
||||||
BulletURDFImporter u2b(m_data->m_guiHelper, m_data->m_pluginManager.getRenderInterface(), globalScaling, flags);
|
BulletURDFImporter u2b(m_data->m_guiHelper, m_data->m_pluginManager.getRenderInterface(), fileIO, globalScaling, flags);
|
||||||
u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer);
|
u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer);
|
||||||
|
|
||||||
bool forceFixedBase = false;
|
bool forceFixedBase = false;
|
||||||
@@ -3067,7 +3107,8 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
|
|||||||
return false;
|
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);
|
u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer);
|
||||||
bool loadOk = u2b.loadURDF(fileName, useFixedBase);
|
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_meshFileName = fileName;
|
||||||
|
|
||||||
urdfColObj.m_geometry.m_meshScale = meshScale;
|
urdfColObj.m_geometry.m_meshScale = meshScale;
|
||||||
|
CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface();
|
||||||
pathPrefix[0] = 0;
|
pathPrefix[0] = 0;
|
||||||
if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024))
|
if (fileIO->findResourcePath(fileName.c_str(), relativeFileName, 1024))
|
||||||
{
|
{
|
||||||
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
|
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
|
||||||
}
|
}
|
||||||
@@ -4112,14 +4153,16 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
|
|||||||
std::string out_found_filename;
|
std::string out_found_filename;
|
||||||
int out_type;
|
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)
|
if (foundFile)
|
||||||
{
|
{
|
||||||
urdfColObj.m_geometry.m_meshFileType = out_type;
|
urdfColObj.m_geometry.m_meshFileType = out_type;
|
||||||
|
|
||||||
if (out_type == UrdfGeometry::FILE_STL)
|
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)
|
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)
|
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
|
else
|
||||||
{
|
{
|
||||||
std::vector<tinyobj::shape_t> shapes;
|
std::vector<tinyobj::shape_t> 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);
|
//shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale);
|
||||||
//static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale)
|
//static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale)
|
||||||
@@ -4309,7 +4353,8 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct
|
|||||||
serverStatusOut.m_type = CMD_CREATE_VISUAL_SHAPE_FAILED;
|
serverStatusOut.m_type = CMD_CREATE_VISUAL_SHAPE_FAILED;
|
||||||
double globalScaling = 1.f;
|
double globalScaling = 1.f;
|
||||||
int flags = 0;
|
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);
|
u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer);
|
||||||
btTransform localInertiaFrame;
|
btTransform localInertiaFrame;
|
||||||
localInertiaFrame.setIdentity();
|
localInertiaFrame.setIdentity();
|
||||||
@@ -4374,12 +4419,12 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct
|
|||||||
const std::string& error_message_prefix = "";
|
const std::string& error_message_prefix = "";
|
||||||
std::string out_found_filename;
|
std::string out_found_filename;
|
||||||
int out_type;
|
int out_type;
|
||||||
if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024))
|
if (fileIO->findResourcePath(fileName.c_str(), relativeFileName, 1024))
|
||||||
{
|
{
|
||||||
b3FileUtils::extractPath(relativeFileName, pathPrefix, 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_meshFileType = out_type;
|
||||||
visualShape.m_geometry.m_meshFileName = fileName;
|
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 relativeFileName[1024];
|
||||||
char pathPrefix[1024];
|
char pathPrefix[1024];
|
||||||
pathPrefix[0] = 0;
|
pathPrefix[0] = 0;
|
||||||
if (b3ResourcePath::findResourcePath(loadSoftBodyArgs.m_fileName, relativeFileName, 1024))
|
if (fileIO->findResourcePath(loadSoftBodyArgs.m_fileName, relativeFileName, 1024))
|
||||||
{
|
{
|
||||||
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
|
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
|
||||||
}
|
}
|
||||||
const std::string& error_message_prefix = "";
|
const std::string& error_message_prefix = "";
|
||||||
std::string out_found_filename;
|
std::string out_found_filename;
|
||||||
int out_type;
|
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<tinyobj::shape_t> shapes;
|
std::vector<tinyobj::shape_t> 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)
|
if (shapes.size() > 0)
|
||||||
{
|
{
|
||||||
const tinyobj::shape_t& shape = shapes[0];
|
const tinyobj::shape_t& shape = shapes[0];
|
||||||
@@ -9802,7 +9849,8 @@ bool PhysicsServerCommandProcessor::processLoadTextureCommand(const struct Share
|
|||||||
char relativeFileName[1024];
|
char relativeFileName[1024];
|
||||||
char pathPrefix[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);
|
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
|
||||||
|
|
||||||
|
|||||||
@@ -1519,6 +1519,8 @@ public:
|
|||||||
btVector3 VRTeleportPos = this->m_physicsServer.getVRTeleportPosition();
|
btVector3 VRTeleportPos = this->m_physicsServer.getVRTeleportPosition();
|
||||||
|
|
||||||
if (gEnableDefaultKeyboardShortcuts)
|
if (gEnableDefaultKeyboardShortcuts)
|
||||||
|
{
|
||||||
|
if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera())
|
||||||
{
|
{
|
||||||
if (key == 'w' && state)
|
if (key == 'w' && state)
|
||||||
{
|
{
|
||||||
@@ -1564,6 +1566,7 @@ public:
|
|||||||
saveCurrentSettingsVR(VRTeleportPos);
|
saveCurrentSettingsVR(VRTeleportPos);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -5,7 +5,7 @@
|
|||||||
#include "PhysicsClientC_API.h"
|
#include "PhysicsClientC_API.h"
|
||||||
#include "PhysicsDirect.h"
|
#include "PhysicsDirect.h"
|
||||||
#include "plugins/b3PluginContext.h"
|
#include "plugins/b3PluginContext.h"
|
||||||
|
#include "../Utils/b3BulletDefaultFileIO.h"
|
||||||
#ifdef _WIN32
|
#ifdef _WIN32
|
||||||
#define WIN32_LEAN_AND_MEAN
|
#define WIN32_LEAN_AND_MEAN
|
||||||
#define VC_EXTRALEAN
|
#define VC_EXTRALEAN
|
||||||
@@ -48,6 +48,7 @@ struct b3Plugin
|
|||||||
|
|
||||||
PFN_GET_RENDER_INTERFACE m_getRendererFunc;
|
PFN_GET_RENDER_INTERFACE m_getRendererFunc;
|
||||||
PFN_GET_COLLISION_INTERFACE m_getCollisionFunc;
|
PFN_GET_COLLISION_INTERFACE m_getCollisionFunc;
|
||||||
|
PFN_GET_FILEIO_INTERFACE m_getFileIOFunc;
|
||||||
|
|
||||||
void* m_userPointer;
|
void* m_userPointer;
|
||||||
|
|
||||||
@@ -65,6 +66,7 @@ struct b3Plugin
|
|||||||
m_processClientCommandsFunc(0),
|
m_processClientCommandsFunc(0),
|
||||||
m_getRendererFunc(0),
|
m_getRendererFunc(0),
|
||||||
m_getCollisionFunc(0),
|
m_getCollisionFunc(0),
|
||||||
|
m_getFileIOFunc(0),
|
||||||
m_userPointer(0)
|
m_userPointer(0)
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
@@ -84,6 +86,7 @@ struct b3Plugin
|
|||||||
m_processClientCommandsFunc = 0;
|
m_processClientCommandsFunc = 0;
|
||||||
m_getRendererFunc = 0;
|
m_getRendererFunc = 0;
|
||||||
m_getCollisionFunc = 0;
|
m_getCollisionFunc = 0;
|
||||||
|
m_getFileIOFunc = 0;
|
||||||
m_userPointer = 0;
|
m_userPointer = 0;
|
||||||
m_isInitialized = false;
|
m_isInitialized = false;
|
||||||
}
|
}
|
||||||
@@ -105,9 +108,11 @@ struct b3PluginManagerInternalData
|
|||||||
int m_activeRendererPluginUid;
|
int m_activeRendererPluginUid;
|
||||||
int m_activeCollisionPluginUid;
|
int m_activeCollisionPluginUid;
|
||||||
int m_numNotificationPlugins;
|
int m_numNotificationPlugins;
|
||||||
|
int m_activeFileIOPluginUid;
|
||||||
|
b3BulletDefaultFileIO m_defaultFileIO;
|
||||||
|
|
||||||
b3PluginManagerInternalData()
|
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,6 +213,7 @@ int b3PluginManager::loadPlugin(const char* pluginPath, const char* postFixStr)
|
|||||||
std::string processClientCommandsStr = std::string("processClientCommands") + postFix;
|
std::string processClientCommandsStr = std::string("processClientCommands") + postFix;
|
||||||
std::string getRendererStr = std::string("getRenderInterface") + postFix;
|
std::string getRendererStr = std::string("getRenderInterface") + postFix;
|
||||||
std::string getCollisionStr = std::string("getCollisionInterface") + 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_initFunc = (PFN_INIT)B3_DYNLIB_IMPORT(pluginHandle, initStr.c_str());
|
||||||
plugin->m_exitFunc = (PFN_EXIT)B3_DYNLIB_IMPORT(pluginHandle, exitStr.c_str());
|
plugin->m_exitFunc = (PFN_EXIT)B3_DYNLIB_IMPORT(pluginHandle, exitStr.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_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_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)
|
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);
|
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;
|
return pluginUniqueId;
|
||||||
}
|
}
|
||||||
@@ -438,7 +455,9 @@ int b3PluginManager::executePluginCommand(int pluginUniqueId, const b3PluginArgu
|
|||||||
return result;
|
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;
|
b3Plugin orgPlugin;
|
||||||
|
|
||||||
@@ -447,14 +466,15 @@ int b3PluginManager::registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT
|
|||||||
pluginHandle->m_pluginHandle = 0;
|
pluginHandle->m_pluginHandle = 0;
|
||||||
pluginHandle->m_ownsPluginHandle = false;
|
pluginHandle->m_ownsPluginHandle = false;
|
||||||
pluginHandle->m_pluginUniqueId = pluginUniqueId;
|
pluginHandle->m_pluginUniqueId = pluginUniqueId;
|
||||||
pluginHandle->m_executeCommandFunc = executeCommandFunc;
|
pluginHandle->m_executeCommandFunc = functions.m_executeCommandFunc;
|
||||||
pluginHandle->m_exitFunc = exitFunc;
|
pluginHandle->m_exitFunc = functions.m_exitFunc;
|
||||||
pluginHandle->m_initFunc = initFunc;
|
pluginHandle->m_initFunc = functions.m_initFunc;
|
||||||
pluginHandle->m_preTickFunc = preTickFunc;
|
pluginHandle->m_preTickFunc = functions.m_preTickFunc;
|
||||||
pluginHandle->m_postTickFunc = postTickFunc;
|
pluginHandle->m_postTickFunc = functions.m_postTickFunc;
|
||||||
pluginHandle->m_getRendererFunc = getRendererFunc;
|
pluginHandle->m_getRendererFunc = functions.m_getRendererFunc;
|
||||||
pluginHandle->m_getCollisionFunc = getCollisionFunc;
|
pluginHandle->m_getCollisionFunc = functions.m_getCollisionFunc;
|
||||||
pluginHandle->m_processClientCommandsFunc = processClientCommandsFunc;
|
pluginHandle->m_processClientCommandsFunc = functions.m_processClientCommandsFunc;
|
||||||
|
pluginHandle->m_getFileIOFunc = functions.m_fileIoFunc;
|
||||||
pluginHandle->m_pluginHandle = 0;
|
pluginHandle->m_pluginHandle = 0;
|
||||||
pluginHandle->m_pluginPath = pluginPath;
|
pluginHandle->m_pluginPath = pluginPath;
|
||||||
pluginHandle->m_userPointer = 0;
|
pluginHandle->m_userPointer = 0;
|
||||||
@@ -502,6 +522,32 @@ UrdfRenderingInterface* b3PluginManager::getRenderInterface()
|
|||||||
return renderer;
|
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)
|
void b3PluginManager::selectCollisionPlugin(int pluginUniqueId)
|
||||||
{
|
{
|
||||||
m_data->m_activeCollisionPluginUid = pluginUniqueId;
|
m_data->m_activeCollisionPluginUid = pluginUniqueId;
|
||||||
|
|||||||
@@ -10,6 +10,37 @@ enum b3PluginManagerTickMode
|
|||||||
B3_PROCESS_CLIENT_COMMANDS_TICK,
|
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
|
class b3PluginManager
|
||||||
{
|
{
|
||||||
struct b3PluginManagerInternalData* m_data;
|
struct b3PluginManagerInternalData* m_data;
|
||||||
@@ -29,11 +60,14 @@ public:
|
|||||||
|
|
||||||
void tickPlugins(double timeStep, b3PluginManagerTickMode tickMode);
|
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);
|
void selectPluginRenderer(int pluginUniqueId);
|
||||||
struct UrdfRenderingInterface* getRenderInterface();
|
struct UrdfRenderingInterface* getRenderInterface();
|
||||||
|
|
||||||
|
void selectFileIOPlugin(int pluginUniqueId);
|
||||||
|
struct CommonFileIOInterface* getFileIOInterface();
|
||||||
|
|
||||||
void selectCollisionPlugin(int pluginUniqueId);
|
void selectCollisionPlugin(int pluginUniqueId);
|
||||||
struct b3PluginCollisionInterface* getCollisionInterface();
|
struct b3PluginCollisionInterface* getCollisionInterface();
|
||||||
};
|
};
|
||||||
|
|||||||
@@ -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 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 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
|
#ifdef __cplusplus
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
1
examples/SharedMemory/plugins/b3PluginFileIOInterface.h
Normal file
1
examples/SharedMemory/plugins/b3PluginFileIOInterface.h
Normal file
@@ -0,0 +1 @@
|
|||||||
|
#error
|
||||||
333
examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp
Normal file
333
examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp
Normal file
@@ -0,0 +1,333 @@
|
|||||||
|
|
||||||
|
|
||||||
|
#include "fileIOPlugin.h"
|
||||||
|
#include "../../SharedMemoryPublic.h"
|
||||||
|
#include "../b3PluginContext.h"
|
||||||
|
#include <stdio.h>
|
||||||
|
#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;i<FILEIO_MAX_FILES;i++)
|
||||||
|
{
|
||||||
|
m_fileHandles[i]=0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
static bool FileIOPluginFindFile(void* userPtr, const char* orgFileName, char* relativeFileName, int maxRelativeFileNameMaxLen)
|
||||||
|
{
|
||||||
|
MyFileIO* fileIo = (MyFileIO*) userPtr;
|
||||||
|
return fileIo->findFile(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<FILEIO_MAX_FILES;i++)
|
||||||
|
{
|
||||||
|
if (m_fileHandles[i]==0)
|
||||||
|
{
|
||||||
|
slot=i;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (slot>=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 (numRead<numBytes && numRead>0)
|
||||||
|
{
|
||||||
|
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;
|
||||||
|
}
|
||||||
24
examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.h
Normal file
24
examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.h
Normal file
@@ -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
|
||||||
@@ -182,7 +182,7 @@ void TinyRendererVisualShapeConverter::setLightSpecularCoeff(float specularCoeff
|
|||||||
m_data->m_hasLightSpecularCoeff = true;
|
m_data->m_hasLightSpecularCoeff = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture2>& texturesOut, b3VisualShapeData& visualShapeOut)
|
static void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture2>& texturesOut, b3VisualShapeData& visualShapeOut, struct CommonFileIOInterface* fileIO)
|
||||||
{
|
{
|
||||||
visualShapeOut.m_visualGeometryType = visual->m_geometry.m_type;
|
visualShapeOut.m_visualGeometryType = visual->m_geometry.m_type;
|
||||||
visualShapeOut.m_dimensions[0] = 0;
|
visualShapeOut.m_dimensions[0] = 0;
|
||||||
@@ -322,7 +322,7 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi
|
|||||||
{
|
{
|
||||||
//glmesh = LoadMeshFromObj(fullPath,visualPathPrefix);
|
//glmesh = LoadMeshFromObj(fullPath,visualPathPrefix);
|
||||||
b3ImportMeshData meshData;
|
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)
|
if (meshData.m_textureImage1)
|
||||||
{
|
{
|
||||||
@@ -338,7 +338,7 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi
|
|||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
case UrdfGeometry::FILE_STL:
|
case UrdfGeometry::FILE_STL:
|
||||||
glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str());
|
glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str(), fileIO);
|
||||||
break;
|
break;
|
||||||
case UrdfGeometry::FILE_COLLADA:
|
case UrdfGeometry::FILE_COLLADA:
|
||||||
{
|
{
|
||||||
@@ -354,7 +354,8 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi
|
|||||||
visualShapeInstances,
|
visualShapeInstances,
|
||||||
upAxisTrans,
|
upAxisTrans,
|
||||||
unitMeterScaling,
|
unitMeterScaling,
|
||||||
upAxis);
|
upAxis,
|
||||||
|
fileIO);
|
||||||
|
|
||||||
glmesh = new GLInstanceGraphicsShape;
|
glmesh = new GLInstanceGraphicsShape;
|
||||||
// int index = 0;
|
// int index = 0;
|
||||||
@@ -541,8 +542,9 @@ static btVector4 sColors[4] =
|
|||||||
|
|
||||||
void TinyRendererVisualShapeConverter::convertVisualShapes(
|
void TinyRendererVisualShapeConverter::convertVisualShapes(
|
||||||
int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame,
|
int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame,
|
||||||
const UrdfLink* linkPtr, const UrdfModel* model,
|
const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUniqueId,
|
||||||
int collisionObjectUniqueId, int bodyUniqueId)
|
int bodyUniqueId, struct CommonFileIOInterface* fileIO)
|
||||||
|
|
||||||
{
|
{
|
||||||
btAssert(linkPtr); // TODO: remove if (not doing it now, because diff will be 50+ lines)
|
btAssert(linkPtr); // TODO: remove if (not doing it now, because diff will be 50+ lines)
|
||||||
if (linkPtr)
|
if (linkPtr)
|
||||||
@@ -663,7 +665,7 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(
|
|||||||
|
|
||||||
{
|
{
|
||||||
B3_PROFILE("convertURDFToVisualShape");
|
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())
|
if (vertices.size() && indices.size())
|
||||||
|
|||||||
@@ -11,7 +11,7 @@ struct TinyRendererVisualShapeConverter : public UrdfRenderingInterface
|
|||||||
|
|
||||||
virtual ~TinyRendererVisualShapeConverter();
|
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);
|
virtual int getNumVisualShapes(int bodyUniqueId);
|
||||||
|
|
||||||
|
|||||||
@@ -42,7 +42,6 @@ B3_SHARED_API int initPlugin_vrSyncPlugin(struct b3PluginContext* context)
|
|||||||
MyClass* obj = new MyClass();
|
MyClass* obj = new MyClass();
|
||||||
context->m_userPointer = obj;
|
context->m_userPointer = obj;
|
||||||
|
|
||||||
printf("hi vrSyncPlugin!\n");
|
|
||||||
return SHARED_MEMORY_MAGIC_NUMBER;
|
return SHARED_MEMORY_MAGIC_NUMBER;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -192,6 +191,4 @@ B3_SHARED_API void exitPlugin_vrSyncPlugin(struct b3PluginContext* context)
|
|||||||
MyClass* obj = (MyClass*)context->m_userPointer;
|
MyClass* obj = (MyClass*)context->m_userPointer;
|
||||||
delete obj;
|
delete obj;
|
||||||
context->m_userPointer = 0;
|
context->m_userPointer = 0;
|
||||||
|
|
||||||
printf("bye vrSyncPlugin!\n");
|
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -22,13 +22,17 @@
|
|||||||
#include <string>
|
#include <string>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
#include <map>
|
#include <map>
|
||||||
|
#ifdef USE_STREAM
|
||||||
#include <fstream>
|
#include <fstream>
|
||||||
|
#else
|
||||||
|
#include "../../CommonInterfaces/CommonFileIOInterface.h"
|
||||||
|
#endif
|
||||||
#include <sstream>
|
#include <sstream>
|
||||||
|
|
||||||
#include "tiny_obj_loader.h"
|
#include "tiny_obj_loader.h"
|
||||||
|
|
||||||
namespace tinyobj
|
namespace tinyobj
|
||||||
{
|
{
|
||||||
|
#ifdef USE_STREAM
|
||||||
//See http://stackoverflow.com/questions/6089231/getting-std-ifstream-to-handle-lf-cr-and-crlf
|
//See http://stackoverflow.com/questions/6089231/getting-std-ifstream-to-handle-lf-cr-and-crlf
|
||||||
std::istream& safeGetline(std::istream& is, std::string& t)
|
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
|
struct vertex_index
|
||||||
{
|
{
|
||||||
int v_idx, vt_idx, vn_idx, dummy;
|
int v_idx, vt_idx, vn_idx, dummy;
|
||||||
@@ -340,7 +344,8 @@ void InitMaterial(material_t& material)
|
|||||||
std::string LoadMtl(
|
std::string LoadMtl(
|
||||||
std::map<std::string, material_t>& material_map,
|
std::map<std::string, material_t>& material_map,
|
||||||
const char* filename,
|
const char* filename,
|
||||||
const char* mtl_basepath)
|
const char* mtl_basepath,
|
||||||
|
CommonFileIOInterface* fileIO)
|
||||||
{
|
{
|
||||||
material_map.clear();
|
material_map.clear();
|
||||||
std::stringstream err;
|
std::stringstream err;
|
||||||
@@ -355,22 +360,44 @@ std::string LoadMtl(
|
|||||||
{
|
{
|
||||||
filepath = std::string(filename);
|
filepath = std::string(filename);
|
||||||
}
|
}
|
||||||
|
#ifdef USE_STREAM
|
||||||
std::ifstream ifs(filepath.c_str());
|
std::ifstream ifs(filepath.c_str());
|
||||||
if (!ifs)
|
if (!ifs)
|
||||||
{
|
{
|
||||||
err << "Cannot open file [" << filepath << "]" << std::endl;
|
err << "Cannot open file [" << filepath << "]" << std::endl;
|
||||||
return err.str();
|
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;
|
material_t material;
|
||||||
|
|
||||||
int maxchars = 8192; // Alloc enough size.
|
int maxchars = 8192; // Alloc enough size.
|
||||||
std::vector<char> buf(maxchars); // Alloc enough size.
|
std::vector<char> buf(maxchars); // Alloc enough size.
|
||||||
|
#ifdef USE_STREAM
|
||||||
while (ifs.peek() != -1)
|
while (ifs.peek() != -1)
|
||||||
|
#else
|
||||||
|
char* line = 0;
|
||||||
|
do
|
||||||
|
#endif
|
||||||
{
|
{
|
||||||
std::string linebuf;
|
std::string linebuf;
|
||||||
|
#ifdef USE_STREAM
|
||||||
safeGetline(ifs, linebuf);
|
safeGetline(ifs, linebuf);
|
||||||
|
#else
|
||||||
|
char tmpBuf[1024];
|
||||||
|
line = fileIO->readLine(fileHandle, tmpBuf, 1024);
|
||||||
|
if (line)
|
||||||
|
{
|
||||||
|
linebuf=line;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
// Trim newline '\r\n' or '\r'
|
// Trim newline '\r\n' or '\r'
|
||||||
if (linebuf.size() > 0)
|
if (linebuf.size() > 0)
|
||||||
@@ -546,9 +573,16 @@ std::string LoadMtl(
|
|||||||
material.unknown_parameter.insert(std::pair<std::string, std::string>(key, value));
|
material.unknown_parameter.insert(std::pair<std::string, std::string>(key, value));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
#ifndef USE_STREAM
|
||||||
|
while (line);
|
||||||
|
#endif
|
||||||
// flush last material.
|
// flush last material.
|
||||||
material_map.insert(std::pair<std::string, material_t>(material.name, material));
|
material_map.insert(std::pair<std::string, material_t>(material.name, material));
|
||||||
|
|
||||||
|
if (fileHandle)
|
||||||
|
{
|
||||||
|
fileIO->fileClose(fileHandle);
|
||||||
|
}
|
||||||
return err.str();
|
return err.str();
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -556,7 +590,8 @@ std::string
|
|||||||
LoadObj(
|
LoadObj(
|
||||||
std::vector<shape_t>& shapes,
|
std::vector<shape_t>& shapes,
|
||||||
const char* filename,
|
const char* filename,
|
||||||
const char* mtl_basepath)
|
const char* mtl_basepath,
|
||||||
|
CommonFileIOInterface* fileIO)
|
||||||
{
|
{
|
||||||
std::string tmp = filename;
|
std::string tmp = filename;
|
||||||
if (!mtl_basepath)
|
if (!mtl_basepath)
|
||||||
@@ -577,13 +612,21 @@ LoadObj(
|
|||||||
MyIndices face;
|
MyIndices face;
|
||||||
|
|
||||||
std::stringstream err;
|
std::stringstream err;
|
||||||
|
#ifdef USE_STREAM
|
||||||
std::ifstream ifs(filename);
|
std::ifstream ifs(filename);
|
||||||
if (!ifs)
|
if (!ifs)
|
||||||
{
|
{
|
||||||
err << "Cannot open file [" << filename << "]" << std::endl;
|
err << "Cannot open file [" << filename << "]" << std::endl;
|
||||||
return err.str();
|
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<float> v;
|
std::vector<float> v;
|
||||||
v.reserve(1024 * 1024);
|
v.reserve(1024 * 1024);
|
||||||
@@ -606,10 +649,24 @@ LoadObj(
|
|||||||
std::string linebuf;
|
std::string linebuf;
|
||||||
linebuf.reserve(maxchars);
|
linebuf.reserve(maxchars);
|
||||||
|
|
||||||
|
#ifdef USE_STREAM
|
||||||
while (ifs.peek() != -1)
|
while (ifs.peek() != -1)
|
||||||
|
#else
|
||||||
|
char* line = 0;
|
||||||
|
do
|
||||||
|
#endif
|
||||||
{
|
{
|
||||||
linebuf.resize(0);
|
linebuf.resize(0);
|
||||||
|
#ifdef USE_STREAM
|
||||||
safeGetline(ifs, linebuf);
|
safeGetline(ifs, linebuf);
|
||||||
|
#else
|
||||||
|
char tmpBuf[1024];
|
||||||
|
line = fileIO->readLine(fileHandle, tmpBuf, 1024);
|
||||||
|
if (line)
|
||||||
|
{
|
||||||
|
linebuf=line;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
// Trim newline '\r\n' or '\r'
|
// Trim newline '\r\n' or '\r'
|
||||||
if (linebuf.size() > 0)
|
if (linebuf.size() > 0)
|
||||||
@@ -720,7 +777,7 @@ LoadObj(
|
|||||||
token += 7;
|
token += 7;
|
||||||
sscanf(token, "%s", namebuf);
|
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())
|
if (!err_mtl.empty())
|
||||||
{
|
{
|
||||||
//faceGroup.resize(0); // for safety
|
//faceGroup.resize(0); // for safety
|
||||||
@@ -789,6 +846,9 @@ LoadObj(
|
|||||||
|
|
||||||
// Ignore unknown command.
|
// Ignore unknown command.
|
||||||
}
|
}
|
||||||
|
#ifndef USE_STREAM
|
||||||
|
while (line);
|
||||||
|
#endif
|
||||||
|
|
||||||
shape_t shape;
|
shape_t shape;
|
||||||
bool ret = exportFaceGroupToShape(shape, v, vn, vt, faceGroup, material, name, allIndices);
|
bool ret = exportFaceGroupToShape(shape, v, vn, vt, faceGroup, material, name, allIndices);
|
||||||
@@ -798,6 +858,10 @@ LoadObj(
|
|||||||
}
|
}
|
||||||
faceGroup.resize(0); // for safety
|
faceGroup.resize(0); // for safety
|
||||||
|
|
||||||
|
if (fileHandle)
|
||||||
|
{
|
||||||
|
fileIO->fileClose(fileHandle);
|
||||||
|
}
|
||||||
return err.str();
|
return err.str();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -10,6 +10,8 @@
|
|||||||
#include <vector>
|
#include <vector>
|
||||||
#include <map>
|
#include <map>
|
||||||
|
|
||||||
|
struct CommonFileIOInterface;
|
||||||
|
|
||||||
namespace tinyobj
|
namespace tinyobj
|
||||||
{
|
{
|
||||||
typedef struct
|
typedef struct
|
||||||
@@ -51,10 +53,19 @@ typedef struct
|
|||||||
/// The function returns error string.
|
/// The function returns error string.
|
||||||
/// Returns empty string when loading .obj success.
|
/// Returns empty string when loading .obj success.
|
||||||
/// 'mtl_basepath' is optional, and used for base path for .mtl file.
|
/// 'mtl_basepath' is optional, and used for base path for .mtl file.
|
||||||
|
#ifdef USE_STREAM
|
||||||
std::string LoadObj(
|
std::string LoadObj(
|
||||||
std::vector<shape_t>& shapes, // [output]
|
std::vector<shape_t>& shapes, // [output]
|
||||||
const char* filename,
|
const char* filename,
|
||||||
const char* mtl_basepath = NULL);
|
const char* mtl_basepath = NULL);
|
||||||
|
#else
|
||||||
|
std::string
|
||||||
|
LoadObj(
|
||||||
|
std::vector<shape_t>& shapes,
|
||||||
|
const char* filename,
|
||||||
|
const char* mtl_basepath,
|
||||||
|
CommonFileIOInterface* fileIO);
|
||||||
|
#endif
|
||||||
|
|
||||||
}; // namespace tinyobj
|
}; // namespace tinyobj
|
||||||
|
|
||||||
|
|||||||
@@ -13,7 +13,7 @@
|
|||||||
#include "LinearMath/btAlignedObjectArray.h"
|
#include "LinearMath/btAlignedObjectArray.h"
|
||||||
#include "LinearMath/btVector3.h"
|
#include "LinearMath/btVector3.h"
|
||||||
#include "Bullet3Common/b3Logging.h"
|
#include "Bullet3Common/b3Logging.h"
|
||||||
|
#include "../CommonInterfaces/CommonFileIOInterface.h"
|
||||||
struct DepthShader : public IShader
|
struct DepthShader : public IShader
|
||||||
{
|
{
|
||||||
Model* m_model;
|
Model* m_model;
|
||||||
@@ -264,11 +264,11 @@ TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer, b3AlignedOb
|
|||||||
m_lightSpecularCoeff = 0.05;
|
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
|
//todo(erwincoumans) move the file loading out of here
|
||||||
char relativeFileName[1024];
|
char relativeFileName[1024];
|
||||||
if (!b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024))
|
if (!fileIO->findResourcePath(fileName, relativeFileName, 1024))
|
||||||
{
|
{
|
||||||
printf("Cannot find file %s\n", fileName);
|
printf("Cannot find file %s\n", fileName);
|
||||||
}
|
}
|
||||||
@@ -335,7 +335,7 @@ void TinyRenderObjectData::registerMeshShape(const float* vertices, int numVerti
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void TinyRenderObjectData::registerMesh2(btAlignedObjectArray<btVector3>& vertices, btAlignedObjectArray<btVector3>& normals, btAlignedObjectArray<int>& indices)
|
void TinyRenderObjectData::registerMesh2(btAlignedObjectArray<btVector3>& vertices, btAlignedObjectArray<btVector3>& normals, btAlignedObjectArray<int>& indices, CommonFileIOInterface* fileIO)
|
||||||
{
|
{
|
||||||
if (0 == m_model)
|
if (0 == m_model)
|
||||||
{
|
{
|
||||||
@@ -344,7 +344,7 @@ void TinyRenderObjectData::registerMesh2(btAlignedObjectArray<btVector3>& vertic
|
|||||||
|
|
||||||
m_model = new Model();
|
m_model = new Model();
|
||||||
char relativeFileName[1024];
|
char relativeFileName[1024];
|
||||||
if (b3ResourcePath::findResourcePath("floor_diffuse.tga", relativeFileName, 1024))
|
if (fileIO->findResourcePath("floor_diffuse.tga", relativeFileName, 1024))
|
||||||
{
|
{
|
||||||
m_model->loadDiffuseTexture(relativeFileName);
|
m_model->loadDiffuseTexture(relativeFileName);
|
||||||
}
|
}
|
||||||
@@ -368,12 +368,12 @@ void TinyRenderObjectData::registerMesh2(btAlignedObjectArray<btVector3>& 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();
|
m_model = new Model();
|
||||||
|
|
||||||
char relativeFileName[1024];
|
char relativeFileName[1024];
|
||||||
if (b3ResourcePath::findResourcePath("floor_diffuse.tga", relativeFileName, 1024))
|
if (fileIO->findResourcePath("floor_diffuse.tga", relativeFileName, 1024))
|
||||||
{
|
{
|
||||||
m_model->loadDiffuseTexture(relativeFileName);
|
m_model->loadDiffuseTexture(relativeFileName);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -41,12 +41,12 @@ struct TinyRenderObjectData
|
|||||||
TinyRenderObjectData(TGAImage& rgbColorBuffer, b3AlignedObjectArray<float>& depthBuffer, b3AlignedObjectArray<float>* shadowBuffer, b3AlignedObjectArray<int>* segmentationMaskBuffer, int objectIndex, int linkIndex);
|
TinyRenderObjectData(TGAImage& rgbColorBuffer, b3AlignedObjectArray<float>& depthBuffer, b3AlignedObjectArray<float>* shadowBuffer, b3AlignedObjectArray<int>* segmentationMaskBuffer, int objectIndex, int linkIndex);
|
||||||
virtual ~TinyRenderObjectData();
|
virtual ~TinyRenderObjectData();
|
||||||
|
|
||||||
void loadModel(const char* fileName);
|
void loadModel(const char* fileName, struct CommonFileIOInterface* fileIO);
|
||||||
void createCube(float HalfExtentsX, float HalfExtentsY, float HalfExtentsZ);
|
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],
|
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);
|
unsigned char* textureImage = 0, int textureWidth = 0, int textureHeight = 0);
|
||||||
|
|
||||||
void registerMesh2(btAlignedObjectArray<btVector3>& vertices, btAlignedObjectArray<btVector3>& normals, btAlignedObjectArray<int>& indices);
|
void registerMesh2(btAlignedObjectArray<btVector3>& vertices, btAlignedObjectArray<btVector3>& normals, btAlignedObjectArray<int>& indices, struct CommonFileIOInterface* fileIO);
|
||||||
|
|
||||||
void* m_userData;
|
void* m_userData;
|
||||||
int m_userIndex;
|
int m_userIndex;
|
||||||
|
|||||||
175
examples/Utils/b3BulletDefaultFileIO.h
Normal file
175
examples/Utils/b3BulletDefaultFileIO.h
Normal file
@@ -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 <stdio.h>
|
||||||
|
#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<B3_FILEIO_MAX_FILES;i++)
|
||||||
|
{
|
||||||
|
m_fileHandles[i]=0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
virtual ~b3BulletDefaultFileIO()
|
||||||
|
{
|
||||||
|
}
|
||||||
|
virtual int fileOpen(const char* fileName, const char* mode)
|
||||||
|
{
|
||||||
|
//search a free slot
|
||||||
|
int slot = -1;
|
||||||
|
for (int i=0;i<B3_FILEIO_MAX_FILES;i++)
|
||||||
|
{
|
||||||
|
if (m_fileHandles[i]==0)
|
||||||
|
{
|
||||||
|
slot=i;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (slot>=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
|
||||||
@@ -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/<exeName> location, then in various folders within 'data' using b3FileUtils
|
//first find in a resource/<exeName> location, then in various folders within 'data' using b3FileUtils
|
||||||
char exePath[B3_MAX_EXE_PATH_LEN];
|
char exePath[B3_MAX_EXE_PATH_LEN];
|
||||||
|
|
||||||
bool res = b3FileUtils::findFile(resourceName, resourcePathOut, resourcePathMaxNumBytes);
|
bool res = findFile(userPointer, resourceName, resourcePathOut, resourcePathMaxNumBytes);
|
||||||
if (res)
|
if (res)
|
||||||
{
|
{
|
||||||
return strlen(resourcePathOut);
|
return strlen(resourcePathOut);
|
||||||
@@ -104,7 +113,7 @@ int b3ResourcePath::findResourcePath(const char* resourceName, char* resourcePat
|
|||||||
char* resourcePathIn = tmpPath.m_path;
|
char* resourcePathIn = tmpPath.m_path;
|
||||||
sprintf(resourcePathIn, "%s/%s", sAdditionalSearchPath, resourceName);
|
sprintf(resourcePathIn, "%s/%s", sAdditionalSearchPath, resourceName);
|
||||||
//printf("try resource at %s\n", resourcePath);
|
//printf("try resource at %s\n", resourcePath);
|
||||||
if (b3FileUtils::findFile(resourcePathIn, resourcePathOut, resourcePathMaxNumBytes))
|
if (findFile(userPointer, resourcePathIn, resourcePathOut, resourcePathMaxNumBytes))
|
||||||
{
|
{
|
||||||
return strlen(resourcePathOut);
|
return strlen(resourcePathOut);
|
||||||
}
|
}
|
||||||
@@ -122,20 +131,20 @@ int b3ResourcePath::findResourcePath(const char* resourceName, char* resourcePat
|
|||||||
char* resourcePathIn = tmpPath.m_path;
|
char* resourcePathIn = tmpPath.m_path;
|
||||||
sprintf(resourcePathIn, "%s../data/%s", pathToExe, resourceName);
|
sprintf(resourcePathIn, "%s../data/%s", pathToExe, resourceName);
|
||||||
//printf("try resource at %s\n", resourcePath);
|
//printf("try resource at %s\n", resourcePath);
|
||||||
if (b3FileUtils::findFile(resourcePathIn, resourcePathOut, resourcePathMaxNumBytes))
|
if (findFile(userPointer, resourcePathIn, resourcePathOut, resourcePathMaxNumBytes))
|
||||||
{
|
{
|
||||||
return strlen(resourcePathOut);
|
return strlen(resourcePathOut);
|
||||||
}
|
}
|
||||||
|
|
||||||
sprintf(resourcePathIn, "%s../resources/%s/%s", pathToExe, &exePath[exeNamePos], resourceName);
|
sprintf(resourcePathIn, "%s../resources/%s/%s", pathToExe, &exePath[exeNamePos], resourceName);
|
||||||
//printf("try resource at %s\n", resourcePath);
|
//printf("try resource at %s\n", resourcePath);
|
||||||
if (b3FileUtils::findFile(resourcePathIn, resourcePathOut, resourcePathMaxNumBytes))
|
if (findFile(userPointer, resourcePathIn, resourcePathOut, resourcePathMaxNumBytes))
|
||||||
{
|
{
|
||||||
return strlen(resourcePathOut);
|
return strlen(resourcePathOut);
|
||||||
}
|
}
|
||||||
sprintf(resourcePathIn, "%s.runfiles/google3/third_party/bullet/data/%s", exePath, resourceName);
|
sprintf(resourcePathIn, "%s.runfiles/google3/third_party/bullet/data/%s", exePath, resourceName);
|
||||||
//printf("try resource at %s\n", resourcePath);
|
//printf("try resource at %s\n", resourcePath);
|
||||||
if (b3FileUtils::findFile(resourcePathIn, resourcePathOut, resourcePathMaxNumBytes))
|
if (findFile(userPointer, resourcePathIn, resourcePathOut, resourcePathMaxNumBytes))
|
||||||
{
|
{
|
||||||
return strlen(resourcePathOut);
|
return strlen(resourcePathOut);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -3,11 +3,13 @@
|
|||||||
|
|
||||||
#include <string>
|
#include <string>
|
||||||
|
|
||||||
|
typedef bool (* PFN_FIND_FILE)(void* userPointer, const char* orgFileName, char* relativeFileName, int maxRelativeFileNameMaxLen);
|
||||||
|
|
||||||
class b3ResourcePath
|
class b3ResourcePath
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
static int getExePath(char* path, int maxPathLenInBytes);
|
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);
|
static void setAdditionalSearchPath(const char* path);
|
||||||
};
|
};
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
@@ -47,7 +47,7 @@ TEST(InvDynCompare, bulletUrdfR2D2)
|
|||||||
|
|
||||||
char relativeFileName[1024];
|
char relativeFileName[1024];
|
||||||
|
|
||||||
ASSERT_TRUE(b3ResourcePath::findResourcePath(kUrdfFile, relativeFileName, 1024));
|
ASSERT_TRUE(b3ResourcePath::findResourcePath(kUrdfFile, relativeFileName, 1024,0));
|
||||||
|
|
||||||
mb_load.setFileName(relativeFileName);
|
mb_load.setFileName(relativeFileName);
|
||||||
mb_load.init();
|
mb_load.init();
|
||||||
|
|||||||
Reference in New Issue
Block a user