diff --git a/Demos3/AllBullet2Demos/BulletDemoEntries.h b/Demos3/AllBullet2Demos/BulletDemoEntries.h index 9d4e479af..54f8d05e7 100644 --- a/Demos3/AllBullet2Demos/BulletDemoEntries.h +++ b/Demos3/AllBullet2Demos/BulletDemoEntries.h @@ -5,6 +5,8 @@ #include "BulletDemoInterface.h" #include "../bullet2/BasicDemo/BasicDemo.h" #include "../bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.h" +#include "../bullet2/FeatherstoneMultiBodyDemo/MultiDofDemo.h" + #include "../bullet2/RagdollDemo/RagdollDemo.h" #include @@ -23,7 +25,7 @@ static BulletDemoEntry allDemos[]= {"BasicDemo",BasicDemo::MyCreateFunc}, {"Ragdoll",RagDollDemo::MyCreateFunc}, {"MultiBody1",FeatherstoneDemo1::MyCreateFunc}, - {"MultiBody2",FeatherstoneDemo2::MyCreateFunc}, + {"MultiDofDemo",MultiDofDemo::MyCreateFunc}, }; diff --git a/Demos3/AllBullet2Demos/premake4.lua b/Demos3/AllBullet2Demos/premake4.lua index 23c40e6a8..32eb5f0e5 100644 --- a/Demos3/AllBullet2Demos/premake4.lua +++ b/Demos3/AllBullet2Demos/premake4.lua @@ -24,6 +24,8 @@ "../bullet2/BasicDemo/Bullet2RigidBodyDemo.h", "../bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp", "../bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.h", + "../bullet2/FeatherstoneMultiBodyDemo/MultiDofDemo.cpp", + "../bullet2/FeatherstoneMultiBodyDemo/MultiDofDemo.h", "../bullet2/BasicDemo/BasicDemo.cpp", "../bullet2/BasicDemo/BasicDemo.h", "../bullet2/RagdollDemo/RagdollDemo.cpp", diff --git a/Demos3/bullet2/BasicDemo/BasicDemo.cpp b/Demos3/bullet2/BasicDemo/BasicDemo.cpp index a24ced148..4b950f2de 100644 --- a/Demos3/bullet2/BasicDemo/BasicDemo.cpp +++ b/Demos3/bullet2/BasicDemo/BasicDemo.cpp @@ -2,11 +2,13 @@ #include "OpenGLWindow/SimpleOpenGL3App.h" #include "btBulletDynamicsCommon.h" #include "Bullet3Common/b3Vector3.h" +#include "BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h" -#define ARRAY_SIZE_X 1 -#define ARRAY_SIZE_Y 1 -#define ARRAY_SIZE_Z 1 +#define ARRAY_SIZE_X 5 +#define ARRAY_SIZE_Y 5 +#define ARRAY_SIZE_Z 5 +static const float scaling=0.35f; BasicDemo::BasicDemo(SimpleOpenGL3App* app) :Bullet2RigidBodyDemo(app) @@ -46,7 +48,14 @@ void BasicDemo::createGround(int cubeShapeId) } void BasicDemo::initPhysics() { - Bullet2RigidBodyDemo::initPhysics(); +// Bullet2RigidBodyDemo::initPhysics(); + + m_config = new btDefaultCollisionConfiguration; + m_dispatcher = new btCollisionDispatcher(m_config); + m_bp = new btDbvtBroadphase(); + m_solver = new btNNCGConstraintSolver(); + m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_bp,m_solver,m_config); + int curColor=0; //create ground int cubeShapeId = m_glApp->registerCubeShape(); @@ -58,7 +67,7 @@ void BasicDemo::initPhysics() { - float halfExtents[]={0.1,1,1,1}; + float halfExtents[]={scaling,scaling,scaling,1}; b3Vector4 colors[4] = { b3MakeVector4(1,0,0,1), @@ -75,7 +84,7 @@ void BasicDemo::initPhysics() 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); diff --git a/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp b/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp index a2e0b4aa2..bce4ad80a 100644 --- a/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp +++ b/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp @@ -54,19 +54,19 @@ m_pickedConstraint(0), m_pickingMultiBodyPoint2Point(0) { - m_config = 0; + m_collisionConfiguration = 0; m_dispatcher = 0; - m_bp = 0; + m_broadphase = 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_collisionConfiguration = new btDefaultCollisionConfiguration; + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + m_broadphase = new btDbvtBroadphase(); m_solver = new btMultiBodyConstraintSolver(); - m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_dispatcher,m_bp,m_solver,m_config); + m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration); } void Bullet2MultiBodyDemo::exitPhysics() @@ -75,19 +75,19 @@ void Bullet2MultiBodyDemo::exitPhysics() m_dynamicsWorld=0; delete m_solver; m_solver=0; - delete m_bp; - m_bp=0; + delete m_broadphase; + m_broadphase=0; delete m_dispatcher; m_dispatcher=0; - delete m_config; - m_config=0; + delete m_collisionConfiguration; + m_collisionConfiguration=0; } Bullet2MultiBodyDemo::~Bullet2MultiBodyDemo() { - btAssert(m_config == 0); + btAssert(m_collisionConfiguration == 0); btAssert(m_dispatcher == 0); - btAssert(m_bp == 0); + btAssert(m_broadphase == 0); btAssert(m_solver == 0); btAssert(m_dynamicsWorld == 0); } @@ -333,7 +333,8 @@ btMultiBody* FeatherstoneDemo1::createFeatherstoneMultiBody(class btMultiBodyDyn btVector3 inertia = btVector3 (91,344,253)*scaling*scaling; - btMultiBody * bod = new btMultiBody(n_links, mass, inertia, settings.m_isFixedBase, settings.m_canSleep); + bool isMultiDof = false; + btMultiBody * bod = new btMultiBody(n_links, mass, inertia, settings.m_isFixedBase, settings.m_canSleep, isMultiDof); // bod->setHasSelfCollision(false); //btQuaternion orn(btVector3(0,0,1),-0.25*SIMD_HALF_PI);//0,0,0,1); @@ -770,7 +771,8 @@ public: bool isFixedBase = true; bool canSleep = true; - btMultiBody * bod = new btMultiBody(n_links, mass, localInertia, isFixedBase, canSleep); + bool isMultiDof = false; + btMultiBody * bod = new btMultiBody(n_links, mass, localInertia, isFixedBase, canSleep, isMultiDof); btTransform tr; tr = offset*transform; diff --git a/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.h b/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.h index 9100a20a1..76c0b62c7 100644 --- a/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.h +++ b/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.h @@ -6,6 +6,7 @@ #include "../../AllBullet2Demos/BulletDemoInterface.h" +#include "LinearMath/btAlignedObjectArray.h" struct btMultiBodySettings { @@ -44,10 +45,11 @@ protected: class btMultiBodyDynamicsWorld* m_dynamicsWorld; class btCollisionDispatcher* m_dispatcher; - class btBroadphaseInterface* m_bp; - class btCollisionConfiguration* m_config; + class btBroadphaseInterface* m_broadphase; + class btCollisionConfiguration* m_collisionConfiguration; class btMultiBodyConstraintSolver* m_solver; + btAlignedObjectArray m_collisionShapes; //btAlignedObjectArray m_linkColliders; public: diff --git a/Demos3/bullet2/FeatherstoneMultiBodyDemo/MultiDofDemo.cpp b/Demos3/bullet2/FeatherstoneMultiBodyDemo/MultiDofDemo.cpp new file mode 100644 index 000000000..7b5fab35d --- /dev/null +++ b/Demos3/bullet2/FeatherstoneMultiBodyDemo/MultiDofDemo.cpp @@ -0,0 +1,402 @@ +#include "MultiDofDemo.h" +#include "OpenGLWindow/SimpleOpenGL3App.h" +#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" + +#include "OpenGLWindow/GLInstancingRenderer.h" +#include "BulletCollision/CollisionShapes/btShapeHull.h" + +static bool g_firstInit = true; +static float scaling = 0.4f; +static float friction = 1.; +#define ARRAY_SIZE_X 5 +#define ARRAY_SIZE_Y 5 +#define ARRAY_SIZE_Z 5 + +//maximum number of objects (and allow user to shoot additional boxes) +#define MAX_PROXIES (ARRAY_SIZE_X*ARRAY_SIZE_Y*ARRAY_SIZE_Z + 1024) + + +#define START_POS_X -5 +//#define START_POS_Y 12 +#define START_POS_Y 2 +#define START_POS_Z -3 + + + +MultiDofDemo::MultiDofDemo(SimpleOpenGL3App* app) +:FeatherstoneDemo1(app) +{ +} +MultiDofDemo::~MultiDofDemo() +{ +} + +void MultiDofDemo::stepSimulation(float deltaTime) +{ + //use a smaller internal timestep, there are stability issues + float internalTimeStep = 1./240.f; + m_dynamicsWorld->stepSimulation(deltaTime,10,internalTimeStep); +// CProfileManager::dumpAll(); +} + + +void MultiDofDemo::initPhysics() +{ + + + + if(g_firstInit) + { + this->m_glApp->m_instancingRenderer->setCameraDistance(btScalar(10.*scaling)); + this->m_glApp->m_instancingRenderer->setCameraPitch(50); + g_firstInit = false; + } + ///collision configuration contains default setup for memory, collision setup + m_collisionConfiguration = new btDefaultCollisionConfiguration(); + + ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + + m_broadphase = new btDbvtBroadphase(); + + //Use the btMultiBodyConstraintSolver for Featherstone btMultiBody support + btMultiBodyConstraintSolver* sol = new btMultiBodyConstraintSolver; + m_solver = sol; + + //use btMultiBodyDynamicsWorld for Featherstone btMultiBody support + btMultiBodyDynamicsWorld* world = new btMultiBodyDynamicsWorld(m_dispatcher,m_broadphase,sol,m_collisionConfiguration); + m_dynamicsWorld = world; +// m_dynamicsWorld->setDebugDrawer(&gDebugDraw); + + m_dynamicsWorld->setGravity(btVector3(0,-10,0)); + + ///create a few basic rigid bodies + btVector3 groundHalfExtents(50,50,50); + btCollisionShape* groundShape = new btBoxShape(groundHalfExtents); + //groundShape->initializePolyhedralFeatures(); +// btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50); + + m_collisionShapes.push_back(groundShape); + + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0,-50,00)); + + ///////////////////////////////////////////////////////////////// + ///////////////////////////////////////////////////////////////// + bool floating = false; + bool damping = true; + bool gyro = true; + int numLinks = 5; + bool spherical = true; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals + bool multibodyOnly = false; + bool canSleep = true; + bool selfCollide = false; + btVector3 linkHalfExtents(0.05, 0.37, 0.1); + btVector3 baseHalfExtents(0.05, 0.37, 0.1); + + btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(world, numLinks, btVector3(-0.4f, 3.f, 0.f), linkHalfExtents, baseHalfExtents, spherical, floating); + //mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm + + mbC->setCanSleep(canSleep); + mbC->setHasSelfCollision(selfCollide); + mbC->setUseGyroTerm(gyro); + // + if(!damping) + { + mbC->setLinearDamping(0.f); + mbC->setAngularDamping(0.f); + }else + { mbC->setLinearDamping(0.1f); + mbC->setAngularDamping(0.9f); + } + // + m_dynamicsWorld->setGravity(btVector3(0, -9.81 ,0)); + ////////////////////////////////////////////// + if(numLinks > 0) + { + btScalar q0 = 45.f * SIMD_PI/ 180.f; + if(!spherical) + if(mbC->isMultiDof()) + mbC->setJointPosMultiDof(0, &q0); + else + mbC->setJointPos(0, q0); + else + { + btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0); + quat0.normalize(); + mbC->setJointPosMultiDof(0, quat0); + } + } + /// + addColliders_testMultiDof(mbC, world, baseHalfExtents, linkHalfExtents); + + int cubeShapeId = m_glApp->registerCubeShape(); + ///////////////////////////////////////////////////////////////// + btScalar groundHeight = -51.55; + if (!multibodyOnly) + { + 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 + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0,groundHeight,0)); + 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,1,1+2);//,1,1+2); + + int index = m_glApp->m_instancingRenderer->registerGraphicsInstance(cubeShapeId,groundTransform.getOrigin(),groundTransform.getRotation(),btVector4(0,1,0,1),groundHalfExtents); + body->setUserIndex(index); + + + + } + ///////////////////////////////////////////////////////////////// + if(!multibodyOnly) + { + btVector3 halfExtents(.5,.5,.5); + btBoxShape* colShape = new btBoxShape(halfExtents); + //btCollisionShape* colShape = new btSphereShape(btScalar(1.)); + m_collisionShapes.push_back(colShape); + + /// Create Dynamic Objects + btTransform startTransform; + startTransform.setIdentity(); + + btScalar mass(1.f); + + //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) + colShape->calculateLocalInertia(mass,localInertia); + + startTransform.setOrigin(btVector3( + btScalar(0.0), + -0.95, + btScalar(0.0))); + + + //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);//,1,1+2); + + int index = m_glApp->m_instancingRenderer->registerGraphicsInstance(cubeShapeId,startTransform.getOrigin(),startTransform.getRotation(),btVector4(1,1,0,1),halfExtents); + body->setUserIndex(index); + + } + + m_glApp->m_instancingRenderer->writeTransforms(); + ///////////////////////////////////////////////////////////////// +} + + +btMultiBody* MultiDofDemo::createFeatherstoneMultiBody_testMultiDof(btMultiBodyDynamicsWorld *pWorld, int numLinks, const btVector3 &basePosition, const btVector3 &baseHalfExtents, const btVector3 &linkHalfExtents, bool spherical, bool floating) +{ + //init the base + btVector3 baseInertiaDiag(0.f, 0.f, 0.f); + float baseMass = 1.f; + + if(baseMass) + { + btCollisionShape *pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); + pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag); + delete pTempBox; + } + + bool canSleep = false; + bool isMultiDof = true; + btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep, isMultiDof); + + btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f); + pMultiBody->setBasePos(basePosition); + pMultiBody->setWorldToBaseRot(baseOriQuat); + btVector3 vel(0, 0, 0); +// pMultiBody->setBaseVel(vel); + + //init the links + btVector3 hingeJointAxis(1, 0, 0); + float linkMass = 1.f; + btVector3 linkInertiaDiag(0.f, 0.f, 0.f); + + btCollisionShape *pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2])); + pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag); + delete pTempBox; + + //y-axis assumed up + btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset + btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset + btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset + + ////// + btScalar q0 = 0.f * SIMD_PI/ 180.f; + btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0); + quat0.normalize(); + ///// + + for(int i = 0; i < numLinks; ++i) + { + if(!spherical) + pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false); + else + //pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false); + pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false); + } + + pMultiBody->finalizeMultiDof(); + + /// + pWorld->addMultiBody(pMultiBody); + /// + return pMultiBody; +} + +void MultiDofDemo::addColliders_testMultiDof(btMultiBody *pMultiBody, btMultiBodyDynamicsWorld *pWorld, const btVector3 &baseHalfExtents, const btVector3 &linkHalfExtents) +{ + int cubeShapeId = m_glApp->registerCubeShape(); + + btAlignedObjectArray world_to_local; + world_to_local.resize(pMultiBody->getNumLinks() + 1); + + btAlignedObjectArray local_origin; + local_origin.resize(pMultiBody->getNumLinks() + 1); + world_to_local[0] = pMultiBody->getWorldToBaseRot(); + local_origin[0] = pMultiBody->getBasePos(); + + { + + 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(baseHalfExtents); + btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(pMultiBody, -1); + col->setCollisionShape(box); + + btTransform tr; + tr.setIdentity(); + tr.setOrigin(local_origin[0]); + tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); + col->setWorldTransform(tr); + + pWorld->addCollisionObject(col, 2,1+2); + + int index = m_glApp->m_instancingRenderer->registerGraphicsInstance(cubeShapeId,tr.getOrigin(),tr.getRotation(),btVector4(0,0,1,1),baseHalfExtents); + col->setUserIndex(index); + + + col->setFriction(friction); + pMultiBody->setBaseCollider(col); + + } + } + + + for (int i=0; i < pMultiBody->getNumLinks(); ++i) + { + const int parent = pMultiBody->getParent(i); + world_to_local[i+1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent+1]; + local_origin[i+1] = local_origin[parent+1] + (quatRotate(world_to_local[i+1].inverse() , pMultiBody->getRVector(i))); + } + + + for (int i=0; i < pMultiBody->getNumLinks(); ++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(linkHalfExtents); + btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, 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); + pWorld->addCollisionObject(col,2,1+2); + int index = m_glApp->m_instancingRenderer->registerGraphicsInstance(cubeShapeId,tr.getOrigin(),tr.getRotation(),btVector4(0,0,1,1),linkHalfExtents); + col->setUserIndex(index); + + + pMultiBody->getLink(i).m_collider=col; + } +} + +void MultiDofDemo::addBoxes_testMultiDof() +{ + //create a few dynamic rigidbodies + // Re-using the same collision is better for memory usage and performance + + btBoxShape* colShape = new btBoxShape(btVector3(1,1,1)); + //btCollisionShape* colShape = new btSphereShape(btScalar(1.)); + m_collisionShapes.push_back(colShape); + + /// Create Dynamic Objects + btTransform startTransform; + startTransform.setIdentity(); + + btScalar mass(1.f); + + //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) + colShape->calculateLocalInertia(mass,localInertia); + + float start_x = START_POS_X - ARRAY_SIZE_X/2; + float start_y = START_POS_Y; + float start_z = START_POS_Z - ARRAY_SIZE_Z/2; + + for (int k=0;kaddRigidBody(body);//,1,1+2); + } + } + } +} diff --git a/Demos3/bullet2/FeatherstoneMultiBodyDemo/MultiDofDemo.h b/Demos3/bullet2/FeatherstoneMultiBodyDemo/MultiDofDemo.h new file mode 100644 index 000000000..70bc5672e --- /dev/null +++ b/Demos3/bullet2/FeatherstoneMultiBodyDemo/MultiDofDemo.h @@ -0,0 +1,34 @@ + +#ifndef MULTI_DOF_DEMO_H +#define MULTI_DOF_DEMO_H + +#include "BulletMultiBodyDemos.h" + +class MultiDofDemo : public FeatherstoneDemo1 +{ + +public: + + MultiDofDemo(SimpleOpenGL3App* app); + virtual ~MultiDofDemo(); + + + static BulletDemoInterface* MyCreateFunc(SimpleOpenGL3App* app) + { + return new MultiDofDemo(app); + } + + virtual void initPhysics(); + + virtual void stepSimulation(float deltaTime); + + + btMultiBody* createFeatherstoneMultiBody_testMultiDof(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition, const btVector3 &baseHalfExtents, const btVector3 &linkHalfExtents, bool spherical = false, bool floating = false); + void addColliders_testMultiDof(btMultiBody *pMultiBody, btMultiBodyDynamicsWorld *pWorld, const btVector3 &baseHalfExtents, const btVector3 &linkHalfExtents); + void addBoxes_testMultiDof(); + + +}; + +#endif //MULTI_DOF_DEMO_H + diff --git a/Demos3/bullet2/RagdollDemo/RagdollDemo.cpp b/Demos3/bullet2/RagdollDemo/RagdollDemo.cpp index 1d545fd48..a4340546e 100644 --- a/Demos3/bullet2/RagdollDemo/RagdollDemo.cpp +++ b/Demos3/bullet2/RagdollDemo/RagdollDemo.cpp @@ -363,7 +363,7 @@ void RagDollDemo::initPhysics() createGround(cubeShapeId); - btVector3 offset(0,10,0); + btVector3 offset(0,3,0); RagDoll* doll = new RagDoll(this->m_dynamicsWorld,offset,m_glApp); diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index 8f6b0b851..a4243ddcd 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -50,7 +50,7 @@ public: const btVector3 &inertia, // inertia of base, in base frame; assumed diagonal bool fixedBase, // whether the base is fixed (true) or can move (false) bool canSleep, - bool multiDof = false + bool multiDof ); ~btMultiBody();