Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

fix some btMultiBody URDF conversion issues in ImportURDFSetup

Browse Source 
remove various vertex format structures and use GLInstanceVertex from #include "OpenGLWindow/GLInstanceGraphicsShape.h"
btMultiBody::setupPrismatic takes an additional argument to allow a shift of inertia tensor, relative to the joint frame (link frame at q=0)
This commit is contained in:
=
2015-02-12 09:11:55 -08:00
parent 70221aeb3e
commit 27aa959059
11 changed files with 473 additions and 142 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp
View File

@@ -326,7 +326,7 @@ btMultiBody* FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMult
if (settings.m_usePrismatic)// && i==(n_links-1))
{
bod->setupPrismatic(child_link_num, mass, inertia, this_link_num,
parent_to_child, joint_axis_child_prismatic, quatRotate(parent_to_child , pos),settings.m_disableParentCollision);
parent_to_child, joint_axis_child_prismatic, quatRotate(parent_to_child , pos),btVector3(0,0,0),settings.m_disableParentCollision);
} else
{

2
Demos3/AllBullet2Demos/main.cpp
View File

@@ -193,7 +193,9 @@ void openURDFDemo(const char* filename)
ImportUrdfSetup* physicsSetup = new ImportUrdfSetup();
physicsSetup->setFileName(filename);
sCurrentDemo = new BasicDemo(app, physicsSetup);
app->setUpAxis(2);
if (sCurrentDemo)
{

52
Demos3/FiniteElementMethod/FiniteElementDemo.cpp
View File

@@ -28,6 +28,7 @@ subject to the following restrictions:
#include <OpenTissue/core/containers/t4mesh/util/t4mesh_block_generator.h>
#include "LinearMath/btAlignedObjectArray.h"
#include "Bullet3AppSupport/CommonParameterInterface.h"
#include "OpenGLWindow/GLInstanceGraphicsShape.h"
//typedef OpenTissue::math::BasicMathTypes<float,size_t> math_types;
typedef OpenTissue::math::BasicMathTypes<double,size_t> math_types;
@@ -100,12 +101,7 @@ FiniteElementDemo::~FiniteElementDemo()
m_app->m_renderer->removeAllInstances();
}
struct MyTetVertex
{
float x,y,z,w;
float nx,ny,nz;
float u,v;
};
void FiniteElementDemo::initPhysics()
{
@@ -151,19 +147,19 @@ void FiniteElementDemo::initPhysics()
int strideInBytes = 9*sizeof(float);
int numVertices =m_data->m_mesh1.m_nodes.size();
btAlignedObjectArray<MyTetVertex> verts;
btAlignedObjectArray<GLInstanceVertex> verts;
verts.resize(numVertices);
for (int n=0;n<m_data->m_mesh1.m_nodes.size();n++)
{
verts[n].x = m_data->m_mesh1.m_nodes[n].m_coord(0);
verts[n].y = m_data->m_mesh1.m_nodes[n].m_coord(1);
verts[n].z = m_data->m_mesh1.m_nodes[n].m_coord(2);
verts[n].w = 1;
verts[n].nx = 0;
verts[n].ny = 1;
verts[n].nz = 0;
verts[n].u = 0.5;
verts[n].v = 0.4;
verts[n].xyzw[0] = m_data->m_mesh1.m_nodes[n].m_coord(0);
verts[n].xyzw[1] = m_data->m_mesh1.m_nodes[n].m_coord(1);
verts[n].xyzw[2] = m_data->m_mesh1.m_nodes[n].m_coord(2);
verts[n].xyzw[3] = 1;
verts[n].normal[0] = 0;
verts[n].normal[1] = 1;
verts[n].normal[2] = 0;
verts[n].uv[0] = 0.5;
verts[n].uv[1] = 0.4;
}
btAlignedObjectArray<int> indices;
@@ -180,7 +176,7 @@ void FiniteElementDemo::initPhysics()
}
m_data->m_tetrahedralMeshRenderIndex = m_app->m_renderer->registerShape(&verts[0].x,verts.size(),&indices[0],indices.size());
m_data->m_tetrahedralMeshRenderIndex = m_app->m_renderer->registerShape(&verts[0].xyzw[0],verts.size(),&indices[0],indices.size());
float pos[4] = {0,0,0,1};
float orn[4] = {0,0,0,1};
@@ -282,19 +278,19 @@ void FiniteElementDemo::renderScene()
int strideInBytes = 9*sizeof(float);
int numVertices =m_data->m_mesh1.m_nodes.size();
btAlignedObjectArray<MyTetVertex> verts;
btAlignedObjectArray<GLInstanceVertex> verts;
verts.resize(numVertices);
for (int n=0;n<m_data->m_mesh1.m_nodes.size();n++)
{
verts[n].x = m_data->m_mesh1.m_nodes[n].m_coord(0);
verts[n].y = m_data->m_mesh1.m_nodes[n].m_coord(1);
verts[n].z = m_data->m_mesh1.m_nodes[n].m_coord(2);
verts[n].w = 1;
verts[n].nx = 0;
verts[n].ny = 1;
verts[n].nz = 0;
verts[n].u = 0.5;
verts[n].v = 0.4;
verts[n].xyzw[0] = m_data->m_mesh1.m_nodes[n].m_coord(0);
verts[n].xyzw[1] = m_data->m_mesh1.m_nodes[n].m_coord(1);
verts[n].xyzw[2] = m_data->m_mesh1.m_nodes[n].m_coord(2);
verts[n].xyzw[3] = 1;
verts[n].normal[0] = 0;
verts[n].normal[1] = 1;
verts[n].normal[2] = 0;
verts[n].uv[0] = 0.5;
verts[n].uv[1] = 0.4;
}
btAlignedObjectArray<int> indices;
@@ -311,7 +307,7 @@ void FiniteElementDemo::renderScene()
}
m_app->m_renderer->updateShape(m_data->m_tetrahedralMeshRenderIndex,&verts[0].x);
m_app->m_renderer->updateShape(m_data->m_tetrahedralMeshRenderIndex,&verts[0].xyzw[0]);
}
m_app->m_renderer->renderScene();

445
Demos3/ImportURDFDemo/ImportURDFSetup.cpp
View File

@@ -6,6 +6,7 @@
#include "../ImportColladaDemo/LoadMeshFromCollada.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "Bullet3Common/b3FileUtils.h"
#include "BulletCollision/CollisionShapes/btShapeHull.h"//to create a tesselation of a generic btConvexShape
static int bodyCollisionFilterGroup=btBroadphaseProxy::CharacterFilter;
static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphaseProxy::CharacterFilter);
@@ -154,8 +155,295 @@ enum MyFileType
FILE_COLLADA=2
};
template <typename T>
btCollisionShape* convertURDFToCollisionShape(const T* visual, const char* pathPrefix)
void convertURDFToVisualShape(const Visual* visual, const char* pathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut)
{
GLInstanceGraphicsShape* glmesh = 0;
btConvexShape* convexColShape = 0;
switch (visual->geometry->type)
{
case Geometry::CYLINDER:
{
printf("processing a cylinder\n");
urdf::Cylinder* cyl = (urdf::Cylinder*)visual->geometry.get();
btAlignedObjectArray<btVector3> vertices;
//int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
int numSteps = 32;
for (int i = 0; i<numSteps; i++)
{
btVector3 vert(cyl->radius*btSin(SIMD_2_PI*(float(i) / numSteps)), cyl->radius*btCos(SIMD_2_PI*(float(i) / numSteps)), cyl->length / 2.);
vertices.push_back(vert);
vert[2] = -cyl->length / 2.;
vertices.push_back(vert);
}
btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
cylZShape->setMargin(0.001);
convexColShape = cylZShape;
break;
}
case Geometry::BOX:
{
printf("processing a box\n");
urdf::Box* box = (urdf::Box*)visual->geometry.get();
btVector3 extents(box->dim.x, box->dim.y, box->dim.z);
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 Geometry::SPHERE:
{
printf("processing a sphere\n");
urdf::Sphere* sphere = (urdf::Sphere*)visual->geometry.get();
btScalar radius = sphere->radius;
btSphereShape* sphereShape = new btSphereShape(radius);
convexColShape = sphereShape;
convexColShape->setMargin(0.001);
break;
break;
}
case Geometry::MESH:
{
if (visual->name.length())
{
printf("visual->name=%s\n", visual->name.c_str());
}
if (visual->geometry)
{
const urdf::Mesh* mesh = (const urdf::Mesh*) visual->geometry.get();
if (mesh->filename.length())
{
const char* filename = mesh->filename.c_str();
printf("mesh->filename=%s\n", filename);
char fullPath[1024];
int fileType = 0;
sprintf(fullPath, "%s%s", pathPrefix, filename);
b3FileUtils::toLower(fullPath);
if (strstr(fullPath, ".dae"))
{
fileType = FILE_COLLADA;
}
if (strstr(fullPath, ".stl"))
{
fileType = FILE_STL;
}
sprintf(fullPath, "%s%s", pathPrefix, filename);
FILE* f = fopen(fullPath, "rb");
if (f)
{
fclose(f);
switch (fileType)
{
case FILE_STL:
{
glmesh = LoadMeshFromSTL(fullPath);
break;
}
case FILE_COLLADA:
{
btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
btTransform upAxisTrans; upAxisTrans.setIdentity();
float unitMeterScaling = 1;
LoadMeshFromCollada(fullPath,
visualShapes,
visualShapeInstances,
upAxisTrans,
unitMeterScaling);
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.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:
{
}
}
if (glmesh && (glmesh->m_numvertices>0))
{
}
else
{
printf("issue extracting mesh from COLLADA/STL file %s\n", fullPath);
}
}
else
{
printf("mesh geometry not found %s\n", fullPath);
}
}
}
break;
}
default:
{
printf("Error: unknown visual geometry type\n");
}
}
//if we have a convex, tesselate into localVertices/localIndices
if (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 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);
}
}
}
btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const char* pathPrefix)
{
btCollisionShape* shape = 0;
@@ -179,12 +467,13 @@ btCollisionShape* convertURDFToCollisionShape(const T* visual, const char* pathP
}
btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
cylZShape->initializePolyhedralFeatures();
cylZShape->setMargin(0.001);
cylZShape->initializePolyhedralFeatures();
//btConvexShape* cylZShape = new btConeShapeZ(cyl->radius,cyl->length);//(vexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
//btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
//btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
cylZShape->setMargin(0.001);
shape = cylZShape;
break;
@@ -447,37 +736,45 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
{
printf("converting visuals of link %s",link->name.c_str());
printf("converting visuals of link %s", link->name.c_str());
{
{
btCompoundShape* tmpGfxShape = new btCompoundShape();
for (int v=0;v<(int)link->visual_array.size();v++)
btAlignedObjectArray<GLInstanceVertex> vertices;
btAlignedObjectArray<int> indices;
btTransform startTrans; startTrans.setIdentity();
int graphicsIndex = -1;
for (int v = 0; v < (int)link->visual_array.size(); v++)
{
const Visual* vis = link->visual_array[v].get();
btCollisionShape* childShape = convertURDFToCollisionShape(vis,pathPrefix);
if (childShape)
btVector3 childPos(vis->origin.position.x, vis->origin.position.y, vis->origin.position.z);
btQuaternion childOrn(vis->origin.rotation.x, vis->origin.rotation.y, vis->origin.rotation.z, vis->origin.rotation.w);
btTransform childTrans;
childTrans.setOrigin(childPos);
childTrans.setRotation(childOrn);
if (1)//!mappings.m_createMultiBody)
{
btVector3 childPos(vis->origin.position.x, vis->origin.position.y, vis->origin.position.z);
btQuaternion childOrn(vis->origin.rotation.x, vis->origin.rotation.y, vis->origin.rotation.z, vis->origin.rotation.w);
btTransform childTrans;
childTrans.setOrigin(childPos);
childTrans.setRotation(childOrn);
if (!mappings.m_createMultiBody)
{
tmpGfxShape->addChildShape(childTrans*inertialFrame.inverse(),childShape);
} else
{
tmpGfxShape->addChildShape(childTrans,childShape);
}
convertURDFToVisualShape(vis, pathPrefix, childTrans*inertialFrame.inverse(), vertices, indices);
}
else
{
convertURDFToVisualShape(vis, pathPrefix, childTrans, vertices, indices);
}
}
if (vertices.size() && indices.size())
{
graphicsIndex = gfxBridge.registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size());
}
btCompoundShape* compoundShape = new btCompoundShape();
compoundShape->setMargin(0.001);
for (int v=0;v<(int)link->collision_array.size();v++)
{
const Collision* col = link->collision_array[v].get();
@@ -489,7 +786,7 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
btTransform childTrans;
childTrans.setOrigin(childPos);
childTrans.setRotation(childOrn);
if (!mappings.m_createMultiBody)
if (1)//!mappings.m_createMultiBody)
{
compoundShape->addChildShape(childTrans*inertialFrame.inverse(),childShape);
} else
@@ -533,9 +830,8 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
//rbci.m_startWorldTransform = inertialFrameInWorldSpace;//linkCenterOfMass;
btRigidBody* body = new btRigidBody(rbci);
world1->addRigidBody(body, bodyCollisionFilterGroup, bodyCollisionFilterMask);
gfxBridge.createCollisionShapeGraphicsObject(tmpGfxShape);
//hack-> transfer user inder from visual to collision shape
compoundShape->setUserIndex(tmpGfxShape->getUserIndex());
compoundShape->setUserIndex(graphicsIndex);
gfxBridge.createRigidBodyGraphicsObject(body, color);
linkInfo->m_bulletRigidBody = body;
@@ -596,10 +892,20 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
printf("Fixed joint (btMultiBody)\n");
//btVector3 dVec = quatRotate(parentComToThisCom.getRotation(),offsetInB.inverse().getOrigin());
btQuaternion rot = parent2joint.inverse().getRotation();
btQuaternion rot = offsetInA.inverse().getRotation();//parent2joint.inverse().getRotation();
//toggle=!toggle;
//mappings.m_bulletMultiBody->setupFixed(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
// rot, parent2joint.getOrigin(), btVector3(0,0,0),disableParentCollision);
mappings.m_bulletMultiBody->setupFixed(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
rot, parent2joint.getOrigin(), btVector3(0,0,0),disableParentCollision);
rot*offsetInB.getRotation(), offsetInA.getOrigin(),-offsetInB.getOrigin(),disableParentCollision);
/*
mappings.m_bulletMultiBody->setupRevolute(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
parent2joint.inverse().getRotation(), jointAxis, offsetInA.getOrigin(),//parent2joint.getOrigin(),
-offsetInB.getOrigin(),
disableParentCollision);
*/
btMatrix3x3 rm(rot);
btScalar y,p,r;
rm.getEulerZYX(y,p,r);
@@ -642,19 +948,23 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
if (mappings.m_createMultiBody)
{
//todo: adjust the center of mass transform and pivot axis properly
mappings.m_bulletMultiBody->setupRevolute(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
/*mappings.m_bulletMultiBody->setupRevolute(
linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
parent2joint.inverse().getRotation(), jointAxis, parent2joint.getOrigin(),
btVector3(0,0,0),//offsetInB.getOrigin(),
disableParentCollision);
*/
/*
mappings.m_bulletMultiBody->setupRevolute(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
parent2joint.inverse().getRotation(), jointAxis, offsetInA.getOrigin(),//parent2joint.getOrigin(),
//parent2joint.inverse().getRotation(), jointAxis, offsetInA.getOrigin(),//parent2joint.getOrigin(),
offsetInA.inverse().getRotation()*offsetInB.getRotation(), quatRotate(offsetInB.inverse().getRotation(),jointAxis), offsetInA.getOrigin(),//parent2joint.getOrigin(),
-offsetInB.getOrigin(),
disableParentCollision);
linkInfo->m_localVisualFrame.setIdentity();
*/
//linkInfo->m_localVisualFrame.setIdentity();
} else
{
//only handle principle axis at the moment,
@@ -711,24 +1021,50 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
{
if (mappings.m_createMultiBody)
{
mappings.m_bulletMultiBody->setupPrismatic(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
parent2joint.inverse().getRotation(),jointAxis,parent2joint.getOrigin(),disableParentCollision);
//mappings.m_bulletMultiBody->setupPrismatic(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
// parent2joint.inverse().getRotation(),jointAxis,parent2joint.getOrigin(),disableParentCollision);
//mappings.m_bulletMultiBody->setupPrismatic(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
// parent2joint.inverse().getRotation(),jointAxis,parent2joint.getOrigin(),disableParentCollision);
mappings.m_bulletMultiBody->setupPrismatic(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
offsetInA.inverse().getRotation()*offsetInB.getRotation(), quatRotate(offsetInB.inverse().getRotation(),jointAxis), offsetInA.getOrigin(),//parent2joint.getOrigin(),
-offsetInB.getOrigin(),
disableParentCollision);
//mappings.m_bulletMultiBody->setupRevolute(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
// parent2joint.getRotation(), jointAxis, parent2joint.getOrigin(),
// offsetInB.getOrigin(),
// disableParentCollision);
} else
{
btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*pp->m_bulletRigidBody, *linkInfo->m_bulletRigidBody, offsetInA, offsetInB);
dof6->setLinearLowerLimit(btVector3(pj->limits->lower,0,0));
dof6->setLinearUpperLimit(btVector3(pj->limits->upper,0,0));
//todo(erwincoumans) for now, we only support principle axis along X, Y or Z
btVector3 axis(pj->axis.x,pj->axis.y,pj->axis.z);
int principleAxis = axis.closestAxis();
switch (principleAxis)
{
case 0:
{
dof6->setLinearLowerLimit(btVector3(pj->limits->lower,0,0));
dof6->setLinearUpperLimit(btVector3(pj->limits->upper,0,0));
break;
}
case 1:
{
dof6->setLinearLowerLimit(btVector3(0,pj->limits->lower,0));
dof6->setLinearUpperLimit(btVector3(0,pj->limits->upper,0));
break;
}
case 2:
default:
{
dof6->setLinearLowerLimit(btVector3(0,0,pj->limits->lower));
dof6->setLinearUpperLimit(btVector3(0,0,pj->limits->upper));
}
};
dof6->setAngularLowerLimit(btVector3(0,0,0));
dof6->setAngularUpperLimit(btVector3(0,0,0));
if (enableConstraints)
world1->addConstraint(dof6,true);
@@ -753,8 +1089,8 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
//btCompoundShape* comp = new btCompoundShape();
//comp->addChildShape(linkInfo->m_localVisualFrame,shape);
gfxBridge.createCollisionShapeGraphicsObject(tmpGfxShape);
compoundShape->setUserIndex(tmpGfxShape->getUserIndex());
compoundShape->setUserIndex(graphicsIndex);
col->setCollisionShape(compoundShape);
btTransform tr;
@@ -826,6 +1162,7 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
gfxBridge.setUpAxis(2);
this->createEmptyDynamicsWorld();
//m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
btIDebugDraw::DBG_DrawConstraints
@@ -834,7 +1171,6 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
);//+btIDebugDraw::DBG_DrawConstraintLimits);
btVector3 gravity(0,0,0);
gravity[upAxis]=-9.8;
@@ -915,23 +1251,9 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
useFeatherstone = !useFeatherstone;
printf("numJoints/DOFS = %d\n", numJoints);
if (0)
bool createGround=true;
if (createGround)
{
btVector3 halfExtents(1,1,1);
btBoxShape* box = new btBoxShape(halfExtents);
box->initializePolyhedralFeatures();
gfxBridge.createCollisionShapeGraphicsObject(box);
btTransform start; start.setIdentity();
btVector3 origin(0,0,0);
origin[upAxis]=5;
start.setOrigin(origin);
btRigidBody* body = createRigidBody(1,start,box);
btVector3 color(0.5,0.5,0.5);
gfxBridge.createRigidBodyGraphicsObject(body,color);
}
{
btVector3 groundHalfExtents(20,20,20);
groundHalfExtents[upAxis]=1.f;
btBoxShape* box = new btBoxShape(groundHalfExtents);
@@ -949,6 +1271,7 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
gfxBridge.createRigidBodyGraphicsObject(body,color);
}
///this extra stepSimulation call makes sure that all the btMultibody transforms are properly propagates.
m_dynamicsWorld->stepSimulation(1. / 240., 0);// 1., 10, 1. / 240.);
}

27
Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp
View File

@@ -27,16 +27,11 @@ static float friction = 1.;
#include "OpenGLWindow/GLInstancingRenderer.h"
#include "BulletCollision/CollisionShapes/btShapeHull.h"
#include "OpenGLWindow/GLInstanceGraphicsShape.h"
#define CONSTRAINT_DEBUG_SIZE 0.2f
static bool prevCanSleep = false;
struct GraphicsVertex
{
float pos[4];
float normal[3];
float texcoord[2];
};
static btVector4 colors[4] =
{
@@ -378,7 +373,7 @@ btMultiBody* FeatherstoneDemo1::createFeatherstoneMultiBody(class btMultiBodyDyn
if (settings.m_usePrismatic)// && i==(n_links-1))
{
bod->setupPrismatic(child_link_num, mass, inertia, this_link_num,
parent_to_child, joint_axis_child_prismatic, quatRotate(parent_to_child , pos),settings.m_disableParentCollision);
parent_to_child, joint_axis_child_prismatic, quatRotate(parent_to_child , pos),btVector3(0,0,0),settings.m_disableParentCollision);
} else
{
@@ -735,22 +730,22 @@ class RagDoll2
int numVertices = hull->numVertices();
int numIndices =hull->numIndices();
btAlignedObjectArray<GraphicsVertex> gvertices;
btAlignedObjectArray<GLInstanceVertex> gvertices;
for (int i=0;i<numVertices;i++)
{
GraphicsVertex vtx;
GLInstanceVertex vtx;
btVector3 pos =hull->getVertexPointer()[i];
vtx.pos[0] = pos.x();
vtx.pos[1] = pos.y();
vtx.pos[2] = pos.z();
vtx.pos[3] = 1.f;
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.texcoord[0] = 0.5f;
vtx.texcoord[1] = 0.5f;
vtx.uv[0] = 0.5f;
vtx.uv[1] = 0.5f;
gvertices.push_back(vtx);
}
@@ -758,7 +753,7 @@ class RagDoll2
for (int i=0;i<numIndices;i++)
indices.push_back(hull->getIndexPointer()[i]);
int shapeId = m_app->m_instancingRenderer->registerShape(&gvertices[0].pos[0],numVertices,&indices[0],numIndices);
int shapeId = m_app->m_instancingRenderer->registerShape(&gvertices[0].xyzw[0],numVertices,&indices[0],numIndices);
int index = m_app->m_instancingRenderer->registerGraphicsInstance(shapeId,body->getWorldTransform().getOrigin(),body->getWorldTransform().getRotation(),color,scaling);
body ->setUserIndex(index);

38
btgui/Bullet3AppSupport/Bullet2RigidBodyDemo.cpp
View File

@@ -7,12 +7,7 @@
#include "BulletCollision/CollisionShapes/btShapeHull.h"//to create a tesselation of a generic btConvexShape
#include "MyDebugDrawer.h"
struct GraphicsVertex
{
float pos[4];
float normal[3];
float texcoord[2];
};
#include "OpenGLWindow/GLInstanceGraphicsShape.h"
struct MyGraphicsPhysicsBridge : public GraphicsPhysicsBridge
@@ -42,7 +37,18 @@ struct MyGraphicsPhysicsBridge : public GraphicsPhysicsBridge
}
}
virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape, const btTransform& parentTransform, btAlignedObjectArray<GraphicsVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut)
virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices)
{
int shapeId = m_glApp->m_renderer->registerShape(vertices, numvertices,indices,numIndices);
return shapeId;
}
virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling)
{
return m_glApp->m_renderer->registerGraphicsInstance(shapeIndex,position,quaternion,color,scaling);
}
virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape, const btTransform& parentTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut)
{
//todo: support all collision shape types
switch (collisionShape->getShapeType())
@@ -82,19 +88,19 @@ struct MyGraphicsPhysicsBridge : public GraphicsPhysicsBridge
for (int v=0;v<3;v++)
{
int index = hull->getIndexPointer()[t*3+v];
GraphicsVertex vtx;
GLInstanceVertex vtx;
btVector3 pos =parentTransform*hull->getVertexPointer()[index];
vtx.pos[0] = pos.x();
vtx.pos[1] = pos.y();
vtx.pos[2] = pos.z();
vtx.pos[3] = 0.f;
vtx.xyzw[0] = pos.x();
vtx.xyzw[1] = pos.y();
vtx.xyzw[2] = pos.z();
vtx.xyzw[3] = 0.f;
vtx.normal[0] =triNormal.x();
vtx.normal[1] =triNormal.y();
vtx.normal[2] =triNormal.z();
vtx.texcoord[0] = 0.5f;
vtx.texcoord[1] = 0.5f;
vtx.uv[0] = 0.5f;
vtx.uv[1] = 0.5f;
indicesOut.push_back(verticesOut.size());
verticesOut.push_back(vtx);
@@ -129,7 +135,7 @@ struct MyGraphicsPhysicsBridge : public GraphicsPhysicsBridge
if (collisionShape->getUserIndex()>=0)
return;
btAlignedObjectArray<GraphicsVertex> vertices;
btAlignedObjectArray<GLInstanceVertex> vertices;
btAlignedObjectArray<int> indices;
btTransform startTrans;startTrans.setIdentity();
@@ -137,7 +143,7 @@ struct MyGraphicsPhysicsBridge : public GraphicsPhysicsBridge
if (vertices.size() && indices.size())
{
int shapeId = m_glApp->m_renderer->registerShape(&vertices[0].pos[0],vertices.size(),&indices[0],indices.size());
int shapeId = m_glApp->m_renderer->registerShape(&vertices[0].xyzw[0],vertices.size(),&indices[0],indices.size());
collisionShape->setUserIndex(shapeId);
}

8
btgui/Bullet3AppSupport/CommonPhysicsSetup.h
View File

@@ -12,7 +12,7 @@ class btCollisionShape;
class btDiscreteDynamicsWorld;
///The GraphicsPhysicsBridge let's the graphics engine create graphics representation and synchronize
///The Bullet 2 GraphicsPhysicsBridge let's the graphics engine create graphics representation and synchronize
struct GraphicsPhysicsBridge
{
virtual ~GraphicsPhysicsBridge() {}
@@ -34,6 +34,11 @@ struct GraphicsPhysicsBridge
{
}
virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices) { return -1; }//, int primitiveType = B3_GL_TRIANGLES, int textureIndex = -1);
virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) { return -1;}
virtual CommonParameterInterface* getParameterInterface()
{
return 0;
@@ -45,6 +50,7 @@ struct GraphicsPhysicsBridge
};
///Bullet 2 specific physics setup, that allows to share code between old and new demo frameworks
struct CommonPhysicsSetup
{
public:

2
btgui/OpenGLWindow/SimpleOpenGL3App.cpp
View File

@@ -143,7 +143,7 @@ SimpleOpenGL3App::SimpleOpenGL3App( const char* title, int width,int height)
b3Assert(glGetError() ==GL_NO_ERROR);
m_instancingRenderer = new GLInstancingRenderer(128*1024,32*1024*1024);
m_instancingRenderer = new GLInstancingRenderer(128*1024,64*1024*1024);
m_renderer = m_instancingRenderer ;
m_instancingRenderer->init();
m_instancingRenderer->resize(width,height);

3
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@@ -167,6 +167,7 @@ void btMultiBody::setupFixed(int i,
}
void btMultiBody::setupPrismatic(int i,
btScalar mass,
const btVector3 &inertia,
@@ -174,6 +175,7 @@ void btMultiBody::setupPrismatic(int i,
const btQuaternion &rotParentToThis,
const btVector3 &jointAxis,
const btVector3 &parentComToThisComOffset,
const btVector3 &thisPivotToThisComOffset,
bool disableParentCollision)
{
if(m_isMultiDof)
@@ -189,6 +191,7 @@ void btMultiBody::setupPrismatic(int i,
m_links[i].setAxisTop(0, 0., 0., 0.);
m_links[i].setAxisBottom(0, jointAxis);
m_links[i].m_eVector = parentComToThisComOffset;
m_links[i].m_dVector = thisPivotToThisComOffset;
m_links[i].m_cachedRotParentToThis = rotParentToThis;
m_links[i].m_jointType = btMultibodyLink::ePrismatic;

18
src/BulletDynamics/Featherstone/btMultiBody.h
View File

@@ -66,15 +66,15 @@ public:
bool disableParentCollision);
void setupPrismatic(int linkIndex, // 0 to num_links-1
btScalar mass,
const btVector3 &inertia, // in my frame; assumed diagonal
int parent,
const btQuaternion &rotParentToThis, // rotate points in parent frame to my frame.
const btVector3 &jointAxis, // in my frame
const btVector3 &parentComToThisComOffset, // vector from parent COM to my COM, in my frame, when q = 0.
bool disableParentCollision=false
);
void setupPrismatic(int i,
btScalar mass,
const btVector3 &inertia,
int parent,
const btQuaternion &rotParentToThis,
const btVector3 &jointAxis,
const btVector3 &parentComToThisComOffset,
const btVector3 &thisPivotToThisComOffset,
bool disableParentCollision);
void setupRevolute(int linkIndex, // 0 to num_links-1
btScalar mass,

2
src/BulletDynamics/Featherstone/btMultiBodyLink.h
View File

@@ -493,7 +493,7 @@ struct btMultibodyLink
case ePrismatic:
{
// m_cachedRotParentToThis never changes, so no need to update
m_cachedRVector = quatRotate(m_cachedRotParentToThis,m_eVector) + pJointPos[0] * getAxisBottom(0);
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector) + pJointPos[0] * getAxisBottom(0);
break;
}