From 27aa95905902c8fc6245fdb8554cfa9b68274c25 Mon Sep 17 00:00:00 2001 From: = <=> Date: Thu, 12 Feb 2015 09:11:55 -0800 Subject: [PATCH] fix some btMultiBody URDF conversion issues in ImportURDFSetup 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) --- .../FeatherstoneMultiBodyDemo.cpp | 2 +- Demos3/AllBullet2Demos/main.cpp | 2 + .../FiniteElementMethod/FiniteElementDemo.cpp | 52 +- Demos3/ImportURDFDemo/ImportURDFSetup.cpp | 457 +++++++++++++++--- .../BulletMultiBodyDemos.cpp | 27 +- .../Bullet2RigidBodyDemo.cpp | 38 +- btgui/Bullet3AppSupport/CommonPhysicsSetup.h | 8 +- btgui/OpenGLWindow/SimpleOpenGL3App.cpp | 2 +- .../Featherstone/btMultiBody.cpp | 5 +- src/BulletDynamics/Featherstone/btMultiBody.h | 20 +- .../Featherstone/btMultiBodyLink.h | 2 +- 11 files changed, 473 insertions(+), 142 deletions(-) diff --git a/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp b/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp index 987928c20..720054914 100644 --- a/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp +++ b/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp @@ -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 { diff --git a/Demos3/AllBullet2Demos/main.cpp b/Demos3/AllBullet2Demos/main.cpp index 7a641407b..e5b8c86a7 100644 --- a/Demos3/AllBullet2Demos/main.cpp +++ b/Demos3/AllBullet2Demos/main.cpp @@ -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) { diff --git a/Demos3/FiniteElementMethod/FiniteElementDemo.cpp b/Demos3/FiniteElementMethod/FiniteElementDemo.cpp index 8612e1f08..05360ed86 100644 --- a/Demos3/FiniteElementMethod/FiniteElementDemo.cpp +++ b/Demos3/FiniteElementMethod/FiniteElementDemo.cpp @@ -28,6 +28,7 @@ subject to the following restrictions: #include #include "LinearMath/btAlignedObjectArray.h" #include "Bullet3AppSupport/CommonParameterInterface.h" +#include "OpenGLWindow/GLInstanceGraphicsShape.h" //typedef OpenTissue::math::BasicMathTypes math_types; typedef OpenTissue::math::BasicMathTypes 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 verts; + btAlignedObjectArray verts; verts.resize(numVertices); for (int n=0;nm_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 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 verts; + btAlignedObjectArray verts; verts.resize(numVertices); for (int n=0;nm_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 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(); diff --git a/Demos3/ImportURDFDemo/ImportURDFSetup.cpp b/Demos3/ImportURDFDemo/ImportURDFSetup.cpp index 964729a7f..ecd4d9125 100644 --- a/Demos3/ImportURDFDemo/ImportURDFSetup.cpp +++ b/Demos3/ImportURDFDemo/ImportURDFSetup.cpp @@ -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 -btCollisionShape* convertURDFToCollisionShape(const T* visual, const char* pathPrefix) + + +void convertURDFToVisualShape(const Visual* visual, const char* pathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& 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 vertices; + + //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float)); + int numSteps = 32; + for (int i = 0; iradius*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 visualShapes; + btAlignedObjectArray 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(); + 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.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: + { + } + } + + + 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(); + 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 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 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 vertices; + btAlignedObjectArray 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 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 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 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 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 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->setupRevolute(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1, - // parent2joint.getRotation(), jointAxis, parent2joint.getOrigin(), - // offsetInB.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); + + } 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); @@ -752,9 +1088,9 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr 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 @@ -833,8 +1170,7 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge) +btIDebugDraw::DBG_DrawAabb );//+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.); } diff --git a/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp b/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp index 61f6a97c1..f2b9b0625 100644 --- a/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp +++ b/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp @@ -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 gvertices; + btAlignedObjectArray gvertices; for (int i=0;igetVertexPointer()[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;igetIndexPointer()[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); diff --git a/btgui/Bullet3AppSupport/Bullet2RigidBodyDemo.cpp b/btgui/Bullet3AppSupport/Bullet2RigidBodyDemo.cpp index db5c98eef..a991023fe 100644 --- a/btgui/Bullet3AppSupport/Bullet2RigidBodyDemo.cpp +++ b/btgui/Bullet3AppSupport/Bullet2RigidBodyDemo.cpp @@ -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& verticesOut, btAlignedObjectArray& 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& verticesOut, btAlignedObjectArray& 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 vertices; + btAlignedObjectArray vertices; btAlignedObjectArray 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); } diff --git a/btgui/Bullet3AppSupport/CommonPhysicsSetup.h b/btgui/Bullet3AppSupport/CommonPhysicsSetup.h index 0039eb1b2..76b803d24 100644 --- a/btgui/Bullet3AppSupport/CommonPhysicsSetup.h +++ b/btgui/Bullet3AppSupport/CommonPhysicsSetup.h @@ -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: diff --git a/btgui/OpenGLWindow/SimpleOpenGL3App.cpp b/btgui/OpenGLWindow/SimpleOpenGL3App.cpp index e2ee9ab20..8e9e94f42 100644 --- a/btgui/OpenGLWindow/SimpleOpenGL3App.cpp +++ b/btgui/OpenGLWindow/SimpleOpenGL3App.cpp @@ -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); diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index dc51b0adf..3786ead2a 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -167,13 +167,15 @@ void btMultiBody::setupFixed(int i, } + void btMultiBody::setupPrismatic(int i, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &jointAxis, - const btVector3 &parentComToThisComOffset, + 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; diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index c837f00c4..2e2f645ff 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -65,16 +65,16 @@ public: const btVector3 &thisPivotToThisComOffset, 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, diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/src/BulletDynamics/Featherstone/btMultiBodyLink.h index c3cbaf9fd..90acef7d3 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyLink.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -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; }