From 70221aeb3e9f50068782e1dd4a6d69aa68a4d617 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 4 Feb 2015 16:56:30 -0800 Subject: [PATCH 1/2] add RenderInstancingDemo allow to use OpenGL2 or OpenGL3 in same binary (will add command-line switch) --- Demos3/AllBullet2Demos/BulletDemoEntries.h | 3 + Demos3/AllBullet2Demos/main.cpp | 239 +++---------------- Demos3/Geometry/RenderInstancingDemo.h | 131 ++++++++++ btgui/OpenGLWindow/CommonRenderInterface.h | 1 + btgui/OpenGLWindow/OpenGL2Include.h | 55 +++++ btgui/OpenGLWindow/SimpleOpenGL2App.cpp | 11 + btgui/OpenGLWindow/SimpleOpenGL2App.h | 4 + btgui/OpenGLWindow/SimpleOpenGL2Renderer.cpp | 199 +++++++++++++++ btgui/OpenGLWindow/SimpleOpenGL2Renderer.h | 81 +++++++ 9 files changed, 518 insertions(+), 206 deletions(-) create mode 100644 Demos3/Geometry/RenderInstancingDemo.h create mode 100644 btgui/OpenGLWindow/OpenGL2Include.h create mode 100644 btgui/OpenGLWindow/SimpleOpenGL2Renderer.cpp create mode 100644 btgui/OpenGLWindow/SimpleOpenGL2Renderer.h diff --git a/Demos3/AllBullet2Demos/BulletDemoEntries.h b/Demos3/AllBullet2Demos/BulletDemoEntries.h index 0eaed02d5..c053eb2f8 100644 --- a/Demos3/AllBullet2Demos/BulletDemoEntries.h +++ b/Demos3/AllBullet2Demos/BulletDemoEntries.h @@ -29,10 +29,12 @@ #include "../bullet2/CollisionDetection/SupportFuncDemo.h" #include "../bullet2/BasicConcepts/CoordinateSystemDemo.h" + #include "../../Demos3/FiniteElementMethod/FiniteElementDemo.h" //#include "../../Demos3/bullet2/SoftDemo/SoftDemo.h" #include "../Geometry/SphereCreation.h" #include "../Geometry/DistributePoints.h" +#include "../Geometry/RenderInstancingDemo.h" #define MYCREATEFUNC(func) \ static BulletDemoInterface* func##CreateFunc(CommonGraphicsApp* app)\ @@ -105,6 +107,7 @@ static BulletDemoEntry allDemos[]= {1,"SphereCreation", &SphereCreation::CreateFunc}, {1,"DistributePoints", &DistributePoints::CreateFunc}, {1,"Coordinate Frames", CoordinateFrameDemoPhysicsCreateFunc}, + {1,"Instanced Rendering", &RenderInstancingDemo::CreateFunc}, // {0,"Soft Body", 0}, // {1,"Cloth1", SoftDemo::CreateFunc}, diff --git a/Demos3/AllBullet2Demos/main.cpp b/Demos3/AllBullet2Demos/main.cpp index 4aa9e6ce6..7a641407b 100644 --- a/Demos3/AllBullet2Demos/main.cpp +++ b/Demos3/AllBullet2Demos/main.cpp @@ -1,12 +1,9 @@ -//#include "OpenGLWindow/OpenGLInclude.h" +#include "OpenGLWindow/OpenGLInclude.h" //#include "OpenGL/gl.h" //#define USE_OPENGL2 -#ifdef USE_OPENGL2 -#include "OpenGLWindow/SimpleOpenGL2App.h" -#else +#include "OpenGLWindow/SimpleOpenGL2App.h" #include "OpenGLWindow/SimpleOpenGL3App.h" -#endif #include "OpenGLWindow/CommonRenderInterface.h" #ifdef __APPLE__ @@ -36,188 +33,10 @@ #include "Bullet3AppSupport/GraphingTexture.h" #include "OpenGLWindow/SimpleCamera.h" +#include "OpenGLWindow/SimpleOpenGL2Renderer.h" CommonGraphicsApp* app=0; -#ifdef USE_OPENGL2 -struct TestRenderer : public CommonRenderInterface -{ - int m_width; - int m_height; - SimpleCamera m_camera; - TestRenderer(int width, int height) - :m_width(width), - m_height(height) - { - - } - virtual void init() - { - - } - virtual void updateCamera(int upAxis) - { - float projection[16]; - float view[16]; - m_camera.setAspectRatio((float)m_width/(float)m_height); - m_camera.update(); - m_camera.getCameraProjectionMatrix(projection); - m_camera.getCameraViewMatrix(view); - GLfloat projMat[16]; - GLfloat viewMat[16]; - for (int i=0;i<16;i++) - { - viewMat[i] = view[i]; - projMat[i] = projection[i]; - } - glMatrixMode(GL_PROJECTION); - glLoadIdentity(); - glMultMatrixf(projMat); - - glMatrixMode(GL_MODELVIEW); - glLoadIdentity(); - glMultMatrixf(viewMat); - } - virtual void removeAllInstances() - { - } - virtual void setCameraDistance(float dist) - { - m_camera.setCameraDistance(dist); - } - virtual void setCameraPitch(float pitch) - { - m_camera.setCameraPitch(pitch); - } - virtual void setCameraTargetPosition(float x, float y, float z) - { - m_camera.setCameraTargetPosition(x,y,z); - } - - virtual void getCameraPosition(float cameraPos[4]) - { - float pos[3]; - m_camera.getCameraPosition(pos); - cameraPos[0] = pos[0]; - cameraPos[1] = pos[1]; - cameraPos[2] = pos[2]; - - } - virtual void getCameraPosition(double cameraPos[4]) - { - float pos[3]; - m_camera.getCameraPosition(pos); - cameraPos[0] = pos[0]; - cameraPos[1] = pos[1]; - cameraPos[2] = pos[2]; - } - - virtual void setCameraTargetPosition(float cameraPos[4]) - { - m_camera.setCameraTargetPosition(cameraPos[0],cameraPos[1],cameraPos[2]); - } - virtual void getCameraTargetPosition(float cameraPos[4]) const - { - m_camera.getCameraTargetPosition(cameraPos); - } - virtual void getCameraTargetPosition(double cameraPos[4]) const - { - cameraPos[0] = 1; - cameraPos[1] = 1; - cameraPos[2] = 1; - } - - virtual void renderScene() - { - } - - - virtual int getScreenWidth() - { - return m_width; - } - virtual int getScreenHeight() - { - return m_height; - } - virtual int registerGraphicsInstance(int shapeIndex, const double* position, const double* quaternion, const double* color, const double* scaling) - { - return 0; - } - virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) - { - return 0; - } - virtual void drawLines(const float* positions, const float color[4], int numPoints, int pointStrideInBytes, const unsigned int* indices, int numIndices, float pointDrawSize) - { - int pointStrideInFloats = pointStrideInBytes/4; - glLineWidth(pointDrawSize); - for (int i=0;im_renderer = new TestRenderer(width,height); -#else - SimpleOpenGL3App* simpleApp = new SimpleOpenGL3App("AllBullet2Demos",width,height); - app = simpleApp; -#endif - s_instancingRenderer = app->m_renderer; + SimpleOpenGL3App* simpleApp=0; + bool useOpenGL2=false; + if (useOpenGL2) + { + app = new SimpleOpenGL2App("AllBullet2Demos",width,height); + app->m_renderer = new SimpleOpenGL2Renderer(width,height); + } else + { + simpleApp = new SimpleOpenGL3App("AllBullet2Demos",width,height); + app = simpleApp; + } + + s_instancingRenderer = app->m_renderer; s_window = app->m_window; prevMouseMoveCallback = s_window->getMouseMoveCallback(); s_window->setMouseMoveCallback(MyMouseMoveCallback); @@ -627,12 +450,16 @@ int main(int argc, char* argv[]) gui = new GwenUserInterface; GL3TexLoader* myTexLoader = new GL3TexLoader; -#ifdef USE_OPENGL2 - Gwen::Renderer::Base* gwenRenderer = new Gwen::Renderer::OpenGL_DebugFont(); -#else - sth_stash* fontstash=simpleApp->getFontStash(); - Gwen::Renderer::Base* gwenRenderer = new GwenOpenGL3CoreRenderer(simpleApp->m_primRenderer,fontstash,width,height,s_window->getRetinaScale(),myTexLoader); -#endif + + Gwen::Renderer::Base* gwenRenderer = 0; + if (useOpenGL2) + { + gwenRenderer = new Gwen::Renderer::OpenGL_DebugFont(); + } else + { + sth_stash* fontstash=simpleApp->getFontStash(); + gwenRenderer = new GwenOpenGL3CoreRenderer(simpleApp->m_primRenderer,fontstash,width,height,s_window->getRetinaScale(),myTexLoader); + } // gui->init(width,height,gwenRenderer,s_window->getRetinaScale()); @@ -822,16 +649,16 @@ int main(int argc, char* argv[]) if (!pauseSimulation) processProfileData(profWindow,false); { -#ifdef USE_OPENGL2 + if (useOpenGL2) { saveOpenGLState(width,height); } -#endif BT_PROFILE("Draw Gwen GUI"); gui->draw(s_instancingRenderer->getScreenWidth(),s_instancingRenderer->getScreenHeight()); -#ifdef USE_OPENGL2 - restoreOpenGLState(); -#endif + if (useOpenGL2) + { + restoreOpenGLState(); + } } } toggle=1-toggle; diff --git a/Demos3/Geometry/RenderInstancingDemo.h b/Demos3/Geometry/RenderInstancingDemo.h new file mode 100644 index 000000000..454d35510 --- /dev/null +++ b/Demos3/Geometry/RenderInstancingDemo.h @@ -0,0 +1,131 @@ +#ifndef RENDER_INSTANCING_DEMO_H +#define RENDER_INSTANCING_DEMO_H + +#include "Bullet3AppSupport/BulletDemoInterface.h" +#include "OpenGLWindow/CommonGraphicsApp.h" +#include "Bullet3Common/b3Quaternion.h" + + + +///quick demo showing the right-handed coordinate system and positive rotations around each axis +class RenderInstancingDemo : public BulletDemoInterface +{ + CommonGraphicsApp* m_app; + float m_x; + float m_y; + float m_z; + b3AlignedObjectArray m_movingInstances; + enum + { + numCubesX = 20, + numCubesY = 20 + }; +public: + + RenderInstancingDemo(CommonGraphicsApp* app) + :m_app(app), + m_x(0), + m_y(0), + m_z(0) + { + m_app->setUpAxis(2); + + { + b3Vector3 extents=b3MakeVector3(100,100,100); + extents[m_app->getUpAxis()]=1; + + int xres = 20; + int yres = 20; + + b3Vector4 color0=b3MakeVector4(0.1, 0.1, 0.1,1); + b3Vector4 color1=b3MakeVector4(0.6, 0.6, 0.6,1); + m_app->registerGrid(xres, yres, color0, color1); + } + + { + int boxId = m_app->registerCubeShape(0.1,0.1,0.1); + + + + for (int i=-numCubesX/2;igetUpAxis()] = 1; + b3Quaternion orn(0,0,0,1); + b3Vector4 color=b3MakeVector4(0.3,0.3,0.3,1); + b3Vector3 scaling=b3MakeVector3(1,1,1); + int instanceId = m_app->m_renderer->registerGraphicsInstance(boxId,pos,orn,color,scaling); + m_movingInstances.push_back(instanceId); + } + } + } + + m_app->m_renderer->writeTransforms(); + } + virtual ~RenderInstancingDemo() + { + m_app->m_renderer->enableBlend(false); + } + + static BulletDemoInterface* CreateFunc(CommonGraphicsApp* app) + { + return new RenderInstancingDemo(app); + } + virtual void physicsDebugDraw(int debugDrawMode) + { + + } + virtual void initPhysics() + { + } + virtual void exitPhysics() + { + + } + virtual void stepSimulation(float deltaTime) + { + m_x+=0.01f; + m_y+=0.01f; + m_z+=0.01f; + int index=0; + for (int i=-numCubesX/2;igetUpAxis()] = 1+1*b3Sin(m_x+i-j); + float orn[4]={0,0,0,1}; + m_app->m_renderer->writeSingleInstanceTransformToCPU(pos,orn,m_movingInstances[index++]); + } + } + m_app->m_renderer->writeTransforms(); + + } + virtual void renderScene() + { + m_app->m_renderer->renderScene(); + } + + + virtual void physicsDebugDraw() + { + + } + virtual bool mouseMoveCallback(float x,float y) + { + return false; + } + virtual bool mouseButtonCallback(int button, int state, float x, float y) + { + return false; + } + virtual bool keyboardCallback(int key, int state) + { + return false; + } + +}; +#endif //RENDER_INSTANCING_DEMO_H + diff --git a/btgui/OpenGLWindow/CommonRenderInterface.h b/btgui/OpenGLWindow/CommonRenderInterface.h index e786672ef..a6aa7181c 100644 --- a/btgui/OpenGLWindow/CommonRenderInterface.h +++ b/btgui/OpenGLWindow/CommonRenderInterface.h @@ -54,6 +54,7 @@ struct CommonRenderInterface virtual void writeSingleInstanceTransformToCPU(const double* position, const double* orientation, int srcIndex)=0; virtual void writeSingleInstanceColorToCPU(float* color, int srcIndex)=0; virtual void writeSingleInstanceColorToCPU(double* color, int srcIndex)=0; + virtual void writeTransforms()=0; virtual void enableBlend(bool blend)=0; }; diff --git a/btgui/OpenGLWindow/OpenGL2Include.h b/btgui/OpenGLWindow/OpenGL2Include.h new file mode 100644 index 000000000..25d48ad33 --- /dev/null +++ b/btgui/OpenGLWindow/OpenGL2Include.h @@ -0,0 +1,55 @@ +/* +Copyright (c) 2012 Advanced Micro Devices, Inc. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +//Originally written by Erwin Coumans + + +#ifndef __OPENGL_INCLUDE_H +#define __OPENGL_INCLUDE_H + + +//think different +#if defined(__APPLE__) && !defined (VMDMESA) +#include +#include +#else + +#include "GlewWindows/GL/glew.h" + +#ifdef _WINDOWS +#include +//#include +//#include +#else +//#include +//#include +#endif //_WINDOWS +#endif //APPLE + +//disable glGetError +//#undef glGetError +//#define glGetError MyGetError +// +//GLenum inline MyGetError() +//{ +// return 0; +//} + +///on Linux only glDrawElementsInstancedARB is defined?!? +//#ifdef __linux +//#define glDrawElementsInstanced glDrawElementsInstancedARB +// +//#endif //__linux + +#endif //__OPENGL_INCLUDE_H + diff --git a/btgui/OpenGLWindow/SimpleOpenGL2App.cpp b/btgui/OpenGLWindow/SimpleOpenGL2App.cpp index b6836cd7c..7730416f0 100644 --- a/btgui/OpenGLWindow/SimpleOpenGL2App.cpp +++ b/btgui/OpenGLWindow/SimpleOpenGL2App.cpp @@ -194,3 +194,14 @@ void SimpleOpenGL2App::drawText( const char* txt, int posX, int posY) { } + +void SimpleOpenGL2App::drawText3D( const char* txt, float posX, float posZY, float posZ, float size) +{ + +} + +void SimpleOpenGL2App::registerGrid(int xres, int yres, float color0[4], float color1[4]) +{ + +} + diff --git a/btgui/OpenGLWindow/SimpleOpenGL2App.h b/btgui/OpenGLWindow/SimpleOpenGL2App.h index 894ac9d8d..5393902c5 100644 --- a/btgui/OpenGLWindow/SimpleOpenGL2App.h +++ b/btgui/OpenGLWindow/SimpleOpenGL2App.h @@ -27,5 +27,9 @@ public: { return 0; } + virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size); + virtual void registerGrid(int xres, int yres, float color0[4], float color1[4]); + + }; #endif //SIMPLE_OPENGL2_APP_H \ No newline at end of file diff --git a/btgui/OpenGLWindow/SimpleOpenGL2Renderer.cpp b/btgui/OpenGLWindow/SimpleOpenGL2Renderer.cpp new file mode 100644 index 000000000..6016e57d0 --- /dev/null +++ b/btgui/OpenGLWindow/SimpleOpenGL2Renderer.cpp @@ -0,0 +1,199 @@ + +#include "SimpleOpenGL2Renderer.h" +#include "OpenGL2Include.h" +#include "Bullet3Common/b3Vector3.h" + + +SimpleOpenGL2Renderer::SimpleOpenGL2Renderer(int width, int height) + :m_width(width), + m_height(height) +{ + +} + +void SimpleOpenGL2Renderer::init() +{ +} + +void SimpleOpenGL2Renderer::updateCamera(int upAxis) +{ + float projection[16]; + float view[16]; + m_camera.setAspectRatio((float)m_width/(float)m_height); + m_camera.update(); + m_camera.getCameraProjectionMatrix(projection); + m_camera.getCameraViewMatrix(view); + GLfloat projMat[16]; + GLfloat viewMat[16]; + for (int i=0;i<16;i++) + { + viewMat[i] = view[i]; + projMat[i] = projection[i]; + } + glMatrixMode(GL_PROJECTION); + glLoadIdentity(); + glMultMatrixf(projMat); + + glMatrixMode(GL_MODELVIEW); + glLoadIdentity(); + glMultMatrixf(viewMat); +} + +void SimpleOpenGL2Renderer::removeAllInstances() +{ +} + +void SimpleOpenGL2Renderer::setCameraDistance(float dist) +{ + m_camera.setCameraDistance(dist); +} + +void SimpleOpenGL2Renderer::setCameraPitch(float pitch) +{ + m_camera.setCameraPitch(pitch); +} + +void SimpleOpenGL2Renderer::setCameraTargetPosition(float x, float y, float z) +{ + m_camera.setCameraTargetPosition(x,y,z); +} + +void SimpleOpenGL2Renderer::getCameraPosition(float cameraPos[4]) +{ + float pos[3]; + m_camera.getCameraPosition(pos); + cameraPos[0] = pos[0]; + cameraPos[1] = pos[1]; + cameraPos[2] = pos[2]; + +} + +void SimpleOpenGL2Renderer::getCameraPosition(double cameraPos[4]) +{ + float pos[3]; + m_camera.getCameraPosition(pos); + cameraPos[0] = pos[0]; + cameraPos[1] = pos[1]; + cameraPos[2] = pos[2]; +} + +void SimpleOpenGL2Renderer::setCameraTargetPosition(float cameraPos[4]) +{ + m_camera.setCameraTargetPosition(cameraPos[0],cameraPos[1],cameraPos[2]); +} + +void SimpleOpenGL2Renderer::getCameraTargetPosition(float cameraPos[4]) const +{ + m_camera.getCameraTargetPosition(cameraPos); +} + +void SimpleOpenGL2Renderer::getCameraTargetPosition(double cameraPos[4]) const +{ + cameraPos[0] = 1; + cameraPos[1] = 1; + cameraPos[2] = 1; +} + +void SimpleOpenGL2Renderer::writeSingleInstanceColorToCPU(float* color, int srcIndex) +{ +} +void SimpleOpenGL2Renderer::writeSingleInstanceColorToCPU(double* color, int srcIndex) +{ + +} +void SimpleOpenGL2Renderer::getCameraViewMatrix(float viewMat[16]) const +{ + b3Assert(0); +} +void SimpleOpenGL2Renderer::getCameraProjectionMatrix(float projMat[16]) const +{ + b3Assert(0); + +} + + +void SimpleOpenGL2Renderer::renderScene() +{ +} + + + + +int SimpleOpenGL2Renderer::registerGraphicsInstance(int shapeIndex, const double* position, const double* quaternion, const double* color, const double* scaling) +{ + return 0; +} + +int SimpleOpenGL2Renderer::registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) +{ + return 0; +} + +void SimpleOpenGL2Renderer::drawLines(const float* positions, const float color[4], int numPoints, int pointStrideInBytes, const unsigned int* indices, int numIndices, float pointDrawSize) +{ + int pointStrideInFloats = pointStrideInBytes/4; + glLineWidth(pointDrawSize); + for (int i=0;i Date: Thu, 12 Feb 2015 09:11:55 -0800 Subject: [PATCH 2/2] 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; }