From f469a2cb49753f313b1733ead800a3b3a77ef0fb Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Tue, 31 May 2016 22:55:13 -0700 Subject: [PATCH] update to tinyrenderer synthetic camera --- .../InverseDynamics/btMultiBodyFromURDF.hpp | 6 +- examples/ExampleBrowser/premake4.lua | 2 + examples/ExtendedTutorials/premake4.lua | 4 - .../ImportSDFDemo/ImportSDFSetup.cpp | 6 +- .../ImportURDFDemo/BulletUrdfImporter.cpp | 387 +++++++++++++++++- .../ImportURDFDemo/BulletUrdfImporter.h | 7 +- .../DefaultVisualShapeConverter.h | 26 -- .../ImportURDFDemo/ImportURDFSetup.cpp | 6 +- .../LinkVisualShapesConverter.h | 3 +- .../Importers/ImportURDFDemo/URDF2Bullet.cpp | 11 +- .../ImportURDFDemo/URDFImporterInterface.h | 2 +- .../InverseDynamicsExample.cpp | 5 +- examples/InverseDynamics/premake4.lua | 4 - .../PhysicsServerCommandProcessor.cpp | 18 +- .../TinyRendererVisualShapeConverter.cpp} | 216 ++++++++-- .../TinyRendererVisualShapeConverter.h | 33 ++ examples/SharedMemory/premake4.lua | 11 +- src/LinearMath/btHashMap.h | 13 + test/InverseDynamics/premake4.lua | 2 - test/SharedMemory/premake4.lua | 28 +- 20 files changed, 675 insertions(+), 115 deletions(-) delete mode 100644 examples/Importers/ImportURDFDemo/DefaultVisualShapeConverter.h rename examples/{Importers/ImportURDFDemo/DefaultVisualShapeConverter.cpp => SharedMemory/TinyRendererVisualShapeConverter.cpp} (66%) create mode 100644 examples/SharedMemory/TinyRendererVisualShapeConverter.h diff --git a/Extras/InverseDynamics/btMultiBodyFromURDF.hpp b/Extras/InverseDynamics/btMultiBodyFromURDF.hpp index 3e5f43978..a38ad268b 100644 --- a/Extras/InverseDynamics/btMultiBodyFromURDF.hpp +++ b/Extras/InverseDynamics/btMultiBodyFromURDF.hpp @@ -11,7 +11,7 @@ #include "../Importers/ImportURDFDemo/URDF2Bullet.h" #include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h" #include "../Importers/ImportURDFDemo/URDF2Bullet.h" -#include "../Importers/ImportURDFDemo/DefaultVisualShapeConverter.h" + /// Create a btMultiBody model from URDF. /// This is adapted from Bullet URDF loader example @@ -45,8 +45,8 @@ public: void init() { this->createEmptyDynamicsWorld(); m_dynamicsWorld->setGravity(m_gravity); - DefaultVisualShapeConverter visualConverter(&m_nogfx); - BulletURDFImporter urdf_importer(&m_nogfx, &visualConverter); + + BulletURDFImporter urdf_importer(&m_nogfx, 0); URDFImporterInterface &u2b(urdf_importer); bool loadOk = u2b.loadURDF(m_filename.c_str(), m_base_fixed); diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index 8b05b1eda..82c63e64d 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -76,6 +76,8 @@ project "App_BulletExampleBrowser" "../SharedMemory/PhysicsLoopBackC_API.h", "../SharedMemory/PhysicsServerCommandProcessor.cpp", "../SharedMemory/PhysicsServerCommandProcessor.h", + "../SharedMemory/TinyRendererVisualShapeConverter.cpp", + "../SharedMemory/TinyRendererVisualShapeConverter.h", "../MultiThreading/MultiThreadingExample.cpp", "../MultiThreading/b3PosixThreadSupport.cpp", "../MultiThreading/b3Win32ThreadSupport.cpp", diff --git a/examples/ExtendedTutorials/premake4.lua b/examples/ExtendedTutorials/premake4.lua index 160affa9d..ea1fc507e 100644 --- a/examples/ExtendedTutorials/premake4.lua +++ b/examples/ExtendedTutorials/premake4.lua @@ -34,7 +34,6 @@ files { "../Importers/ImportObjDemo/LoadMeshFromObj.cpp", "../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp", "../Importers/ImportURDFDemo/BulletUrdfImporter.cpp", - "../Importers/ImportURDFDemo/DefaultVisualShapeConverter.cpp", "../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp", "../Importers/ImportURDFDemo/URDF2Bullet.cpp", "../Importers/ImportURDFDemo/UrdfParser.cpp", @@ -86,7 +85,6 @@ files { "../Importers/ImportObjDemo/LoadMeshFromObj.cpp", "../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp", "../Importers/ImportURDFDemo/BulletUrdfImporter.cpp", - "../Importers/ImportURDFDemo/DefaultVisualShapeConverter.cpp", "../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp", "../Importers/ImportURDFDemo/URDF2Bullet.cpp", "../Importers/ImportURDFDemo/UrdfParser.cpp", @@ -151,7 +149,6 @@ files { "../Importers/ImportObjDemo/LoadMeshFromObj.cpp", "../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp", "../Importers/ImportURDFDemo/BulletUrdfImporter.cpp", - "../Importers/ImportURDFDemo/DefaultVisualShapeConverter.cpp", "../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp", "../Importers/ImportURDFDemo/URDF2Bullet.cpp", "../Importers/ImportURDFDemo/UrdfParser.cpp", @@ -212,7 +209,6 @@ files { "../Importers/ImportObjDemo/LoadMeshFromObj.cpp", "../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp", "../Importers/ImportURDFDemo/BulletUrdfImporter.cpp", - "../Importers/ImportURDFDemo/DefaultVisualShapeConverter.cpp", "../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp", "../Importers/ImportURDFDemo/URDF2Bullet.cpp", "../Importers/ImportURDFDemo/UrdfParser.cpp", diff --git a/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp b/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp index f8e61c032..54883298b 100644 --- a/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp +++ b/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp @@ -12,7 +12,7 @@ #include "../../Utils/b3ResourcePath.h" #include "../ImportURDFDemo/BulletUrdfImporter.h" -#include "../Importers/ImportURDFDemo/DefaultVisualShapeConverter.h" + #include "../ImportURDFDemo/URDF2Bullet.h" @@ -203,8 +203,8 @@ void ImportSDFSetup::initPhysics() m_dynamicsWorld->setGravity(gravity); - DefaultVisualShapeConverter visualConverter(m_guiHelper); - BulletURDFImporter u2b(m_guiHelper, &visualConverter); + + BulletURDFImporter u2b(m_guiHelper, 0); bool loadOk = u2b.loadSDF(m_fileName); diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 1aac66af3..7e9624d64 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -37,7 +37,7 @@ struct BulletURDFInternalData UrdfParser m_urdfParser; struct GUIHelperInterface* m_guiHelper; char m_pathPrefix[1024]; - + btHashMap m_linkColors; btAlignedObjectArray m_allocatedCollisionShapes; LinkVisualShapesConverter* m_customVisualShapesConverter; @@ -232,14 +232,6 @@ void BulletURDFImporter::getLinkChildIndices(int linkIndex, btAlignedObjectArray } } -bool BulletURDFImporter::getLinkColor(int linkIndex, btVector4& colorRGBA) const -{ - if (m_data->m_customVisualShapesConverter) - { - return m_data->m_customVisualShapesConverter->getLinkColor(linkIndex, colorRGBA); - } - return false; -} std::string BulletURDFImporter::getLinkName(int linkIndex) const { @@ -585,16 +577,385 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co return shape; } -int BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionShape* colShape) const + +static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut) +{ + + + GLInstanceGraphicsShape* glmesh = 0; + + btConvexShape* convexColShape = 0; + + switch (visual->m_geometry.m_type) + { + case URDF_GEOM_CYLINDER: + { + btAlignedObjectArray vertices; + + //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float)); + int numSteps = 32; + for (int i = 0; im_geometry.m_cylinderRadius; + btScalar cylLength = visual->m_geometry.m_cylinderLength; + + btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i) / numSteps)), cylRadius*btCos(SIMD_2_PI*(float(i) / numSteps)), cylLength / 2.); + vertices.push_back(vert); + vert[2] = -cylLength / 2.; + vertices.push_back(vert); + } + + btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); + cylZShape->setMargin(0.001); + convexColShape = cylZShape; + break; + } + case URDF_GEOM_BOX: + { + + btVector3 extents = visual->m_geometry.m_boxSize; + + btBoxShape* boxShape = new btBoxShape(extents*0.5f); + //btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5); + convexColShape = boxShape; + convexColShape->setMargin(0.001); + break; + } + case URDF_GEOM_SPHERE: + { + btScalar radius = visual->m_geometry.m_sphereRadius; + btSphereShape* sphereShape = new btSphereShape(radius); + convexColShape = sphereShape; + convexColShape->setMargin(0.001); + break; + + break; + } + case URDF_GEOM_MESH: + { + if (visual->m_name.length()) + { + //b3Printf("visual->name=%s\n", visual->m_name.c_str()); + } + if (1)//visual->m_geometry) + { + if (visual->m_geometry.m_meshFileName.length()) + { + const char* filename = visual->m_geometry.m_meshFileName.c_str(); + //b3Printf("mesh->filename=%s\n", filename); + char fullPath[1024]; + int fileType = 0; + + char tmpPathPrefix[1024]; + std::string xml_string; + int maxPathLen = 1024; + b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen); + + char visualPathPrefix[1024]; + sprintf(visualPathPrefix,"%s%s",urdfPathPrefix,tmpPathPrefix); + + + sprintf(fullPath, "%s%s", urdfPathPrefix, filename); + b3FileUtils::toLower(fullPath); + if (strstr(fullPath, ".dae")) + { + fileType = FILE_COLLADA; + } + if (strstr(fullPath, ".stl")) + { + fileType = FILE_STL; + } + if (strstr(fullPath,".obj")) + { + fileType = FILE_OBJ; + } + + + sprintf(fullPath, "%s%s", urdfPathPrefix, filename); + FILE* f = fopen(fullPath, "rb"); + if (f) + { + fclose(f); + + switch (fileType) + { + case FILE_OBJ: + { + glmesh = LoadMeshFromObj(fullPath,visualPathPrefix); + break; + } + + case FILE_STL: + { + glmesh = LoadMeshFromSTL(fullPath); + break; + } + case FILE_COLLADA: + { + + btAlignedObjectArray visualShapes; + btAlignedObjectArray visualShapeInstances; + btTransform upAxisTrans; upAxisTrans.setIdentity(); + float unitMeterScaling = 1; + int upAxis = 2; + + LoadMeshFromCollada(fullPath, + visualShapes, + visualShapeInstances, + upAxisTrans, + unitMeterScaling, + upAxis); + + glmesh = new GLInstanceGraphicsShape; + // int index = 0; + glmesh->m_indices = new b3AlignedObjectArray(); + glmesh->m_vertices = new b3AlignedObjectArray(); + + for (int i = 0; im_shapeIndex]; + + b3AlignedObjectArray verts; + verts.resize(gfxShape->m_vertices->size()); + + int baseIndex = glmesh->m_vertices->size(); + + for (int i = 0; im_vertices->size(); i++) + { + verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0]; + verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1]; + verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2]; + verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0]; + verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1]; + verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0]; + verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1]; + verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2]; + verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3]; + + } + + int curNumIndices = glmesh->m_indices->size(); + int additionalIndices = gfxShape->m_indices->size(); + glmesh->m_indices->resize(curNumIndices + additionalIndices); + for (int k = 0; km_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex; + } + + //compensate upAxisTrans and unitMeterScaling here + btMatrix4x4 upAxisMat; + upAxisMat.setIdentity(); +// upAxisMat.setPureRotation(upAxisTrans.getRotation()); + btMatrix4x4 unitMeterScalingMat; + unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling)); + btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform; + //btMatrix4x4 worldMat = instance->m_worldTransform; + int curNumVertices = glmesh->m_vertices->size(); + int additionalVertices = verts.size(); + glmesh->m_vertices->reserve(curNumVertices + additionalVertices); + + for (int v = 0; vm_vertices->push_back(verts[v]); + } + } + glmesh->m_numIndices = glmesh->m_indices->size(); + glmesh->m_numvertices = glmesh->m_vertices->size(); + //glmesh = LoadMeshFromCollada(fullPath); + + break; + } + default: + { + b3Warning("Error: unsupported file type for Visual mesh: %s\n", fullPath); + btAssert(0); + } + } + + + if (glmesh && glmesh->m_vertices && (glmesh->m_numvertices>0)) + { + //apply the geometry scaling + for (int i=0;im_vertices->size();i++) + { + glmesh->m_vertices->at(i).xyzw[0] *= visual->m_geometry.m_meshScale[0]; + glmesh->m_vertices->at(i).xyzw[1] *= visual->m_geometry.m_meshScale[1]; + glmesh->m_vertices->at(i).xyzw[2] *= visual->m_geometry.m_meshScale[2]; + } + + } + else + { + b3Warning("issue extracting mesh from COLLADA/STL file %s\n", fullPath); + } + + } + else + { + b3Warning("mesh geometry not found %s\n", fullPath); + } + + + } + } + + + break; + } + default: + { + b3Warning("Error: unknown visual geometry type\n"); + } + } + + //if we have a convex, tesselate into localVertices/localIndices + if ((glmesh==0) && convexColShape) + { + btShapeHull* hull = new btShapeHull(convexColShape); + hull->buildHull(0.0); + { + // int strideInBytes = 9*sizeof(float); + int numVertices = hull->numVertices(); + int numIndices = hull->numIndices(); + + + glmesh = new GLInstanceGraphicsShape; + // int index = 0; + glmesh->m_indices = new b3AlignedObjectArray(); + glmesh->m_vertices = new b3AlignedObjectArray(); + + + for (int i = 0; i < numVertices; i++) + { + GLInstanceVertex vtx; + btVector3 pos = hull->getVertexPointer()[i]; + vtx.xyzw[0] = pos.x(); + vtx.xyzw[1] = pos.y(); + vtx.xyzw[2] = pos.z(); + vtx.xyzw[3] = 1.f; + pos.normalize(); + vtx.normal[0] = pos.x(); + vtx.normal[1] = pos.y(); + vtx.normal[2] = pos.z(); + vtx.uv[0] = 0.5f; + vtx.uv[1] = 0.5f; + glmesh->m_vertices->push_back(vtx); + } + + btAlignedObjectArray indices; + for (int i = 0; i < numIndices; i++) + { + glmesh->m_indices->push_back(hull->getIndexPointer()[i]); + } + + glmesh->m_numvertices = glmesh->m_vertices->size(); + glmesh->m_numIndices = glmesh->m_indices->size(); + } + delete hull; + delete convexColShape; + convexColShape = 0; + + } + + if (glmesh && glmesh->m_numIndices>0 && glmesh->m_numvertices >0) + { + + int baseIndex = verticesOut.size(); + + + + for (int i = 0; i < glmesh->m_indices->size(); i++) + { + indicesOut.push_back(glmesh->m_indices->at(i) + baseIndex); + } + + for (int i = 0; i < glmesh->m_vertices->size(); i++) + { + GLInstanceVertex& v = glmesh->m_vertices->at(i); + btVector3 vert(v.xyzw[0],v.xyzw[1],v.xyzw[2]); + btVector3 vt = visualTransform*vert; + v.xyzw[0] = vt[0]; + v.xyzw[1] = vt[1]; + v.xyzw[2] = vt[2]; + btVector3 triNormal(v.normal[0],v.normal[1],v.normal[2]); + triNormal = visualTransform.getBasis()*triNormal; + v.normal[0] = triNormal[0]; + v.normal[1] = triNormal[1]; + v.normal[2] = triNormal[2]; + verticesOut.push_back(v); + } + } + delete glmesh; + +} + + +int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const +{ + int graphicsIndex = -1; + btAlignedObjectArray vertices; + btAlignedObjectArray indices; + btTransform startTrans; startTrans.setIdentity(); + + const UrdfModel& model = m_data->m_urdfParser.getModel(); + UrdfLink* const* linkPtr = model.m_links.getAtIndex(linkIndex); + if (linkPtr) + { + + const UrdfLink* link = *linkPtr; + + for (int v = 0; v < link->m_visualArray.size();v++) + { + const UrdfVisual& vis = link->m_visualArray[v]; + btTransform childTrans = vis.m_linkLocalFrame; + btHashString matName(vis.m_materialName.c_str()); + UrdfMaterial *const * matPtr = model.m_materials[matName]; + if (matPtr) + { + UrdfMaterial *const mat = *matPtr; + //printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]); + m_data->m_linkColors.insert(linkIndex,mat->m_rgbaColor); + } + convertURDFToVisualShapeInternal(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices); + + + } + } + if (vertices.size() && indices.size()) + { + graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size()); + } + return graphicsIndex; +} + + +bool BulletURDFImporter::getLinkColor(int linkIndex, btVector4& colorRGBA) const +{ + const btVector4* rgbaPtr = m_data->m_linkColors[linkIndex]; + if (rgbaPtr) + { + colorRGBA = *rgbaPtr; + return true; + } + return false; +} + + +void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj) const { - int graphicsIndex = -1; if (m_data->m_customVisualShapesConverter) { const UrdfModel& model = m_data->m_urdfParser.getModel(); - graphicsIndex = m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, model, colShape); + m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, model, colObj); } - return graphicsIndex; } diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h index c1a2b7c41..14032e388 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h @@ -37,7 +37,7 @@ public: virtual std::string getLinkName(int linkIndex) const; virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const; - + virtual std::string getJointName(int linkIndex) const; virtual void getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const; @@ -46,7 +46,9 @@ public: virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const; - virtual int convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionShape* colShape) const; + virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const; + + virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj) const; ///todo(erwincoumans) refactor this convertLinkCollisionShapes/memory allocation @@ -55,6 +57,7 @@ public: virtual int getNumAllocatedCollisionShapes() const; virtual class btCollisionShape* getAllocatedCollisionShape(int index); + }; diff --git a/examples/Importers/ImportURDFDemo/DefaultVisualShapeConverter.h b/examples/Importers/ImportURDFDemo/DefaultVisualShapeConverter.h deleted file mode 100644 index fc59cd0a7..000000000 --- a/examples/Importers/ImportURDFDemo/DefaultVisualShapeConverter.h +++ /dev/null @@ -1,26 +0,0 @@ -#ifndef DEFAULT_VISUAL_SHAPE_CONVERTER_H -#define DEFAULT_VISUAL_SHAPE_CONVERTER_H - - -#include "LinkVisualShapesConverter.h" - -struct DefaultVisualShapeConverter : public LinkVisualShapesConverter -{ - - struct DefaultVisualShapeConverterInternalData* m_data; - - DefaultVisualShapeConverter(struct GUIHelperInterface* guiHelper); - - virtual ~DefaultVisualShapeConverter(); - - virtual int convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionShape* colShape); - - virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const; - - -}; - - - - -#endif //DEFAULT_VISUAL_SHAPE_CONVERTER_H \ No newline at end of file diff --git a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp index e9c224a82..e7a1607ad 100644 --- a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp +++ b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp @@ -10,7 +10,7 @@ #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" #include "../CommonInterfaces/CommonParameterInterface.h" #include "../../Utils/b3ResourcePath.h" -#include "DefaultVisualShapeConverter.h" + #include "BulletUrdfImporter.h" @@ -200,9 +200,7 @@ void ImportUrdfSetup::initPhysics() m_dynamicsWorld->setGravity(gravity); - - DefaultVisualShapeConverter visualConverter(m_guiHelper); - BulletURDFImporter u2b(m_guiHelper, &visualConverter); + BulletURDFImporter u2b(m_guiHelper, 0); bool loadOk = u2b.loadURDF(m_fileName); diff --git a/examples/Importers/ImportURDFDemo/LinkVisualShapesConverter.h b/examples/Importers/ImportURDFDemo/LinkVisualShapesConverter.h index bff2ef263..a9c941dc0 100644 --- a/examples/Importers/ImportURDFDemo/LinkVisualShapesConverter.h +++ b/examples/Importers/ImportURDFDemo/LinkVisualShapesConverter.h @@ -3,8 +3,7 @@ struct LinkVisualShapesConverter { - virtual int convertVisualShapes(int linkIndex, const char* pathPrefix, const class btTransform& localInertiaFrame, const struct UrdfModel& model, class btCollisionShape* colShape)=0; - virtual bool getLinkColor(int linkIndex, class btVector4& colorRGBA) const = 0; + virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const class btTransform& localInertiaFrame, const struct UrdfModel& model, class btCollisionObject* colObj)=0; }; #endif //LINK_VISUAL_SHAPES_CONVERTER_H diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index e3deca4cd..fbe9f13f8 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -213,7 +213,9 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat btCompoundShape* compoundShape = u2b.convertLinkCollisionShapes(urdfLinkIndex,pathPrefix,localInertialFrame); - int graphicsIndex = u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,compoundShape); + int graphicsIndex = u2b.convertLinkVisualShapes(urdfLinkIndex,pathPrefix,localInertialFrame); + + if (compoundShape) @@ -246,6 +248,8 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat creation.createRigidBodyGraphicsInstance(urdfLinkIndex, body, color, graphicsIndex); cache.registerRigidBody(urdfLinkIndex, body, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame); + + //untested: u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,body); } else { if (cache.m_bulletMultiBody==0) @@ -385,6 +389,8 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat btVector4 color = selectColor2();//(0.0,0.0,0.5); u2b.getLinkColor(urdfLinkIndex,color); creation.createCollisionObjectGraphicsInstance(urdfLinkIndex,col,color); + + u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,col); btScalar friction = 0.5f; @@ -398,6 +404,9 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat cache.m_bulletMultiBody->setBaseCollider(col); } } + } else + { + //u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,compoundShape); } } diff --git a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h index 1bdd3a277..345ef7d5f 100644 --- a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h +++ b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h @@ -44,7 +44,7 @@ public: ///quick hack: need to rethink the API/dependencies of this virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const { return -1;} - virtual int convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionShape* colShape) const { return -1;} + virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj) const { } virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const = 0; }; diff --git a/examples/InverseDynamics/InverseDynamicsExample.cpp b/examples/InverseDynamics/InverseDynamicsExample.cpp index f5abaa726..20cff9204 100644 --- a/examples/InverseDynamics/InverseDynamicsExample.cpp +++ b/examples/InverseDynamics/InverseDynamicsExample.cpp @@ -23,7 +23,6 @@ subject to the following restrictions: #include "../Importers/ImportURDFDemo/BulletUrdfImporter.h" #include "../Importers/ImportURDFDemo/URDF2Bullet.h" #include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h" -#include "../Importers/ImportURDFDemo/DefaultVisualShapeConverter.h" #include "../CommonInterfaces/CommonMultiBodyBase.h" @@ -156,8 +155,8 @@ void InverseDynamicsExample::initPhysics() { - DefaultVisualShapeConverter visualConverter(m_guiHelper); - BulletURDFImporter u2b(m_guiHelper, &visualConverter); + + BulletURDFImporter u2b(m_guiHelper,0); bool loadOk = u2b.loadURDF("kuka_lwr/kuka.urdf"); if (loadOk) { diff --git a/examples/InverseDynamics/premake4.lua b/examples/InverseDynamics/premake4.lua index 93786253f..3871526ce 100644 --- a/examples/InverseDynamics/premake4.lua +++ b/examples/InverseDynamics/premake4.lua @@ -34,7 +34,6 @@ files { "../Importers/ImportObjDemo/LoadMeshFromObj.cpp", "../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp", "../Importers/ImportURDFDemo/BulletUrdfImporter.cpp", - "../Importers/ImportURDFDemo/DefaultVisualShapeConverter.cpp", "../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp", "../Importers/ImportURDFDemo/URDF2Bullet.cpp", "../Importers/ImportURDFDemo/UrdfParser.cpp", @@ -86,7 +85,6 @@ files { "../Importers/ImportObjDemo/LoadMeshFromObj.cpp", "../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp", "../Importers/ImportURDFDemo/BulletUrdfImporter.cpp", - "../Importers/ImportURDFDemo/DefaultVisualShapeConverter.cpp", "../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp", "../Importers/ImportURDFDemo/URDF2Bullet.cpp", "../Importers/ImportURDFDemo/UrdfParser.cpp", @@ -151,7 +149,6 @@ files { "../Importers/ImportObjDemo/LoadMeshFromObj.cpp", "../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp", "../Importers/ImportURDFDemo/BulletUrdfImporter.cpp", - "../Importers/ImportURDFDemo/DefaultVisualShapeConverter.cpp", "../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp", "../Importers/ImportURDFDemo/URDF2Bullet.cpp", "../Importers/ImportURDFDemo/UrdfParser.cpp", @@ -210,7 +207,6 @@ files { "../Importers/ImportObjDemo/LoadMeshFromObj.cpp", "../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp", "../Importers/ImportURDFDemo/BulletUrdfImporter.cpp", - "../Importers/ImportURDFDemo/DefaultVisualShapeConverter.cpp", "../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp", "../Importers/ImportURDFDemo/URDF2Bullet.cpp", "../Importers/ImportURDFDemo/UrdfParser.cpp", diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 45cce8ccc..330455446 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -4,7 +4,7 @@ #include "../Importers/ImportURDFDemo/BulletUrdfImporter.h" #include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h" #include "../Importers/ImportURDFDemo/URDF2Bullet.h" -#include "../Importers/ImportURDFDemo/DefaultVisualShapeConverter.h" +#include "TinyRendererVisualShapeConverter.h" #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" @@ -288,7 +288,6 @@ struct PhysicsServerCommandProcessorInternalData ///handle management btAlignedObjectArray m_bodyHandles; int m_numUsedHandles; // number of active handles - int m_firstFreeHandle; // free handles list InternalBodyHandle* getHandle(int handle) { @@ -411,6 +410,7 @@ struct PhysicsServerCommandProcessorInternalData btVector3 m_hitPos; btScalar m_oldPickingDist; bool m_prevCanSleep; + TinyRendererVisualShapeConverter m_visualConverter; PhysicsServerCommandProcessorInternalData() : @@ -693,8 +693,8 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto } - DefaultVisualShapeConverter visualConverter(m_data->m_guiHelper); - BulletURDFImporter u2b(m_data->m_guiHelper, &visualConverter); + + BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter); bool loadOk = u2b.loadURDF(fileName, useFixedBase); @@ -972,8 +972,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm int width, height; int numPixelsCopied = 0; - - + if (clientCmd.m_requestPixelDataArguments.m_startPixelIndex==0) + { + m_data->m_visualConverter.render(); + } m_data->m_guiHelper->copyCameraImageData(0,0,0,0,0,&width,&height,0); int numTotalPixels = width*height; @@ -1591,6 +1593,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { m_data->m_guiHelper->getRenderInterface()->removeAllInstances(); } + if (m_data) + { + m_data->m_visualConverter.resetAll(); + } deleteDynamicsWorld(); createEmptyDynamicsWorld(); m_data->exitHandles(); diff --git a/examples/Importers/ImportURDFDemo/DefaultVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp similarity index 66% rename from examples/Importers/ImportURDFDemo/DefaultVisualShapeConverter.cpp rename to examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index 594a8bd5c..50bee85c6 100644 --- a/examples/Importers/ImportURDFDemo/DefaultVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -12,26 +12,26 @@ subject to the following restrictions: */ -#include "DefaultVisualShapeConverter.h" +#include "TinyRendererVisualShapeConverter.h" -#include "URDFImporterInterface.h" +#include "../Importers/ImportURDFDemo/URDFImporterInterface.h" #include "btBulletCollisionCommon.h" -#include "../ImportObjDemo/LoadMeshFromObj.h" -#include "../ImportSTLDemo/LoadMeshFromSTL.h" -#include "../ImportColladaDemo/LoadMeshFromCollada.h" +#include "../Importers/ImportObjDemo/LoadMeshFromObj.h" +#include "../Importers/ImportSTLDemo/LoadMeshFromSTL.h" +#include "../Importers/ImportColladaDemo/LoadMeshFromCollada.h" #include "BulletCollision/CollisionShapes/btShapeHull.h"//to create a tesselation of a generic btConvexShape -#include "../../CommonInterfaces/CommonGUIHelperInterface.h" +#include "../CommonInterfaces/CommonGUIHelperInterface.h" #include "Bullet3Common/b3FileUtils.h" #include -#include "../../Utils/b3ResourcePath.h" - - +#include "../Utils/b3ResourcePath.h" +#include "../TinyRenderer/TinyRenderer.h" +#include "../OpenGLWindow/SimpleCamera.h" #include #include -#include "UrdfParser.h" +#include "../Importers/ImportURDFDemo/UrdfParser.h" enum MyFileType @@ -41,21 +41,53 @@ enum MyFileType MY_FILE_OBJ=3, }; - -struct DefaultVisualShapeConverterInternalData +struct TinyRendererObjectArray { - struct GUIHelperInterface* m_guiHelper; - //char m_pathPrefix[1024]; - btHashMap m_linkColors; + btAlignedObjectArray< TinyRenderObjectData*> m_renderObjects; +}; + +#define START_WIDTH 640 +#define START_HEIGHT 480 + +struct TinyRendererVisualShapeConverterInternalData +{ + + btHashMap m_swRenderInstances; + + int m_upAxis; + int m_swWidth; + int m_swHeight; + TGAImage m_rgbColorBuffer; + b3AlignedObjectArray m_depthBuffer; + SimpleCamera m_camera; + + TinyRendererVisualShapeConverterInternalData() + :m_upAxis(2), + m_swWidth(START_WIDTH), + m_swHeight(START_HEIGHT), + m_rgbColorBuffer(START_WIDTH,START_HEIGHT,TGAImage::RGB) + { + m_depthBuffer.resize(m_swWidth*m_swHeight); + } + }; -DefaultVisualShapeConverter::DefaultVisualShapeConverter(struct GUIHelperInterface* guiHelper) + +TinyRendererVisualShapeConverter::TinyRendererVisualShapeConverter() { - m_data = new DefaultVisualShapeConverterInternalData(); - m_data->m_guiHelper = guiHelper; + m_data = new TinyRendererVisualShapeConverterInternalData(); + + float dist = 1.5; + float pitch = -80; + float yaw = 10; + float targetPos[3]={0,0,0}; + m_data->m_camera.setCameraUpAxis(m_data->m_upAxis); + resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]); + + } -DefaultVisualShapeConverter::~DefaultVisualShapeConverter() +TinyRendererVisualShapeConverter::~TinyRendererVisualShapeConverter() { delete m_data; } @@ -385,8 +417,9 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref -int DefaultVisualShapeConverter::convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionShape* colShape) +void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colObj) { + btAlignedObjectArray vertices; btAlignedObjectArray indices; btTransform startTrans; startTrans.setIdentity(); @@ -408,29 +441,140 @@ int DefaultVisualShapeConverter::convertVisualShapes(int linkIndex, const char* { UrdfMaterial *const mat = *matPtr; //printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]); - m_data->m_linkColors.insert(linkIndex,mat->m_rgbaColor); + //m_data->m_linkColors.insert(linkIndex,mat->m_rgbaColor); } + + TinyRendererObjectArray** visualsPtr = m_data->m_swRenderInstances[colObj]; + if (visualsPtr==0) + { + m_data->m_swRenderInstances.insert(colObj,new TinyRendererObjectArray); + } + visualsPtr = m_data->m_swRenderInstances[colObj]; + btAssert(visualsPtr); + TinyRendererObjectArray* visuals = *visualsPtr; + convertURDFToVisualShape(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices); - - + + if (vertices.size() && indices.size()) + { + TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_swWidth,m_data->m_swHeight,m_data->m_rgbColorBuffer,m_data->m_depthBuffer); + tinyObj->registerMeshShape(&vertices[0].xyzw[0],vertices.size(),&indices[0],indices.size()); + visuals->m_renderObjects.push_back(tinyObj); + } } } - if (vertices.size() && indices.size()) - { - graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size()); - } - return graphicsIndex; } -bool DefaultVisualShapeConverter::getLinkColor(int linkIndex, btVector4& colorRGBA) const +void TinyRendererVisualShapeConverter::setUpAxis(int axis) { - const btVector4* rgbaPtr = m_data->m_linkColors[linkIndex]; - if (rgbaPtr) - { - colorRGBA = *rgbaPtr; - return true; - } - return false; + m_data->m_upAxis = axis; + m_data->m_camera.setCameraUpAxis(axis); + m_data->m_camera.update(); +} +void TinyRendererVisualShapeConverter::resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ) +{ + m_data->m_camera.setCameraDistance(camDist); + m_data->m_camera.setCameraPitch(pitch); + m_data->m_camera.setCameraYaw(yaw); + m_data->m_camera.setCameraTargetPosition(camPosX,camPosY,camPosZ); + m_data->m_camera.setAspectRatio((float)m_data->m_swWidth/(float)m_data->m_swHeight); + m_data->m_camera.update(); + } +void TinyRendererVisualShapeConverter::clearBuffers(TGAColor& clearColor) +{ + for(int y=0;ym_swHeight;++y) + { + for(int x=0;xm_swWidth;++x) + { + m_data->m_rgbColorBuffer.set(x,y,clearColor); + m_data->m_depthBuffer[x+y*m_data->m_swWidth] = -1e30f; + } + } + +} + + + +void TinyRendererVisualShapeConverter::render() +{ + //clear the color buffer + TGAColor clearColor; + clearColor.bgra[0] = 255; + clearColor.bgra[1] = 255; + clearColor.bgra[2] = 255; + clearColor.bgra[3] = 255; + + clearBuffers(clearColor); + + + ATTRIBUTE_ALIGNED16(btScalar modelMat[16]); + ATTRIBUTE_ALIGNED16(float viewMat[16]); + ATTRIBUTE_ALIGNED16(float projMat[16]); + + m_data->m_camera.getCameraProjectionMatrix(projMat); + m_data->m_camera.getCameraViewMatrix(viewMat); + + btVector3 lightDirWorld(-5,200,-40); + switch (m_data->m_upAxis) + { + case 1: + lightDirWorld = btVector3(-50.f,100,30); + break; + case 2: + lightDirWorld = btVector3(-50.f,30,100); + break; + default:{} + }; + + lightDirWorld.normalize(); + + + for (int i=0;im_swRenderInstances.size();i++) + { + TinyRendererObjectArray** visualArrayPtr = m_data->m_swRenderInstances.getAtIndex(i); + if (0==visualArrayPtr) + continue;//can this ever happen? + TinyRendererObjectArray* visualArray = *visualArrayPtr; + + btHashPtr colObjHash = m_data->m_swRenderInstances.getKeyAtIndex(i); + + + const btCollisionObject* colObj = (btCollisionObject*) colObjHash.getPointer(); + + for (int v=0;vm_renderObjects.size();v++) + { + + TinyRenderObjectData* renderObj = visualArray->m_renderObjects[v]; + + + //sync the object transform + const btTransform& tr = colObj->getWorldTransform(); + tr.getOpenGLMatrix(modelMat); + + for (int i=0;i<4;i++) + { + for (int j=0;j<4;j++) + { + + renderObj->m_projectionMatrix[i][j] = projMat[i+4*j]; + renderObj->m_modelMatrix[i][j] = modelMat[i+4*j]; + renderObj->m_viewMatrix[i][j] = viewMat[i+4*j]; + renderObj->m_localScaling = colObj->getCollisionShape()->getLocalScaling(); + renderObj->m_lightDirWorld = lightDirWorld; + } + } + TinyRenderer::renderObject(*renderObj); + } + } + m_data->m_rgbColorBuffer.write_tga_file("camera.tga"); + +} + +void TinyRendererVisualShapeConverter::resetAll() +{ + //todo: free memory + m_data->m_swRenderInstances.clear(); +} diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.h b/examples/SharedMemory/TinyRendererVisualShapeConverter.h new file mode 100644 index 000000000..1256641ff --- /dev/null +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.h @@ -0,0 +1,33 @@ +#ifndef TINY_RENDERER_VISUAL_SHAPE_CONVERTER_H +#define TINY_RENDERER_VISUAL_SHAPE_CONVERTER_H + + +#include "../Importers/ImportURDFDemo/LinkVisualShapesConverter.h" + +struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter +{ + + struct TinyRendererVisualShapeConverterInternalData* m_data; + + TinyRendererVisualShapeConverter(); + + virtual ~TinyRendererVisualShapeConverter(); + + virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colShape); + + void setUpAxis(int axis); + + void resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ); + + void clearBuffers(struct TGAColor& clearColor); + + void resetAll(); + + void render(); + +}; + + + + +#endif //TINY_RENDERER_VISUAL_SHAPE_CONVERTER_H diff --git a/examples/SharedMemory/premake4.lua b/examples/SharedMemory/premake4.lua index dd9c6c923..0616a8865 100644 --- a/examples/SharedMemory/premake4.lua +++ b/examples/SharedMemory/premake4.lua @@ -40,14 +40,21 @@ files { "PhysicsLoopBackC_API.h", "PhysicsServerCommandProcessor.cpp", "PhysicsServerCommandProcessor.h", + "TinyRendererVisualShapeConverter.cpp", + "TinyRendererVisualShapeConverter.h", + "../TinyRenderer/geometry.cpp", + "../TinyRenderer/model.cpp", + "../TinyRenderer/tgaimage.cpp", + "../TinyRenderer/our_gl.cpp", + "../TinyRenderer/TinyRenderer.cpp", + "../OpenGLWindow/SimpleCamera.cpp", + "../OpenGLWindow/SimpleCamera.h", "../Importers/ImportURDFDemo/ConvertRigidBodies2MultiBody.h", "../Importers/ImportURDFDemo/MultiBodyCreationInterface.h", "../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp", "../Importers/ImportURDFDemo/MyMultiBodyCreator.h", "../Importers/ImportURDFDemo/BulletUrdfImporter.cpp", "../Importers/ImportURDFDemo/BulletUrdfImporter.h", - "../Importers/ImportURDFDemo/DefaultVisualShapeConverter.cpp", - "../Importers/ImportURDFDemo/DefaultVisualShapeConverter.h", "../Importers/ImportURDFDemo/UrdfParser.cpp", "../Importers/ImportURDFDemo/urdfStringSplit.cpp", "../Importers/ImportURDFDemo/UrdfParser.cpp", diff --git a/src/LinearMath/btHashMap.h b/src/LinearMath/btHashMap.h index af9727b7a..ca6f326b4 100644 --- a/src/LinearMath/btHashMap.h +++ b/src/LinearMath/btHashMap.h @@ -395,6 +395,19 @@ protected: return &m_valueArray[index]; } + Key getKeyAtIndex(int index) + { + btAssert(index < m_keyArray.size()); + return m_keyArray[index]; + } + + const Key getKeyAtIndex(int index) const + { + btAssert(index < m_keyArray.size()); + return m_keyArray[index]; + } + + Value* operator[](const Key& key) { return find(key); } diff --git a/test/InverseDynamics/premake4.lua b/test/InverseDynamics/premake4.lua index fb35a53e1..498cf94d8 100644 --- a/test/InverseDynamics/premake4.lua +++ b/test/InverseDynamics/premake4.lua @@ -73,8 +73,6 @@ "../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h", "../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp", "../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.h", - "../../examples/Importers/ImportURDFDemo/DefaultVisualShapeConverter.cpp", - "../../examples/Importers/ImportURDFDemo/DefaultVisualShapeConverter.h", "../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp", "../../examples/Importers/ImportURDFDemo/UrdfParser.cpp", "../../examples/Importers/ImportURDFDemo/UrdfParser.h", diff --git a/test/SharedMemory/premake4.lua b/test/SharedMemory/premake4.lua index 1b6d6dd2a..ef70b0139 100644 --- a/test/SharedMemory/premake4.lua +++ b/test/SharedMemory/premake4.lua @@ -66,6 +66,15 @@ project ("Test_PhysicsServerLoopBack") "../../examples/SharedMemory/Win32SharedMemory.h", "../../examples/SharedMemory/PosixSharedMemory.cpp", "../../examples/SharedMemory/PosixSharedMemory.h", + "../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp", + "../../examples/SharedMemory/TinyRendererVisualShapeConverter.h", + "../../examples/OpenGLWindow/SimpleCamera.cpp", + "../../examples/OpenGLWindow/SimpleCamera.h", + "../../examples/TinyRenderer/geometry.cpp", + "../../examples/TinyRenderer/model.cpp", + "../../examples/TinyRenderer/tgaimage.cpp", + "../../examples/TinyRenderer/our_gl.cpp", + "../../examples/TinyRenderer/TinyRenderer.cpp", "../../examples/Utils/b3ResourcePath.cpp", "../../examples/Utils/b3ResourcePath.h", "../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp", @@ -78,7 +87,6 @@ project ("Test_PhysicsServerLoopBack") "../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp", "../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp", "../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp", - "../../examples/Importers/ImportURDFDemo/DefaultVisualShapeConverter.cpp", "../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp", "../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp", "../../examples/Importers/ImportURDFDemo/UrdfParser.cpp", @@ -124,6 +132,15 @@ project ("Test_PhysicsServerLoopBack") "../../examples/SharedMemory/Win32SharedMemory.h", "../../examples/SharedMemory/PosixSharedMemory.cpp", "../../examples/SharedMemory/PosixSharedMemory.h", + "../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp", + "../../examples/SharedMemory/TinyRendererVisualShapeConverter.h", + "../../examples/TinyRenderer/geometry.cpp", + "../../examples/TinyRenderer/model.cpp", + "../../examples/TinyRenderer/tgaimage.cpp", + "../../examples/TinyRenderer/our_gl.cpp", + "../../examples/TinyRenderer/TinyRenderer.cpp", + "../../examples/OpenGLWindow/SimpleCamera.cpp", + "../../examples/OpenGLWindow/SimpleCamera.h", "../../examples/Utils/b3ResourcePath.cpp", "../../examples/Utils/b3ResourcePath.h", "../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp", @@ -136,7 +153,6 @@ project ("Test_PhysicsServerLoopBack") "../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp", "../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp", "../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp", - "../../examples/Importers/ImportURDFDemo/DefaultVisualShapeConverter.cpp", "../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp", "../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp", "../../examples/Importers/ImportURDFDemo/UrdfParser.cpp", @@ -214,6 +230,13 @@ project ("Test_PhysicsServerInProcessExampleBrowser") "../../examples/SharedMemory/Win32SharedMemory.h", "../../examples/SharedMemory/PosixSharedMemory.cpp", "../../examples/SharedMemory/PosixSharedMemory.h", + "../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp", + "../../examples/SharedMemory/TinyRendererVisualShapeConverter.h", + "../../examples/TinyRenderer/geometry.cpp", + "../../examples/TinyRenderer/model.cpp", + "../../examples/TinyRenderer/tgaimage.cpp", + "../../examples/TinyRenderer/our_gl.cpp", + "../../examples/TinyRenderer/TinyRenderer.cpp", "../../examples/Utils/b3ResourcePath.cpp", "../../examples/Utils/b3ResourcePath.h", "../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp", @@ -226,7 +249,6 @@ project ("Test_PhysicsServerInProcessExampleBrowser") "../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp", "../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp", "../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp", - "../../examples/Importers/ImportURDFDemo/DefaultVisualShapeConverter.cpp", "../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp", "../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp", "../../examples/Importers/ImportURDFDemo/UrdfParser.cpp",