From 48e5a3a9ed57396607cda24f0ac2ea5c477f3e72 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 7 Aug 2018 15:18:58 -0700 Subject: [PATCH] Cleanup SerialChains demo --- examples/MultiBody/SerialChains.cpp | 105 +++++++++++++--------------- 1 file changed, 49 insertions(+), 56 deletions(-) diff --git a/examples/MultiBody/SerialChains.cpp b/examples/MultiBody/SerialChains.cpp index c4c3d0565..ac545662c 100644 --- a/examples/MultiBody/SerialChains.cpp +++ b/examples/MultiBody/SerialChains.cpp @@ -42,8 +42,9 @@ public: m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); } - 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); + btMultiBody* createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical = false, bool floating = false); + void createGround(const btVector3& halfExtents = btVector3(50, 50, 50), btScalar zOffSet = btScalar(-1.55)); + void addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents); }; static bool g_fixedBase = true; @@ -94,7 +95,6 @@ void SerialChains::initPhysics() g_fixedBase = !g_fixedBase; } - btMultiBodyConstraintSolver* sol; btMLCPSolverInterface* mlcp; switch (g_constraintSolverType++) { @@ -147,8 +147,8 @@ void SerialChains::initPhysics() btVector3 linkHalfExtents(0.05, 0.37, 0.1); btVector3 baseHalfExtents(0.05, 0.37, 0.1); - btMultiBody* mbC1 = createFeatherstoneMultiBody_testMultiDof(world, numLinks, btVector3(-0.4f, 3.f, 0.f), linkHalfExtents, baseHalfExtents, spherical, g_fixedBase); - btMultiBody* mbC2 = createFeatherstoneMultiBody_testMultiDof(world, numLinks, btVector3(-0.4f, 3.0f, 0.5f), linkHalfExtents, baseHalfExtents, spherical, g_fixedBase); + btMultiBody* mbC1 = createFeatherstoneMultiBody(world, numLinks, btVector3(-0.4f, 3.f, 0.f), linkHalfExtents, baseHalfExtents, spherical, g_fixedBase); + btMultiBody* mbC2 = createFeatherstoneMultiBody(world, numLinks, btVector3(-0.4f, 3.0f, 0.5f), linkHalfExtents, baseHalfExtents, spherical, g_fixedBase); mbC1->setCanSleep(canSleep); mbC1->setHasSelfCollision(selfCollide); @@ -182,7 +182,7 @@ void SerialChains::initPhysics() } } /// - addColliders_testMultiDof(mbC1, world, baseHalfExtents, linkHalfExtents); + addColliders(mbC1, world, baseHalfExtents, linkHalfExtents); mbC2->setCanSleep(canSleep); mbC2->setHasSelfCollision(selfCollide); @@ -216,70 +216,38 @@ void SerialChains::initPhysics() } } /// - addColliders_testMultiDof(mbC2, world, baseHalfExtents, linkHalfExtents); + addColliders(mbC2, world, baseHalfExtents, linkHalfExtents); ///////////////////////////////////////////////////////////////// btScalar groundHeight = -51.55; - if (!multibodyOnly) - { - btScalar mass(0.); + btScalar mass(0.); - //rigidbody is dynamic if and only if mass is non zero, otherwise static - bool isDynamic = (mass != 0.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) - groundShape->calculateLocalInertia(mass, localInertia); + 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); + //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); - } + //add the body to the dynamics world + m_dynamicsWorld->addRigidBody(body, 1, 1 + 2); //,1,1+2); ///////////////////////////////////////////////////////////////// - if (!multibodyOnly) - { - btVector3 halfExtents(.5, .5, .5); - btBoxShape* colShape = new btBoxShape(halfExtents); - 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.0, - 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); - } + createGround(); m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); ///////////////////////////////////////////////////////////////// } -btMultiBody* SerialChains::createFeatherstoneMultiBody_testMultiDof(btMultiBodyDynamicsWorld* pWorld, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical, bool fixedBase) +btMultiBody* SerialChains::createFeatherstoneMultiBody(btMultiBodyDynamicsWorld* pWorld, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical, bool fixedBase) { //init the base btVector3 baseInertiaDiag(0.f, 0.f, 0.f); @@ -338,7 +306,32 @@ btMultiBody* SerialChains::createFeatherstoneMultiBody_testMultiDof(btMultiBodyD return pMultiBody; } -void SerialChains::addColliders_testMultiDof(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents) +void SerialChains::createGround(const btVector3& halfExtents, btScalar zOffSet) +{ + btCollisionShape* groundShape = new btBoxShape(halfExtents); + m_collisionShapes.push_back(groundShape); + + // rigidbody is dynamic if and only if mass is non zero, otherwise static + btScalar mass(0.); + const 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 + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0, -halfExtents.z() + zOffSet, 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); +} + +void SerialChains::addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents) { btAlignedObjectArray world_to_local; world_to_local.resize(pMultiBody->getNumLinks() + 1);