update to tinyrenderer synthetic camera

This commit is contained in:
erwin coumans
2016-05-31 22:55:13 -07:00
parent 14aa666c6f
commit f469a2cb49
20 changed files with 675 additions and 115 deletions

View File

@@ -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);

View File

@@ -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",

View File

@@ -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",

View File

@@ -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);

View File

@@ -37,7 +37,7 @@ struct BulletURDFInternalData
UrdfParser m_urdfParser;
struct GUIHelperInterface* m_guiHelper;
char m_pathPrefix[1024];
btHashMap<btHashInt,btVector4> m_linkColors;
btAlignedObjectArray<btCollisionShape*> 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<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut)
{
GLInstanceGraphicsShape* glmesh = 0;
btConvexShape* convexColShape = 0;
switch (visual->m_geometry.m_type)
{
case URDF_GEOM_CYLINDER:
{
btAlignedObjectArray<btVector3> vertices;
//int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
int numSteps = 32;
for (int i = 0; i<numSteps; i++)
{
btScalar cylRadius = visual->m_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<GLInstanceGraphicsShape> visualShapes;
btAlignedObjectArray<ColladaGraphicsInstance> 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<int>();
glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
for (int i = 0; i<visualShapeInstances.size(); i++)
{
ColladaGraphicsInstance* instance = &visualShapeInstances[i];
GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex];
b3AlignedObjectArray<GLInstanceVertex> verts;
verts.resize(gfxShape->m_vertices->size());
int baseIndex = glmesh->m_vertices->size();
for (int i = 0; i<gfxShape->m_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; k<additionalIndices; k++)
{
glmesh->m_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; v<verts.size(); v++)
{
btVector3 pos(verts[v].xyzw[0], verts[v].xyzw[1], verts[v].xyzw[2]);
pos = worldMat*pos;
verts[v].xyzw[0] = float(pos[0]);
verts[v].xyzw[1] = float(pos[1]);
verts[v].xyzw[2] = float(pos[2]);
glmesh->m_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;i<glmesh->m_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<int>();
glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
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<int> 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<GLInstanceVertex> vertices;
btAlignedObjectArray<int> 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;
}

View File

@@ -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);
};

View File

@@ -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

View File

@@ -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);

View File

@@ -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

View File

@@ -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)
@@ -386,6 +390,8 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
u2b.getLinkColor(urdfLinkIndex,color);
creation.createCollisionObjectGraphicsInstance(urdfLinkIndex,col,color);
u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,col);
btScalar friction = 0.5f;
col->setFriction(friction);
@@ -398,6 +404,9 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
cache.m_bulletMultiBody->setBaseCollider(col);
}
}
} else
{
//u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,compoundShape);
}
}

View File

@@ -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;
};

View File

@@ -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)
{

View File

@@ -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",

View File

@@ -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<InternalBodyHandle> 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();

View File

@@ -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 <string>
#include "../../Utils/b3ResourcePath.h"
#include "../Utils/b3ResourcePath.h"
#include "../TinyRenderer/TinyRenderer.h"
#include "../OpenGLWindow/SimpleCamera.h"
#include <iostream>
#include <fstream>
#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<btHashInt,btVector4> m_linkColors;
btAlignedObjectArray< TinyRenderObjectData*> m_renderObjects;
};
#define START_WIDTH 640
#define START_HEIGHT 480
struct TinyRendererVisualShapeConverterInternalData
{
btHashMap<btHashPtr,TinyRendererObjectArray*> m_swRenderInstances;
int m_upAxis;
int m_swWidth;
int m_swHeight;
TGAImage m_rgbColorBuffer;
b3AlignedObjectArray<float> 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<GLInstanceVertex> vertices;
btAlignedObjectArray<int> 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;y<m_data->m_swHeight;++y)
{
for(int x=0;x<m_data->m_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;i<m_data->m_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;v<visualArray->m_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();
}

View File

@@ -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

View File

@@ -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",

View File

@@ -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);
}

View File

@@ -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",

View File

@@ -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",