00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016 #include "btSimpleBroadphase.h"
00017 #include "BulletCollision/BroadphaseCollision/btDispatcher.h"
00018 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
00019
00020 #include "LinearMath/btVector3.h"
00021 #include "LinearMath/btTransform.h"
00022 #include "LinearMath/btMatrix3x3.h"
00023 #include "LinearMath/btAabbUtil2.h"
00024
00025 #include <new>
00026
00027 extern int gOverlappingPairs;
00028
00029 void btSimpleBroadphase::validate()
00030 {
00031 for (int i=0;i<m_numHandles;i++)
00032 {
00033 for (int j=i+1;j<m_numHandles;j++)
00034 {
00035 btAssert(&m_pHandles[i] != &m_pHandles[j]);
00036 }
00037 }
00038
00039 }
00040
00041 btSimpleBroadphase::btSimpleBroadphase(int maxProxies, btOverlappingPairCache* overlappingPairCache)
00042 :m_pairCache(overlappingPairCache),
00043 m_ownsPairCache(false),
00044 m_invalidPair(0)
00045 {
00046
00047 if (!overlappingPairCache)
00048 {
00049 void* mem = btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16);
00050 m_pairCache = new (mem)btHashedOverlappingPairCache();
00051 m_ownsPairCache = true;
00052 }
00053
00054
00055 m_pHandlesRawPtr = btAlignedAlloc(sizeof(btSimpleBroadphaseProxy)*maxProxies,16);
00056 m_pHandles = new(m_pHandlesRawPtr) btSimpleBroadphaseProxy[maxProxies];
00057 m_maxHandles = maxProxies;
00058 m_numHandles = 0;
00059 m_firstFreeHandle = 0;
00060 m_LastHandleIndex = -1;
00061
00062
00063 {
00064 for (int i = m_firstFreeHandle; i < maxProxies; i++)
00065 {
00066 m_pHandles[i].SetNextFree(i + 1);
00067 m_pHandles[i].m_uniqueId = i+2;
00068 }
00069 m_pHandles[maxProxies - 1].SetNextFree(0);
00070
00071 }
00072
00073 }
00074
00075 btSimpleBroadphase::~btSimpleBroadphase()
00076 {
00077 btAlignedFree(m_pHandlesRawPtr);
00078
00079 if (m_ownsPairCache)
00080 {
00081 m_pairCache->~btOverlappingPairCache();
00082 btAlignedFree(m_pairCache);
00083 }
00084 }
00085
00086
00087 btBroadphaseProxy* btSimpleBroadphase::createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* ,void* multiSapProxy)
00088 {
00089 if (m_numHandles >= m_maxHandles)
00090 {
00091 btAssert(0);
00092 return 0;
00093 }
00094 btAssert(aabbMin[0]<= aabbMax[0] && aabbMin[1]<= aabbMax[1] && aabbMin[2]<= aabbMax[2]);
00095
00096 int newHandleIndex = allocHandle();
00097 btSimpleBroadphaseProxy* proxy = new (&m_pHandles[newHandleIndex])btSimpleBroadphaseProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy);
00098
00099 return proxy;
00100 }
00101
00102 class RemovingOverlapCallback : public btOverlapCallback
00103 {
00104 protected:
00105 virtual bool processOverlap(btBroadphasePair& pair)
00106 {
00107 (void)pair;
00108 btAssert(0);
00109 return false;
00110 }
00111 };
00112
00113 class RemovePairContainingProxy
00114 {
00115
00116 btBroadphaseProxy* m_targetProxy;
00117 public:
00118 virtual ~RemovePairContainingProxy()
00119 {
00120 }
00121 protected:
00122 virtual bool processOverlap(btBroadphasePair& pair)
00123 {
00124 btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy0);
00125 btSimpleBroadphaseProxy* proxy1 = static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy1);
00126
00127 return ((m_targetProxy == proxy0 || m_targetProxy == proxy1));
00128 };
00129 };
00130
00131 void btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg,btDispatcher* dispatcher)
00132 {
00133
00134 btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(proxyOrg);
00135 freeHandle(proxy0);
00136
00137 m_pairCache->removeOverlappingPairsContainingProxy(proxyOrg,dispatcher);
00138
00139
00140
00141 }
00142
00143 void btSimpleBroadphase::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
00144 {
00145 const btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy);
00146 aabbMin = sbp->m_aabbMin;
00147 aabbMax = sbp->m_aabbMax;
00148 }
00149
00150 void btSimpleBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* )
00151 {
00152 btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy);
00153 sbp->m_aabbMin = aabbMin;
00154 sbp->m_aabbMax = aabbMax;
00155 }
00156
00157 void btSimpleBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin,const btVector3& aabbMax)
00158 {
00159 for (int i=0; i <= m_LastHandleIndex; i++)
00160 {
00161 btSimpleBroadphaseProxy* proxy = &m_pHandles[i];
00162 if(!proxy->m_clientObject)
00163 {
00164 continue;
00165 }
00166 rayCallback.process(proxy);
00167 }
00168 }
00169
00170
00171 void btSimpleBroadphase::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback)
00172 {
00173 for (int i=0; i <= m_LastHandleIndex; i++)
00174 {
00175 btSimpleBroadphaseProxy* proxy = &m_pHandles[i];
00176 if(!proxy->m_clientObject)
00177 {
00178 continue;
00179 }
00180 if (TestAabbAgainstAabb2(aabbMin,aabbMax,proxy->m_aabbMin,proxy->m_aabbMax))
00181 {
00182 callback.process(proxy);
00183 }
00184 }
00185 }
00186
00187
00188
00189
00190
00191
00192
00193 bool btSimpleBroadphase::aabbOverlap(btSimpleBroadphaseProxy* proxy0,btSimpleBroadphaseProxy* proxy1)
00194 {
00195 return proxy0->m_aabbMin[0] <= proxy1->m_aabbMax[0] && proxy1->m_aabbMin[0] <= proxy0->m_aabbMax[0] &&
00196 proxy0->m_aabbMin[1] <= proxy1->m_aabbMax[1] && proxy1->m_aabbMin[1] <= proxy0->m_aabbMax[1] &&
00197 proxy0->m_aabbMin[2] <= proxy1->m_aabbMax[2] && proxy1->m_aabbMin[2] <= proxy0->m_aabbMax[2];
00198
00199 }
00200
00201
00202
00203
00204 class CheckOverlapCallback : public btOverlapCallback
00205 {
00206 public:
00207 virtual bool processOverlap(btBroadphasePair& pair)
00208 {
00209 return (!btSimpleBroadphase::aabbOverlap(static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy0),static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy1)));
00210 }
00211 };
00212
00213 void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
00214 {
00215
00216 int i,j;
00217 if (m_numHandles >= 0)
00218 {
00219 int new_largest_index = -1;
00220 for (i=0; i <= m_LastHandleIndex; i++)
00221 {
00222 btSimpleBroadphaseProxy* proxy0 = &m_pHandles[i];
00223 if(!proxy0->m_clientObject)
00224 {
00225 continue;
00226 }
00227 new_largest_index = i;
00228 for (j=i+1; j <= m_LastHandleIndex; j++)
00229 {
00230 btSimpleBroadphaseProxy* proxy1 = &m_pHandles[j];
00231 btAssert(proxy0 != proxy1);
00232 if(!proxy1->m_clientObject)
00233 {
00234 continue;
00235 }
00236
00237 btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0);
00238 btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1);
00239
00240 if (aabbOverlap(p0,p1))
00241 {
00242 if ( !m_pairCache->findPair(proxy0,proxy1))
00243 {
00244 m_pairCache->addOverlappingPair(proxy0,proxy1);
00245 }
00246 } else
00247 {
00248 if (!m_pairCache->hasDeferredRemoval())
00249 {
00250 if ( m_pairCache->findPair(proxy0,proxy1))
00251 {
00252 m_pairCache->removeOverlappingPair(proxy0,proxy1,dispatcher);
00253 }
00254 }
00255 }
00256 }
00257 }
00258
00259 m_LastHandleIndex = new_largest_index;
00260
00261 if (m_ownsPairCache && m_pairCache->hasDeferredRemoval())
00262 {
00263
00264 btBroadphasePairArray& overlappingPairArray = m_pairCache->getOverlappingPairArray();
00265
00266
00267 overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
00268
00269 overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
00270 m_invalidPair = 0;
00271
00272
00273 btBroadphasePair previousPair;
00274 previousPair.m_pProxy0 = 0;
00275 previousPair.m_pProxy1 = 0;
00276 previousPair.m_algorithm = 0;
00277
00278
00279 for (i=0;i<overlappingPairArray.size();i++)
00280 {
00281
00282 btBroadphasePair& pair = overlappingPairArray[i];
00283
00284 bool isDuplicate = (pair == previousPair);
00285
00286 previousPair = pair;
00287
00288 bool needsRemoval = false;
00289
00290 if (!isDuplicate)
00291 {
00292 bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
00293
00294 if (hasOverlap)
00295 {
00296 needsRemoval = false;
00297 } else
00298 {
00299 needsRemoval = true;
00300 }
00301 } else
00302 {
00303
00304 needsRemoval = true;
00305
00306 btAssert(!pair.m_algorithm);
00307 }
00308
00309 if (needsRemoval)
00310 {
00311 m_pairCache->cleanOverlappingPair(pair,dispatcher);
00312
00313
00314
00315 pair.m_pProxy0 = 0;
00316 pair.m_pProxy1 = 0;
00317 m_invalidPair++;
00318 gOverlappingPairs--;
00319 }
00320
00321 }
00322
00324 #define CLEAN_INVALID_PAIRS 1
00325 #ifdef CLEAN_INVALID_PAIRS
00326
00327
00328 overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
00329
00330 overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
00331 m_invalidPair = 0;
00332 #endif//CLEAN_INVALID_PAIRS
00333
00334 }
00335 }
00336 }
00337
00338
00339 bool btSimpleBroadphase::testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
00340 {
00341 btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0);
00342 btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1);
00343 return aabbOverlap(p0,p1);
00344 }
00345
00346 void btSimpleBroadphase::resetPool(btDispatcher* dispatcher)
00347 {
00348
00349 }