4 #include "../BulletFileLoader/btBulletFile.h" 76 for (
int m = 0; m < manifoldArray.
size(); m++)
81 int matchingManifoldIndex = -1;
82 for (
int q = 0; q < numContactManifolds; q++)
86 matchingManifoldIndex = q;
89 if (matchingManifoldIndex >= 0)
91 existingManifold->
deSerialize(contactManifolds[matchingManifoldIndex]);
99 manifoldArray.
clear();
109 bool isFixedBase = mbd->m_baseMass == 0;
110 bool canSleep =
false;
115 baseWorldPos.
deSerialize(mbd->m_baseWorldPosition);
118 baseWorldRot.
deSerialize(mbd->m_baseWorldOrientation);
123 baseAngVel.
deSerialize(mbd->m_baseAngularVelocity);
127 for (
int i = 0; i < mbd->m_numLinks; i++)
134 switch (mbd->m_links[i].m_jointType)
177 bool isFixedBase = mbd->m_baseMass == 0;
178 bool canSleep =
false;
185 baseWorldPos.
deSerialize(mbd->m_baseWorldPosition);
188 baseWorldOrn.
deSerialize(mbd->m_baseWorldOrientation);
193 for (
int i = 0; i < mbd->m_numLinks; i++)
196 localInertiaDiagonal.
deSerialize(mbd->m_links[i].m_linkInertia);
198 parentRotToThis.
deSerialize(mbd->m_links[i].m_zeroRotParentToThis);
200 parentComToThisPivotOffset.
deSerialize(mbd->m_links[i].m_parentComToThisPivotOffset);
202 thisPivotToThisComOffset.
deSerialize(mbd->m_links[i].m_thisPivotToThisComOffset);
204 switch (mbd->m_links[i].m_jointType)
208 mb->
setupFixed(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
209 parentRotToThis, parentComToThisPivotOffset, thisPivotToThisComOffset);
217 jointAxis.
deSerialize(mbd->m_links[i].m_jointAxisBottom[0]);
218 bool disableParentCollision =
true;
219 mb->
setupPrismatic(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
220 parentRotToThis, jointAxis, parentComToThisPivotOffset, thisPivotToThisComOffset, disableParentCollision);
229 jointAxis.
deSerialize(mbd->m_links[i].m_jointAxisTop[0]);
230 bool disableParentCollision =
true;
231 mb->
setupRevolute(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
232 parentRotToThis, jointAxis, parentComToThisPivotOffset, thisPivotToThisComOffset, disableParentCollision);
241 bool disableParentCollision =
true;
242 mb->
setupSpherical(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
243 parentRotToThis, parentComToThisPivotOffset, thisPivotToThisComOffset, disableParentCollision);
293 printf(
"btMultiBodyWorldImporter::convertAllObjects error: mismatch in number of links in a body (expected %d, found %d).\n", mbd->
m_numLinks, mb->getNumLinks() );
331 printf(
"btMultiBodyWorldImporter::convertAllObjects error: cannot find btRigidBody with bodyUniqueId %d\n", uid);
337 printf(
"Error in btMultiBodyWorldImporter::convertAllObjects: didn't find bodyUniqueId: %d\n", uid);
378 printf(
"btMultiBodyWorldImporter::convertAllObjects error: mismatch in number of links in a body (expected %d, found %d).\n", mbd->
m_numLinks, mb->getNumLinks() );
416 printf(
"btMultiBodyWorldImporter::convertAllObjects error: cannot find btRigidBody with bodyUniqueId %d\n", uid);
422 printf(
"Error in btMultiBodyWorldImporter::convertAllObjects: didn't find bodyUniqueId: %d\n", uid);
458 for (
int i = 0; i < bulletFile2->m_multiBodies.size(); i++)
473 for (
int i = 0; i < m_data->m_mbMap.size(); i++)
487 for (
int i = 0; i < bulletFile2->m_multiBodyLinkColliders.size(); i++)
499 if (shapePtr && *shapePtr)
524 int mbLinkIndex = mblcd->
m_link;
526 bool isDynamic = (mbLinkIndex < 0 && multiBody->
hasFixedBase()) ?
false :
true;
531 int colGroup = 0, colMask = 0;
533 if (collisionFlags & URDF_HAS_COLLISION_GROUP)
535 collisionFilterGroup = colGroup;
537 if (collisionFlags & URDF_HAS_COLLISION_MASK)
539 collisionFilterMask = colMask;
542 m_data->m_mbDynamicsWorld->addCollisionObject(col, collisionFilterGroup, collisionFilterMask);
547 printf(
"error: no shape found\n");
552 world1->addCollisionObject(col, collisionFilterGroup, collisionFilterMask);
558 for (
int i = 0; i < m_data->m_mbMap.size(); i++)
566 m_data->m_mbDynamicsWorld->addMultiBody(mb);
btTransformFloatData m_worldTransform
static const btRigidBody * upcast(const btCollisionObject *colObj)
to keep collision detection and dynamics separate we don't store a rigidbody pointer but a rigidbody ...
virtual int getNumMultibodies() const
void deSerialize(const struct btQuaternionFloatData &dataIn)
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 ...
void setupRevolute(int linkIndex, btScalar mass, const btVector3 &inertia, int parentIndex, const btQuaternion &rotParentToThis, const btVector3 &jointAxis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision=false)
btAlignedObjectArray< bStructHandle * > m_contactManifolds
void setupFixed(int linkIndex, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool deprecatedDisableParentCollision=true)
void deSerializeDouble(const struct btVector3DoubleData &dataIn)
btCollisionObjectDoubleData * m_body1
virtual void updateAabbs()
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
void setJointVelMultiDof(int i, const double *qdot)
btVector3FloatData m_angularVelocity
static btCollisionObjectDoubleData * getBody1FromContactManifold(btPersistentManifoldDoubleData *manifold)
virtual void dispatchAllCollisionPairs(btOverlappingPairCache *pairCache, const btDispatcherInfo &dispatchInfo, btDispatcher *dispatcher)=0
btCollisionObjectFloatData * m_body1
class btMultiBodyLinkCollider * m_collider
btCollisionObjectDoubleData m_colObjData
do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 ...
void setupSpherical(int linkIndex, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision=false)
The btCollisionShape class provides an interface for collision shapes that can be shared among btColl...
btCollisionObjectArray & getCollisionObjectArray()
void setJointPosMultiDof(int i, const double *q)
void setBaseCollider(btMultiBodyLinkCollider *collider)
btSpatialMotionVector m_absFrameLocVelocity
btVector3DoubleData m_linearVelocity
struct btMultiBodyWorldImporterInternalData * m_data
void updateCollisionObjectWorldTransforms(btAlignedObjectArray< btQuaternion > &scratch_q, btAlignedObjectArray< btVector3 > &scratch_m)
void setWorldToBaseRot(const btQuaternion &rot)
virtual void computeOverlappingPairs()
the computeOverlappingPairs is usually already called by performDiscreteCollisionDetection (or stepSi...
btVector3 getBaseOmega() const
virtual ~btMultiBodyWorldImporter()
The btHashMap template class implements a generic and lightweight hashmap.
void setRestitution(btScalar rest)
btCollisionObjectFloatData m_collisionObjectData
int getNumCollisionObjects() const
virtual void deleteAllData()
delete all memory collision shapes, rigid bodies, constraints etc.
void setBaseVel(const btVector3 &vel)
void clear()
clear the array, deallocated memory. Generally it is better to use array.resize(0), to reduce performance overhead of run-time memory (de)allocations.
The btOverlappingPairCache provides an interface for overlapping pair management (add, remove, storage), used by the btBroadphaseInterface broadphases.
virtual void setCollisionShape(btCollisionShape *collisionShape)
btQuaternion inverse() const
Return the inverse of this quaternion.
virtual btOverlappingPairCache * getOverlappingPairCache()=0
void forwardKinematics(btAlignedObjectArray< btQuaternion > &scratch_q, btAlignedObjectArray< btVector3 > &scratch_m)
void setNumContacts(int cachedPoints)
the setNumContacts API is usually not used, except when you gather/fill all contacts manually ...
do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 ...
void setupPrismatic(int i, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &jointAxis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision)
void setHasSelfCollision(bool hasSelfCollision)
btBroadphaseProxy * getBroadphaseHandle()
void syncMultiBody(T *mbd, btMultiBody *mb, btMultiBodyWorldImporterInternalData *m_data, btAlignedObjectArray< btQuaternion > &scratchQ, btAlignedObjectArray< btVector3 > &scratchM)
btTransform getBaseWorldTransform() const
void setFriction(btScalar frict)
virtual btBroadphasePairArray & getOverlappingPairArray()=0
const btCollisionObject * getBody0() const
do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 ...
void setBaseOmega(const btVector3 &omega)
virtual void deleteAllData()
delete all memory collision shapes, rigid bodies, constraints etc.
btMultiBodyDynamicsWorld * m_mbDynamicsWorld
btMultiBody * getMultiBody(int mbIndex)
void deSerialize(const struct btVector3DoubleData &dataIn)
void insert(const Key &key, const Value &value)
void setLinearVelocity(const btVector3 &lin_vel)
bool hasFixedBase() const
The btRigidBody is the main class for rigid body objects.
void setJointVel(int i, btScalar qdot)
void setJointPos(int i, btScalar q)
void syncContactManifolds(T **contactManifolds, int numContactManifolds, btMultiBodyWorldImporterInternalData *m_data)
do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 ...
btCollisionObjectDoubleData * m_body0
void setWorldTransform(const btTransform &worldTrans)
btDispatcher * getDispatcher()
virtual int getNumOverlappingPairs() const =0
The btBroadphaseProxy is the main class that can be used with the Bullet broadphases.
btCollisionAlgorithm * m_algorithm
const btMultibodyLink & getLink(int index) const
btVector3 can be used to represent 3D points and vectors.
btVector3DoubleData m_angularVelocity
virtual void getAllContactManifolds(btManifoldArray &manifoldArray)=0
int size() const
return the number of elements in the array
btHashMap< btHashPtr, btMultiBody * > m_mbMap
btMultiBodyWorldImporter(class btMultiBodyDynamicsWorld *world)
virtual int getNumManifolds() const =0
The btBulletWorldImporter is a starting point to import .bullet files.
virtual bool convertAllObjects(bParse::btBulletFile *bulletFile2)
virtual bool convertAllObjects(bParse::btBulletFile *file)
btSpatialMotionVector m_absFrameTotVelocity
btCollisionObjectDoubleData m_collisionObjectData
btDispatcherInfo & getDispatchInfo()
btTransformDoubleData m_worldTransform
const btCollisionObject * getBody1() const
btAlignedObjectArray< bStructHandle * > m_multiBodies
void setAngularVelocity(const btVector3 &ang_vel)
btVector3FloatData m_linearVelocity
static btCollisionObjectDoubleData * getBody0FromContactManifold(btPersistentManifoldDoubleData *manifold)
btTransform m_cachedWorldTransform
do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 ...
void * findLibPointer(void *ptr)
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
void deSerialize(const struct btPersistentManifoldDoubleData *manifoldDataPtr)
btCollisionObjectFloatData * m_body0
void deSerializeFloat(const struct btVector3FloatData &dataIn)
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
void setBasePos(const btVector3 &pos)
btAlignedObjectArray< bStructHandle * > m_rigidBodies
const btBroadphaseInterface * getBroadphase() const
btMultiBodyDoubleData * m_multiBody
The btMultiBodyDynamicsWorld adds Featherstone multi body dynamics to Bullet This implementation is s...
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
void convertMultiBody(T *mbd, btMultiBodyWorldImporterInternalData *m_data)
const btVector3 getBaseVel() const
The btBroadphasePair class contains a pair of aabb-overlapping objects.