fix various warning

add b3ResourcePath utility, to locate resources relative to executable
This commit is contained in:
Erwin Coumans (Google)
2015-07-03 18:17:14 -07:00
parent 4a9441c5fd
commit 8517e85b21
18 changed files with 176 additions and 92 deletions

View File

@@ -85,8 +85,6 @@ SET(App_ExampleBrowser_SRCS
../Importers/ImportURDFDemo/URDF2Bullet.cpp ../Importers/ImportURDFDemo/URDF2Bullet.cpp
../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp ../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
../Importers/ImportURDFDemo/MyMultiBodyCreator.h ../Importers/ImportURDFDemo/MyMultiBodyCreator.h
../Importers/ImportURDFDemo/ROSURDFImporter.cpp
../Importers/ImportURDFDemo/ROSURDFImporter.h
../Importers/ImportURDFDemo/UrdfParser.cpp ../Importers/ImportURDFDemo/UrdfParser.cpp
../Importers/ImportURDFDemo/urdfStringSplit.cpp ../Importers/ImportURDFDemo/urdfStringSplit.cpp
../Importers/ImportURDFDemo/urdfStringSplit.h ../Importers/ImportURDFDemo/urdfStringSplit.h
@@ -141,6 +139,8 @@ SET(App_ExampleBrowser_SRCS
../ThirdPartyLibs/urdf/boost_replacement/string_split.h ../ThirdPartyLibs/urdf/boost_replacement/string_split.h
../Utils/b3Clock.cpp ../Utils/b3Clock.cpp
../Utils/b3Clock.h ../Utils/b3Clock.h
../Utils/b3ResourcePath.cpp
../Utils/b3ResourcePath.h
${ExampleBrowser_SRCS} ${ExampleBrowser_SRCS}
${ExampleBrowser_HDRS} ${ExampleBrowser_HDRS}
${BULLET_PHYSICS_SOURCE_DIR}/build3/bullet.rc ${BULLET_PHYSICS_SOURCE_DIR}/build3/bullet.rc

View File

@@ -77,6 +77,7 @@
"../ThirdPartyLibs/Wavefront/tiny_obj_loader.*", "../ThirdPartyLibs/Wavefront/tiny_obj_loader.*",
"../ThirdPartyLibs/tinyxml/*", "../ThirdPartyLibs/tinyxml/*",
"../Utils/b3Clock.*", "../Utils/b3Clock.*",
"../Utils/b3ResourcePath.*",
"../GyroscopicDemo/GyroscopicSetup.cpp", "../GyroscopicDemo/GyroscopicSetup.cpp",
"../GyroscopicDemo/GyroscopicSetup.h", "../GyroscopicDemo/GyroscopicSetup.h",
"../ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/pose.cpp", "../ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/pose.cpp",

View File

@@ -24,6 +24,7 @@ subject to the following restrictions:
#include "../OpenGLWindow/SimpleOpenGL3App.h" #include "../OpenGLWindow/SimpleOpenGL3App.h"
#include "LoadMeshFromCollada.h" #include "LoadMeshFromCollada.h"
#include "Bullet3Common/b3FileUtils.h" #include "Bullet3Common/b3FileUtils.h"
#include "../../Utils/b3ResourcePath.h"
#include "../CommonInterfaces/CommonRigidBodyBase.h" #include "../CommonInterfaces/CommonRigidBodyBase.h"
@@ -86,9 +87,9 @@ void ImportColladaSetup::initPhysics()
char relativeFileName[1024]; char relativeFileName[1024];
b3FileUtils f; if (!b3ResourcePath::findResourcePath(fileName,relativeFileName,1024))
if (!f.findFile(fileName,relativeFileName,1024)) return;
return;
btVector3 shift(0,0,0); btVector3 shift(0,0,0);
btVector3 scaling(1,1,1); btVector3 scaling(1,1,1);
@@ -209,4 +210,4 @@ void ImportColladaSetup::initPhysics()
class CommonExampleInterface* ImportColladaCreateFunc(struct CommonExampleOptions& options) class CommonExampleInterface* ImportColladaCreateFunc(struct CommonExampleOptions& options)
{ {
return new ImportColladaSetup(options.m_guiHelper); return new ImportColladaSetup(options.m_guiHelper);
} }

View File

@@ -169,7 +169,7 @@ void readLibraryGeometries(TiXmlDocument& doc, btAlignedObjectArray<GLInstanceGr
const char* sem = input->Attribute("semantic"); const char* sem = input->Attribute("semantic");
std::string semName(sem); std::string semName(sem);
// printf("sem=%s\n",sem); // printf("sem=%s\n",sem);
const char* src = input->Attribute("source"); // const char* src = input->Attribute("source");
// printf("src=%s\n",src); // printf("src=%s\n",src);
const char* srcIdRef = input->Attribute("source"); const char* srcIdRef = input->Attribute("source");
std::string source_name; std::string source_name;
@@ -192,12 +192,12 @@ void readLibraryGeometries(TiXmlDocument& doc, btAlignedObjectArray<GLInstanceGr
std::string normalSourceName; std::string normalSourceName;
int primitiveCount; int primitiveCount;
primitive->QueryIntAttribute("count", &primitiveCount); primitive->QueryIntAttribute("count", &primitiveCount);
bool positionAndNormalInVertex=false;
int indexStride=1; int indexStride=1;
int posOffset = 0; int posOffset = 0;
int normalOffset = 0; int normalOffset = 0;
int numIndices = 0; int numIndices = 0;
{ {
for (TiXmlElement* input = primitive->FirstChildElement("input");input != NULL;input = input->NextSiblingElement("input")) for (TiXmlElement* input = primitive->FirstChildElement("input");input != NULL;input = input->NextSiblingElement("input"))
{ {
const char* sem = input->Attribute("semantic"); const char* sem = input->Attribute("semantic");
@@ -206,7 +206,8 @@ void readLibraryGeometries(TiXmlDocument& doc, btAlignedObjectArray<GLInstanceGr
if ((offset+1)>indexStride) if ((offset+1)>indexStride)
indexStride=offset+1; indexStride=offset+1;
//printf("sem=%s\n",sem); //printf("sem=%s\n",sem);
const char* src = input->Attribute("source"); // const char* src = input->Attribute("source");
//printf("src=%s\n",src); //printf("src=%s\n",src);
const char* srcIdRef = input->Attribute("source"); const char* srcIdRef = input->Attribute("source");
std::string source_name; std::string source_name;
@@ -226,7 +227,6 @@ void readLibraryGeometries(TiXmlDocument& doc, btAlignedObjectArray<GLInstanceGr
{ {
normalSourceName = vs->m_normalArrayId; normalSourceName = vs->m_normalArrayId;
normalOffset = offset; normalOffset = offset;
positionAndNormalInVertex = true;
} }
} }
if (semName=="NORMAL") if (semName=="NORMAL")
@@ -234,7 +234,6 @@ void readLibraryGeometries(TiXmlDocument& doc, btAlignedObjectArray<GLInstanceGr
btAssert(normalSourceName.length()==0); btAssert(normalSourceName.length()==0);
normalSourceName = source_name; normalSourceName = source_name;
normalOffset = offset; normalOffset = offset;
positionAndNormalInVertex = false;
} }
} }
numIndices = primitiveCount * 3; numIndices = primitiveCount * 3;
@@ -418,7 +417,7 @@ void readNodeHierarchy(TiXmlElement* node,btHashMap<btHashString,int>& name2Shap
int* shapeIndexPtr = name2Shape[geomUrl]; int* shapeIndexPtr = name2Shape[geomUrl];
if (shapeIndexPtr) if (shapeIndexPtr)
{ {
int index = *shapeIndexPtr; // int index = *shapeIndexPtr;
printf("found geom with index %d\n", *shapeIndexPtr); printf("found geom with index %d\n", *shapeIndexPtr);
ColladaGraphicsInstance& instance = visualShapeInstances.expand(); ColladaGraphicsInstance& instance = visualShapeInstances.expand();
instance.m_shapeIndex = *shapeIndexPtr; instance.m_shapeIndex = *shapeIndexPtr;
@@ -555,7 +554,7 @@ void getUnitMeterScalingAndUpAxisTransform(TiXmlDocument& doc, btTransform& tr,
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)
{ {
GLInstanceGraphicsShape* instance = 0; // GLInstanceGraphicsShape* instance = 0;
//usually COLLADA files don't have that many visual geometries/shapes //usually COLLADA files don't have that many visual geometries/shapes
visualShapes.reserve(32); visualShapes.reserve(32);
@@ -745,4 +744,4 @@ void LoadMeshFromColladaAssimp(const char* relativeFileName, btAlignedObjectArra
} }
#endif //COMPARE_WITH_ASSIMP #endif //COMPARE_WITH_ASSIMP

View File

@@ -117,7 +117,7 @@ ATTRIBUTE_ALIGNED16(class) btMatrix4x4
return m_el[0].w() * v.x() + m_el[1].w() * v.y() + m_el[2].w() * v.z() + m_el[3].w() * v.w(); return m_el[0].w() * v.x() + m_el[1].w() * v.y() + m_el[2].w() * v.z() + m_el[3].w() * v.w();
} }
SIMD_FORCE_INLINE btMatrix4x4 SIMD_FORCE_INLINE btMatrix4x4 &
operator*=(const btMatrix4x4& m) operator*=(const btMatrix4x4& m)
{ {
setValue( setValue(
@@ -125,6 +125,7 @@ ATTRIBUTE_ALIGNED16(class) btMatrix4x4
m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]),m.tdotw(m_el[1]), m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]),m.tdotw(m_el[1]),
m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]),m.tdotw(m_el[2]), m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]),m.tdotw(m_el[2]),
m.tdotx(m_el[3]), m.tdoty(m_el[3]), m.tdotz(m_el[3]),m.tdotw(m_el[3])); m.tdotx(m_el[3]), m.tdoty(m_el[3]), m.tdotz(m_el[3]),m.tdotw(m_el[3]));
return *this;
} }

View File

@@ -6,6 +6,8 @@
#include "btBulletDynamicsCommon.h" #include "btBulletDynamicsCommon.h"
#include "../OpenGLWindow/SimpleOpenGL3App.h" #include "../OpenGLWindow/SimpleOpenGL3App.h"
#include "Wavefront2GLInstanceGraphicsShape.h" #include "Wavefront2GLInstanceGraphicsShape.h"
#include "../../Utils/b3ResourcePath.h"
#include "Bullet3Common/b3FileUtils.h"
#include "../CommonInterfaces/CommonRigidBodyBase.h" #include "../CommonInterfaces/CommonRigidBodyBase.h"
@@ -54,30 +56,17 @@ void ImportObjSetup::initPhysics()
this->createEmptyDynamicsWorld(); this->createEmptyDynamicsWorld();
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe); m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe);
const char* fileName = "samurai_monastry.obj";
char relativeFileName[1024];
const char* prefix[]={"./data/","../data/","../../data/","../../../data/","../../../../data/"}; const char* fileName = "samurai_monastry.obj";
int prefixIndex=-1; char relativeFileName[1024];
{ if (b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024))
{
int numPrefixes = sizeof(prefix)/sizeof(char*); char pathPrefix[1024];
for (int i=0;i<numPrefixes;i++) b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
{
FILE* f = 0;
sprintf(relativeFileName,"%s%s",prefix[i],fileName);
f = fopen(relativeFileName,"r");
if (f)
{
fclose(f);
prefixIndex = i;
break;
}
}
}
if (prefixIndex<0)
return;
btVector3 shift(0,0,0); btVector3 shift(0,0,0);
btVector3 scaling(10,10,10); btVector3 scaling(10,10,10);
@@ -86,7 +75,7 @@ void ImportObjSetup::initPhysics()
{ {
std::vector<tinyobj::shape_t> shapes; std::vector<tinyobj::shape_t> shapes;
std::string err = tinyobj::LoadObj(shapes, relativeFileName, prefix[prefixIndex]); std::string err = tinyobj::LoadObj(shapes, relativeFileName, pathPrefix);
GLInstanceGraphicsShape* gfxShape = btgCreateGraphicsShapeFromWavefrontObj(shapes); GLInstanceGraphicsShape* gfxShape = btgCreateGraphicsShapeFromWavefrontObj(shapes);
@@ -106,7 +95,12 @@ void ImportObjSetup::initPhysics()
m_guiHelper->getRenderInterface()->registerGraphicsInstance(shapeId,position,orn,color,scaling); m_guiHelper->getRenderInterface()->registerGraphicsInstance(shapeId,position,orn,color,scaling);
} }}
else
{
b3Warning("Cannot find %s\n", fileName);
}
} }
CommonExampleInterface* ImportObjCreateFunc(struct CommonExampleOptions& options) CommonExampleInterface* ImportObjCreateFunc(struct CommonExampleOptions& options)

View File

@@ -6,6 +6,7 @@
#include "../OpenGLWindow/SimpleOpenGL3App.h" #include "../OpenGLWindow/SimpleOpenGL3App.h"
#include "LoadMeshFromSTL.h" #include "LoadMeshFromSTL.h"
#include "../CommonInterfaces/CommonRigidBodyBase.h" #include "../CommonInterfaces/CommonRigidBodyBase.h"
#include "../../Utils/b3ResourcePath.h"
@@ -52,28 +53,12 @@ void ImportSTLSetup::initPhysics()
const char* fileName = "l_finger_tip.stl"; const char* fileName = "l_finger_tip.stl";
char relativeFileName[1024]; char relativeFileName[1024];
const char* prefix[]={"./data/","../data/","../../data/","../../../data/","../../../../data/"}; if (!b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024))
int prefixIndex=-1; {
{ b3Warning("Cannot find file %s\n", fileName);
return;
int numPrefixes = sizeof(prefix)/sizeof(char*); }
for (int i=0;i<numPrefixes;i++)
{
FILE* f = 0;
sprintf(relativeFileName,"%s%s",prefix[i],fileName);
f = fopen(relativeFileName,"r");
if (f)
{
fclose(f);
prefixIndex = i;
break;
}
}
}
if (prefixIndex<0)
return;
btVector3 shift(0,0,0); btVector3 shift(0,0,0);
btVector3 scaling(10,10,10); btVector3 scaling(10,10,10);

View File

@@ -24,6 +24,7 @@ subject to the following restrictions:
#include "../CommonInterfaces/CommonGUIHelperInterface.h" #include "../CommonInterfaces/CommonGUIHelperInterface.h"
#include "Bullet3Common/b3FileUtils.h" #include "Bullet3Common/b3FileUtils.h"
#include <string> #include <string>
#include "../../Utils/b3ResourcePath.h"
@@ -101,7 +102,8 @@ bool BulletURDFImporter::loadURDF(const char* fileName)
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);
std::string xml_string; std::string xml_string;
m_data->m_pathPrefix[0] = 0; m_data->m_pathPrefix[0] = 0;
@@ -390,7 +392,7 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
upAxis); upAxis);
glmesh = new GLInstanceGraphicsShape; glmesh = new GLInstanceGraphicsShape;
int index = 0; // int index = 0;
glmesh->m_indices = new b3AlignedObjectArray<int>(); glmesh->m_indices = new b3AlignedObjectArray<int>();
glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>(); glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
@@ -501,7 +503,7 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
glmesh = new GLInstanceGraphicsShape; glmesh = new GLInstanceGraphicsShape;
int index = 0; // int index = 0;
glmesh->m_indices = new b3AlignedObjectArray<int>(); glmesh->m_indices = new b3AlignedObjectArray<int>();
glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>(); glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
@@ -702,7 +704,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
upAxis ); upAxis );
glmesh = new GLInstanceGraphicsShape; glmesh = new GLInstanceGraphicsShape;
int index = 0; // int index = 0;
glmesh->m_indices = new b3AlignedObjectArray<int>(); glmesh->m_indices = new b3AlignedObjectArray<int>();
glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>(); glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();

View File

@@ -9,7 +9,9 @@
#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h" #include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#include "../CommonInterfaces/CommonParameterInterface.h" #include "../CommonInterfaces/CommonParameterInterface.h"
#ifdef ENABLE_ROS_URDF
#include "ROSURDFImporter.h" #include "ROSURDFImporter.h"
#endif
#include "BulletUrdfImporter.h" #include "BulletUrdfImporter.h"
@@ -197,18 +199,20 @@ void ImportUrdfSetup::initPhysics()
//now print the tree using the new interface //now print the tree using the new interface
URDFImporterInterface* bla=0; URDFImporterInterface* bla=0;
static bool newURDF = false; static bool newURDF = true;
newURDF = !newURDF;
if (newURDF) if (newURDF)
{ {
b3Printf("using new URDF\n"); b3Printf("using new URDF\n");
bla = new BulletURDFImporter(m_guiHelper); bla = new BulletURDFImporter(m_guiHelper);
} else }
#ifdef USE_ROS_URDF
else
{ {
b3Printf("using ROS URDF\n"); b3Printf("using ROS URDF\n");
bla = new ROSURDFImporter(m_guiHelper); bla = new ROSURDFImporter(m_guiHelper);
} }
newURDF = !newURDF;
#endif//USE_ROS_URDF
URDFImporterInterface& u2b = *bla; URDFImporterInterface& u2b = *bla;
bool loadOk = u2b.loadURDF(m_fileName); bool loadOk = u2b.loadURDF(m_fileName);

View File

@@ -12,8 +12,8 @@
#include "URDFJointTypes.h" #include "URDFJointTypes.h"
MyMultiBodyCreator::MyMultiBodyCreator(GUIHelperInterface* guiHelper) MyMultiBodyCreator::MyMultiBodyCreator(GUIHelperInterface* guiHelper)
:m_guiHelper(guiHelper), :m_bulletMultiBody(0),
m_bulletMultiBody(0) m_guiHelper(guiHelper)
{ {
} }

View File

@@ -152,7 +152,7 @@ bool ROSURDFImporter::loadURDF(const char* fileName)
m_data->m_robot->getLinks(m_data->m_links); m_data->m_robot->getLinks(m_data->m_links);
//initialize the 'index' of each link //initialize the 'index' of each link
for (int i=0;i<m_data->m_links.size();i++) for (int i=0;i<(int)m_data->m_links.size();i++)
{ {
m_data->m_links[i]->m_link_index = i; m_data->m_links[i]->m_link_index = i;
} }
@@ -413,7 +413,7 @@ void ROSconvertURDFToVisualShape(const Visual* visual, const char* urdfPathPrefi
upAxis); upAxis);
glmesh = new GLInstanceGraphicsShape; glmesh = new GLInstanceGraphicsShape;
int index = 0; //int index = 0;
glmesh->m_indices = new b3AlignedObjectArray<int>(); glmesh->m_indices = new b3AlignedObjectArray<int>();
glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>(); glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
@@ -524,7 +524,7 @@ void ROSconvertURDFToVisualShape(const Visual* visual, const char* urdfPathPrefi
glmesh = new GLInstanceGraphicsShape; glmesh = new GLInstanceGraphicsShape;
int index = 0; // int index = 0;
glmesh->m_indices = new b3AlignedObjectArray<int>(); glmesh->m_indices = new b3AlignedObjectArray<int>();
glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>(); glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
@@ -726,7 +726,7 @@ btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const cha
upAxis ); upAxis );
glmesh = new GLInstanceGraphicsShape; glmesh = new GLInstanceGraphicsShape;
int index = 0; // int index = 0;
glmesh->m_indices = new b3AlignedObjectArray<int>(); glmesh->m_indices = new b3AlignedObjectArray<int>();
glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>(); glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();

View File

@@ -73,9 +73,10 @@ void printTree(const URDFImporterInterface& u2b, int linkIndex, int indentationL
struct URDF2BulletCachedData struct URDF2BulletCachedData
{ {
URDF2BulletCachedData() URDF2BulletCachedData()
:m_totalNumJoints1(0), :
m_currentMultiBodyLinkIndex(-1), m_currentMultiBodyLinkIndex(-1),
m_bulletMultiBody(0) m_bulletMultiBody(0),
m_totalNumJoints1(0)
{ {
} }

View File

@@ -18,7 +18,7 @@ static bool parseVector4(btVector4& vec4, const std::string& vector_str)
btArray<std::string> pieces; btArray<std::string> pieces;
btArray<float> rgba; btArray<float> rgba;
urdfStringSplit(pieces, vector_str, urdfIsAnyOf(" ")); urdfStringSplit(pieces, vector_str, urdfIsAnyOf(" "));
for (unsigned int i = 0; i < pieces.size(); ++i) for (int i = 0; i < pieces.size(); ++i)
{ {
if (!pieces[i].empty()) if (!pieces[i].empty())
{ {
@@ -39,7 +39,7 @@ static bool parseVector3(btVector3& vec3, const std::string& vector_str, ErrorLo
btArray<std::string> pieces; btArray<std::string> pieces;
btArray<float> rgba; btArray<float> rgba;
urdfStringSplit(pieces, vector_str, urdfIsAnyOf(" ")); urdfStringSplit(pieces, vector_str, urdfIsAnyOf(" "));
for (unsigned int i = 0; i < pieces.size(); ++i) for (int i = 0; i < pieces.size(); ++i)
{ {
if (!pieces[i].empty()) if (!pieces[i].empty())
{ {
@@ -844,4 +844,4 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger)
return initTreeAndRoot(logger); return initTreeAndRoot(logger);
} }

View File

@@ -4,7 +4,7 @@
#include "PosixSharedMemory.h" #include "PosixSharedMemory.h"
#include "Win32SharedMemory.h" #include "Win32SharedMemory.h"
#include "../Importers/ImportURDFDemo/ROSURDFImporter.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"
@@ -132,7 +132,7 @@ bool PhysicsServer::loadUrdf(const char* fileName, const btVector3& pos, const b
bool useMultiBody, bool useFixedBase) bool useMultiBody, bool useFixedBase)
{ {
ROSURDFImporter u2b(m_guiHelper); BulletURDFImporter u2b(m_guiHelper);
bool loadOk = u2b.loadURDF(fileName); bool loadOk = u2b.loadURDF(fileName);
if (loadOk) if (loadOk)
{ {

View File

@@ -22,8 +22,9 @@ files {
"../Importers/ImportURDFDemo/MultiBodyCreationInterface.h", "../Importers/ImportURDFDemo/MultiBodyCreationInterface.h",
"../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp", "../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp",
"../Importers/ImportURDFDemo/MyMultiBodyCreator.h", "../Importers/ImportURDFDemo/MyMultiBodyCreator.h",
"../Importers/ImportURDFDemo/ROSURDFImporter.cpp", "../Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
"../Importers/ImportURDFDemo/ROSURDFImporter.h", "../Importers/ImportURDFDemo/BulletUrdfImporter.h",
"../Importers/ImportURDFDemo/UrdfParser.cpp",
"../Importers/ImportURDFDemo/BulletUrdfImporter.cpp", "../Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
"../Importers/ImportURDFDemo/BulletUrdfImporter.h", "../Importers/ImportURDFDemo/BulletUrdfImporter.h",
"../Importers/ImportURDFDemo/urdfStringSplit.cpp", "../Importers/ImportURDFDemo/urdfStringSplit.cpp",
@@ -31,6 +32,7 @@ files {
"../Importers/ImportURDFDemo/UrdfParser.h", "../Importers/ImportURDFDemo/UrdfParser.h",
"../Importers/ImportURDFDemo/URDF2Bullet.cpp", "../Importers/ImportURDFDemo/URDF2Bullet.cpp",
"../Importers/ImportURDFDemo/URDF2Bullet.h", "../Importers/ImportURDFDemo/URDF2Bullet.h",
"../Utils/b3ResourcePath.cpp",
"../Importers/ImportURDFDemo/URDFImporterInterface.h", "../Importers/ImportURDFDemo/URDFImporterInterface.h",
"../Importers/ImportURDFDemo/URDFJointTypes.h", "../Importers/ImportURDFDemo/URDFJointTypes.h",
"../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp", "../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp",

View File

@@ -0,0 +1,80 @@
#include "b3ResourcePath.h"
#include "Bullet3Common/b3Logging.h"
#ifdef __APPLE__
#include <mach-o/dyld.h> /* _NSGetExecutablePath */
#else
#ifdef _WIN32
#else
//not Mac, not Windows, let's cross the fingers it is Linux :-)
#include <unistd.h>
#endif
#endif
#include "Bullet3Common/b3FileUtils.h"
#define B3_MAX_EXE_PATH_LEN 4096
int b3ResourcePath::getExePath(char* path, int maxPathLenInBytes)
{
int numBytes = 0;
#if __APPLE__
uint32_t bufsize = uint32_t(maxPathLenInBytes);
if (_NSGetExecutablePath(path, &bufsize)!=0)
{
b3Warning("Cannot find executable path\n");
return false;
} else
{
numBytes = strlen(path);
}
#else
#ifdef _WIN32
#error not yet
#else
numBytes = (int)readlink("/proc/self/exe", path, maxPathLenInBytes-1);
if (numBytes > 0)
{
path[numBytes] = 0;
} else
{
b3Warning("Cannot find executable path\n");
}
#endif //_WIN32
#endif //__APPLE__
return numBytes;
}
int b3ResourcePath::findResourcePath(const char* resourceName, char* resourcePath, int resourcePathMaxNumBytes)
{
//first find in a resource/<exeName> location, then in various folders within 'data' using b3FileUtils
char exePath[B3_MAX_EXE_PATH_LEN];
int l = b3ResourcePath::getExePath(exePath, B3_MAX_EXE_PATH_LEN);
if (l)
{
char pathToExe[B3_MAX_EXE_PATH_LEN];
int exeNamePos = b3FileUtils::extractPath(exePath,pathToExe,B3_MAX_EXE_PATH_LEN);
if (exeNamePos)
{
sprintf(resourcePath,"%s../resources/%s/%s",pathToExe,&exePath[exeNamePos],resourceName);
//printf("try resource at %s\n", resourcePath);
if (b3FileUtils::findFile(resourcePath, resourcePath, resourcePathMaxNumBytes))
{
return strlen(resourcePath);
}
}
}
bool res = b3FileUtils::findFile(resourceName, resourcePath, resourcePathMaxNumBytes);
if (res)
{
return strlen(resourcePath);
}
return 0;
}

View File

@@ -0,0 +1,13 @@
#ifndef _B3_RESOURCE_PATH_H
#define _B3_RESOURCE_PATH_H
#include <string>
class b3ResourcePath
{
public:
static int getExePath(char* path, int maxPathLenInBytes);
static int findResourcePath(const char* sourceName, char* resourcePath, int maxResourcePathLenInBytes);
};
#endif

View File

@@ -15,7 +15,7 @@ struct b3FileUtils
{ {
} }
bool findFile(const char* orgFileName, char* relativeFileName, int maxRelativeFileNameMaxLen) static bool findFile(const char* orgFileName, char* relativeFileName, int maxRelativeFileNameMaxLen)
{ {
FILE* f=0; FILE* f=0;
f = fopen(orgFileName,"rb"); f = fopen(orgFileName,"rb");
@@ -33,7 +33,6 @@ struct b3FileUtils
f=0; f=0;
bool fileFound = false; bool fileFound = false;
int result = 0;
for (int i=0;!f && i<numPrefixes;i++) for (int i=0;!f && i<numPrefixes;i++)
{ {
@@ -64,7 +63,7 @@ struct b3FileUtils
const char * oriptr; const char * oriptr;
const char * patloc; const char * patloc;
// find how many times the pattern occurs in the original string // find how many times the pattern occurs in the original string
for (oriptr = name; patloc = strstr(oriptr, pattern); oriptr = patloc + patlen) for (oriptr = name; (patloc = strstr(oriptr, pattern)); oriptr = patloc + patlen)
{ {
patcnt++; patcnt++;
} }
@@ -73,7 +72,7 @@ struct b3FileUtils
static void extractPath(const char* fileName, char* path, int maxPathLength) static int extractPath(const char* fileName, char* path, int maxPathLength)
{ {
const char* stripped = strip2(fileName, "/"); const char* stripped = strip2(fileName, "/");
stripped = strip2(stripped, "\\"); stripped = strip2(stripped, "\\");
@@ -91,12 +90,14 @@ struct b3FileUtils
path[len]=0; path[len]=0;
} else } else
{ {
len = 0;
b3Assert(maxPathLength>0); b3Assert(maxPathLength>0);
if (maxPathLength>0) if (maxPathLength>0)
{ {
path[0] = 0; path[len] = 0;
} }
} }
return len;
} }
static char toLowerChar(const char t) static char toLowerChar(const char t)