73 lines
2.7 KiB
C++
73 lines
2.7 KiB
C++
#include "MyMultiBodyCreator.h"
|
|
|
|
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
|
|
|
|
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
|
|
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
|
|
|
|
#include "BulletCollision/CollisionShapes/btCompoundShape.h"
|
|
|
|
#include "btBulletDynamicsCommon.h"
|
|
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
|
|
|
|
|
|
MyMultiBodyCreator::MyMultiBodyCreator(GUIHelperInterface* guiHelper)
|
|
:m_guiHelper(guiHelper),
|
|
m_bulletMultiBody(0)
|
|
{
|
|
}
|
|
|
|
|
|
class btMultiBody* MyMultiBodyCreator::allocateMultiBody(int /* urdfLinkIndex */, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep, bool multiDof)
|
|
{
|
|
// m_urdf2mbLink.resize(totalNumJoints+1,-2);
|
|
m_mb2urdfLink.resize(totalNumJoints+1,-2);
|
|
|
|
m_bulletMultiBody = new btMultiBody(totalNumJoints,mass,localInertiaDiagonal,isFixedBase,canSleep,multiDof);
|
|
return m_bulletMultiBody;
|
|
}
|
|
|
|
class btRigidBody* MyMultiBodyCreator::allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape)
|
|
{
|
|
btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, colShape, localInertiaDiagonal);
|
|
rbci.m_startWorldTransform = initialWorldTrans;
|
|
btRigidBody* body = new btRigidBody(rbci);
|
|
return body;
|
|
}
|
|
|
|
class btMultiBodyLinkCollider* MyMultiBodyCreator::allocateMultiBodyLinkCollider(int /*urdfLinkIndex*/, int mbLinkIndex, btMultiBody* multiBody)
|
|
{
|
|
btMultiBodyLinkCollider* mbCol= new btMultiBodyLinkCollider(multiBody, mbLinkIndex);
|
|
return mbCol;
|
|
}
|
|
|
|
|
|
class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder)
|
|
{
|
|
btGeneric6DofSpring2Constraint* c = new btGeneric6DofSpring2Constraint(rbA,rbB,offsetInA, offsetInB, (RotateOrder)rotateOrder);
|
|
return c;
|
|
}
|
|
|
|
void MyMultiBodyCreator::addLinkMapping(int urdfLinkIndex, int mbLinkIndex)
|
|
{
|
|
// m_urdf2mbLink[urdfLinkIndex] = mbLinkIndex;
|
|
m_mb2urdfLink[mbLinkIndex] = urdfLinkIndex;
|
|
}
|
|
|
|
void MyMultiBodyCreator::createRigidBodyGraphicsInstance(int linkIndex, btRigidBody* body, const btVector3& colorRgba, int graphicsIndex)
|
|
{
|
|
|
|
m_guiHelper->createRigidBodyGraphicsObject(body, colorRgba);
|
|
}
|
|
|
|
void MyMultiBodyCreator::createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* colObj, const btVector3& colorRgba)
|
|
{
|
|
m_guiHelper->createCollisionObjectGraphicsObject(colObj,colorRgba);
|
|
}
|
|
|
|
btMultiBody* MyMultiBodyCreator::getBulletMultiBody()
|
|
{
|
|
return m_bulletMultiBody;
|
|
}
|
|
|