Bullet Collision Detection & Physics Library: btMultiBodyWorldImporter.cpp Source File

Bullet Collision Detection & Physics Library

btMultiBodyWorldImporter.cpp
Go to the documentation of this file.
1 #include "btMultiBodyWorldImporter.h"
2
3 #include "LinearMath/btSerializer.h"
4 #include "../BulletFileLoader/btBulletFile.h"
5 #include "btBulletWorldImporter.h"
6 #include "btBulletDynamicsCommon.h"
7 #include "BulletDynamics/Featherstone/btMultiBody.h"
8 #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
9 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
10
11 struct btMultiBodyWorldImporterInternalData
12 {
13 btMultiBodyDynamicsWorld* m_mbDynamicsWorld;
14 btHashMap<btHashPtr, btMultiBody*> m_mbMap;
15 };
16
17 btMultiBodyWorldImporter::btMultiBodyWorldImporter(btMultiBodyDynamicsWorld* world)
18 : btBulletWorldImporter(world)
19 {
20 m_data = new btMultiBodyWorldImporterInternalData;
21 m_data->m_mbDynamicsWorld = world;
22 }
23
24 btMultiBodyWorldImporter::~btMultiBodyWorldImporter()
25 {
26 delete m_data;
27 }
28
29 void btMultiBodyWorldImporter::deleteAllData()
30 {
31 btBulletWorldImporter::deleteAllData();
32 }
33
34 static btCollisionObjectDoubleData* getBody0FromContactManifold(btPersistentManifoldDoubleData* manifold)
35 {
36 return (btCollisionObjectDoubleData*)manifold->m_body0;
37 }
38 static btCollisionObjectDoubleData* getBody1FromContactManifold(btPersistentManifoldDoubleData* manifold)
39 {
40 return (btCollisionObjectDoubleData*)manifold->m_body1;
41 }
42 static btCollisionObjectFloatData* getBody0FromContactManifold(btPersistentManifoldFloatData* manifold)
43 {
44 return (btCollisionObjectFloatData*)manifold->m_body0;
45 }
46 static btCollisionObjectFloatData* getBody1FromContactManifold(btPersistentManifoldFloatData* manifold)
47 {
48 return (btCollisionObjectFloatData*)manifold->m_body1;
49 }
50
51 template <class T>
52 void syncContactManifolds(T** contactManifolds, int numContactManifolds, btMultiBodyWorldImporterInternalData* m_data)
53 {
54 m_data->m_mbDynamicsWorld->updateAabbs();
55 m_data->m_mbDynamicsWorld->computeOverlappingPairs();
56 btDispatcher* dispatcher = m_data->m_mbDynamicsWorld->getDispatcher();
57
58 btDispatcherInfo& dispatchInfo = m_data->m_mbDynamicsWorld->getDispatchInfo();
59
60 if (dispatcher)
61 {
62 btOverlappingPairCache* pairCache = m_data->m_mbDynamicsWorld->getBroadphase()->getOverlappingPairCache();
63 if (dispatcher)
64 {
65 dispatcher->dispatchAllCollisionPairs(pairCache, dispatchInfo, dispatcher);
66 }
67 int numExistingManifolds = m_data->m_mbDynamicsWorld->getDispatcher()->getNumManifolds();
68 btManifoldArray manifoldArray;
69 for (int i = 0; i < pairCache->getNumOverlappingPairs(); i++)
70 {
71 btBroadphasePair& pair = pairCache->getOverlappingPairArray()[i];
72 if (pair.m_algorithm)
73 {
74 pair.m_algorithm->getAllContactManifolds(manifoldArray);
75 //for each existing manifold, search a matching manifoldData and reconstruct
76 for (int m = 0; m < manifoldArray.size(); m++)
77 {
78 btPersistentManifold* existingManifold = manifoldArray[m];
79 int uid0 = existingManifold->getBody0()->getBroadphaseHandle()->m_uniqueId;
80 int uid1 = existingManifold->getBody1()->getBroadphaseHandle()->m_uniqueId;
81 int matchingManifoldIndex = -1;
82 for (int q = 0; q < numContactManifolds; q++)
83 {
84 if (uid0 == getBody0FromContactManifold(contactManifolds[q])->m_uniqueId && uid1 == getBody1FromContactManifold(contactManifolds[q])->m_uniqueId)
85 {
86 matchingManifoldIndex = q;
87 }
88 }
89 if (matchingManifoldIndex >= 0)
90 {
91 existingManifold->deSerialize(contactManifolds[matchingManifoldIndex]);
92 }
93 else
94 {
95 existingManifold->setNumContacts(0);
96 //printf("Issue: cannot find maching contact manifold (%d, %d), may cause issues in determinism.\n", uid0, uid1);
97 }
98
99 manifoldArray.clear();
100 }
101 }
102 }
103 }
104 }
105
106 template <class T>
107 void syncMultiBody(T* mbd, btMultiBody* mb, btMultiBodyWorldImporterInternalData* m_data, btAlignedObjectArray<btQuaternion>& scratchQ, btAlignedObjectArray<btVector3>& scratchM)
108 {
109 bool isFixedBase = mbd->m_baseMass == 0;
110 bool canSleep = false;
111 btVector3 baseInertia;
112 baseInertia.deSerialize(mbd->m_baseInertia);
113
114 btVector3 baseWorldPos;
115 baseWorldPos.deSerialize(mbd->m_baseWorldPosition);
116 mb->setBasePos(baseWorldPos);
117 btQuaternion baseWorldRot;
118 baseWorldRot.deSerialize(mbd->m_baseWorldOrientation);
119 mb->setWorldToBaseRot(baseWorldRot.inverse());
120 btVector3 baseLinVal;
121 baseLinVal.deSerialize(mbd->m_baseLinearVelocity);
122 btVector3 baseAngVel;
123 baseAngVel.deSerialize(mbd->m_baseAngularVelocity);
124 mb->setBaseVel(baseLinVal);
125 mb->setBaseOmega(baseAngVel);
126
127 for (int i = 0; i < mbd->m_numLinks; i++)
128 {
129 mb->getLink(i).m_absFrameTotVelocity.m_topVec.deSerialize(mbd->m_links[i].m_absFrameTotVelocityTop);
130 mb->getLink(i).m_absFrameTotVelocity.m_bottomVec.deSerialize(mbd->m_links[i].m_absFrameTotVelocityBottom);
131 mb->getLink(i).m_absFrameLocVelocity.m_topVec.deSerialize(mbd->m_links[i].m_absFrameLocVelocityTop);
132 mb->getLink(i).m_absFrameLocVelocity.m_bottomVec.deSerialize(mbd->m_links[i].m_absFrameLocVelocityBottom);
133
134 switch (mbd->m_links[i].m_jointType)
135 {
136 case btMultibodyLink::eFixed:
137 {
138 break;
139 }
140 case btMultibodyLink::ePrismatic:
141 {
142 mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
143 mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
144 break;
145 }
146 case btMultibodyLink::eRevolute:
147 {
148 mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
149 mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
150 break;
151 }
152 case btMultibodyLink::eSpherical:
153 {
154 btScalar jointPos[4] = {(btScalar)mbd->m_links[i].m_jointPos[0], (btScalar)mbd->m_links[i].m_jointPos[1], (btScalar)mbd->m_links[i].m_jointPos[2], (btScalar)mbd->m_links[i].m_jointPos[3]};
155 btScalar jointVel[3] = {(btScalar)mbd->m_links[i].m_jointVel[0], (btScalar)mbd->m_links[i].m_jointVel[1], (btScalar)mbd->m_links[i].m_jointVel[2]};
156 mb->setJointPosMultiDof(i, jointPos);
157 mb->setJointVelMultiDof(i, jointVel);
158
159 break;
160 }
161 case btMultibodyLink::ePlanar:
162 {
163 break;
164 }
165 default:
166 {
167 }
168 }
169 }
170 mb->forwardKinematics(scratchQ, scratchM);
171 mb->updateCollisionObjectWorldTransforms(scratchQ, scratchM);
172 }
173
174 template <class T>
175 void convertMultiBody(T* mbd, btMultiBodyWorldImporterInternalData* m_data)
176 {
177 bool isFixedBase = mbd->m_baseMass == 0;
178 bool canSleep = false;
179 btVector3 baseInertia;
180 baseInertia.deSerialize(mbd->m_baseInertia);
181 btMultiBody* mb = new btMultiBody(mbd->m_numLinks, mbd->m_baseMass, baseInertia, isFixedBase, canSleep);
182 mb->setHasSelfCollision(false);
183
184 btVector3 baseWorldPos;
185 baseWorldPos.deSerialize(mbd->m_baseWorldPosition);
186
187 btQuaternion baseWorldOrn;
188 baseWorldOrn.deSerialize(mbd->m_baseWorldOrientation);
189 mb->setBasePos(baseWorldPos);
190 mb->setWorldToBaseRot(baseWorldOrn.inverse());
191
192 m_data->m_mbMap.insert(mbd, mb);
193 for (int i = 0; i < mbd->m_numLinks; i++)
194 {
195 btVector3 localInertiaDiagonal;
196 localInertiaDiagonal.deSerialize(mbd->m_links[i].m_linkInertia);
197 btQuaternion parentRotToThis;
198 parentRotToThis.deSerialize(mbd->m_links[i].m_zeroRotParentToThis);
199 btVector3 parentComToThisPivotOffset;
200 parentComToThisPivotOffset.deSerialize(mbd->m_links[i].m_parentComToThisPivotOffset);
201 btVector3 thisPivotToThisComOffset;
202 thisPivotToThisComOffset.deSerialize(mbd->m_links[i].m_thisPivotToThisComOffset);
203
204 switch (mbd->m_links[i].m_jointType)
205 {
206 case btMultibodyLink::eFixed:
207 {
208 mb->setupFixed(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
209 parentRotToThis, parentComToThisPivotOffset, thisPivotToThisComOffset);
210 //search for the collider
211 //mbd->m_links[i].m_linkCollider
212 break;
213 }
214 case btMultibodyLink::ePrismatic:
215 {
216 btVector3 jointAxis;
217 jointAxis.deSerialize(mbd->m_links[i].m_jointAxisBottom[0]);
218 bool disableParentCollision = true; //todo
219 mb->setupPrismatic(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
220 parentRotToThis, jointAxis, parentComToThisPivotOffset, thisPivotToThisComOffset, disableParentCollision);
221 mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
222 mb->finalizeMultiDof();
223 mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
224 break;
225 }
226 case btMultibodyLink::eRevolute:
227 {
228 btVector3 jointAxis;
229 jointAxis.deSerialize(mbd->m_links[i].m_jointAxisTop[0]);
230 bool disableParentCollision = true; //todo
231 mb->setupRevolute(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
232 parentRotToThis, jointAxis, parentComToThisPivotOffset, thisPivotToThisComOffset, disableParentCollision);
233 mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
234 mb->finalizeMultiDof();
235 mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
236 break;
237 }
238 case btMultibodyLink::eSpherical:
239 {
240 btAssert(0);
241 bool disableParentCollision = true; //todo
242 mb->setupSpherical(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
243 parentRotToThis, parentComToThisPivotOffset, thisPivotToThisComOffset, disableParentCollision);
244 btScalar jointPos[4] = {(btScalar)mbd->m_links[i].m_jointPos[0], (btScalar)mbd->m_links[i].m_jointPos[1], (btScalar)mbd->m_links[i].m_jointPos[2], (btScalar)mbd->m_links[i].m_jointPos[3]};
245 btScalar jointVel[3] = {(btScalar)mbd->m_links[i].m_jointVel[0], (btScalar)mbd->m_links[i].m_jointVel[1], (btScalar)mbd->m_links[i].m_jointVel[2]};
246 mb->setJointPosMultiDof(i, jointPos);
247 mb->finalizeMultiDof();
248 mb->setJointVelMultiDof(i, jointVel);
249
250 break;
251 }
252 case btMultibodyLink::ePlanar:
253 {
254 btAssert(0);
255 break;
256 }
257 default:
258 {
259 btAssert(0);
260 }
261 }
262 }
263 }
264
265 bool btMultiBodyWorldImporter::convertAllObjects(bParse::btBulletFile* bulletFile2)
266 {
267 bool result = false;
268 btAlignedObjectArray<btQuaternion> scratchQ;
269 btAlignedObjectArray<btVector3> scratchM;
270
271 if (m_importerFlags & eRESTORE_EXISTING_OBJECTS)
272 {
273 //check if the snapshot is valid for the existing world
274 //equal number of objects, # links etc
275 if ((bulletFile2->m_multiBodies.size() != m_data->m_mbDynamicsWorld->getNumMultibodies()))
276 {
277 printf("btMultiBodyWorldImporter::convertAllObjects error: expected %d multibodies, got %d.\n", m_data->m_mbDynamicsWorld->getNumMultibodies(), bulletFile2->m_multiBodies.size());
278 result = false;
279 return result;
280 }
281 result = true;
282
283 //convert all multibodies
284 if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
285 {
286 //for (int i = 0; i < bulletFile2->m_multiBodies.size(); i++)
287 for (int i = bulletFile2->m_multiBodies.size() - 1; i >= 0; i--)
288 {
289 btMultiBodyDoubleData* mbd = (btMultiBodyDoubleData*)bulletFile2->m_multiBodies[i];
290 btMultiBody* mb = m_data->m_mbDynamicsWorld->getMultiBody(i);
291 if (mbd->m_numLinks != mb->getNumLinks())
292 {
293 printf("btMultiBodyWorldImporter::convertAllObjects error: mismatch in number of links in a body (expected %d, found %d).\n", mbd->m_numLinks, mb->getNumLinks() );
294 result = false;
295 return result;
296 } else
297 {
298 syncMultiBody(mbd, mb, m_data, scratchQ, scratchM);
299 }
300 }
301
302 for (int i = bulletFile2->m_rigidBodies.size() - 1; i >= 0; i--)
303 {
304 btRigidBodyDoubleData* rbd = (btRigidBodyDoubleData*)bulletFile2->m_rigidBodies[i];
305 int foundRb = -1;
306 int uid = rbd->m_collisionObjectData.m_uniqueId;
307 for (int i = 0; i < m_data->m_mbDynamicsWorld->getNumCollisionObjects(); i++)
308 {
309 if (uid == m_data->m_mbDynamicsWorld->getCollisionObjectArray()[i]->getBroadphaseHandle()->m_uniqueId)
310 {
311 foundRb = i;
312 break;
313 }
314 }
315 if (foundRb >= 0)
316 {
317 btRigidBody* rb = btRigidBody::upcast(m_data->m_mbDynamicsWorld->getCollisionObjectArray()[foundRb]);
318 if (rb)
319 {
320 btTransform tr;
321 tr.deSerializeDouble(rbd->m_collisionObjectData.m_worldTransform);
322 rb->setWorldTransform(tr);
323 btVector3 linVel, angVel;
324 linVel.deSerializeDouble(rbd->m_linearVelocity);
325 angVel.deSerializeDouble(rbd->m_angularVelocity);
326 rb->setLinearVelocity(linVel);
327 rb->setAngularVelocity(angVel);
328 }
329 else
330 {
331 printf("btMultiBodyWorldImporter::convertAllObjects error: cannot find btRigidBody with bodyUniqueId %d\n", uid);
332 result = false;
333 }
334 }
335 else
336 {
337 printf("Error in btMultiBodyWorldImporter::convertAllObjects: didn't find bodyUniqueId: %d\n", uid);
338 result = false;
339 }
340 }
341
342 //todo: check why body1 pointer is not properly deserialized
343 for (int i = 0; i < bulletFile2->m_contactManifolds.size(); i++)
344 {
345 btPersistentManifoldDoubleData* manifoldData = (btPersistentManifoldDoubleData*)bulletFile2->m_contactManifolds[i];
346 {
347 void* ptr = bulletFile2->findLibPointer(manifoldData->m_body0);
348 if (ptr)
349 {
350 manifoldData->m_body0 = (btCollisionObjectDoubleData*)ptr;
351 }
352 }
353
354 {
355 void* ptr = bulletFile2->findLibPointer(manifoldData->m_body1);
356 if (ptr)
357 {
358 manifoldData->m_body1 = (btCollisionObjectDoubleData*)ptr;
359 }
360 }
361 }
362
363 if (bulletFile2->m_contactManifolds.size())
364 {
365 syncContactManifolds((btPersistentManifoldDoubleData**)&bulletFile2->m_contactManifolds[0], bulletFile2->m_contactManifolds.size(), m_data);
366 }
367 }
368 else
369 {
370 //single precision version
371 //for (int i = 0; i < bulletFile2->m_multiBodies.size(); i++)
372 for (int i = bulletFile2->m_multiBodies.size() - 1; i >= 0; i--)
373 {
374 btMultiBodyFloatData* mbd = (btMultiBodyFloatData*)bulletFile2->m_multiBodies[i];
375 btMultiBody* mb = m_data->m_mbDynamicsWorld->getMultiBody(i);
376 if (mbd->m_numLinks != mb->getNumLinks())
377 {
378 printf("btMultiBodyWorldImporter::convertAllObjects error: mismatch in number of links in a body (expected %d, found %d).\n", mbd->m_numLinks, mb->getNumLinks() );
379 result = false;
380 return result;
381 } else
382 {
383 syncMultiBody(mbd, mb, m_data, scratchQ, scratchM);
384 }
385 }
386
387 for (int i = bulletFile2->m_rigidBodies.size() - 1; i >= 0; i--)
388 {
389 btRigidBodyFloatData* rbd = (btRigidBodyFloatData*)bulletFile2->m_rigidBodies[i];
390 int foundRb = -1;
391 int uid = rbd->m_collisionObjectData.m_uniqueId;
392 for (int i = 0; i < m_data->m_mbDynamicsWorld->getNumCollisionObjects(); i++)
393 {
394 if (uid == m_data->m_mbDynamicsWorld->getCollisionObjectArray()[i]->getBroadphaseHandle()->m_uniqueId)
395 {
396 foundRb = i;
397 break;
398 }
399 }
400 if (foundRb >= 0)
401 {
402 btRigidBody* rb = btRigidBody::upcast(m_data->m_mbDynamicsWorld->getCollisionObjectArray()[foundRb]);
403 if (rb)
404 {
405 btTransform tr;
406 tr.deSerializeFloat(rbd->m_collisionObjectData.m_worldTransform);
407 rb->setWorldTransform(tr);
408 btVector3 linVel, angVel;
409 linVel.deSerializeFloat(rbd->m_linearVelocity);
410 angVel.deSerializeFloat(rbd->m_angularVelocity);
411 rb->setLinearVelocity(linVel);
412 rb->setAngularVelocity(angVel);
413 }
414 else
415 {
416 printf("btMultiBodyWorldImporter::convertAllObjects error: cannot find btRigidBody with bodyUniqueId %d\n", uid);
417 result = false;
418 }
419 }
420 else
421 {
422 printf("Error in btMultiBodyWorldImporter::convertAllObjects: didn't find bodyUniqueId: %d\n", uid);
423 result = false;
424 }
425 }
426
427 //todo: check why body1 pointer is not properly deserialized
428 for (int i = 0; i < bulletFile2->m_contactManifolds.size(); i++)
429 {
430 btPersistentManifoldFloatData* manifoldData = (btPersistentManifoldFloatData*)bulletFile2->m_contactManifolds[i];
431 {
432 void* ptr = bulletFile2->findLibPointer(manifoldData->m_body0);
433 if (ptr)
434 {
435 manifoldData->m_body0 = (btCollisionObjectFloatData*)ptr;
436 }
437 }
438 {
439 void* ptr = bulletFile2->findLibPointer(manifoldData->m_body1);
440 if (ptr)
441 {
442 manifoldData->m_body1 = (btCollisionObjectFloatData*)ptr;
443 }
444 }
445 }
446
447 if (bulletFile2->m_contactManifolds.size())
448 {
449 syncContactManifolds((btPersistentManifoldFloatData**)&bulletFile2->m_contactManifolds[0], bulletFile2->m_contactManifolds.size(), m_data);
450 }
451 }
452 }
453 else
454 {
455 result = btBulletWorldImporter::convertAllObjects(bulletFile2);
456
457 //convert all multibodies
458 for (int i = 0; i < bulletFile2->m_multiBodies.size(); i++)
459 {
460 if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
461 {
462 btMultiBodyDoubleData* mbd = (btMultiBodyDoubleData*)bulletFile2->m_multiBodies[i];
463 convertMultiBody(mbd, m_data);
464 }
465 else
466 {
467 btMultiBodyFloatData* mbd = (btMultiBodyFloatData*)bulletFile2->m_multiBodies[i];
468 convertMultiBody(mbd, m_data);
469 }
470 }
471
472 //forward kinematics, so that the link world transforms are valid, for collision detection
473 for (int i = 0; i < m_data->m_mbMap.size(); i++)
474 {
475 btMultiBody** ptr = m_data->m_mbMap.getAtIndex(i);
476 if (ptr)
477 {
478 btMultiBody* mb = *ptr;
479 mb->finalizeMultiDof();
480 btVector3 linvel = mb->getBaseVel();
481 btVector3 angvel = mb->getBaseOmega();
482 mb->forwardKinematics(scratchQ, scratchM);
483 }
484 }
485
486 //convert all multibody link colliders
487 for (int i = 0; i < bulletFile2->m_multiBodyLinkColliders.size(); i++)
488 {
489 if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
490 {
491 btMultiBodyLinkColliderDoubleData* mblcd = (btMultiBodyLinkColliderDoubleData*)bulletFile2->m_multiBodyLinkColliders[i];
492
493 btMultiBody** ptr = m_data->m_mbMap[mblcd->m_multiBody];
494 if (ptr)
495 {
496 btMultiBody* multiBody = *ptr;
497
498 btCollisionShape** shapePtr = m_shapeMap.find(mblcd->m_colObjData.m_collisionShape);
499 if (shapePtr && *shapePtr)
500 {
501 btTransform startTransform;
502 mblcd->m_colObjData.m_worldTransform.m_origin.m_floats[3] = 0.f;
503 startTransform.deSerializeDouble(mblcd->m_colObjData.m_worldTransform);
504
505 btCollisionShape* shape = (btCollisionShape*)*shapePtr;
506 if (shape)
507 {
508 btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(multiBody, mblcd->m_link);
509 col->setCollisionShape(shape);
510 //btCollisionObject* body = createCollisionObject(startTransform,shape,mblcd->m_colObjData.m_name);
511 col->setFriction(btScalar(mblcd->m_colObjData.m_friction));
512 col->setRestitution(btScalar(mblcd->m_colObjData.m_restitution));
513 //m_bodyMap.insert(colObjData,body);
514 if (mblcd->m_link == -1)
515 {
516 col->setWorldTransform(multiBody->getBaseWorldTransform());
517 multiBody->setBaseCollider(col);
518 }
519 else
520 {
521 col->setWorldTransform(multiBody->getLink(mblcd->m_link).m_cachedWorldTransform);
522 multiBody->getLink(mblcd->m_link).m_collider = col;
523 }
524 int mbLinkIndex = mblcd->m_link;
525
526 bool isDynamic = (mbLinkIndex < 0 && multiBody->hasFixedBase()) ? false : true;
527 int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
528 int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
529
530 #if 0
531 int colGroup = 0, colMask = 0;
532 int collisionFlags = mblcd->m_colObjData.m_collisionFlags;
533 if (collisionFlags & URDF_HAS_COLLISION_GROUP)
534 {
535 collisionFilterGroup = colGroup;
536 }
537 if (collisionFlags & URDF_HAS_COLLISION_MASK)
538 {
539 collisionFilterMask = colMask;
540 }
541 #endif
542 m_data->m_mbDynamicsWorld->addCollisionObject(col, collisionFilterGroup, collisionFilterMask);
543 }
544 }
545 else
546 {
547 printf("error: no shape found\n");
548 }
549 #if 0
550 //base and fixed? -> static, otherwise flag as dynamic
551
552 world1->addCollisionObject(col, collisionFilterGroup, collisionFilterMask);
553 #endif
554 }
555 }
556 }
557
558 for (int i = 0; i < m_data->m_mbMap.size(); i++)
559 {
560 btMultiBody** ptr = m_data->m_mbMap.getAtIndex(i);
561 if (ptr)
562 {
563 btMultiBody* mb = *ptr;
564 mb->finalizeMultiDof();
565
566 m_data->m_mbDynamicsWorld->addMultiBody(mb);
567 }
568 }
569 }
570 return result;
571 }
btMultiBody::getBaseVel
const btVector3 getBaseVel() const
Definition: btMultiBody.h:189
btCollisionWorld::computeOverlappingPairs
virtual void computeOverlappingPairs()
the computeOverlappingPairs is usually already called by performDiscreteCollisionDetection (or stepSi...
Definition: btCollisionWorld.cpp:215
btBroadphaseProxy
The btBroadphaseProxy is the main class that can be used with the Bullet broadphases.
Definition: btBroadphaseProxy.h:86
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:60
btMultiBody::setupRevolute
void setupRevolute(int i, btScalar mass, const btVector3 &inertia, int parentIndex, const btQuaternion &rotParentToThis, const btVector3 &jointAxis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision=false)
Definition: btMultiBody.cpp:216
btPersistentManifoldDoubleData
Definition: btPersistentManifold.h:272
btPersistentManifoldDoubleData::m_body1
btCollisionObjectDoubleData * m_body1
Definition: btPersistentManifold.h:314
btMultiBodyWorldImporter::m_data
struct btMultiBodyWorldImporterInternalData * m_data
Definition: btMultiBodyWorldImporter.h:8
btOverlappingPairCache::getOverlappingPairArray
virtual btBroadphasePairArray & getOverlappingPairArray()=0
btTransformDoubleData::m_origin
btVector3DoubleData m_origin
Definition: btTransform.h:253
btMultiBody::setJointPosMultiDof
void setJointPosMultiDof(int i, const double *q)
Definition: btMultiBody.cpp:409
btQuaternion
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:50
btCollisionObjectDoubleData::m_uniqueId
int m_uniqueId
Definition: btCollisionObject.h:634
btPersistentManifold::setNumContacts
void setNumContacts(int cachedPoints)
the setNumContacts API is usually not used, except when you gather/fill all contacts manually
Definition: btPersistentManifold.h:125
btMultibodyLink::eFixed
@ eFixed
Definition: btMultiBodyLink.h:76
btMultiBodyDynamicsWorld.h
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
btBroadphaseProxy::DefaultFilter
@ DefaultFilter
Definition: btBroadphaseProxy.h:92
convertMultiBody
void convertMultiBody(T *mbd, btMultiBodyWorldImporterInternalData *m_data)
Definition: btMultiBodyWorldImporter.cpp:175
btMultibodyLink::ePrismatic
@ ePrismatic
Definition: btMultiBodyLink.h:73
btDispatcher
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition: btDispatcher.h:77
btMultiBody::setWorldToBaseRot
void setWorldToBaseRot(const btQuaternion &rot)
Definition: btMultiBody.h:236
btCollisionObjectDoubleData
do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
Definition: btCollisionObject.h:604
btCollisionObjectFloatData
do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
Definition: btCollisionObject.h:639
btQuaternion::inverse
btQuaternion inverse() const
Return the inverse of this quaternion.
Definition: btQuaternion.h:497
bParse::btBulletFile::m_multiBodies
btAlignedObjectArray< bStructHandle * > m_multiBodies
Definition: btBulletFile.h:34
btMultiBody::setJointVelMultiDof
void setJointVelMultiDof(int i, const double *qdot)
Definition: btMultiBody.cpp:432
btMultiBodyWorldImporter::convertAllObjects
virtual bool convertAllObjects(bParse::btBulletFile *bulletFile2)
Definition: btMultiBodyWorldImporter.cpp:265
btPersistentManifold::deSerialize
void deSerialize(const struct btPersistentManifoldDoubleData *manifoldDataPtr)
Definition: btPersistentManifold.cpp:360
btMultiBodyWorldImporterInternalData::m_mbMap
btHashMap< btHashPtr, btMultiBody * > m_mbMap
Definition: btMultiBodyWorldImporter.cpp:14
btPersistentManifold::getBody0
const btCollisionObject * getBody0() const
Definition: btPersistentManifold.h:105
btMultibodyLink::m_absFrameTotVelocity
btSpatialMotionVector m_absFrameTotVelocity
Definition: btMultiBodyLink.h:68
btDispatcher::getNumManifolds
virtual int getNumManifolds() const =0
btAlignedObjectArray::clear
void clear()
clear the array, deallocated memory. Generally it is better to use array.resize(0),...
Definition: btAlignedObjectArray.h:176
getBody0FromContactManifold
static btCollisionObjectDoubleData * getBody0FromContactManifold(btPersistentManifoldDoubleData *manifold)
Definition: btMultiBodyWorldImporter.cpp:34
btOverlappingPairCache::getNumOverlappingPairs
virtual int getNumOverlappingPairs() const =0
btRigidBodyDoubleData::m_collisionObjectData
btCollisionObjectDoubleData m_collisionObjectData
Definition: btRigidBody.h:616
btMultiBody::updateCollisionObjectWorldTransforms
void updateCollisionObjectWorldTransforms(btAlignedObjectArray< btQuaternion > &world_to_local, btAlignedObjectArray< btVector3 > &local_origin)
Definition: btMultiBody.cpp:2126
btPersistentManifoldFloatData::m_body0
btCollisionObjectFloatData * m_body0
Definition: btPersistentManifold.h:360
btBroadphaseProxy::m_uniqueId
int m_uniqueId
Definition: btBroadphaseProxy.h:106
btVector3DoubleData::m_floats
double m_floats[4]
Definition: btVector3.h:1288
btMultiBodyWorldImporterInternalData
Definition: btMultiBodyWorldImporter.cpp:12
btCollisionObjectDoubleData::m_worldTransform
btTransformDoubleData m_worldTransform
Definition: btCollisionObject.h:610
btPersistentManifoldFloatData
Definition: btPersistentManifold.h:319
btCollisionObjectDoubleData::m_collisionShape
void * m_collisionShape
Definition: btCollisionObject.h:606
bParse::btBulletFile::m_rigidBodies
btAlignedObjectArray< bStructHandle * > m_rigidBodies
Definition: btBulletFile.h:40
btCollisionWorld::getBroadphase
const btBroadphaseInterface * getBroadphase() const
Definition: btCollisionWorld.h:117
btMultiBody::getBaseWorldTransform
btTransform getBaseWorldTransform() const
Definition: btMultiBody.h:222
getBody1FromContactManifold
static btCollisionObjectDoubleData * getBody1FromContactManifold(btPersistentManifoldDoubleData *manifold)
Definition: btMultiBodyWorldImporter.cpp:38
btMultiBody::setBaseVel
void setBaseVel(const btVector3 &vel)
Definition: btMultiBody.h:230
btCollisionObjectDoubleData::m_restitution
double m_restitution
Definition: btCollisionObject.h:621
btCollisionObjectFloatData::m_uniqueId
int m_uniqueId
Definition: btCollisionObject.h:669
btMultibodyLink::m_cachedWorldTransform
btTransform m_cachedWorldTransform
Definition: btMultiBodyLink.h:141
btWorldImporter::deleteAllData
virtual void deleteAllData()
delete all memory collision shapes, rigid bodies, constraints etc.
Definition: btWorldImporter.cpp:33
btMultiBody::setupPrismatic
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)
Definition: btMultiBody.cpp:178
btRigidBody::setLinearVelocity
void setLinearVelocity(const btVector3 &lin_vel)
Definition: btRigidBody.h:407
btCollisionShape
The btCollisionShape class provides an interface for collision shapes that can be shared among btColl...
Definition: btCollisionShape.h:28
btAssert
#define btAssert(x)
Definition: btScalar.h:153
btHashMap< btHashPtr, btMultiBody * >
btCollisionObject::setRestitution
void setRestitution(btScalar rest)
Definition: btCollisionObject.h:299
bParse::btBulletFile::m_contactManifolds
btAlignedObjectArray< bStructHandle * > m_contactManifolds
Definition: btBulletFile.h:54
btMultiBody::setBaseCollider
void setBaseCollider(btMultiBodyLinkCollider *collider)
Definition: btMultiBody.h:124
bParse::btBulletFile
Definition: btBulletFile.h:29
btCollisionObject::setCollisionShape
virtual void setCollisionShape(btCollisionShape *collisionShape)
Definition: btCollisionObject.h:219
btBroadphaseProxy::AllFilter
@ AllFilter
Definition: btBroadphaseProxy.h:98
btMultiBodyLinkCollider.h
btBulletDynamicsCommon.h
btCollisionObjectFloatData::m_worldTransform
btTransformFloatData m_worldTransform
Definition: btCollisionObject.h:645
btMultiBody::getBaseOmega
btVector3 getBaseOmega() const
Definition: btMultiBody.h:208
btPersistentManifoldFloatData::m_body1
btCollisionObjectFloatData * m_body1
Definition: btPersistentManifold.h:361
btMultiBodyWorldImporterInternalData::m_mbDynamicsWorld
btMultiBodyDynamicsWorld * m_mbDynamicsWorld
Definition: btMultiBodyWorldImporter.cpp:13
btMultiBody::hasFixedBase
bool hasFixedBase() const
Definition: btMultiBody.h:505
btMultiBodyLinkColliderDoubleData::m_multiBody
btMultiBodyDoubleData * m_multiBody
Definition: btMultiBodyLinkCollider.h:152
btOverlappingPairCache
The btOverlappingPairCache provides an interface for overlapping pair management (add,...
Definition: btOverlappingPairCache.h:51
btCollisionObject::getBroadphaseHandle
btBroadphaseProxy * getBroadphaseHandle()
Definition: btCollisionObject.h:383
btCollisionObject::setFriction
void setFriction(btScalar frict)
Definition: btCollisionObject.h:308
btMultiBody::setJointPos
void setJointPos(int i, btScalar q)
Definition: btMultiBody.cpp:402
btCollisionObject::setWorldTransform
void setWorldTransform(const btTransform &worldTrans)
Definition: btCollisionObject.h:377
btCollisionWorld::getNumCollisionObjects
int getNumCollisionObjects() const
Definition: btCollisionWorld.h:427
btMultiBody::setBaseOmega
void setBaseOmega(const btVector3 &omega)
Definition: btMultiBody.h:241
btBroadphasePair::m_algorithm
btCollisionAlgorithm * m_algorithm
Definition: btBroadphaseProxy.h:211
bParse::bFile::findLibPointer
void * findLibPointer(void *ptr)
Definition: bFile.cpp:1439
btSerializer.h
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btMultiBodyLinkCollider
Definition: btMultiBodyLinkCollider.h:33
btRigidBodyFloatData::m_collisionObjectData
btCollisionObjectFloatData m_collisionObjectData
Definition: btRigidBody.h:590
btMultiBody::getLink
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:114
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
btMultiBodyDoubleData::m_numLinks
int m_numLinks
Definition: btMultiBody.h:844
btMultibodyLink::eSpherical
@ eSpherical
Definition: btMultiBodyLink.h:74
btMultiBody::setHasSelfCollision
void setHasSelfCollision(bool hasSelfCollision)
Definition: btMultiBody.h:572
btMultiBody::setupSpherical
void setupSpherical(int i, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision=false)
Definition: btMultiBody.cpp:252
btMultibodyLink::eRevolute
@ eRevolute
Definition: btMultiBodyLink.h:72
btSpatialMotionVector::m_bottomVec
btVector3 m_bottomVec
Definition: btSpatialAlgebra.h:96
btPersistentManifold
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
Definition: btPersistentManifold.h:65
btMultiBody
Definition: btMultiBody.h:51
btMultiBody::setJointVel
void setJointVel(int i, btScalar qdot)
Definition: btMultiBody.cpp:427
btMultiBodyWorldImporter::deleteAllData
virtual void deleteAllData()
delete all memory collision shapes, rigid bodies, constraints etc.
Definition: btMultiBodyWorldImporter.cpp:29
btPersistentManifoldDoubleData::m_body0
btCollisionObjectDoubleData * m_body0
Definition: btPersistentManifold.h:313
btRigidBodyDoubleData
do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
Definition: btRigidBody.h:615
btRigidBodyFloatData
do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
Definition: btRigidBody.h:589
btMultiBodyDynamicsWorld::getMultiBody
btMultiBody * getMultiBody(int mbIndex)
Definition: btMultiBodyDynamicsWorld.h:71
btMultiBodyWorldImporter::btMultiBodyWorldImporter
btMultiBodyWorldImporter(class btMultiBodyDynamicsWorld *world)
Definition: btMultiBodyWorldImporter.cpp:17
btMultiBodyWorldImporter.h
btMultibodyLink::m_collider
class btMultiBodyLinkCollider * m_collider
Definition: btMultiBodyLink.h:132
btWorldImporter::m_importerFlags
int m_importerFlags
Definition: btWorldImporter.h:72
btDispatcherInfo
Definition: btDispatcher.h:31
btAlignedObjectArray< btPersistentManifold * >
btMultiBodyLinkColliderDoubleData::m_colObjData
btCollisionObjectDoubleData m_colObjData
Definition: btMultiBodyLinkCollider.h:151
bParse::bFile::getFlags
int getFlags() const
Definition: bFile.h:122
btRigidBody::setAngularVelocity
void setAngularVelocity(const btVector3 &ang_vel)
Definition: btRigidBody.h:413
btBulletWorldImporter
The btBulletWorldImporter is a starting point to import .bullet files.
Definition: btBulletWorldImporter.h:33
btMultiBody.h
btMultibodyLink::m_absFrameLocVelocity
btSpatialMotionVector m_absFrameLocVelocity
Definition: btMultiBodyLink.h:68
btRigidBodyDoubleData::m_linearVelocity
btVector3DoubleData m_linearVelocity
Definition: btRigidBody.h:618
btMultiBodyDoubleData
do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
Definition: btMultiBody.h:837
btDispatcher::dispatchAllCollisionPairs
virtual void dispatchAllCollisionPairs(btOverlappingPairCache *pairCache, const btDispatcherInfo &dispatchInfo, btDispatcher *dispatcher)=0
btMultiBody::forwardKinematics
void forwardKinematics(btAlignedObjectArray< btQuaternion > &world_to_local, btAlignedObjectArray< btVector3 > &local_origin)
Definition: btMultiBody.cpp:2083
btHashMap::insert
void insert(const Key &key, const Value &value)
Definition: btHashMap.h:264
btCollisionWorld::updateAabbs
virtual void updateAabbs()
Definition: btCollisionWorld.cpp:197
btCollisionObjectDoubleData::m_collisionFlags
int m_collisionFlags
Definition: btCollisionObject.h:626
btVector3::deSerializeDouble
void deSerializeDouble(const struct btVector3DoubleData &dataIn)
Definition: btVector3.h:1311
btRigidBodyFloatData::m_angularVelocity
btVector3FloatData m_angularVelocity
Definition: btRigidBody.h:593
btMultiBodyFloatData
do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
Definition: btMultiBody.h:854
btRigidBodyFloatData::m_linearVelocity
btVector3FloatData m_linearVelocity
Definition: btRigidBody.h:592
btMultiBodyFloatData::m_numLinks
int m_numLinks
Definition: btMultiBody.h:862
btMultiBodyDynamicsWorld::getNumMultibodies
virtual int getNumMultibodies() const
Definition: btMultiBodyDynamicsWorld.h:66
btBulletWorldImporter.h
btQuaternion::deSerialize
void deSerialize(const struct btQuaternionFloatData &dataIn)
Definition: btQuaternion.h:1009
btVector3::deSerialize
void deSerialize(const struct btVector3DoubleData &dataIn)
Definition: btVector3.h:1330
btCollisionAlgorithm::getAllContactManifolds
virtual void getAllContactManifolds(btManifoldArray &manifoldArray)=0
btCollisionWorld::getDispatcher
btDispatcher * getDispatcher()
Definition: btCollisionWorld.h:132
btVector3::deSerializeFloat
void deSerializeFloat(const struct btVector3FloatData &dataIn)
Definition: btVector3.h:1298
btSpatialMotionVector::m_topVec
btVector3 m_topVec
Definition: btSpatialAlgebra.h:96
btBroadphaseInterface::getOverlappingPairCache
virtual btOverlappingPairCache * getOverlappingPairCache()=0
btBulletWorldImporter::convertAllObjects
virtual bool convertAllObjects(bParse::btBulletFile *file)
Definition: btBulletWorldImporter.cpp:86
btPersistentManifold::getBody1
const btCollisionObject * getBody1() const
Definition: btPersistentManifold.h:106
eRESTORE_EXISTING_OBJECTS
@ eRESTORE_EXISTING_OBJECTS
Definition: btWorldImporter.h:63
btCollisionWorld::getCollisionObjectArray
btCollisionObjectArray & getCollisionObjectArray()
Definition: btCollisionWorld.h:476
btMultiBodyDynamicsWorld
The btMultiBodyDynamicsWorld adds Featherstone multi body dynamics to Bullet This implementation is s...
Definition: btMultiBodyDynamicsWorld.h:32
btMultiBodyLinkColliderDoubleData
Definition: btMultiBodyLinkCollider.h:150
syncMultiBody
void syncMultiBody(T *mbd, btMultiBody *mb, btMultiBodyWorldImporterInternalData *m_data, btAlignedObjectArray< btQuaternion > &scratchQ, btAlignedObjectArray< btVector3 > &scratchM)
Definition: btMultiBodyWorldImporter.cpp:107
btTransform::deSerializeFloat
void deSerializeFloat(const struct btTransformFloatData &dataIn)
Definition: btTransform.h:274
btTransform::deSerializeDouble
void deSerializeDouble(const struct btTransformDoubleData &dataIn)
Definition: btTransform.h:280
btMultiBodyLinkColliderDoubleData::m_link
int m_link
Definition: btMultiBodyLinkCollider.h:153
btCollisionWorld::getDispatchInfo
btDispatcherInfo & getDispatchInfo()
Definition: btCollisionWorld.h:490
btMultibodyLink::ePlanar
@ ePlanar
Definition: btMultiBodyLink.h:75
btMultiBody::setBasePos
void setBasePos(const btVector3 &pos)
Definition: btMultiBody.h:210
btBroadphasePair
The btBroadphasePair class contains a pair of aabb-overlapping objects.
Definition: btBroadphaseProxy.h:179
btRigidBody::upcast
static const btRigidBody * upcast(const btCollisionObject *colObj)
to keep collision detection and dynamics separate we don't store a rigidbody pointer but a rigidbody ...
Definition: btRigidBody.h:189
btCollisionObjectDoubleData::m_friction
double m_friction
Definition: btCollisionObject.h:617
btMultiBody::finalizeMultiDof
void finalizeMultiDof()
Definition: btMultiBody.cpp:343
btMultiBodyWorldImporter::~btMultiBodyWorldImporter
virtual ~btMultiBodyWorldImporter()
Definition: btMultiBodyWorldImporter.cpp:24
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:142
bParse::FD_DOUBLE_PRECISION
@ FD_DOUBLE_PRECISION
Definition: bFile.h:35
btRigidBodyDoubleData::m_angularVelocity
btVector3DoubleData m_angularVelocity
Definition: btRigidBody.h:619
syncContactManifolds
void syncContactManifolds(T **contactManifolds, int numContactManifolds, btMultiBodyWorldImporterInternalData *m_data)
Definition: btMultiBodyWorldImporter.cpp:52
btMultiBody::setupFixed
void setupFixed(int i, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool deprecatedDisableParentCollision=true)
Definition: btMultiBody.cpp:150