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

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

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
17btMultiBodyWorldImporter::btMultiBodyWorldImporter(btMultiBodyDynamicsWorld* world)
18 : btBulletWorldImporter(world)
19{
20 m_data = new btMultiBodyWorldImporterInternalData;
21 m_data->m_mbDynamicsWorld = world;
22}
23
24btMultiBodyWorldImporter::~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
51template <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
106template <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
174template <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}
btBulletDynamicsCommon.h
btBulletFile.h
btBulletWorldImporter.h
btMultiBodyDynamicsWorld.h
btMultiBodyLinkCollider.h
getBody1FromContactManifold
static btCollisionObjectDoubleData * getBody1FromContactManifold(btPersistentManifoldDoubleData *manifold)
Definition btMultiBodyWorldImporter.cpp:38
syncMultiBody
void syncMultiBody(T *mbd, btMultiBody *mb, btMultiBodyWorldImporterInternalData *m_data, btAlignedObjectArray< btQuaternion > &scratchQ, btAlignedObjectArray< btVector3 > &scratchM)
Definition btMultiBodyWorldImporter.cpp:107
syncContactManifolds
void syncContactManifolds(T **contactManifolds, int numContactManifolds, btMultiBodyWorldImporterInternalData *m_data)
Definition btMultiBodyWorldImporter.cpp:52
convertMultiBody
void convertMultiBody(T *mbd, btMultiBodyWorldImporterInternalData *m_data)
Definition btMultiBodyWorldImporter.cpp:175
getBody0FromContactManifold
static btCollisionObjectDoubleData * getBody0FromContactManifold(btPersistentManifoldDoubleData *manifold)
Definition btMultiBodyWorldImporter.cpp:34
btMultiBodyWorldImporter.h
btMultiBody.h
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition btScalar.h:314
btAssert
#define btAssert(x)
Definition btScalar.h:153
btSerializer.h
eRESTORE_EXISTING_OBJECTS
@ eRESTORE_EXISTING_OBJECTS
Definition btWorldImporter.h:63
bParse::bFile::getFlags
int getFlags() const
Definition bFile.h:122
bParse::bFile::findLibPointer
void * findLibPointer(void *ptr)
Definition bFile.cpp:1440
bParse::btBulletFile
Definition btBulletFile.h:29
bParse::btBulletFile::m_rigidBodies
btAlignedObjectArray< bStructHandle * > m_rigidBodies
Definition btBulletFile.h:40
bParse::btBulletFile::m_multiBodies
btAlignedObjectArray< bStructHandle * > m_multiBodies
Definition btBulletFile.h:34
bParse::btBulletFile::m_contactManifolds
btAlignedObjectArray< bStructHandle * > m_contactManifolds
Definition btBulletFile.h:54
bParse::btBulletFile::m_multiBodyLinkColliders
btAlignedObjectArray< bStructHandle * > m_multiBodyLinkColliders
Definition btBulletFile.h:36
btAlignedObjectArray< btPersistentManifold * >
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition btAlignedObjectArray.h:142
btAlignedObjectArray::clear
void clear()
clear the array, deallocated memory. Generally it is better to use array.resize(0),...
Definition btAlignedObjectArray.h:176
btBroadphaseInterface::getOverlappingPairCache
virtual btOverlappingPairCache * getOverlappingPairCache()=0
btBulletWorldImporter
The btBulletWorldImporter is a starting point to import .bullet files.
Definition btBulletWorldImporter.h:33
btBulletWorldImporter::convertAllObjects
virtual bool convertAllObjects(bParse::btBulletFile *file)
Definition btBulletWorldImporter.cpp:86
btCollisionAlgorithm::getAllContactManifolds
virtual void getAllContactManifolds(btManifoldArray &manifoldArray)=0
btCollisionObject::setRestitution
void setRestitution(btScalar rest)
Definition btCollisionObject.h:311
btCollisionObject::getBroadphaseHandle
btBroadphaseProxy * getBroadphaseHandle()
Definition btCollisionObject.h:395
btCollisionObject::setCollisionShape
virtual void setCollisionShape(btCollisionShape *collisionShape)
Definition btCollisionObject.h:221
btCollisionObject::setWorldTransform
void setWorldTransform(const btTransform &worldTrans)
Definition btCollisionObject.h:389
btCollisionObject::setFriction
void setFriction(btScalar frict)
Definition btCollisionObject.h:320
btCollisionShape
The btCollisionShape class provides an interface for collision shapes that can be shared among btColl...
Definition btCollisionShape.h:28
btCollisionWorld::updateAabbs
virtual void updateAabbs()
Definition btCollisionWorld.cpp:197
btCollisionWorld::getDispatcher
btDispatcher * getDispatcher()
Definition btCollisionWorld.h:132
btCollisionWorld::getDispatchInfo
btDispatcherInfo & getDispatchInfo()
Definition btCollisionWorld.h:490
btCollisionWorld::getNumCollisionObjects
int getNumCollisionObjects() const
Definition btCollisionWorld.h:427
btCollisionWorld::getCollisionObjectArray
btCollisionObjectArray & getCollisionObjectArray()
Definition btCollisionWorld.h:476
btCollisionWorld::getBroadphase
const btBroadphaseInterface * getBroadphase() const
Definition btCollisionWorld.h:117
btCollisionWorld::computeOverlappingPairs
virtual void computeOverlappingPairs()
the computeOverlappingPairs is usually already called by performDiscreteCollisionDetection (or stepSi...
Definition btCollisionWorld.cpp:214
btDiscreteDynamicsWorld::addCollisionObject
virtual void addCollisionObject(btCollisionObject *collisionObject, int collisionFilterGroup=btBroadphaseProxy::StaticFilter, int collisionFilterMask=btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter)
Definition btDiscreteDynamicsWorld.cpp:517
btDispatcher
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition btDispatcher.h:77
btDispatcher::getNumManifolds
virtual int getNumManifolds() const =0
btDispatcher::dispatchAllCollisionPairs
virtual void dispatchAllCollisionPairs(btOverlappingPairCache *pairCache, const btDispatcherInfo &dispatchInfo, btDispatcher *dispatcher)=0
btHashMap
The btHashMap template class implements a generic and lightweight hashmap.
Definition btHashMap.h:220
btHashMap::insert
void insert(const Key &key, const Value &value)
Definition btHashMap.h:264
btHashMap::size
int size() const
Definition btHashMap.h:373
btHashMap::getAtIndex
const Value * getAtIndex(int index) const
Definition btHashMap.h:378
btHashMap::find
const Value * find(const Key &key) const
Definition btHashMap.h:424
btMultiBodyDynamicsWorld
The btMultiBodyDynamicsWorld adds Featherstone multi body dynamics to Bullet This implementation is s...
Definition btMultiBodyDynamicsWorld.h:32
btMultiBodyDynamicsWorld::getMultiBody
btMultiBody * getMultiBody(int mbIndex)
Definition btMultiBodyDynamicsWorld.h:71
btMultiBodyDynamicsWorld::getNumMultibodies
virtual int getNumMultibodies() const
Definition btMultiBodyDynamicsWorld.h:66
btMultiBodyDynamicsWorld::addMultiBody
virtual void addMultiBody(btMultiBody *body, int group=btBroadphaseProxy::DefaultFilter, int mask=btBroadphaseProxy::AllFilter)
Definition btMultiBodyDynamicsWorld.cpp:26
btMultiBodyLinkCollider
Definition btMultiBodyLinkCollider.h:33
btMultiBodyWorldImporter::deleteAllData
virtual void deleteAllData()
delete all memory collision shapes, rigid bodies, constraints etc.
Definition btMultiBodyWorldImporter.cpp:29
btMultiBodyWorldImporter::convertAllObjects
virtual bool convertAllObjects(bParse::btBulletFile *bulletFile2)
Definition btMultiBodyWorldImporter.cpp:265
btMultiBodyWorldImporter::m_data
struct btMultiBodyWorldImporterInternalData * m_data
Definition btMultiBodyWorldImporter.h:8
btMultiBodyWorldImporter::~btMultiBodyWorldImporter
virtual ~btMultiBodyWorldImporter()
Definition btMultiBodyWorldImporter.cpp:24
btMultiBodyWorldImporter::btMultiBodyWorldImporter
btMultiBodyWorldImporter(class btMultiBodyDynamicsWorld *world)
Definition btMultiBodyWorldImporter.cpp:17
btMultiBody
Definition btMultiBody.h:51
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:182
btMultiBody::setJointVelMultiDof
void setJointVelMultiDof(int i, const double *qdot)
Definition btMultiBody.cpp:438
btMultiBody::finalizeMultiDof
void finalizeMultiDof()
Definition btMultiBody.cpp:347
btMultiBody::setHasSelfCollision
void setHasSelfCollision(bool hasSelfCollision)
Definition btMultiBody.h:631
btMultiBody::setBaseVel
void setBaseVel(const btVector3 &vel)
Definition btMultiBody.h:250
btMultiBody::setBaseOmega
void setBaseOmega(const btVector3 &omega)
Definition btMultiBody.h:269
btMultiBody::getNumLinks
int getNumLinks() const
Definition btMultiBody.h:166
btMultiBody::getLink
const btMultibodyLink & getLink(int index) const
Definition btMultiBody.h:114
btMultiBody::getBaseOmega
btVector3 getBaseOmega() const
Definition btMultiBody.h:208
btMultiBody::setBasePos
void setBasePos(const btVector3 &pos)
Definition btMultiBody.h:210
btMultiBody::setJointPosMultiDof
void setJointPosMultiDof(int i, const double *q)
Definition btMultiBody.cpp:415
btMultiBody::updateCollisionObjectWorldTransforms
void updateCollisionObjectWorldTransforms(btAlignedObjectArray< btQuaternion > &world_to_local, btAlignedObjectArray< btVector3 > &local_origin)
Definition btMultiBody.cpp:2172
btMultiBody::setJointVel
void setJointVel(int i, btScalar qdot)
Definition btMultiBody.cpp:433
btMultiBody::hasFixedBase
bool hasFixedBase() const
Definition btMultiBody.cpp:680
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:256
btMultiBody::setJointPos
void setJointPos(int i, btScalar q)
Definition btMultiBody.cpp:408
btMultiBody::getBaseWorldTransform
btTransform getBaseWorldTransform() const
Definition btMultiBody.h:228
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:220
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:154
btMultiBody::setWorldToBaseRot
void setWorldToBaseRot(const btQuaternion &rot)
Definition btMultiBody.h:257
btMultiBody::forwardKinematics
void forwardKinematics(btAlignedObjectArray< btQuaternion > &world_to_local, btAlignedObjectArray< btVector3 > &local_origin)
Definition btMultiBody.cpp:2129
btMultiBody::getBaseVel
const btVector3 getBaseVel() const
Definition btMultiBody.h:189
btMultiBody::setBaseCollider
void setBaseCollider(btMultiBodyLinkCollider *collider)
Definition btMultiBody.h:124
btOverlappingPairCache
The btOverlappingPairCache provides an interface for overlapping pair management (add,...
Definition btOverlappingPairCache.h:51
btOverlappingPairCache::getOverlappingPairArray
virtual btBroadphasePairArray & getOverlappingPairArray()=0
btOverlappingPairCache::getNumOverlappingPairs
virtual int getNumOverlappingPairs() const =0
btPersistentManifold
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
Definition btPersistentManifold.h:65
btPersistentManifold::getBody0
const btCollisionObject * getBody0() const
Definition btPersistentManifold.h:105
btPersistentManifold::getBody1
const btCollisionObject * getBody1() const
Definition btPersistentManifold.h:106
btPersistentManifold::deSerialize
void deSerialize(const struct btPersistentManifoldDoubleData *manifoldDataPtr)
Definition btPersistentManifold.cpp:360
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
btQuaternion
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition btQuaternion.h:50
btQuaternion::inverse
btQuaternion inverse() const
Return the inverse of this quaternion.
Definition btQuaternion.h:497
btQuaternion::deSerialize
void deSerialize(const struct btQuaternionFloatData &dataIn)
Definition btQuaternion.h:1009
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition btRigidBody.h:60
btRigidBody::setAngularVelocity
void setAngularVelocity(const btVector3 &ang_vel)
Definition btRigidBody.h:451
btRigidBody::setLinearVelocity
void setLinearVelocity(const btVector3 &lin_vel)
Definition btRigidBody.h:442
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
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition btTransform.h:30
btTransform::deSerializeFloat
void deSerializeFloat(const struct btTransformFloatData &dataIn)
Definition btTransform.h:275
btTransform::deSerializeDouble
void deSerializeDouble(const struct btTransformDoubleData &dataIn)
Definition btTransform.h:281
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition btVector3.h:82
btVector3::deSerialize
void deSerialize(const struct btVector3DoubleData &dataIn)
Definition btVector3.h:1330
btVector3::deSerializeFloat
void deSerializeFloat(const struct btVector3FloatData &dataIn)
Definition btVector3.h:1298
btVector3::deSerializeDouble
void deSerializeDouble(const struct btVector3DoubleData &dataIn)
Definition btVector3.h:1311
btWorldImporter::m_importerFlags
int m_importerFlags
Definition btWorldImporter.h:72
btWorldImporter::deleteAllData
virtual void deleteAllData()
delete all memory collision shapes, rigid bodies, constraints etc.
Definition btWorldImporter.cpp:33
btWorldImporter::m_shapeMap
btHashMap< btHashPtr, btCollisionShape * > m_shapeMap
Definition btWorldImporter.h:99
bParse::FD_DOUBLE_PRECISION
@ FD_DOUBLE_PRECISION
Definition bFile.h:35
btBroadphasePair
The btBroadphasePair class contains a pair of aabb-overlapping objects.
Definition btBroadphaseProxy.h:179
btBroadphasePair::m_algorithm
btCollisionAlgorithm * m_algorithm
Definition btBroadphaseProxy.h:211
btBroadphaseProxy::AllFilter
@ AllFilter
Definition btBroadphaseProxy.h:98
btBroadphaseProxy::DefaultFilter
@ DefaultFilter
Definition btBroadphaseProxy.h:92
btBroadphaseProxy::StaticFilter
@ StaticFilter
Definition btBroadphaseProxy.h:93
btBroadphaseProxy::m_uniqueId
int m_uniqueId
Definition btBroadphaseProxy.h:106
btCollisionObjectDoubleData
do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
Definition btCollisionObject.h:616
btCollisionObjectDoubleData::m_restitution
double m_restitution
Definition btCollisionObject.h:633
btCollisionObjectDoubleData::m_collisionFlags
int m_collisionFlags
Definition btCollisionObject.h:638
btCollisionObjectDoubleData::m_uniqueId
int m_uniqueId
Definition btCollisionObject.h:646
btCollisionObjectDoubleData::m_collisionShape
void * m_collisionShape
Definition btCollisionObject.h:618
btCollisionObjectDoubleData::m_worldTransform
btTransformDoubleData m_worldTransform
Definition btCollisionObject.h:622
btCollisionObjectDoubleData::m_friction
double m_friction
Definition btCollisionObject.h:629
btCollisionObjectFloatData
do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
Definition btCollisionObject.h:651
btCollisionObjectFloatData::m_uniqueId
int m_uniqueId
Definition btCollisionObject.h:681
btCollisionObjectFloatData::m_worldTransform
btTransformFloatData m_worldTransform
Definition btCollisionObject.h:657
btDispatcherInfo
Definition btDispatcher.h:31
btMultiBodyDoubleData
do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
Definition btMultiBody.h:921
btMultiBodyDoubleData::m_numLinks
int m_numLinks
Definition btMultiBody.h:928
btMultiBodyFloatData
do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
Definition btMultiBody.h:938
btMultiBodyFloatData::m_numLinks
int m_numLinks
Definition btMultiBody.h:946
btMultiBodyLinkColliderDoubleData
Definition btMultiBodyLinkCollider.h:167
btMultiBodyLinkColliderDoubleData::m_link
int m_link
Definition btMultiBodyLinkCollider.h:170
btMultiBodyLinkColliderDoubleData::m_multiBody
btMultiBodyDoubleData * m_multiBody
Definition btMultiBodyLinkCollider.h:169
btMultiBodyLinkColliderDoubleData::m_colObjData
btCollisionObjectDoubleData m_colObjData
Definition btMultiBodyLinkCollider.h:168
btMultiBodyWorldImporterInternalData
Definition btMultiBodyWorldImporter.cpp:12
btMultiBodyWorldImporterInternalData::m_mbDynamicsWorld
btMultiBodyDynamicsWorld * m_mbDynamicsWorld
Definition btMultiBodyWorldImporter.cpp:13
btMultiBodyWorldImporterInternalData::m_mbMap
btHashMap< btHashPtr, btMultiBody * > m_mbMap
Definition btMultiBodyWorldImporter.cpp:14
btMultibodyLink::m_collider
class btMultiBodyLinkCollider * m_collider
Definition btMultiBodyLink.h:132
btMultibodyLink::ePlanar
@ ePlanar
Definition btMultiBodyLink.h:75
btMultibodyLink::ePrismatic
@ ePrismatic
Definition btMultiBodyLink.h:73
btMultibodyLink::eRevolute
@ eRevolute
Definition btMultiBodyLink.h:72
btMultibodyLink::eSpherical
@ eSpherical
Definition btMultiBodyLink.h:74
btMultibodyLink::eFixed
@ eFixed
Definition btMultiBodyLink.h:76
btMultibodyLink::m_absFrameTotVelocity
btSpatialMotionVector m_absFrameTotVelocity
Definition btMultiBodyLink.h:68
btMultibodyLink::m_cachedWorldTransform
btTransform m_cachedWorldTransform
Definition btMultiBodyLink.h:141
btMultibodyLink::m_absFrameLocVelocity
btSpatialMotionVector m_absFrameLocVelocity
Definition btMultiBodyLink.h:68
btPersistentManifoldDoubleData
Definition btPersistentManifold.h:272
btPersistentManifoldDoubleData::m_body0
btCollisionObjectDoubleData * m_body0
Definition btPersistentManifold.h:313
btPersistentManifoldDoubleData::m_body1
btCollisionObjectDoubleData * m_body1
Definition btPersistentManifold.h:314
btPersistentManifoldFloatData
Definition btPersistentManifold.h:319
btPersistentManifoldFloatData::m_body1
btCollisionObjectFloatData * m_body1
Definition btPersistentManifold.h:361
btPersistentManifoldFloatData::m_body0
btCollisionObjectFloatData * m_body0
Definition btPersistentManifold.h:360
btRigidBodyDoubleData
do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
Definition btRigidBody.h:662
btRigidBodyDoubleData::m_angularVelocity
btVector3DoubleData m_angularVelocity
Definition btRigidBody.h:666
btRigidBodyDoubleData::m_collisionObjectData
btCollisionObjectDoubleData m_collisionObjectData
Definition btRigidBody.h:663
btRigidBodyDoubleData::m_linearVelocity
btVector3DoubleData m_linearVelocity
Definition btRigidBody.h:665
btRigidBodyFloatData
do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
Definition btRigidBody.h:636
btRigidBodyFloatData::m_angularVelocity
btVector3FloatData m_angularVelocity
Definition btRigidBody.h:640
btRigidBodyFloatData::m_collisionObjectData
btCollisionObjectFloatData m_collisionObjectData
Definition btRigidBody.h:637
btRigidBodyFloatData::m_linearVelocity
btVector3FloatData m_linearVelocity
Definition btRigidBody.h:639
btSpatialMotionVector::m_bottomVec
btVector3 m_bottomVec
Definition btSpatialAlgebra.h:96
btSpatialMotionVector::m_topVec
btVector3 m_topVec
Definition btSpatialAlgebra.h:96
btTransformDoubleData::m_origin
btVector3DoubleData m_origin
Definition btTransform.h:254
btVector3DoubleData::m_floats
double m_floats[4]
Definition btVector3.h:1288