00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016 #include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h"
00017 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
00018 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
00019 #include "BulletCollision/BroadphaseCollision/btDbvt.h"
00020 #include "LinearMath/btIDebugDraw.h"
00021 #include "LinearMath/btAabbUtil2.h"
00022 #include "btManifoldResult.h"
00023 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
00024
00025 btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped)
00026 :btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
00027 m_isSwapped(isSwapped),
00028 m_sharedManifold(ci.m_manifold)
00029 {
00030 m_ownsManifold = false;
00031
00032 const btCollisionObjectWrapper* colObjWrap = m_isSwapped? body1Wrap : body0Wrap;
00033 btAssert (colObjWrap->getCollisionShape()->isCompound());
00034
00035 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->getCollisionShape());
00036 m_compoundShapeRevision = compoundShape->getUpdateRevision();
00037
00038
00039 preallocateChildAlgorithms(body0Wrap,body1Wrap);
00040 }
00041
00042 void btCompoundCollisionAlgorithm::preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
00043 {
00044 const btCollisionObjectWrapper* colObjWrap = m_isSwapped? body1Wrap : body0Wrap;
00045 const btCollisionObjectWrapper* otherObjWrap = m_isSwapped? body0Wrap : body1Wrap;
00046 btAssert (colObjWrap->getCollisionShape()->isCompound());
00047
00048 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->getCollisionShape());
00049
00050 int numChildren = compoundShape->getNumChildShapes();
00051 int i;
00052
00053 m_childCollisionAlgorithms.resize(numChildren);
00054 for (i=0;i<numChildren;i++)
00055 {
00056 if (compoundShape->getDynamicAabbTree())
00057 {
00058 m_childCollisionAlgorithms[i] = 0;
00059 } else
00060 {
00061
00062 const btCollisionShape* childShape = compoundShape->getChildShape(i);
00063
00064 btCollisionObjectWrapper childWrap(colObjWrap,childShape,colObjWrap->getCollisionObject(),colObjWrap->getWorldTransform(),-1,i);
00065 m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(&childWrap,otherObjWrap,m_sharedManifold);
00066 }
00067 }
00068 }
00069
00070 void btCompoundCollisionAlgorithm::removeChildAlgorithms()
00071 {
00072 int numChildren = m_childCollisionAlgorithms.size();
00073 int i;
00074 for (i=0;i<numChildren;i++)
00075 {
00076 if (m_childCollisionAlgorithms[i])
00077 {
00078 m_childCollisionAlgorithms[i]->~btCollisionAlgorithm();
00079 m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
00080 }
00081 }
00082 }
00083
00084 btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm()
00085 {
00086 removeChildAlgorithms();
00087 }
00088
00089
00090
00091
00092 struct btCompoundLeafCallback : btDbvt::ICollide
00093 {
00094
00095 public:
00096
00097 const btCollisionObjectWrapper* m_compoundColObjWrap;
00098 const btCollisionObjectWrapper* m_otherObjWrap;
00099 btDispatcher* m_dispatcher;
00100 const btDispatcherInfo& m_dispatchInfo;
00101 btManifoldResult* m_resultOut;
00102 btCollisionAlgorithm** m_childCollisionAlgorithms;
00103 btPersistentManifold* m_sharedManifold;
00104
00105 btCompoundLeafCallback (const btCollisionObjectWrapper* compoundObjWrap,const btCollisionObjectWrapper* otherObjWrap,btDispatcher* dispatcher,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut,btCollisionAlgorithm** childCollisionAlgorithms,btPersistentManifold* sharedManifold)
00106 :m_compoundColObjWrap(compoundObjWrap),m_otherObjWrap(otherObjWrap),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut),
00107 m_childCollisionAlgorithms(childCollisionAlgorithms),
00108 m_sharedManifold(sharedManifold)
00109 {
00110
00111 }
00112
00113
00114 void ProcessChildShape(const btCollisionShape* childShape,int index)
00115 {
00116 btAssert(index>=0);
00117 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(m_compoundColObjWrap->getCollisionShape());
00118 btAssert(index<compoundShape->getNumChildShapes());
00119
00120
00121
00122 btTransform orgTrans = m_compoundColObjWrap->getWorldTransform();
00123 btTransform orgInterpolationTrans = m_compoundColObjWrap->getWorldTransform();
00124 const btTransform& childTrans = compoundShape->getChildTransform(index);
00125 btTransform newChildWorldTrans = orgTrans*childTrans ;
00126
00127
00128 btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
00129 childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0);
00130 m_otherObjWrap->getCollisionShape()->getAabb(m_otherObjWrap->getWorldTransform(),aabbMin1,aabbMax1);
00131
00132 if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
00133 {
00134
00135 btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap,childShape,m_compoundColObjWrap->getCollisionObject(),newChildWorldTrans,-1,index);
00136
00137
00138
00139 if (!m_childCollisionAlgorithms[index])
00140 m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(&compoundWrap,m_otherObjWrap,m_sharedManifold);
00141
00142
00143 const btCollisionObjectWrapper* tmpWrap = 0;
00144
00146 if (m_resultOut->getBody0Internal() == m_compoundColObjWrap->getCollisionObject())
00147 {
00148 tmpWrap = m_resultOut->getBody0Wrap();
00149 m_resultOut->setBody0Wrap(&compoundWrap);
00150 m_resultOut->setShapeIdentifiersA(-1,index);
00151 } else
00152 {
00153 tmpWrap = m_resultOut->getBody1Wrap();
00154 m_resultOut->setBody1Wrap(&compoundWrap);
00155 m_resultOut->setShapeIdentifiersB(-1,index);
00156 }
00157
00158
00159 m_childCollisionAlgorithms[index]->processCollision(&compoundWrap,m_otherObjWrap,m_dispatchInfo,m_resultOut);
00160
00161 #if 0
00162 if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
00163 {
00164 btVector3 worldAabbMin,worldAabbMax;
00165 m_dispatchInfo.m_debugDraw->drawAabb(aabbMin0,aabbMax0,btVector3(1,1,1));
00166 m_dispatchInfo.m_debugDraw->drawAabb(aabbMin1,aabbMax1,btVector3(1,1,1));
00167 }
00168 #endif
00169
00170 if (m_resultOut->getBody0Internal() == m_compoundColObjWrap->getCollisionObject())
00171 {
00172 m_resultOut->setBody0Wrap(tmpWrap);
00173 } else
00174 {
00175 m_resultOut->setBody1Wrap(tmpWrap);
00176 }
00177
00178 }
00179 }
00180 void Process(const btDbvtNode* leaf)
00181 {
00182 int index = leaf->dataAsInt;
00183
00184 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(m_compoundColObjWrap->getCollisionShape());
00185 const btCollisionShape* childShape = compoundShape->getChildShape(index);
00186
00187 #if 0
00188 if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
00189 {
00190 btVector3 worldAabbMin,worldAabbMax;
00191 btTransform orgTrans = m_compoundColObjWrap->getWorldTransform();
00192 btTransformAabb(leaf->volume.Mins(),leaf->volume.Maxs(),0.,orgTrans,worldAabbMin,worldAabbMax);
00193 m_dispatchInfo.m_debugDraw->drawAabb(worldAabbMin,worldAabbMax,btVector3(1,0,0));
00194 }
00195 #endif
00196
00197 ProcessChildShape(childShape,index);
00198
00199 }
00200 };
00201
00202
00203
00204
00205
00206
00207 void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
00208 {
00209 const btCollisionObjectWrapper* colObjWrap = m_isSwapped? body1Wrap : body0Wrap;
00210 const btCollisionObjectWrapper* otherObjWrap = m_isSwapped? body0Wrap : body1Wrap;
00211
00212 btAssert (colObjWrap->getCollisionShape()->isCompound());
00213 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->getCollisionShape());
00214
00217 if (compoundShape->getUpdateRevision() != m_compoundShapeRevision)
00218 {
00220 removeChildAlgorithms();
00221
00222 preallocateChildAlgorithms(body0Wrap,body1Wrap);
00223 }
00224
00225
00226 const btDbvt* tree = compoundShape->getDynamicAabbTree();
00227
00228 btCompoundLeafCallback callback(colObjWrap,otherObjWrap,m_dispatcher,dispatchInfo,resultOut,&m_childCollisionAlgorithms[0],m_sharedManifold);
00229
00233 {
00234 int i;
00235 btManifoldArray manifoldArray;
00236 for (i=0;i<m_childCollisionAlgorithms.size();i++)
00237 {
00238 if (m_childCollisionAlgorithms[i])
00239 {
00240 m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray);
00241 for (int m=0;m<manifoldArray.size();m++)
00242 {
00243 if (manifoldArray[m]->getNumContacts())
00244 {
00245 resultOut->setPersistentManifold(manifoldArray[m]);
00246 resultOut->refreshContactPoints();
00247 resultOut->setPersistentManifold(0);
00248 }
00249 }
00250 manifoldArray.resize(0);
00251 }
00252 }
00253 }
00254
00255 if (tree)
00256 {
00257
00258 btVector3 localAabbMin,localAabbMax;
00259 btTransform otherInCompoundSpace;
00260 otherInCompoundSpace = colObjWrap->getWorldTransform().inverse() * otherObjWrap->getWorldTransform();
00261 otherObjWrap->getCollisionShape()->getAabb(otherInCompoundSpace,localAabbMin,localAabbMax);
00262
00263 const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
00264
00265 tree->collideTV(tree->m_root,bounds,callback);
00266
00267 } else
00268 {
00269
00270 int numChildren = m_childCollisionAlgorithms.size();
00271 int i;
00272 for (i=0;i<numChildren;i++)
00273 {
00274 callback.ProcessChildShape(compoundShape->getChildShape(i),i);
00275 }
00276 }
00277
00278 {
00279
00280 int numChildren = m_childCollisionAlgorithms.size();
00281 int i;
00282 btManifoldArray manifoldArray;
00283 const btCollisionShape* childShape = 0;
00284 btTransform orgTrans;
00285 btTransform orgInterpolationTrans;
00286 btTransform newChildWorldTrans;
00287 btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
00288
00289 for (i=0;i<numChildren;i++)
00290 {
00291 if (m_childCollisionAlgorithms[i])
00292 {
00293 childShape = compoundShape->getChildShape(i);
00294
00295 orgTrans = colObjWrap->getWorldTransform();
00296 orgInterpolationTrans = colObjWrap->getWorldTransform();
00297 const btTransform& childTrans = compoundShape->getChildTransform(i);
00298 newChildWorldTrans = orgTrans*childTrans ;
00299
00300
00301 childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0);
00302 otherObjWrap->getCollisionShape()->getAabb(otherObjWrap->getWorldTransform(),aabbMin1,aabbMax1);
00303
00304 if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
00305 {
00306 m_childCollisionAlgorithms[i]->~btCollisionAlgorithm();
00307 m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
00308 m_childCollisionAlgorithms[i] = 0;
00309 }
00310 }
00311 }
00312 }
00313 }
00314
00315 btScalar btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
00316 {
00317 btAssert(0);
00318
00319 btCollisionObject* colObj = m_isSwapped? body1 : body0;
00320 btCollisionObject* otherObj = m_isSwapped? body0 : body1;
00321
00322 btAssert (colObj->getCollisionShape()->isCompound());
00323
00324 btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
00325
00326
00327
00328
00329
00330
00331
00332
00333 btScalar hitFraction = btScalar(1.);
00334
00335 int numChildren = m_childCollisionAlgorithms.size();
00336 int i;
00337 btTransform orgTrans;
00338 btScalar frac;
00339 for (i=0;i<numChildren;i++)
00340 {
00341
00342
00343
00344 orgTrans = colObj->getWorldTransform();
00345
00346 const btTransform& childTrans = compoundShape->getChildTransform(i);
00347
00348 colObj->setWorldTransform( orgTrans*childTrans );
00349
00350
00351
00352 frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut);
00353 if (frac<hitFraction)
00354 {
00355 hitFraction = frac;
00356 }
00357
00358
00359 colObj->setWorldTransform( orgTrans);
00360 }
00361 return hitFraction;
00362
00363 }
00364
00365
00366