From ed73bce9dad2cafcbe72508f8920f83394f2fd86 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sun, 5 Jan 2014 21:58:30 -0800 Subject: [PATCH] Move Bullet 2.x demos in a single demo 'AllBullet2Demos', for now only BasicDemo and FeatherstoneDemo is implemented. --- Demos3/AllBullet2Demos/BulletDemoEntries.h | 53 ++ Demos3/AllBullet2Demos/BulletDemoInterface.h | 62 ++ Demos3/AllBullet2Demos/main.cpp | 124 ++- Demos3/AllBullet2Demos/premake4.lua | 6 + Demos3/bullet2/BasicDemo/BasicDemo.cpp | 130 +++ Demos3/bullet2/BasicDemo/BasicDemo.h | 29 + Demos3/bullet2/BasicDemo/main.cpp | 455 ----------- Demos3/bullet2/BasicDemo/premake4.lua | 38 - .../BulletMultiBodyDemos.cpp | 633 +++++++++++++++ .../BulletMultiBodyDemos.h | 113 +++ .../FeatherstoneMultiBodyDemo/main.cpp | 742 ------------------ .../FeatherstoneMultiBodyDemo/premake4.lua | 38 - btgui/OpenGLWindow/GLInstancingRenderer.cpp | 17 +- btgui/OpenGLWindow/GLInstancingRenderer.h | 1 + build3/premake4.lua | 4 +- .../BasicDemo/Bullet2RigidBodyDemo.cpp | 202 +++++ .../bullet2/BasicDemo/Bullet2RigidBodyDemo.h | 43 + .../b3ConvexHullContact.cpp | 59 +- 18 files changed, 1419 insertions(+), 1330 deletions(-) create mode 100644 Demos3/AllBullet2Demos/BulletDemoEntries.h create mode 100644 Demos3/AllBullet2Demos/BulletDemoInterface.h create mode 100644 Demos3/bullet2/BasicDemo/BasicDemo.cpp create mode 100644 Demos3/bullet2/BasicDemo/BasicDemo.h delete mode 100644 Demos3/bullet2/BasicDemo/main.cpp delete mode 100644 Demos3/bullet2/BasicDemo/premake4.lua create mode 100644 Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp create mode 100644 Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.h delete mode 100644 Demos3/bullet2/FeatherstoneMultiBodyDemo/main.cpp delete mode 100644 Demos3/bullet2/FeatherstoneMultiBodyDemo/premake4.lua create mode 100644 demos3/bullet2/BasicDemo/Bullet2RigidBodyDemo.cpp create mode 100644 demos3/bullet2/BasicDemo/Bullet2RigidBodyDemo.h diff --git a/Demos3/AllBullet2Demos/BulletDemoEntries.h b/Demos3/AllBullet2Demos/BulletDemoEntries.h new file mode 100644 index 000000000..031b0bebe --- /dev/null +++ b/Demos3/AllBullet2Demos/BulletDemoEntries.h @@ -0,0 +1,53 @@ + +#ifndef BULLET_DEMO_ENTRIES_H +#define BULLET_DEMO_ENTRIES_H + +#include "BulletDemoInterface.h"" +#include "../bullet2/BasicDemo/BasicDemo.h" +#include "../bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.h" +#include + +struct BulletDemoEntry +{ + const char* m_name; + BulletDemoInterface::CreateFunc* m_createFunc; +}; + + +static BulletDemoEntry allDemos[]= +{ + + //{"emptydemo",EmptyBulletDemo::MyCreateFunc}, + {"BasicDemo",BasicDemo::MyCreateFunc}, + {"MultiBody1",FeatherstoneDemo1::MyCreateFunc}, + {"MultiBody2",FeatherstoneDemo2::MyCreateFunc}, + +}; + + +static const char* startFileName = "startDemo.txt"; + +static void saveCurrentDemoEntry(int currentEntry) +{ + FILE* f = fopen(startFileName,"w"); + if (f) + { + fprintf(f,"%d\n",currentEntry); + fclose(f); + } +}; + +static int loadCurrentDemoEntry() +{ + int currentEntry= 0; + FILE* f = fopen(startFileName,"r"); + if (f) + { + fscanf(f,"%d",¤tEntry); + fclose(f); + } + return currentEntry; +}; + +#endif//BULLET_DEMO_ENTRIES_H + diff --git a/Demos3/AllBullet2Demos/BulletDemoInterface.h b/Demos3/AllBullet2Demos/BulletDemoInterface.h new file mode 100644 index 000000000..8e69b05bd --- /dev/null +++ b/Demos3/AllBullet2Demos/BulletDemoInterface.h @@ -0,0 +1,62 @@ +#ifndef DEMO_INTERFACE_H +#define DEMO_INTERFACE_H + +struct SimpleOpenGL3App; + +class BulletDemoInterface +{ +public: + + typedef class BulletDemoInterface* (CreateFunc)(SimpleOpenGL3App* app); + + virtual ~BulletDemoInterface() + { + } + + virtual void initPhysics()=0; + virtual void exitPhysics()=0; + virtual void stepSimulation(float deltaTime)=0; + virtual void renderScene()=0; + virtual bool mouseMoveCallback(float x,float y)=0; + virtual bool mouseButtonCallback(int button, int state, float x, float y)=0; + virtual bool keyboardCallback(int key, int state)=0; + +}; + +class EmptyBulletDemo : public BulletDemoInterface +{ +public: + static BulletDemoInterface* MyCreateFunc(SimpleOpenGL3App* app) + { + return new EmptyBulletDemo(); + } + + virtual void initPhysics() + { + } + virtual void exitPhysics() + { + } + virtual void stepSimulation(float deltaTime) + { + } + virtual void renderScene() + { + } + 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 //DEMO_INTERFACE_H + diff --git a/Demos3/AllBullet2Demos/main.cpp b/Demos3/AllBullet2Demos/main.cpp index 9e5379b04..46b0e1cf1 100644 --- a/Demos3/AllBullet2Demos/main.cpp +++ b/Demos3/AllBullet2Demos/main.cpp @@ -4,13 +4,60 @@ #include #include "../GpuDemos/gwenUserInterface.h" +#include "BulletDemoEntries.h" -GwenUserInterface* gui = 0; +#define DEMO_SELECTION_COMBOBOX 13 + +static SimpleOpenGL3App* app=0; +static GwenUserInterface* gui = 0; +static int sCurrentDemoIndex = 0; +static BulletDemoInterface* sCurrentDemo = 0; +static b3AlignedObjectArray allNames; + +extern bool useShadowMap; +static bool wireframe=false; +void MyKeyboardCallback(int key, int state) +{ + + bool handled = false; + if (sCurrentDemo) + { + handled = sCurrentDemo->keyboardCallback(key,state); + } + //checkout: is it desired to ignore keys, if the demo already handles them? + //if (handled) + // return; + + if (key=='w' && state) + { + wireframe=!wireframe; + if (wireframe) + { + glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); + } else + { + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); + } + } + + if (key=='s' && state) + { + useShadowMap=!useShadowMap; + } + + if (key==B3G_ESCAPE && app && app->m_window) + { + app->m_window->setRequestExit(); + } + +} static void MyMouseMoveCallback( float x, float y) { bool handled = false; - if (gui) + if (sCurrentDemo) + handled = sCurrentDemo->mouseMoveCallback(x,y); + if (!handled && gui) handled = gui->mouseMoveCallback(x,y); if (!handled) b3DefaultMouseMoveCallback(x,y); @@ -19,33 +66,59 @@ static void MyMouseButtonCallback(int button, int state, float x, float y) { bool handled = false; //try picking first - if (gui) + if (sCurrentDemo) + handled = sCurrentDemo->mouseButtonCallback(button,state,x,y); + + if (!handled && gui) handled = gui->mouseButtonCallback(button,state,x,y); if (!handled) b3DefaultMouseButtonCallback(button,state,x,y); } +#include +void selectDemo(int demoIndex) +{ + sCurrentDemoIndex = demoIndex; + int numDemos = sizeof(allDemos)/sizeof(BulletDemoEntry); + if (demoIndex>numDemos) + demoIndex = 0; + + if (sCurrentDemo) + { + sCurrentDemo->exitPhysics(); + app->m_instancingRenderer->removeAllInstances(); + delete sCurrentDemo; + sCurrentDemo=0; + } + + if (allDemos[demoIndex].m_createFunc && app) + { + sCurrentDemo = (*allDemos[demoIndex].m_createFunc)(app); + if (sCurrentDemo) + sCurrentDemo->initPhysics(); + + } +} void MyComboBoxCallback(int comboId, const char* item) { printf("comboId = %d, item = %s\n",comboId, item); - /*int numDemos = demoNames.size(); - for (int i=0;im_instancingRenderer->setCameraDistance(13); app->m_instancingRenderer->setCameraPitch(0); app->m_instancingRenderer->setCameraTargetPosition(b3MakeVector3(0,0,0)); app->m_window->setMouseMoveCallback(MyMouseMoveCallback); app->m_window->setMouseButtonCallback(MyMouseButtonCallback); - + app->m_window->setKeyboardCallback(MyKeyboardCallback); GLint err = glGetError(); assert(err==GL_NO_ERROR); @@ -70,10 +143,18 @@ int main(int argc, char* argv[]) gui = new GwenUserInterface; gui->init(width,height,fontstash,app->m_window->getRetinaScale()); - const char* names[] = {"test1", "test2","test3"}; - gui->registerComboBox(13,3,&names[0],1); - const char* names2[] = {"comboF", "comboG","comboH"}; - gui->registerComboBox(2,3,&names2[0],1); + int numDemos = sizeof(allDemos)/sizeof(BulletDemoEntry); + + for (int i=0;iregisterComboBox(DEMO_SELECTION_COMBOBOX,allNames.size(),&allNames[0],sCurrentDemoIndex); + + //const char* names2[] = {"comboF", "comboG","comboH"}; + //gui->registerComboBox(2,3,&names2[0],0); gui->setComboBoxCallback(MyComboBoxCallback); @@ -93,6 +174,11 @@ int main(int argc, char* argv[]) sprintf(bla,"Simple test frame %d", frameCount); app->drawText(bla,10,10); + if (sCurrentDemo) + { + sCurrentDemo->stepSimulation(1./60.f); + sCurrentDemo->renderScene(); + } static int toggle = 1; if (1) @@ -103,8 +189,8 @@ int main(int argc, char* argv[]) app->swapBuffer(); } while (!app->m_window->requestedExit()); + selectDemo(0); delete gui; - delete app; return 0; } diff --git a/Demos3/AllBullet2Demos/premake4.lua b/Demos3/AllBullet2Demos/premake4.lua index da9429780..ff371d317 100644 --- a/Demos3/AllBullet2Demos/premake4.lua +++ b/Demos3/AllBullet2Demos/premake4.lua @@ -20,6 +20,12 @@ files { "**.cpp", "**.h", + "../bullet2/BasicDemo/Bullet2RigidBodyDemo.cpp", + "../bullet2/BasicDemo/Bullet2RigidBodyDemo.h", + "../bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp", + "../bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.h", + "../bullet2/BasicDemo/BasicDemo.cpp", + "../bullet2/BasicDemo/BasicDemo.h", "../../src/Bullet3Common/**.cpp", "../../src/Bullet3Common/**.h", "../../btgui/Timing/b3Clock.cpp", diff --git a/Demos3/bullet2/BasicDemo/BasicDemo.cpp b/Demos3/bullet2/BasicDemo/BasicDemo.cpp new file mode 100644 index 000000000..ce642b8fc --- /dev/null +++ b/Demos3/bullet2/BasicDemo/BasicDemo.cpp @@ -0,0 +1,130 @@ +#include "BasicDemo.h" +#include "OpenGLWindow/SimpleOpenGL3App.h" +#include "btBulletDynamicsCommon.h" +#include "Bullet3Common/b3Vector3.h" + +#define ARRAY_SIZE_X 5 +#define ARRAY_SIZE_Y 5 +#define ARRAY_SIZE_Z 5 + + +BasicDemo::BasicDemo(SimpleOpenGL3App* app) +:Bullet2RigidBodyDemo(app) +{ +} + +BasicDemo::~BasicDemo() +{ +} + +void BasicDemo::initPhysics() +{ + Bullet2RigidBodyDemo::initPhysics(); + int curColor=0; + //create ground + int cubeShapeId = m_glApp->registerCubeShape(); + float pos[]={0,0,0}; + float orn[]={0,0,0,1}; + + + { + float color[]={0.3,0.3,1,1}; + float halfExtents[]={50,50,50,1}; + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0,-50,0)); + m_glApp->m_instancingRenderer->registerGraphicsInstance(cubeShapeId,groundTransform.getOrigin(),groundTransform.getRotation(),color,halfExtents); + btBoxShape* groundShape = new btBoxShape(btVector3(btScalar(halfExtents[0]),btScalar(halfExtents[1]),btScalar(halfExtents[2]))); + //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here: + { + btScalar mass(0.); + //rigidbody is dynamic if and only if mass is non zero, otherwise static + bool isDynamic = (mass != 0.f); + btVector3 localInertia(0,0,0); + if (isDynamic) + groundShape->calculateLocalInertia(mass,localInertia); + //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia); + btRigidBody* body = new btRigidBody(rbInfo); + //add the body to the dynamics world + m_dynamicsWorld->addRigidBody(body); + } + } + + { + float halfExtents[]={1,1,1,1}; + b3Vector4 colors[4] = + { + b3MakeVector4(1,0,0,1), + b3MakeVector4(0,1,0,1), + b3MakeVector4(0,1,1,1), + b3MakeVector4(1,1,0,1), + }; + + + + btTransform startTransform; + startTransform.setIdentity(); + btScalar mass = 1.f; + btVector3 localInertia; + btBoxShape* colShape = new btBoxShape(btVector3(halfExtents[0],halfExtents[1],halfExtents[2])); + colShape ->calculateLocalInertia(mass,localInertia); + + for (int k=0;km_instancingRenderer->registerGraphicsInstance(cubeShapeId,startTransform.getOrigin(),startTransform.getRotation(),color,halfExtents); + + //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia); + btRigidBody* body = new btRigidBody(rbInfo); + + + m_dynamicsWorld->addRigidBody(body); + } + } + } + } + + m_glApp->m_instancingRenderer->writeTransforms(); +} +void BasicDemo::exitPhysics() +{ + + Bullet2RigidBodyDemo::exitPhysics(); +} +void BasicDemo::renderScene() +{ + //sync graphics -> physics world transforms + { + for (int i=0;igetNumCollisionObjects();i++) + { + btVector3 pos = m_dynamicsWorld->getCollisionObjectArray()[i]->getWorldTransform().getOrigin(); + btQuaternion orn = m_dynamicsWorld->getCollisionObjectArray()[i]->getWorldTransform().getRotation(); + m_glApp->m_instancingRenderer->writeSingleInstanceTransformToCPU(pos,orn,i); + } + m_glApp->m_instancingRenderer->writeTransforms(); + } + + m_glApp->m_instancingRenderer->renderScene(); +} + + +void BasicDemo::stepSimulation(float dt) +{ + m_dynamicsWorld->stepSimulation(dt); +} diff --git a/Demos3/bullet2/BasicDemo/BasicDemo.h b/Demos3/bullet2/BasicDemo/BasicDemo.h new file mode 100644 index 000000000..1fa00c727 --- /dev/null +++ b/Demos3/bullet2/BasicDemo/BasicDemo.h @@ -0,0 +1,29 @@ +#ifndef BASIC_DEMO_H +#define BASIC_DEMO_H + +#include "LinearMath/btVector3.h" +#include "Bullet2RigidBodyDemo.h" + + + +class BasicDemo : public Bullet2RigidBodyDemo +{ + +public: + + static BulletDemoInterface* MyCreateFunc(SimpleOpenGL3App* app) + { + return new BasicDemo(app); + } + + BasicDemo(SimpleOpenGL3App* app); + virtual ~BasicDemo(); + + void initPhysics(); + void exitPhysics(); + void renderScene(); + void stepSimulation(float dt); +}; + + +#endif //BASIC_DEMO_H diff --git a/Demos3/bullet2/BasicDemo/main.cpp b/Demos3/bullet2/BasicDemo/main.cpp deleted file mode 100644 index 3c9f77f45..000000000 --- a/Demos3/bullet2/BasicDemo/main.cpp +++ /dev/null @@ -1,455 +0,0 @@ - -#define ARRAY_SIZE_X 5 -#define ARRAY_SIZE_Y 5 -#define ARRAY_SIZE_Z 5 - - -#include "OpenGLWindow/SimpleOpenGL3App.h" -#include "Bullet3Common/b3Vector3.h" -#include "assert.h" -#include - -#include "btBulletDynamicsCommon.h" - -class Bullet2RigidBodyDemo -{ -protected: - btDiscreteDynamicsWorld* m_dynamicsWorld; - btCollisionDispatcher* m_dispatcher; - btBroadphaseInterface* m_bp; - btCollisionConfiguration* m_config; - btConstraintSolver* m_solver; - -public: - Bullet2RigidBodyDemo() - { - m_config = 0; - m_dispatcher = 0; - m_bp = 0; - m_solver = 0; - m_dynamicsWorld = 0; - } - virtual void initPhysics() - { - m_config = new btDefaultCollisionConfiguration; - m_dispatcher = new btCollisionDispatcher(m_config); - m_bp = new btDbvtBroadphase(); - m_solver = new btSequentialImpulseConstraintSolver(); - m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_bp,m_solver,m_config); - } - virtual void exitPhysics() - { - delete m_dynamicsWorld; - m_dynamicsWorld=0; - delete m_solver; - m_solver=0; - delete m_bp; - m_bp=0; - delete m_dispatcher; - m_dispatcher=0; - delete m_config; - m_config=0; - } - - virtual ~Bullet2RigidBodyDemo() - { - btAssert(m_config == 0); - btAssert(m_dispatcher == 0); - btAssert(m_bp == 0); - btAssert(m_solver == 0); - btAssert(m_dynamicsWorld == 0); - } - -}; - -class BasicDemo : public Bullet2RigidBodyDemo -{ - - btRigidBody* m_pickedBody; - btTypedConstraint* m_pickedConstraint; - btVector3 m_oldPickingPos; - btVector3 m_hitPos; - btScalar m_oldPickingDist; - - - -public: - SimpleOpenGL3App* m_glApp; - - - BasicDemo(SimpleOpenGL3App* app) - :m_glApp(app), - m_pickedBody(0), - m_pickedConstraint(0) - { - } - virtual ~BasicDemo() - { - } - - void initPhysics() - { - Bullet2RigidBodyDemo::initPhysics(); - - //create ground - int cubeShapeId = m_glApp->registerCubeShape(); - float pos[]={0,0,0}; - float orn[]={0,0,0,1}; - - - { - float color[]={0.3,0.3,1,1}; - float halfExtents[]={50,50,50,1}; - btTransform groundTransform; - groundTransform.setIdentity(); - groundTransform.setOrigin(btVector3(0,-50,0)); - m_glApp->m_instancingRenderer->registerGraphicsInstance(cubeShapeId,groundTransform.getOrigin(),groundTransform.getRotation(),color,halfExtents); - btBoxShape* groundShape = new btBoxShape(btVector3(btScalar(halfExtents[0]),btScalar(halfExtents[1]),btScalar(halfExtents[2]))); - //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here: - { - btScalar mass(0.); - //rigidbody is dynamic if and only if mass is non zero, otherwise static - bool isDynamic = (mass != 0.f); - btVector3 localInertia(0,0,0); - if (isDynamic) - groundShape->calculateLocalInertia(mass,localInertia); - //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects - btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); - btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia); - btRigidBody* body = new btRigidBody(rbInfo); - //add the body to the dynamics world - m_dynamicsWorld->addRigidBody(body); - } - } - - { - float halfExtents[]={1,1,1,1}; - b3Vector4 colors[4] = - { - b3MakeVector4(1,0,0,1), - b3MakeVector4(0,1,0,1), - b3MakeVector4(0,1,1,1), - b3MakeVector4(1,1,0,1), - }; - - - - btTransform startTransform; - startTransform.setIdentity(); - btScalar mass = 1.f; - btVector3 localInertia; - btBoxShape* colShape = new btBoxShape(btVector3(halfExtents[0],halfExtents[1],halfExtents[2])); - colShape ->calculateLocalInertia(mass,localInertia); - - for (int k=0;km_instancingRenderer->registerGraphicsInstance(cubeShapeId,startTransform.getOrigin(),startTransform.getRotation(),color,halfExtents); - - //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects - btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform); - btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia); - btRigidBody* body = new btRigidBody(rbInfo); - - - m_dynamicsWorld->addRigidBody(body); - } - } - } - } - - m_glApp->m_instancingRenderer->writeTransforms(); - } - void exitPhysics() - { - Bullet2RigidBodyDemo::exitPhysics(); - } - void drawObjects() - { - //sync graphics -> physics world transforms - { - for (int i=0;igetNumCollisionObjects();i++) - { - btVector3 pos = m_dynamicsWorld->getCollisionObjectArray()[i]->getWorldTransform().getOrigin(); - btQuaternion orn = m_dynamicsWorld->getCollisionObjectArray()[i]->getWorldTransform().getRotation(); - m_glApp->m_instancingRenderer->writeSingleInstanceTransformToCPU(pos,orn,i); - } - m_glApp->m_instancingRenderer->writeTransforms(); - } - - m_glApp->m_instancingRenderer->renderScene(); - } - - btVector3 getRayTo(int x,int y) - { - if (!m_glApp->m_instancingRenderer) - { - btAssert(0); - return btVector3(0,0,0); - } - - float top = 1.f; - float bottom = -1.f; - float nearPlane = 1.f; - float tanFov = (top-bottom)*0.5f / nearPlane; - float fov = b3Scalar(2.0) * b3Atan(tanFov); - - btVector3 camPos,camTarget; - m_glApp->m_instancingRenderer->getCameraPosition(camPos); - m_glApp->m_instancingRenderer->getCameraTargetPosition(camTarget); - - btVector3 rayFrom = camPos; - btVector3 rayForward = (camTarget-camPos); - rayForward.normalize(); - float farPlane = 10000.f; - rayForward*= farPlane; - - btVector3 rightOffset; - btVector3 m_cameraUp=btVector3(0,1,0); - btVector3 vertical = m_cameraUp; - - btVector3 hor; - hor = rayForward.cross(vertical); - hor.normalize(); - vertical = hor.cross(rayForward); - vertical.normalize(); - - float tanfov = tanf(0.5f*fov); - - - hor *= 2.f * farPlane * tanfov; - vertical *= 2.f * farPlane * tanfov; - - b3Scalar aspect; - float width = m_glApp->m_instancingRenderer->getScreenWidth(); - float height = m_glApp->m_instancingRenderer->getScreenHeight(); - - aspect = width / height; - - hor*=aspect; - - - btVector3 rayToCenter = rayFrom + rayForward; - btVector3 dHor = hor * 1.f/width; - btVector3 dVert = vertical * 1.f/height; - - - btVector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical; - rayTo += btScalar(x) * dHor; - rayTo -= btScalar(y) * dVert; - return rayTo; - } - - - bool mouseMoveCallback(float x,float y) - { -// if (m_data->m_altPressed!=0 || m_data->m_controlPressed!=0) - // return false; - - if (m_pickedBody && m_pickedConstraint) - { - btPoint2PointConstraint* pickCon = static_cast(m_pickedConstraint); - if (pickCon) - { - //keep it at the same picking distance - btVector3 newRayTo = getRayTo(x,y); - btVector3 rayFrom; - btVector3 oldPivotInB = pickCon->getPivotInB(); - btVector3 newPivotB; - m_glApp->m_instancingRenderer->getCameraPosition(rayFrom); - btVector3 dir = newRayTo-rayFrom; - dir.normalize(); - dir *= m_oldPickingDist; - - newPivotB = rayFrom + dir; - pickCon->setPivotB(newPivotB); - } - } - - return false; - } - bool mouseButtonCallback(int button, int state, float x, float y) - { - - if (state==1) - { - if(button==0)// && (m_data->m_altPressed==0 && m_data->m_controlPressed==0)) - { - btVector3 camPos; - m_glApp->m_instancingRenderer->getCameraPosition(camPos); - - btVector3 rayFrom = camPos; - btVector3 rayTo = getRayTo(x,y); - - btCollisionWorld::ClosestRayResultCallback rayCallback(rayFrom,rayTo); - m_dynamicsWorld->rayTest(rayFrom,rayTo,rayCallback); - if (rayCallback.hasHit()) - { - - btVector3 pickPos = rayCallback.m_hitPointWorld; - btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject); - if (body) - { - //other exclusions? - if (!(body->isStaticObject() || body->isKinematicObject())) - { - m_pickedBody = body; - m_pickedBody->setActivationState(DISABLE_DEACTIVATION); - //printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ()); - btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos; - btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body,localPivot); - m_dynamicsWorld->addConstraint(p2p,true); - m_pickedConstraint = p2p; - btScalar mousePickClamping = 30.f; - p2p->m_setting.m_impulseClamp = mousePickClamping; - //very weak constraint for picking - p2p->m_setting.m_tau = 0.001f; - } - } - - -// pickObject(pickPos, rayCallback.m_collisionObject); - m_oldPickingPos = rayTo; - m_hitPos = pickPos; - m_oldPickingDist = (pickPos-rayFrom).length(); -// printf("hit !\n"); - //add p2p - } - - } - } else - { - if (button==0) - { - if (m_pickedConstraint) - { - m_dynamicsWorld->removeConstraint(m_pickedConstraint); - delete m_pickedConstraint; - m_pickedConstraint=0; - m_pickedBody = 0; - } - //remove p2p - } - } - - //printf("button=%d, state=%d\n",button,state); - return false; - } - - void stepSimulation() - { - m_dynamicsWorld->stepSimulation(1./60,0); - } -}; - - - -BasicDemo* sDemo = 0; - -static void MyMouseMoveCallback( float x, float y) -{ - bool handled = false; - if (sDemo) - handled = sDemo->mouseMoveCallback(x,y); - if (!handled) - b3DefaultMouseMoveCallback(x,y); -} -static void MyMouseButtonCallback(int button, int state, float x, float y) -{ - bool handled = false; - //try picking first - if (sDemo) - handled = sDemo->mouseButtonCallback(button,state,x,y); - - if (!handled) - b3DefaultMouseButtonCallback(button,state,x,y); -} - -void MyKeyboardCallback(int key, int state) -{ - - if (key==B3G_ESCAPE && sDemo->m_glApp->m_window) - { - sDemo->m_glApp->m_window->setRequestExit(); - } - if (key=='w') - { - glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); - } - if (key=='s') - { - glPolygonMode( GL_FRONT_AND_BACK, GL_FILL); - } - -// if (sDemo) - // sDemo->keyboardCallback(key,state); - - b3DefaultKeyboardCallback(key,state); -} - - -int main(int argc, char* argv[]) -{ - - float dt = 1./120.f; -#ifdef BT_DEBUG - char* name = "Bullet 2 CPU BasicDemo (Debug build=SLOW)"; -#else - char* name = "Bullet 2 CPU BasicDemo"; -#endif - - - SimpleOpenGL3App* app = new SimpleOpenGL3App(name,1024,768); - app->m_instancingRenderer->setCameraDistance(40); - app->m_instancingRenderer->setCameraPitch(0); - app->m_instancingRenderer->setCameraTargetPosition(b3MakeVector3(0,0,0)); - - app->m_window->setMouseMoveCallback(MyMouseMoveCallback); - app->m_window->setMouseButtonCallback(MyMouseButtonCallback); - app->m_window->setKeyboardCallback(MyKeyboardCallback); - - BasicDemo* demo = new BasicDemo(app); - demo->initPhysics(); - sDemo = demo; - - GLint err = glGetError(); - assert(err==GL_NO_ERROR); - - do - { - GLint err = glGetError(); - assert(err==GL_NO_ERROR); - app->m_instancingRenderer->init(); - app->m_instancingRenderer->updateCamera(); - - demo->stepSimulation(); - demo->drawObjects(); - app->drawGrid(10,0.01); - char bla[1024]; - static int frameCount = 0; - frameCount++; - sprintf(bla,"Simulation frame %d", frameCount); - - app->drawText(bla,10,10); - app->swapBuffer(); - } while (!app->m_window->requestedExit()); - - - demo->exitPhysics(); - delete demo; - - delete app; - return 0; -} diff --git a/Demos3/bullet2/BasicDemo/premake4.lua b/Demos3/bullet2/BasicDemo/premake4.lua deleted file mode 100644 index 0007a678f..000000000 --- a/Demos3/bullet2/BasicDemo/premake4.lua +++ /dev/null @@ -1,38 +0,0 @@ - - project "App2_BasicDemo" - - language "C++" - - kind "ConsoleApp" - targetdir "../../../bin" - - includedirs { - ".", - "../../../src", - "../../../btgui" - } - - initOpenGL() - initGlew() - - links{"gwen", "BulletDynamics", "BulletCollision","LinearMath", - "OpenGL_Window", "OpenGL_TrueTypeFont" - } - - files { - "**.cpp", - "**.h", - "../../../src/Bullet3Common/**.cpp", - "../../../src/Bullet3Common/**.h", - "../../../btgui/Timing/b3Clock.cpp", - "../../../btgui/Timing/b3Clock.h" - - } - - if os.is("Linux") then - links ("X11") - end - - if os.is("MacOSX") then - links{"Cocoa.framework"} - end diff --git a/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp b/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp new file mode 100644 index 000000000..1036809f1 --- /dev/null +++ b/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp @@ -0,0 +1,633 @@ + + +#define ARRAY_SIZE_X 5 +#define ARRAY_SIZE_Y 5 +#define ARRAY_SIZE_Z 5 +static float scaling = 1.f; +static float friction = 1.; + +#include "BulletMultiBodyDemos.h" + + +#include "OpenGLWindow/SimpleOpenGL3App.h" +#include "Bullet3Common/b3Vector3.h" +#include "assert.h" +#include + +#include "btBulletDynamicsCommon.h" + +#include "BulletDynamics/Featherstone/btMultiBody.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyLink.h" +#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h" +#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h" +#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" + +static b3Vector4 colors[4] = +{ + b3MakeVector4(1,0,0,1), + b3MakeVector4(0,1,0,1), + b3MakeVector4(0,1,1,1), + b3MakeVector4(1,1,0,1), +}; + + + +Bullet2MultiBodyDemo::Bullet2MultiBodyDemo(SimpleOpenGL3App* app) +:m_glApp(app), +m_pickedBody(0), +m_pickedConstraint(0), +m_pickingMultiBodyPoint2Point(0) + +{ + m_config = 0; + m_dispatcher = 0; + m_bp = 0; + m_solver = 0; + m_dynamicsWorld = 0; +} +void Bullet2MultiBodyDemo::initPhysics() +{ + m_config = new btDefaultCollisionConfiguration; + m_dispatcher = new btCollisionDispatcher(m_config); + m_bp = new btDbvtBroadphase(); + m_solver = new btMultiBodyConstraintSolver(); + m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_dispatcher,m_bp,m_solver,m_config); +} + +void Bullet2MultiBodyDemo::exitPhysics() +{ + delete m_dynamicsWorld; + m_dynamicsWorld=0; + delete m_solver; + m_solver=0; + delete m_bp; + m_bp=0; + delete m_dispatcher; + m_dispatcher=0; + delete m_config; + m_config=0; +} + +Bullet2MultiBodyDemo::~Bullet2MultiBodyDemo() +{ + btAssert(m_config == 0); + btAssert(m_dispatcher == 0); + btAssert(m_bp == 0); + btAssert(m_solver == 0); + btAssert(m_dynamicsWorld == 0); +} + + + +btVector3 Bullet2MultiBodyDemo::getRayTo(int x,int y) +{ + if (!m_glApp->m_instancingRenderer) + { + btAssert(0); + return btVector3(0,0,0); + } + + float top = 1.f; + float bottom = -1.f; + float nearPlane = 1.f; + float tanFov = (top-bottom)*0.5f / nearPlane; + float fov = b3Scalar(2.0) * b3Atan(tanFov); + + btVector3 camPos,camTarget; + m_glApp->m_instancingRenderer->getCameraPosition(camPos); + m_glApp->m_instancingRenderer->getCameraTargetPosition(camTarget); + + btVector3 rayFrom = camPos; + btVector3 rayForward = (camTarget-camPos); + rayForward.normalize(); + float farPlane = 10000.f; + rayForward*= farPlane; + + btVector3 rightOffset; + btVector3 m_cameraUp=btVector3(0,1,0); + btVector3 vertical = m_cameraUp; + + btVector3 hor; + hor = rayForward.cross(vertical); + hor.normalize(); + vertical = hor.cross(rayForward); + vertical.normalize(); + + float tanfov = tanf(0.5f*fov); + + + hor *= 2.f * farPlane * tanfov; + vertical *= 2.f * farPlane * tanfov; + + b3Scalar aspect; + float width = m_glApp->m_instancingRenderer->getScreenWidth(); + float height = m_glApp->m_instancingRenderer->getScreenHeight(); + + aspect = width / height; + + hor*=aspect; + + + btVector3 rayToCenter = rayFrom + rayForward; + btVector3 dHor = hor * 1.f/width; + btVector3 dVert = vertical * 1.f/height; + + + btVector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical; + rayTo += btScalar(x) * dHor; + rayTo -= btScalar(y) * dVert; + return rayTo; +} + + +bool Bullet2MultiBodyDemo::mouseMoveCallback(float x,float y) +{ +// if (m_data->m_altPressed!=0 || m_data->m_controlPressed!=0) +// return false; + + if (m_pickedBody && m_pickedConstraint) + { + btPoint2PointConstraint* pickCon = static_cast(m_pickedConstraint); + if (pickCon) + { + //keep it at the same picking distance + btVector3 newRayTo = getRayTo(x,y); + btVector3 rayFrom; + btVector3 oldPivotInB = pickCon->getPivotInB(); + btVector3 newPivotB; + m_glApp->m_instancingRenderer->getCameraPosition(rayFrom); + btVector3 dir = newRayTo-rayFrom; + dir.normalize(); + dir *= m_oldPickingDist; + + newPivotB = rayFrom + dir; + pickCon->setPivotB(newPivotB); + } + } + if (m_pickingMultiBodyPoint2Point) + { + //keep it at the same picking distance + + btVector3 newRayTo = getRayTo(x,y); + btVector3 rayFrom; + btVector3 oldPivotInB = m_pickingMultiBodyPoint2Point->getPivotInB(); + btVector3 newPivotB; + btVector3 camPos; + m_glApp->m_instancingRenderer->getCameraPosition(camPos); + rayFrom = camPos; + btVector3 dir = newRayTo-rayFrom; + dir.normalize(); + dir *= m_oldPickingDist; + + newPivotB = rayFrom + dir; + + m_pickingMultiBodyPoint2Point->setPivotInB(newPivotB); + } + + return false; +} +bool Bullet2MultiBodyDemo::mouseButtonCallback(int button, int state, float x, float y) +{ + + if (state==1) + { + if(button==0)// && (m_data->m_altPressed==0 && m_data->m_controlPressed==0)) + { + btVector3 camPos; + m_glApp->m_instancingRenderer->getCameraPosition(camPos); + + btVector3 rayFrom = camPos; + btVector3 rayTo = getRayTo(x,y); + + btCollisionWorld::ClosestRayResultCallback rayCallback(rayFrom,rayTo); + m_dynamicsWorld->rayTest(rayFrom,rayTo,rayCallback); + if (rayCallback.hasHit()) + { + + btVector3 pickPos = rayCallback.m_hitPointWorld; + btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject); + if (body) + { + //other exclusions? + if (!(body->isStaticObject() || body->isKinematicObject())) + { + m_pickedBody = body; + m_pickedBody->setActivationState(DISABLE_DEACTIVATION); + //printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ()); + btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos; + btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body,localPivot); + m_dynamicsWorld->addConstraint(p2p,true); + m_pickedConstraint = p2p; + btScalar mousePickClamping = 30.f; + p2p->m_setting.m_impulseClamp = mousePickClamping; + //very weak constraint for picking + p2p->m_setting.m_tau = 0.001f; + } + } else + { + btMultiBodyLinkCollider* multiCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(rayCallback.m_collisionObject); + if (multiCol && multiCol->m_multiBody) + { + multiCol->m_multiBody->setCanSleep(false); + + btVector3 pivotInA = multiCol->m_multiBody->worldPosToLocal(multiCol->m_link, pickPos); + + btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(multiCol->m_multiBody,multiCol->m_link,0,pivotInA,pickPos); + //if you add too much energy to the system, causing high angular velocities, simulation 'explodes' + //see also http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=949 + //so we try to avoid it by clamping the maximum impulse (force) that the mouse pick can apply + //it is not satisfying, hopefully we find a better solution (higher order integrator, using joint friction using a zero-velocity target motor with limited force etc?) + + p2p->setMaxAppliedImpulse(20*scaling); + + btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_dynamicsWorld; + world->addMultiBodyConstraint(p2p); + m_pickingMultiBodyPoint2Point =p2p; + } + } + + +// pickObject(pickPos, rayCallback.m_collisionObject); + m_oldPickingPos = rayTo; + m_hitPos = pickPos; + m_oldPickingDist = (pickPos-rayFrom).length(); +// printf("hit !\n"); + //add p2p + } + + } + } else + { + if (button==0) + { + if (m_pickedConstraint) + { + m_dynamicsWorld->removeConstraint(m_pickedConstraint); + delete m_pickedConstraint; + m_pickedConstraint=0; + m_pickedBody = 0; + } + + if (m_pickingMultiBodyPoint2Point) + { + m_pickingMultiBodyPoint2Point->getMultiBodyA()->setCanSleep(true); + btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_dynamicsWorld; + world->removeMultiBodyConstraint(m_pickingMultiBodyPoint2Point); + delete m_pickingMultiBodyPoint2Point; + m_pickingMultiBodyPoint2Point = 0; + } + //remove p2p + } + } + + //printf("button=%d, state=%d\n",button,state); + return false; +} + + + + + + + + + + + + + +FeatherstoneDemo1::FeatherstoneDemo1(SimpleOpenGL3App* app) +:Bullet2MultiBodyDemo(app) +{ +} +FeatherstoneDemo1::~FeatherstoneDemo1() +{ +} + + + +btMultiBody* FeatherstoneDemo1::createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, const btMultiBodySettings& settings) +{ + int curColor=0; + + + int cubeShapeId = m_glApp->registerCubeShape(); + + int n_links = settings.m_numLinks; + float mass = 13.5*scaling; + btVector3 inertia = btVector3 (91,344,253)*scaling*scaling; + + + btMultiBody * bod = new btMultiBody(n_links, mass, inertia, settings.m_isFixedBase, settings.m_canSleep); +// bod->setHasSelfCollision(false); + + //btQuaternion orn(btVector3(0,0,1),-0.25*SIMD_HALF_PI);//0,0,0,1); + btQuaternion orn(0,0,0,1); + bod->setBasePos(settings.m_basePosition); + bod->setWorldToBaseRot(orn); + btVector3 vel(0,0,0); + bod->setBaseVel(vel); + + { + + btVector3 joint_axis_hinge(1,0,0); + btVector3 joint_axis_prismatic(0,0,1); + btQuaternion parent_to_child = orn.inverse(); + btVector3 joint_axis_child_prismatic = quatRotate(parent_to_child ,joint_axis_prismatic); + btVector3 joint_axis_child_hinge = quatRotate(parent_to_child , joint_axis_hinge); + + int this_link_num = -1; + int link_num_counter = 0; + + + + btVector3 pos = btVector3 (0,0,9.0500002)*scaling; + + btVector3 joint_axis_position = btVector3 (0,0,4.5250001)*scaling; + + for (int i=0;i0) + initial_joint_angle = -0.06f; + + const int child_link_num = link_num_counter++; + + + + 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); + + } else + { + bod->setupRevolute(child_link_num, mass, inertia, this_link_num,parent_to_child, joint_axis_child_hinge, + joint_axis_position,quatRotate(parent_to_child , (pos - joint_axis_position)),settings.m_disableParentCollision); + } + bod->setJointPos(child_link_num, initial_joint_angle); + this_link_num = i; + + if (0)//!useGroundShape && i==4) + { + btVector3 pivotInAworld(0,20,46); + btVector3 pivotInAlocal = bod->worldPosToLocal(i, pivotInAworld); + btVector3 pivotInBworld = pivotInAworld; + btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(bod,i,&btTypedConstraint::getFixedBody(),pivotInAlocal,pivotInBworld); + world->addMultiBodyConstraint(p2p); + } + //add some constraint limit + if (settings.m_usePrismatic) + { + // btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(bod,n_links-1,2,3); + + if (settings.m_createConstraints) + { + btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(bod,i,-1,1); + world->addMultiBodyConstraint(con); + } + + } else + { + if (settings.m_createConstraints) + { + if (1) + { + btMultiBodyJointMotor* con = new btMultiBodyJointMotor(bod,i,0,500000); + world->addMultiBodyConstraint(con); + } + + btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(bod,i,-1,1); + world->addMultiBodyConstraint(con); + } + + } + } + } + + //add a collider for the base + { + + btAlignedObjectArray world_to_local; + world_to_local.resize(n_links+1); + + btAlignedObjectArray local_origin; + local_origin.resize(n_links+1); + world_to_local[0] = bod->getWorldToBaseRot(); + local_origin[0] = bod->getBasePos(); + //float halfExtents[3]={7.5,0.05,4.5}; + float halfExtents[3]={7.5,0.45,4.5}; + { + + float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1}; + float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()}; + + + if (1) + { + btCollisionShape* box = new btBoxShape(btVector3(halfExtents[0],halfExtents[1],halfExtents[2])*scaling); + btRigidBody* body = new btRigidBody(mass,0,box,inertia); + btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(bod,-1); + + + + + body->setCollisionShape(box); + col->setCollisionShape(box); + + btTransform tr; + tr.setIdentity(); + tr.setOrigin(local_origin[0]); + tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); + body->setWorldTransform(tr); + col->setWorldTransform(tr); + + b3Vector4 color = colors[curColor++]; + curColor&=3; + + int index = m_glApp->m_instancingRenderer->registerGraphicsInstance(cubeShapeId,tr.getOrigin(),tr.getRotation(),color,halfExtents); + col->setUserIndex(index); + + + + + world->addCollisionObject(col,short(btBroadphaseProxy::DefaultFilter),short(btBroadphaseProxy::AllFilter)); + col->setFriction(friction); + bod->setBaseCollider(col); + + } + } + + + for (int i=0;igetNumLinks();i++) + { + const int parent = bod->getParent(i); + world_to_local[i+1] = bod->getParentToLocalRot(i) * world_to_local[parent+1]; + local_origin[i+1] = local_origin[parent+1] + (quatRotate(world_to_local[i+1].inverse() , bod->getRVector(i))); + } + + + for (int i=0;igetNumLinks();i++) + { + + btVector3 posr = local_origin[i+1]; + float pos[4]={posr.x(),posr.y(),posr.z(),1}; + + float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; + + btCollisionShape* box = new btBoxShape(btVector3(halfExtents[0],halfExtents[1],halfExtents[2])*scaling); + btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(bod,i); + + col->setCollisionShape(box); + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); + col->setWorldTransform(tr); + col->setFriction(friction); + + + b3Vector4 color = colors[curColor++]; + curColor&=3; + + int index = m_glApp->m_instancingRenderer->registerGraphicsInstance(cubeShapeId,tr.getOrigin(),tr.getRotation(),color,halfExtents); + col->setUserIndex(index); + + + + world->addCollisionObject(col,short(btBroadphaseProxy::DefaultFilter),short(btBroadphaseProxy::AllFilter)); + + bod->getLink(i).m_collider=col; + //app->drawBox(halfExtents, pos,quat); + } + + } + world->addMultiBody(bod); + + return bod; +} + +void FeatherstoneDemo1::addColliders_testMultiDof(btMultiBody *pMultiBody, btMultiBodyDynamicsWorld *pWorld, const btVector3 &baseHalfExtents, const btVector3 &linkHalfExtents) +{ +} +void FeatherstoneDemo1::addBoxes_testMultiDof() +{ +} + +void FeatherstoneDemo1::createGround() +{ + //create ground + int cubeShapeId = m_glApp->registerCubeShape(); + float pos[]={0,0,0}; + float orn[]={0,0,0,1}; + + + { + float color[]={0.3,0.3,1,1}; + float halfExtents[]={50,50,50,1}; + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0,-50,0)); + btBoxShape* groundShape = new btBoxShape(btVector3(btScalar(halfExtents[0]),btScalar(halfExtents[1]),btScalar(halfExtents[2]))); + //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here: + { + btScalar mass(0.); + //rigidbody is dynamic if and only if mass is non zero, otherwise static + bool isDynamic = (mass != 0.f); + btVector3 localInertia(0,0,0); + if (isDynamic) + groundShape->calculateLocalInertia(mass,localInertia); + //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia); + btRigidBody* body = new btRigidBody(rbInfo); + + int index = m_glApp->m_instancingRenderer->registerGraphicsInstance(cubeShapeId,groundTransform.getOrigin(),groundTransform.getRotation(),color,halfExtents); + body ->setUserIndex(index); + + //add the body to the dynamics world + m_dynamicsWorld->addRigidBody(body); + } + } +} + +void FeatherstoneDemo1::initPhysics() +{ + + Bullet2MultiBodyDemo::initPhysics(); + + createGround(); + + btMultiBodySettings settings; + settings.m_isFixedBase = false; + settings.m_basePosition.setValue(0,10,0); + settings.m_numLinks = 10; + btMultiBody* mb = createFeatherstoneMultiBody(m_dynamicsWorld,settings); + + + m_glApp->m_instancingRenderer->writeTransforms(); +} + + + +void FeatherstoneDemo1::exitPhysics() +{ + Bullet2MultiBodyDemo::exitPhysics(); +} + +void FeatherstoneDemo1::renderScene() +{ + //sync graphics -> physics world transforms + { + for (int i=0;igetNumCollisionObjects();i++) + { + btCollisionObject* col = m_dynamicsWorld->getCollisionObjectArray()[i]; + + btVector3 pos = col->getWorldTransform().getOrigin(); + btQuaternion orn = col->getWorldTransform().getRotation(); + int index = col->getUserIndex(); + m_glApp->m_instancingRenderer->writeSingleInstanceTransformToCPU(pos,orn,index); + } + m_glApp->m_instancingRenderer->writeTransforms(); + } + + m_glApp->m_instancingRenderer->renderScene(); +} + + +void FeatherstoneDemo1::stepSimulation(float deltaTime) +{ + m_dynamicsWorld->stepSimulation(deltaTime,0); +// CProfileManager::dumpAll(); +} + + + +FeatherstoneDemo2::FeatherstoneDemo2(SimpleOpenGL3App* app) +:FeatherstoneDemo1(app) +{ +} + +FeatherstoneDemo2::~FeatherstoneDemo2() +{ +} + +void FeatherstoneDemo2::initPhysics() +{ + Bullet2MultiBodyDemo::initPhysics(); + + createGround(); + + btMultiBodySettings settings; + settings.m_isFixedBase = false; + settings.m_basePosition.setValue(0,20,0); + settings.m_numLinks = 3; + settings.m_usePrismatic = true; + btMultiBody* mb = createFeatherstoneMultiBody(m_dynamicsWorld,settings); + + + m_glApp->m_instancingRenderer->writeTransforms(); +} \ No newline at end of file diff --git a/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.h b/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.h new file mode 100644 index 000000000..2a9b230b7 --- /dev/null +++ b/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.h @@ -0,0 +1,113 @@ + +#ifndef BULLET_MULTI_BODY_DEMOS_H +#define BULLET_MULTI_BODY_DEMOS_H + +#include "LinearMath/btVector3.h" + +#include "../../AllBullet2Demos\BulletDemoInterface.h" + + +struct btMultiBodySettings +{ + btMultiBodySettings() + { + m_numLinks = 0; + m_basePosition.setZero(); + m_isFixedBase = true; + m_usePrismatic = false; + m_canSleep = true; + m_createConstraints = false; + m_disableParentCollision = false; + } + int m_numLinks; + btVector3 m_basePosition; + bool m_isFixedBase; + bool m_usePrismatic; + bool m_canSleep; + bool m_createConstraints; + bool m_disableParentCollision; +}; + +class Bullet2MultiBodyDemo : public BulletDemoInterface +{ +protected: + + SimpleOpenGL3App* m_glApp; + + class btRigidBody* m_pickedBody; + class btTypedConstraint* m_pickedConstraint; + btVector3 m_oldPickingPos; + btVector3 m_hitPos; + btScalar m_oldPickingDist; + + class btMultiBodyPoint2Point* m_pickingMultiBodyPoint2Point; + + class btMultiBodyDynamicsWorld* m_dynamicsWorld; + class btCollisionDispatcher* m_dispatcher; + class btBroadphaseInterface* m_bp; + class btCollisionConfiguration* m_config; + class btMultiBodyConstraintSolver* m_solver; + + //btAlignedObjectArray m_linkColliders; + +public: + Bullet2MultiBodyDemo(SimpleOpenGL3App* app); + virtual void initPhysics(); + virtual void exitPhysics(); + virtual ~Bullet2MultiBodyDemo(); + btVector3 getRayTo(int x,int y); + virtual bool mouseMoveCallback(float x,float y); + virtual bool mouseButtonCallback(int button, int state, float x, float y); + virtual bool keyboardCallback(int key, int state) + { + return false; + } +}; + +class FeatherstoneDemo1 : public Bullet2MultiBodyDemo +{ + +public: + + FeatherstoneDemo1(SimpleOpenGL3App* app); + virtual ~FeatherstoneDemo1(); + + + static BulletDemoInterface* MyCreateFunc(SimpleOpenGL3App* app) + { + return new FeatherstoneDemo1(app); + } + + class btMultiBody* createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, const btMultiBodySettings& settings); + + void addColliders_testMultiDof(btMultiBody *pMultiBody, btMultiBodyDynamicsWorld *pWorld, const btVector3 &baseHalfExtents, const btVector3 &linkHalfExtents); + void addBoxes_testMultiDof(); + + void createGround(); + virtual void initPhysics(); + virtual void exitPhysics(); + virtual void renderScene(); + virtual void stepSimulation(float deltaTime); +}; + + +class FeatherstoneDemo2 : public FeatherstoneDemo1 +{ + +public: + + FeatherstoneDemo2(SimpleOpenGL3App* app); + virtual ~FeatherstoneDemo2(); + + + static BulletDemoInterface* MyCreateFunc(SimpleOpenGL3App* app) + { + return new FeatherstoneDemo2(app); + } + + virtual void initPhysics(); +}; + + +#endif //BULLET_MULTI_BODY_DEMOS_H + diff --git a/Demos3/bullet2/FeatherstoneMultiBodyDemo/main.cpp b/Demos3/bullet2/FeatherstoneMultiBodyDemo/main.cpp deleted file mode 100644 index 685603917..000000000 --- a/Demos3/bullet2/FeatherstoneMultiBodyDemo/main.cpp +++ /dev/null @@ -1,742 +0,0 @@ - -#define ARRAY_SIZE_X 5 -#define ARRAY_SIZE_Y 5 -#define ARRAY_SIZE_Z 5 -float scaling = 1.f; -float friction = 1.; - - -#include "OpenGLWindow/SimpleOpenGL3App.h" -#include "Bullet3Common/b3Vector3.h" -#include "assert.h" -#include - -#include "btBulletDynamicsCommon.h" - -#include "BulletDynamics/Featherstone/btMultiBody.h" -#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" -#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" -#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" -#include "BulletDynamics/Featherstone/btMultiBodyLink.h" -#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h" -#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h" -#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" - -static b3Vector4 colors[4] = -{ - b3MakeVector4(1,0,0,1), - b3MakeVector4(0,1,0,1), - b3MakeVector4(0,1,1,1), - b3MakeVector4(1,1,0,1), -}; - - -struct btMultiBodySettings -{ - btMultiBodySettings() - { - m_numLinks = 0; - m_basePosition.setZero(); - m_isFixedBase = true; - m_usePrismatic = false; - m_canSleep = true; - m_createConstraints = false; - m_disableParentCollision = false; - } - int m_numLinks; - btVector3 m_basePosition; - bool m_isFixedBase; - bool m_usePrismatic; - bool m_canSleep; - bool m_createConstraints; - bool m_disableParentCollision; -}; - -class Bullet2MultiBodyDemo -{ -protected: - btMultiBodyDynamicsWorld* m_dynamicsWorld; - btCollisionDispatcher* m_dispatcher; - btBroadphaseInterface* m_bp; - btCollisionConfiguration* m_config; - btMultiBodyConstraintSolver* m_solver; - -public: - Bullet2MultiBodyDemo() - { - m_config = 0; - m_dispatcher = 0; - m_bp = 0; - m_solver = 0; - m_dynamicsWorld = 0; - } - virtual void initPhysics() - { - m_config = new btDefaultCollisionConfiguration; - m_dispatcher = new btCollisionDispatcher(m_config); - m_bp = new btDbvtBroadphase(); - m_solver = new btMultiBodyConstraintSolver(); - m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_dispatcher,m_bp,m_solver,m_config); - } - virtual void exitPhysics() - { - delete m_dynamicsWorld; - m_dynamicsWorld=0; - delete m_solver; - m_solver=0; - delete m_bp; - m_bp=0; - delete m_dispatcher; - m_dispatcher=0; - delete m_config; - m_config=0; - } - - virtual ~Bullet2MultiBodyDemo() - { - btAssert(m_config == 0); - btAssert(m_dispatcher == 0); - btAssert(m_bp == 0); - btAssert(m_solver == 0); - btAssert(m_dynamicsWorld == 0); - } - -}; - -class BasicDemo : public Bullet2MultiBodyDemo -{ - SimpleOpenGL3App* m_glApp; - - btRigidBody* m_pickedBody; - btTypedConstraint* m_pickedConstraint; - btVector3 m_oldPickingPos; - btVector3 m_hitPos; - btScalar m_oldPickingDist; - - class btMultiBodyPoint2Point* m_pickingMultiBodyPoint2Point; - btAlignedObjectArray m_linkColliders; - -public: - BasicDemo(SimpleOpenGL3App* app) - :m_glApp(app), - m_pickedBody(0), - m_pickedConstraint(0), - m_pickingMultiBodyPoint2Point(0) - { - } - virtual ~BasicDemo() - { - } - - btMultiBody* createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, const btMultiBodySettings& settings) - { - static int curColor=0; - - - int cubeShapeId = m_glApp->registerCubeShape(); - - int n_links = settings.m_numLinks; - float mass = 13.5*scaling; - btVector3 inertia = btVector3 (91,344,253)*scaling*scaling; - - - btMultiBody * bod = new btMultiBody(n_links, mass, inertia, settings.m_isFixedBase, settings.m_canSleep); - // bod->setHasSelfCollision(false); - - //btQuaternion orn(btVector3(0,0,1),-0.25*SIMD_HALF_PI);//0,0,0,1); - btQuaternion orn(0,0,0,1); - bod->setBasePos(settings.m_basePosition); - bod->setWorldToBaseRot(orn); - btVector3 vel(0,0,0); - bod->setBaseVel(vel); - - { - - btVector3 joint_axis_hinge(1,0,0); - btVector3 joint_axis_prismatic(0,0,1); - btQuaternion parent_to_child = orn.inverse(); - btVector3 joint_axis_child_prismatic = quatRotate(parent_to_child ,joint_axis_prismatic); - btVector3 joint_axis_child_hinge = quatRotate(parent_to_child , joint_axis_hinge); - - int this_link_num = -1; - int link_num_counter = 0; - - - - btVector3 pos = btVector3 (0,0,9.0500002)*scaling; - - btVector3 joint_axis_position = btVector3 (0,0,4.5250001)*scaling; - - for (int i=0;i0) - initial_joint_angle = -0.06f; - - const int child_link_num = link_num_counter++; - - - - 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); - - } else - { - bod->setupRevolute(child_link_num, mass, inertia, this_link_num,parent_to_child, joint_axis_child_hinge, - joint_axis_position,quatRotate(parent_to_child , (pos - joint_axis_position)),settings.m_disableParentCollision); - } - bod->setJointPos(child_link_num, initial_joint_angle); - this_link_num = i; - - if (0)//!useGroundShape && i==4) - { - btVector3 pivotInAworld(0,20,46); - btVector3 pivotInAlocal = bod->worldPosToLocal(i, pivotInAworld); - btVector3 pivotInBworld = pivotInAworld; - btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(bod,i,&btTypedConstraint::getFixedBody(),pivotInAlocal,pivotInBworld); - world->addMultiBodyConstraint(p2p); - } - //add some constraint limit - if (settings.m_usePrismatic) - { - // btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(bod,n_links-1,2,3); - - if (settings.m_createConstraints) - { - btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(bod,i,-1,1); - world->addMultiBodyConstraint(con); - } - - } else - { - if (settings.m_createConstraints) - { - if (1) - { - btMultiBodyJointMotor* con = new btMultiBodyJointMotor(bod,i,0,500000); - world->addMultiBodyConstraint(con); - } - - btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(bod,i,-1,1); - world->addMultiBodyConstraint(con); - } - - } - } - } - - //add a collider for the base - { - - btAlignedObjectArray world_to_local; - world_to_local.resize(n_links+1); - - btAlignedObjectArray local_origin; - local_origin.resize(n_links+1); - world_to_local[0] = bod->getWorldToBaseRot(); - local_origin[0] = bod->getBasePos(); - //float halfExtents[3]={7.5,0.05,4.5}; - float halfExtents[3]={7.5,0.45,4.5}; - { - - float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1}; - float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()}; - - - if (1) - { - btCollisionShape* box = new btBoxShape(btVector3(halfExtents[0],halfExtents[1],halfExtents[2])*scaling); - btRigidBody* body = new btRigidBody(mass,0,box,inertia); - btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(bod,-1); - - - - - body->setCollisionShape(box); - col->setCollisionShape(box); - - btTransform tr; - tr.setIdentity(); - tr.setOrigin(local_origin[0]); - tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); - body->setWorldTransform(tr); - col->setWorldTransform(tr); - - b3Vector4 color = colors[curColor++]; - curColor&=3; - - int index = m_glApp->m_instancingRenderer->registerGraphicsInstance(cubeShapeId,tr.getOrigin(),tr.getRotation(),color,halfExtents); - col->setUserIndex(index); - - - - - world->addCollisionObject(col,short(btBroadphaseProxy::DefaultFilter),short(btBroadphaseProxy::AllFilter)); - col->setFriction(friction); - bod->setBaseCollider(col); - - } - } - - - for (int i=0;igetNumLinks();i++) - { - const int parent = bod->getParent(i); - world_to_local[i+1] = bod->getParentToLocalRot(i) * world_to_local[parent+1]; - local_origin[i+1] = local_origin[parent+1] + (quatRotate(world_to_local[i+1].inverse() , bod->getRVector(i))); - } - - - for (int i=0;igetNumLinks();i++) - { - - btVector3 posr = local_origin[i+1]; - float pos[4]={posr.x(),posr.y(),posr.z(),1}; - - float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; - - btCollisionShape* box = new btBoxShape(btVector3(halfExtents[0],halfExtents[1],halfExtents[2])*scaling); - btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(bod,i); - - col->setCollisionShape(box); - btTransform tr; - tr.setIdentity(); - tr.setOrigin(posr); - tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); - col->setWorldTransform(tr); - col->setFriction(friction); - - - b3Vector4 color = colors[curColor++]; - curColor&=3; - - int index = m_glApp->m_instancingRenderer->registerGraphicsInstance(cubeShapeId,tr.getOrigin(),tr.getRotation(),color,halfExtents); - col->setUserIndex(index); - - - - world->addCollisionObject(col,short(btBroadphaseProxy::DefaultFilter),short(btBroadphaseProxy::AllFilter)); - - bod->getLink(i).m_collider=col; - //app->drawBox(halfExtents, pos,quat); - } - - } - world->addMultiBody(bod); - - return bod; - } - - void addColliders_testMultiDof(btMultiBody *pMultiBody, btMultiBodyDynamicsWorld *pWorld, const btVector3 &baseHalfExtents, const btVector3 &linkHalfExtents) - { - } - void addBoxes_testMultiDof() - { - } - - void initPhysics() - { - - Bullet2MultiBodyDemo::initPhysics(); - - //create ground - int cubeShapeId = m_glApp->registerCubeShape(); - float pos[]={0,0,0}; - float orn[]={0,0,0,1}; - - - { - float color[]={0.3,0.3,1,1}; - float halfExtents[]={50,50,50,1}; - btTransform groundTransform; - groundTransform.setIdentity(); - groundTransform.setOrigin(btVector3(0,-50,0)); - btBoxShape* groundShape = new btBoxShape(btVector3(btScalar(halfExtents[0]),btScalar(halfExtents[1]),btScalar(halfExtents[2]))); - //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here: - { - btScalar mass(0.); - //rigidbody is dynamic if and only if mass is non zero, otherwise static - bool isDynamic = (mass != 0.f); - btVector3 localInertia(0,0,0); - if (isDynamic) - groundShape->calculateLocalInertia(mass,localInertia); - //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects - btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); - btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia); - btRigidBody* body = new btRigidBody(rbInfo); - - int index = m_glApp->m_instancingRenderer->registerGraphicsInstance(cubeShapeId,groundTransform.getOrigin(),groundTransform.getRotation(),color,halfExtents); - body ->setUserIndex(index); - - //add the body to the dynamics world - m_dynamicsWorld->addRigidBody(body); - } - } -#if 0 - - { - float halfExtents[]={1,1,1,1}; - - - - btTransform startTransform; - startTransform.setIdentity(); - btScalar mass = 1.f; - btVector3 localInertia; - btBoxShape* colShape = new btBoxShape(btVector3(halfExtents[0],halfExtents[1],halfExtents[2])); - colShape ->calculateLocalInertia(mass,localInertia); - - for (int k=0;km_instancingRenderer->registerGraphicsInstance(cubeShapeId,startTransform.getOrigin(),startTransform.getRotation(),color,halfExtents); - - //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects - btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform); - btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia); - btRigidBody* body = new btRigidBody(rbInfo); - body->setUserIndex(index); - - m_dynamicsWorld->addRigidBody(body); - } - } - } - } -#endif - btMultiBodySettings settings; - settings.m_isFixedBase = false; - settings.m_basePosition.setValue(0,10,0); - settings.m_numLinks = 10; - btMultiBody* mb = createFeatherstoneMultiBody(m_dynamicsWorld,settings); - - m_glApp->m_instancingRenderer->writeTransforms(); - } - - - - - - void exitPhysics() - { - Bullet2MultiBodyDemo::exitPhysics(); - } - void drawObjects() - { - //sync graphics -> physics world transforms - { - for (int i=0;igetNumCollisionObjects();i++) - { - btCollisionObject* col = m_dynamicsWorld->getCollisionObjectArray()[i]; - - btVector3 pos = col->getWorldTransform().getOrigin(); - btQuaternion orn = col->getWorldTransform().getRotation(); - int index = col->getUserIndex(); - m_glApp->m_instancingRenderer->writeSingleInstanceTransformToCPU(pos,orn,index); - } - m_glApp->m_instancingRenderer->writeTransforms(); - } - - m_glApp->m_instancingRenderer->renderScene(); - } - - btVector3 getRayTo(int x,int y) - { - if (!m_glApp->m_instancingRenderer) - { - btAssert(0); - return btVector3(0,0,0); - } - - float top = 1.f; - float bottom = -1.f; - float nearPlane = 1.f; - float tanFov = (top-bottom)*0.5f / nearPlane; - float fov = b3Scalar(2.0) * b3Atan(tanFov); - - btVector3 camPos,camTarget; - m_glApp->m_instancingRenderer->getCameraPosition(camPos); - m_glApp->m_instancingRenderer->getCameraTargetPosition(camTarget); - - btVector3 rayFrom = camPos; - btVector3 rayForward = (camTarget-camPos); - rayForward.normalize(); - float farPlane = 10000.f; - rayForward*= farPlane; - - btVector3 rightOffset; - btVector3 m_cameraUp=btVector3(0,1,0); - btVector3 vertical = m_cameraUp; - - btVector3 hor; - hor = rayForward.cross(vertical); - hor.normalize(); - vertical = hor.cross(rayForward); - vertical.normalize(); - - float tanfov = tanf(0.5f*fov); - - - hor *= 2.f * farPlane * tanfov; - vertical *= 2.f * farPlane * tanfov; - - b3Scalar aspect; - float width = m_glApp->m_instancingRenderer->getScreenWidth(); - float height = m_glApp->m_instancingRenderer->getScreenHeight(); - - aspect = width / height; - - hor*=aspect; - - - btVector3 rayToCenter = rayFrom + rayForward; - btVector3 dHor = hor * 1.f/width; - btVector3 dVert = vertical * 1.f/height; - - - btVector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical; - rayTo += btScalar(x) * dHor; - rayTo -= btScalar(y) * dVert; - return rayTo; - } - - - bool mouseMoveCallback(float x,float y) - { -// if (m_data->m_altPressed!=0 || m_data->m_controlPressed!=0) - // return false; - - if (m_pickedBody && m_pickedConstraint) - { - btPoint2PointConstraint* pickCon = static_cast(m_pickedConstraint); - if (pickCon) - { - //keep it at the same picking distance - btVector3 newRayTo = getRayTo(x,y); - btVector3 rayFrom; - btVector3 oldPivotInB = pickCon->getPivotInB(); - btVector3 newPivotB; - m_glApp->m_instancingRenderer->getCameraPosition(rayFrom); - btVector3 dir = newRayTo-rayFrom; - dir.normalize(); - dir *= m_oldPickingDist; - - newPivotB = rayFrom + dir; - pickCon->setPivotB(newPivotB); - } - } - if (m_pickingMultiBodyPoint2Point) - { - //keep it at the same picking distance - - btVector3 newRayTo = getRayTo(x,y); - btVector3 rayFrom; - btVector3 oldPivotInB = m_pickingMultiBodyPoint2Point->getPivotInB(); - btVector3 newPivotB; - btVector3 camPos; - m_glApp->m_instancingRenderer->getCameraPosition(camPos); - rayFrom = camPos; - btVector3 dir = newRayTo-rayFrom; - dir.normalize(); - dir *= m_oldPickingDist; - - newPivotB = rayFrom + dir; - - m_pickingMultiBodyPoint2Point->setPivotInB(newPivotB); - } - - return false; - } - bool mouseButtonCallback(int button, int state, float x, float y) - { - - if (state==1) - { - if(button==0)// && (m_data->m_altPressed==0 && m_data->m_controlPressed==0)) - { - btVector3 camPos; - m_glApp->m_instancingRenderer->getCameraPosition(camPos); - - btVector3 rayFrom = camPos; - btVector3 rayTo = getRayTo(x,y); - - btCollisionWorld::ClosestRayResultCallback rayCallback(rayFrom,rayTo); - m_dynamicsWorld->rayTest(rayFrom,rayTo,rayCallback); - if (rayCallback.hasHit()) - { - - btVector3 pickPos = rayCallback.m_hitPointWorld; - btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject); - if (body) - { - //other exclusions? - if (!(body->isStaticObject() || body->isKinematicObject())) - { - m_pickedBody = body; - m_pickedBody->setActivationState(DISABLE_DEACTIVATION); - //printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ()); - btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos; - btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body,localPivot); - m_dynamicsWorld->addConstraint(p2p,true); - m_pickedConstraint = p2p; - btScalar mousePickClamping = 30.f; - p2p->m_setting.m_impulseClamp = mousePickClamping; - //very weak constraint for picking - p2p->m_setting.m_tau = 0.001f; - } - } else - { - btMultiBodyLinkCollider* multiCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(rayCallback.m_collisionObject); - if (multiCol && multiCol->m_multiBody) - { - multiCol->m_multiBody->setCanSleep(false); - - btVector3 pivotInA = multiCol->m_multiBody->worldPosToLocal(multiCol->m_link, pickPos); - - btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(multiCol->m_multiBody,multiCol->m_link,0,pivotInA,pickPos); - //if you add too much energy to the system, causing high angular velocities, simulation 'explodes' - //see also http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=949 - //so we try to avoid it by clamping the maximum impulse (force) that the mouse pick can apply - //it is not satisfying, hopefully we find a better solution (higher order integrator, using joint friction using a zero-velocity target motor with limited force etc?) - - p2p->setMaxAppliedImpulse(20*scaling); - - btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_dynamicsWorld; - world->addMultiBodyConstraint(p2p); - m_pickingMultiBodyPoint2Point =p2p; - } - } - - -// pickObject(pickPos, rayCallback.m_collisionObject); - m_oldPickingPos = rayTo; - m_hitPos = pickPos; - m_oldPickingDist = (pickPos-rayFrom).length(); -// printf("hit !\n"); - //add p2p - } - - } - } else - { - if (button==0) - { - if (m_pickedConstraint) - { - m_dynamicsWorld->removeConstraint(m_pickedConstraint); - delete m_pickedConstraint; - m_pickedConstraint=0; - m_pickedBody = 0; - } - - if (m_pickingMultiBodyPoint2Point) - { - m_pickingMultiBodyPoint2Point->getMultiBodyA()->setCanSleep(true); - btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_dynamicsWorld; - world->removeMultiBodyConstraint(m_pickingMultiBodyPoint2Point); - delete m_pickingMultiBodyPoint2Point; - m_pickingMultiBodyPoint2Point = 0; - } - //remove p2p - } - } - - //printf("button=%d, state=%d\n",button,state); - return false; - } - - void stepSimulation() - { - m_dynamicsWorld->stepSimulation(1./60,0); -// CProfileManager::dumpAll(); - } -}; - - - -BasicDemo* sDemo = 0; - -static void MyMouseMoveCallback( float x, float y) -{ - bool handled = false; - if (sDemo) - handled = sDemo->mouseMoveCallback(x,y); - if (!handled) - b3DefaultMouseMoveCallback(x,y); -} -static void MyMouseButtonCallback(int button, int state, float x, float y) -{ - bool handled = false; - //try picking first - if (sDemo) - handled = sDemo->mouseButtonCallback(button,state,x,y); - - if (!handled) - b3DefaultMouseButtonCallback(button,state,x,y); -} - - -int main(int argc, char* argv[]) -{ - - float dt = 1./120.f; -#ifdef BT_DEBUG - char* name = "Bullet 2 CPU FeatherstoneMultiBodyDemo (Debug build=SLOW)"; -#else - char* name = "Bullet 2 CPU FeatherstoneMultiBodyDemo"; -#endif - - - SimpleOpenGL3App* app = new SimpleOpenGL3App(name,1024,768); - app->m_instancingRenderer->setCameraDistance(40); - app->m_instancingRenderer->setCameraPitch(0); - app->m_instancingRenderer->setCameraTargetPosition(b3MakeVector3(0,0,0)); - - app->m_window->setMouseMoveCallback(MyMouseMoveCallback); - app->m_window->setMouseButtonCallback(MyMouseButtonCallback); - - BasicDemo* demo = new BasicDemo(app); - demo->initPhysics(); - sDemo = demo; - - GLint err = glGetError(); - assert(err==GL_NO_ERROR); - - do - { - GLint err = glGetError(); - assert(err==GL_NO_ERROR); - app->m_instancingRenderer->init(); - app->m_instancingRenderer->updateCamera(); - - demo->stepSimulation(); - demo->drawObjects(); - app->drawGrid(10,0.01); - char bla[1024]; - static int frameCount = 0; - frameCount++; - sprintf(bla,"Simulation frame %d", frameCount); - - app->drawText(bla,10,10); - app->swapBuffer(); - } while (!app->m_window->requestedExit()); - - - demo->exitPhysics(); - delete demo; - - delete app; - return 0; -} diff --git a/Demos3/bullet2/FeatherstoneMultiBodyDemo/premake4.lua b/Demos3/bullet2/FeatherstoneMultiBodyDemo/premake4.lua deleted file mode 100644 index fc4bafc8e..000000000 --- a/Demos3/bullet2/FeatherstoneMultiBodyDemo/premake4.lua +++ /dev/null @@ -1,38 +0,0 @@ - - project "App2_FeatherstoneMultiBodyDemo" - - language "C++" - - kind "ConsoleApp" - targetdir "../../../bin" - - includedirs { - ".", - "../../../src", - "../../../btgui" - } - - initOpenGL() - initGlew() - - links{"gwen", "BulletDynamics", "BulletCollision","LinearMath", - "OpenGL_Window", "OpenGL_TrueTypeFont" - } - - files { - "**.cpp", - "**.h", - "../../../src/Bullet3Common/**.cpp", - "../../../src/Bullet3Common/**.h", - "../../../btgui/Timing/b3Clock.cpp", - "../../../btgui/Timing/b3Clock.h" - - } - - if os.is("Linux") then - links ("X11") - end - - if os.is("MacOSX") then - links{"Cocoa.framework"} - end diff --git a/btgui/OpenGLWindow/GLInstancingRenderer.cpp b/btgui/OpenGLWindow/GLInstancingRenderer.cpp index 98c8fbffb..578903cea 100644 --- a/btgui/OpenGLWindow/GLInstancingRenderer.cpp +++ b/btgui/OpenGLWindow/GLInstancingRenderer.cpp @@ -358,6 +358,8 @@ GLInstancingRenderer::GLInstancingRenderer(int maxNumObjectCapacity, int maxShap { m_data = new InternalDataRenderer; + m_data->m_totalNumInstances = 0; + sData2 = m_data; m_data->m_instance_positions_ptr.resize(m_maxNumObjectCapacity*4); @@ -367,12 +369,8 @@ GLInstancingRenderer::GLInstancingRenderer(int maxNumObjectCapacity, int maxShap } -GLInstancingRenderer::~GLInstancingRenderer() +void GLInstancingRenderer::removeAllInstances() { - delete m_data->m_shadowMap; - glDeleteTextures(1,&m_data->m_shadowTexture); - glDeleteTextures(1,&m_data->m_defaultTexturehandle); - for (int i=0;im_index_vbo) @@ -386,6 +384,15 @@ GLInstancingRenderer::~GLInstancingRenderer() delete m_graphicsInstances[i]; } m_graphicsInstances.clear(); +} + +GLInstancingRenderer::~GLInstancingRenderer() +{ + delete m_data->m_shadowMap; + glDeleteTextures(1,&m_data->m_shadowTexture); + glDeleteTextures(1,&m_data->m_defaultTexturehandle); + + removeAllInstances(); sData2=0; diff --git a/btgui/OpenGLWindow/GLInstancingRenderer.h b/btgui/OpenGLWindow/GLInstancingRenderer.h index cac4cc453..fd64a57e7 100644 --- a/btgui/OpenGLWindow/GLInstancingRenderer.h +++ b/btgui/OpenGLWindow/GLInstancingRenderer.h @@ -66,6 +66,7 @@ public: void InitShaders(); void CleanupShaders(); + void removeAllInstances(); void updateShape(int shapeIndex, const float* vertices); diff --git a/build3/premake4.lua b/build3/premake4.lua index 693b148f2..717e5c090 100644 --- a/build3/premake4.lua +++ b/build3/premake4.lua @@ -114,9 +114,7 @@ include "../Demos3/SimpleOpenGL3" include "../src/BulletDynamics" include "../src/BulletCollision" include "../src/LinearMath" - include "../Demos3/bullet2/BasicDemo" - include "../Demos3/bullet2/FeatherstoneMultiBodyDemo" - + include "../src/Bullet3Dynamics" include "../src/Bullet3Common" include "../src/Bullet3Geometry" diff --git a/demos3/bullet2/BasicDemo/Bullet2RigidBodyDemo.cpp b/demos3/bullet2/BasicDemo/Bullet2RigidBodyDemo.cpp new file mode 100644 index 000000000..5a6b5dde5 --- /dev/null +++ b/demos3/bullet2/BasicDemo/Bullet2RigidBodyDemo.cpp @@ -0,0 +1,202 @@ +#include "Bullet2RigidBodyDemo.h" +#include "btBulletDynamicsCommon.h" +#include "OpenGLWindow/SimpleOpenGL3App.h" + +Bullet2RigidBodyDemo::Bullet2RigidBodyDemo(SimpleOpenGL3App* app) + :m_glApp(app), + m_pickedBody(0), + m_pickedConstraint(0) +{ + m_config = 0; + m_dispatcher = 0; + m_bp = 0; + m_solver = 0; + m_dynamicsWorld = 0; +} +void Bullet2RigidBodyDemo::initPhysics() +{ + m_config = new btDefaultCollisionConfiguration; + m_dispatcher = new btCollisionDispatcher(m_config); + m_bp = new btDbvtBroadphase(); + m_solver = new btSequentialImpulseConstraintSolver(); + m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_bp,m_solver,m_config); +} + +void Bullet2RigidBodyDemo::exitPhysics() +{ + delete m_dynamicsWorld; + m_dynamicsWorld=0; + delete m_solver; + m_solver=0; + delete m_bp; + m_bp=0; + delete m_dispatcher; + m_dispatcher=0; + delete m_config; + m_config=0; +} + +Bullet2RigidBodyDemo::~Bullet2RigidBodyDemo() +{ + btAssert(m_config == 0); + btAssert(m_dispatcher == 0); + btAssert(m_bp == 0); + btAssert(m_solver == 0); + btAssert(m_dynamicsWorld == 0); +} + +btVector3 Bullet2RigidBodyDemo::getRayTo(int x,int y) +{ + if (!m_glApp->m_instancingRenderer) + { + btAssert(0); + return btVector3(0,0,0); + } + + float top = 1.f; + float bottom = -1.f; + float nearPlane = 1.f; + float tanFov = (top-bottom)*0.5f / nearPlane; + float fov = b3Scalar(2.0) * b3Atan(tanFov); + + btVector3 camPos,camTarget; + m_glApp->m_instancingRenderer->getCameraPosition(camPos); + m_glApp->m_instancingRenderer->getCameraTargetPosition(camTarget); + + btVector3 rayFrom = camPos; + btVector3 rayForward = (camTarget-camPos); + rayForward.normalize(); + float farPlane = 10000.f; + rayForward*= farPlane; + + btVector3 rightOffset; + btVector3 m_cameraUp=btVector3(0,1,0); + btVector3 vertical = m_cameraUp; + + btVector3 hor; + hor = rayForward.cross(vertical); + hor.normalize(); + vertical = hor.cross(rayForward); + vertical.normalize(); + + float tanfov = tanf(0.5f*fov); + + + hor *= 2.f * farPlane * tanfov; + vertical *= 2.f * farPlane * tanfov; + + b3Scalar aspect; + float width = m_glApp->m_instancingRenderer->getScreenWidth(); + float height = m_glApp->m_instancingRenderer->getScreenHeight(); + + aspect = width / height; + + hor*=aspect; + + + btVector3 rayToCenter = rayFrom + rayForward; + btVector3 dHor = hor * 1.f/width; + btVector3 dVert = vertical * 1.f/height; + + + btVector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical; + rayTo += btScalar(x) * dHor; + rayTo -= btScalar(y) * dVert; + return rayTo; +} + + +bool Bullet2RigidBodyDemo::mouseMoveCallback(float x,float y) +{ +// if (m_data->m_altPressed!=0 || m_data->m_controlPressed!=0) +// return false; + + if (m_pickedBody && m_pickedConstraint) + { + btPoint2PointConstraint* pickCon = static_cast(m_pickedConstraint); + if (pickCon) + { + //keep it at the same picking distance + btVector3 newRayTo = getRayTo(x,y); + btVector3 rayFrom; + btVector3 oldPivotInB = pickCon->getPivotInB(); + btVector3 newPivotB; + m_glApp->m_instancingRenderer->getCameraPosition(rayFrom); + btVector3 dir = newRayTo-rayFrom; + dir.normalize(); + dir *= m_oldPickingDist; + + newPivotB = rayFrom + dir; + pickCon->setPivotB(newPivotB); + } + } + + return false; +} +bool Bullet2RigidBodyDemo::mouseButtonCallback(int button, int state, float x, float y) +{ + + if (state==1) + { + if(button==0)// && (m_data->m_altPressed==0 && m_data->m_controlPressed==0)) + { + btVector3 camPos; + m_glApp->m_instancingRenderer->getCameraPosition(camPos); + + btVector3 rayFrom = camPos; + btVector3 rayTo = getRayTo(x,y); + + btCollisionWorld::ClosestRayResultCallback rayCallback(rayFrom,rayTo); + m_dynamicsWorld->rayTest(rayFrom,rayTo,rayCallback); + if (rayCallback.hasHit()) + { + + btVector3 pickPos = rayCallback.m_hitPointWorld; + btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject); + if (body) + { + //other exclusions? + if (!(body->isStaticObject() || body->isKinematicObject())) + { + m_pickedBody = body; + m_pickedBody->setActivationState(DISABLE_DEACTIVATION); + //printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ()); + btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos; + btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body,localPivot); + m_dynamicsWorld->addConstraint(p2p,true); + m_pickedConstraint = p2p; + btScalar mousePickClamping = 30.f; + p2p->m_setting.m_impulseClamp = mousePickClamping; + //very weak constraint for picking + p2p->m_setting.m_tau = 0.001f; + } + } + + +// pickObject(pickPos, rayCallback.m_collisionObject); + m_oldPickingPos = rayTo; + m_hitPos = pickPos; + m_oldPickingDist = (pickPos-rayFrom).length(); +// printf("hit !\n"); + //add p2p + } + + } + } else + { + if (button==0) + { + if (m_pickedConstraint) + { + m_dynamicsWorld->removeConstraint(m_pickedConstraint); + delete m_pickedConstraint; + m_pickedConstraint=0; + m_pickedBody = 0; + } + //remove p2p + } + } + + //printf("button=%d, state=%d\n",button,state); + return false; +} diff --git a/demos3/bullet2/BasicDemo/Bullet2RigidBodyDemo.h b/demos3/bullet2/BasicDemo/Bullet2RigidBodyDemo.h new file mode 100644 index 000000000..fe1e352cb --- /dev/null +++ b/demos3/bullet2/BasicDemo/Bullet2RigidBodyDemo.h @@ -0,0 +1,43 @@ +#ifndef BULLET2_RIGIDBODY_DEMO_H +#define BULLET2_RIGIDBODY_DEMO_H + +#include "LinearMath/btVector3.h" + +#include "../../AllBullet2Demos\BulletDemoInterface.h" + +class Bullet2RigidBodyDemo : public BulletDemoInterface +{ +protected: + class btDiscreteDynamicsWorld* m_dynamicsWorld; + class btCollisionDispatcher* m_dispatcher; + class btBroadphaseInterface* m_bp; + class btCollisionConfiguration* m_config; + class btConstraintSolver* m_solver; + + class btRigidBody* m_pickedBody; + class btTypedConstraint* m_pickedConstraint; + btVector3 m_oldPickingPos; + btVector3 m_hitPos; + btScalar m_oldPickingDist; + + +public: + + class SimpleOpenGL3App* m_glApp; + + Bullet2RigidBodyDemo(SimpleOpenGL3App* app); + virtual void initPhysics(); + virtual void exitPhysics(); + + virtual ~Bullet2RigidBodyDemo(); + btVector3 getRayTo(int x,int y); + virtual bool mouseMoveCallback(float x,float y); + virtual bool mouseButtonCallback(int button, int state, float x, float y); + virtual bool keyboardCallback(int key, int state) + { + return false; + } + +}; + +#endif //BULLET2_RIGIDBODY_DEMO_H diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp b/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp index 733a53ced..3699628cb 100644 --- a/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp +++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp @@ -3108,35 +3108,6 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray* m_dmins.resize(nPairs); if (splitSearchSepAxisConvex) { - - { - B3_PROFILE("findSeparatingAxisVertexFaceKernel"); - b3BufferInfoCL bInfo[] = { - b3BufferInfoCL( pairs->getBufferCL(), true ), - b3BufferInfoCL( bodyBuf->getBufferCL(),true), - b3BufferInfoCL( gpuCollidables.getBufferCL(),true), - b3BufferInfoCL( convexData.getBufferCL(),true), - b3BufferInfoCL( gpuVertices.getBufferCL(),true), - b3BufferInfoCL( gpuUniqueEdges.getBufferCL(),true), - b3BufferInfoCL( gpuFaces.getBufferCL(),true), - b3BufferInfoCL( gpuIndices.getBufferCL(),true), - b3BufferInfoCL( clAabbsWorldSpace.getBufferCL(),true), - b3BufferInfoCL( m_sepNormals.getBufferCL()), - b3BufferInfoCL( m_hasSeparatingNormals.getBufferCL()), - b3BufferInfoCL( m_dmins.getBufferCL()) - }; - - b3LauncherCL launcher(m_queue, m_findSeparatingAxisVertexFaceKernel,"findSeparatingAxisVertexFaceKernel"); - launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); - launcher.setConst( nPairs ); - - int num = nPairs; - launcher.launch1D( num); - clFinish(m_queue); - } - - - if (useMprGpu) { nContacts = m_totalContactsOut.at(0); @@ -3180,8 +3151,36 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray* } } - } else + } //else { + + { + B3_PROFILE("findSeparatingAxisVertexFaceKernel"); + b3BufferInfoCL bInfo[] = { + b3BufferInfoCL( pairs->getBufferCL(), true ), + b3BufferInfoCL( bodyBuf->getBufferCL(),true), + b3BufferInfoCL( gpuCollidables.getBufferCL(),true), + b3BufferInfoCL( convexData.getBufferCL(),true), + b3BufferInfoCL( gpuVertices.getBufferCL(),true), + b3BufferInfoCL( gpuUniqueEdges.getBufferCL(),true), + b3BufferInfoCL( gpuFaces.getBufferCL(),true), + b3BufferInfoCL( gpuIndices.getBufferCL(),true), + b3BufferInfoCL( clAabbsWorldSpace.getBufferCL(),true), + b3BufferInfoCL( m_sepNormals.getBufferCL()), + b3BufferInfoCL( m_hasSeparatingNormals.getBufferCL()), + b3BufferInfoCL( m_dmins.getBufferCL()) + }; + + b3LauncherCL launcher(m_queue, m_findSeparatingAxisVertexFaceKernel,"findSeparatingAxisVertexFaceKernel"); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( nPairs ); + + int num = nPairs; + launcher.launch1D( num); + clFinish(m_queue); + } + + { B3_PROFILE("findSeparatingAxisEdgeEdgeKernel");