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

77
examples/MultiBody/SerialChains.cpp
View File

@@ -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,12 +216,10 @@ void SerialChains::initPhysics()
}
}
///
addColliders_testMultiDof(mbC2, world, baseHalfExtents, linkHalfExtents);
addColliders(mbC2, world, baseHalfExtents, linkHalfExtents);
/////////////////////////////////////////////////////////////////
btScalar groundHeight = -51.55;
if (!multibodyOnly)
{
btScalar mass(0.);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
@@ -240,46 +238,16 @@ void SerialChains::initPhysics()
//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<btQuaternion> world_to_local;
world_to_local.resize(pMultiBody->getNumLinks() + 1);