Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Cleanup SerialChains demo

Browse Source
This commit is contained in:
Jeongseok Lee
2018-08-07 15:18:58 -07:00
parent 340236ee29
commit 48e5a3a9ed
1 changed files with 49 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files

105
examples/MultiBody/SerialChains.cpp
View File

@@ -42,8 +42,9 @@ public:
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); 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); btMultiBody* createFeatherstoneMultiBody(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 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; static bool g_fixedBase = true;
@@ -94,7 +95,6 @@ void SerialChains::initPhysics()
g_fixedBase = !g_fixedBase; g_fixedBase = !g_fixedBase;
} }
btMultiBodyConstraintSolver* sol;
btMLCPSolverInterface* mlcp; btMLCPSolverInterface* mlcp;
switch (g_constraintSolverType++) switch (g_constraintSolverType++)
{ {
@@ -147,8 +147,8 @@ void SerialChains::initPhysics()
btVector3 linkHalfExtents(0.05, 0.37, 0.1); btVector3 linkHalfExtents(0.05, 0.37, 0.1);
btVector3 baseHalfExtents(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* mbC1 = createFeatherstoneMultiBody(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* mbC2 = createFeatherstoneMultiBody(world, numLinks, btVector3(-0.4f, 3.0f, 0.5f), linkHalfExtents, baseHalfExtents, spherical, g_fixedBase);
mbC1->setCanSleep(canSleep); mbC1->setCanSleep(canSleep);
mbC1->setHasSelfCollision(selfCollide); 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->setCanSleep(canSleep);
mbC2->setHasSelfCollision(selfCollide); 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; 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 //rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f); bool isDynamic = (mass != 0.f);
btVector3 localInertia(0, 0, 0); btVector3 localInertia(0, 0, 0);
if (isDynamic) if (isDynamic)
groundShape->calculateLocalInertia(mass, localInertia); groundShape->calculateLocalInertia(mass, localInertia);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
groundTransform.setIdentity(); groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, groundHeight, 0)); groundTransform.setOrigin(btVector3(0, groundHeight, 0));
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia); btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia);
btRigidBody* body = new btRigidBody(rbInfo); btRigidBody* body = new btRigidBody(rbInfo);
//add the body to the dynamics world //add the body to the dynamics world
m_dynamicsWorld->addRigidBody(body, 1, 1 + 2); //,1,1+2); 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 createGround();
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);
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); 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 //init the base
btVector3 baseInertiaDiag(0.f, 0.f, 0.f); btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
@@ -338,7 +306,32 @@ btMultiBody* SerialChains::createFeatherstoneMultiBody_testMultiDof(btMultiBodyD
return pMultiBody; 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<btQuaternion> world_to_local; btAlignedObjectArray<btQuaternion> world_to_local;
world_to_local.resize(pMultiBody->getNumLinks() + 1); world_to_local.resize(pMultiBody->getNumLinks() + 1);