SpuGatheringCollisionTask.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 
00016 #include "SpuGatheringCollisionTask.h"
00017 
00018 //#define DEBUG_SPU_COLLISION_DETECTION 1
00019 #include "../SpuDoubleBuffer.h"
00020 
00021 #include "../SpuCollisionTaskProcess.h"
00022 #include "../SpuGatheringCollisionDispatcher.h" //for SPU_BATCHSIZE_BROADPHASE_PAIRS
00023 
00024 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
00025 #include "../SpuContactManifoldCollisionAlgorithm.h"
00026 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
00027 #include "SpuContactResult.h"
00028 #include "BulletCollision/CollisionShapes/btOptimizedBvh.h"
00029 #include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h"
00030 #include "BulletCollision/CollisionShapes/btSphereShape.h"
00031 #include "BulletCollision/CollisionShapes/btConvexPointCloudShape.h"
00032 
00033 #include "BulletCollision/CollisionShapes/btCapsuleShape.h"
00034 
00035 #include "BulletCollision/CollisionShapes/btConvexShape.h"
00036 #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
00037 #include "BulletCollision/CollisionShapes/btConvexHullShape.h"
00038 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
00039 
00040 #include "SpuMinkowskiPenetrationDepthSolver.h"
00041 //#include "SpuEpaPenetrationDepthSolver.h"
00042 #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
00043 
00044 
00045 #include "boxBoxDistance.h"
00046 #include "BulletMultiThreaded/vectormath2bullet.h"
00047 #include "SpuCollisionShapes.h" //definition of SpuConvexPolyhedronVertexData
00048 #include "BulletCollision/CollisionDispatch/btBoxBoxDetector.h"
00049 #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
00050 #include "BulletCollision/CollisionShapes/btTriangleShape.h"
00051 
00052 #ifdef __SPU__
00053 
00054 #ifndef USE_LIBSPE2
00055 //#define USE_SOFTWARE_CACHE 1
00056 #endif
00057 #endif //__SPU__
00058 
00059 int gSkippedCol = 0;
00060 int gProcessedCol = 0;
00061 
00064 #if USE_SOFTWARE_CACHE
00065 #include <spu_intrinsics.h>
00066 #include <sys/spu_thread.h>
00067 #include <sys/spu_event.h>
00068 #include <stdint.h>
00069 #define SPE_CACHE_NWAY                  4
00070 //#define SPE_CACHE_NSETS               32, 16
00071 #define SPE_CACHE_NSETS                 8
00072 //#define SPE_CACHELINE_SIZE            512
00073 #define SPE_CACHELINE_SIZE              128
00074 #define SPE_CACHE_SET_TAGID(set)        15
00075 
00076 #include "../Extras/software_cache/cache/include/spe_cache.h"
00077 
00078 
00079 int g_CacheMisses=0;
00080 int g_CacheHits=0;
00081 
00082 #if 0 // Added to allow cache misses and hits to be tracked, change this to 1 to restore unmodified version
00083 #define spe_cache_read(ea)              _spe_cache_lookup_xfer_wait_(ea, 0, 1)
00084 #else
00085 #define spe_cache_read(ea)              \
00086 ({                                                              \
00087     int set, idx, line, byte;                                   \
00088     _spe_cache_nway_lookup_(ea, set, idx);                      \
00089                                                                 \
00090     if (btUnlikely(idx < 0)) {                                  \
00091         ++g_CacheMisses;                        \
00092             idx = _spe_cache_miss_(ea, set, -1);                        \
00093         spu_writech(22, SPE_CACHE_SET_TAGMASK(set));            \
00094         spu_mfcstat(MFC_TAG_UPDATE_ALL);                        \
00095     }                                                           \
00096     else                            \
00097     {                               \
00098         ++g_CacheHits;              \
00099     }                               \
00100     line = _spe_cacheline_num_(set, idx);                       \
00101     byte = _spe_cacheline_byte_offset_(ea);                     \
00102     (void *) &spe_cache_mem[line + byte];                       \
00103 })
00104 
00105 #endif
00106 
00107 #endif // USE_SOFTWARE_CACHE
00108 
00109 bool gUseEpa = false;
00110 
00111 #ifdef USE_SN_TUNER
00112 #include <LibSN_SPU.h>
00113 #endif //USE_SN_TUNER
00114 
00115 #if defined (__SPU__) && !defined (USE_LIBSPE2)
00116 #include <spu_printf.h>
00117 #elif defined (USE_LIBSPE2)
00118 #define spu_printf(a)
00119 #else
00120 #define IGNORE_ALIGNMENT 1
00121 #include <stdio.h>
00122 #include <stdlib.h>
00123 #define spu_printf printf
00124 
00125 #endif
00126 
00127 //int gNumConvexPoints0=0;
00128 
00130 ATTRIBUTE_ALIGNED16(struct)     CollisionTask_LocalStoreMemory
00131 {
00134 
00135         ATTRIBUTE_ALIGNED16(btBroadphasePair    gBroadphasePairsBuffer[SPU_BATCHSIZE_BROADPHASE_PAIRS]);
00136         DoubleBuffer<unsigned char, MIDPHASE_WORKUNIT_PAGE_SIZE> g_workUnitTaskBuffers;
00137         ATTRIBUTE_ALIGNED16(char gSpuContactManifoldAlgoBuffer [sizeof(SpuContactManifoldCollisionAlgorithm)+16]);
00138         ATTRIBUTE_ALIGNED16(char gColObj0Buffer [sizeof(btCollisionObject)+16]);
00139         ATTRIBUTE_ALIGNED16(char gColObj1Buffer [sizeof(btCollisionObject)+16]);
00141         ATTRIBUTE_ALIGNED16(int spuIndices[16]);
00142         btPersistentManifold    gPersistentManifoldBuffer;
00143         CollisionShape_LocalStoreMemory gCollisionShapes[2];
00144         bvhMeshShape_LocalStoreMemory bvhShapeData;
00145         ATTRIBUTE_ALIGNED16(SpuConvexPolyhedronVertexData convexVertexData[2]);
00146         CompoundShape_LocalStoreMemory compoundShapeData[2];
00147                 
00150         btCollisionObject*      m_lsColObj0Ptr;
00151         btCollisionObject*      m_lsColObj1Ptr;
00152         btBroadphasePair* m_pairsPointer;
00153         btPersistentManifold*   m_lsManifoldPtr;
00154         SpuContactManifoldCollisionAlgorithm*   m_lsCollisionAlgorithmPtr;
00155 
00156         bool    needsDmaPutContactManifoldAlgo;
00157 
00158         btCollisionObject* getColObj0()
00159         {
00160                 return m_lsColObj0Ptr;
00161         }
00162         btCollisionObject* getColObj1()
00163         {
00164                 return m_lsColObj1Ptr;
00165         }
00166 
00167 
00168         btBroadphasePair* getBroadphasePairPtr()
00169         {
00170                 return m_pairsPointer;
00171         }
00172 
00173         SpuContactManifoldCollisionAlgorithm*   getlocalCollisionAlgorithm()
00174         {
00175                 return m_lsCollisionAlgorithmPtr;
00176         }
00177         
00178         btPersistentManifold*   getContactManifoldPtr()
00179         {
00180                 return m_lsManifoldPtr;
00181         }
00182 };
00183 
00184 
00185 #if defined(__CELLOS_LV2__) || defined(USE_LIBSPE2) 
00186 
00187 ATTRIBUTE_ALIGNED16(CollisionTask_LocalStoreMemory      gLocalStoreMemory);
00188 
00189 void* createCollisionLocalStoreMemory()
00190 {
00191         return &gLocalStoreMemory;
00192 }
00193 void deleteCollisionLocalStoreMemory()
00194 {
00195 }
00196 #else
00197 
00198 btAlignedObjectArray<CollisionTask_LocalStoreMemory*> sLocalStorePointers;
00199 
00200 void* createCollisionLocalStoreMemory()
00201 {
00202     CollisionTask_LocalStoreMemory* localStore = (CollisionTask_LocalStoreMemory*)btAlignedAlloc( sizeof(CollisionTask_LocalStoreMemory),16);
00203     sLocalStorePointers.push_back(localStore);
00204     return localStore;
00205 }
00206 
00207 void deleteCollisionLocalStoreMemory()
00208 {
00209     for (int i=0;i<sLocalStorePointers.size();i++)
00210     {
00211         btAlignedFree(sLocalStorePointers[i]);
00212     }
00213     sLocalStorePointers.clear();
00214 }
00215 
00216 #endif
00217 
00218 void    ProcessSpuConvexConvexCollision(SpuCollisionPairInput* wuInput, CollisionTask_LocalStoreMemory* lsMemPtr, SpuContactResult& spuContacts);
00219 
00220 
00221 SIMD_FORCE_INLINE void small_cache_read(void* buffer, ppu_address_t ea, size_t size)
00222 {
00223 #if USE_SOFTWARE_CACHE
00224         // Check for alignment requirements. We need to make sure the entire request fits within one cache line,
00225         // so the first and last bytes should fall on the same cache line
00226         btAssert((ea & ~SPE_CACHELINE_MASK) == ((ea + size - 1) & ~SPE_CACHELINE_MASK));
00227 
00228         void* ls = spe_cache_read(ea);
00229         memcpy(buffer, ls, size);
00230 #else
00231         stallingUnalignedDmaSmallGet(buffer,ea,size);
00232 #endif
00233 }
00234 
00235 SIMD_FORCE_INLINE void small_cache_read_triple( void* ls0, ppu_address_t ea0,
00236                                                                                                 void* ls1, ppu_address_t ea1,
00237                                                                                                 void* ls2, ppu_address_t ea2,
00238                                                                                                 size_t size)
00239 {
00240                 btAssert(size<16);
00241                 ATTRIBUTE_ALIGNED16(char        tmpBuffer0[32]);
00242                 ATTRIBUTE_ALIGNED16(char        tmpBuffer1[32]);
00243                 ATTRIBUTE_ALIGNED16(char        tmpBuffer2[32]);
00244 
00245                 uint32_t i;
00246                 
00247 
00249                 char* localStore0 = (char*)ls0;
00250                 uint32_t last4BitsOffset = ea0 & 0x0f;
00251                 char* tmpTarget0 = tmpBuffer0 + last4BitsOffset;
00252 #ifdef __SPU__
00253                 cellDmaSmallGet(tmpTarget0,ea0,size,DMA_TAG(1),0,0);
00254 #else
00255                 tmpTarget0 = (char*)cellDmaSmallGetReadOnly(tmpTarget0,ea0,size,DMA_TAG(1),0,0);
00256 #endif
00257 
00258 
00259                 char* localStore1 = (char*)ls1;
00260                 last4BitsOffset = ea1 & 0x0f;
00261                 char* tmpTarget1 = tmpBuffer1 + last4BitsOffset;
00262 #ifdef __SPU__
00263                 cellDmaSmallGet(tmpTarget1,ea1,size,DMA_TAG(1),0,0);
00264 #else
00265                 tmpTarget1 = (char*)cellDmaSmallGetReadOnly(tmpTarget1,ea1,size,DMA_TAG(1),0,0);
00266 #endif
00267                 
00268                 char* localStore2 = (char*)ls2;
00269                 last4BitsOffset = ea2 & 0x0f;
00270                 char* tmpTarget2 = tmpBuffer2 + last4BitsOffset;
00271 #ifdef __SPU__
00272                 cellDmaSmallGet(tmpTarget2,ea2,size,DMA_TAG(1),0,0);
00273 #else
00274                 tmpTarget2 = (char*)cellDmaSmallGetReadOnly(tmpTarget2,ea2,size,DMA_TAG(1),0,0);
00275 #endif
00276                 
00277                 
00278                 cellDmaWaitTagStatusAll( DMA_MASK(1) );
00279 
00280                 //this is slowish, perhaps memcpy on SPU is smarter?
00281                 for (i=0; btLikely( i<size );i++)
00282                 {
00283                         localStore0[i] = tmpTarget0[i];
00284                         localStore1[i] = tmpTarget1[i];
00285                         localStore2[i] = tmpTarget2[i];
00286                 }
00287 
00288                 
00289 }
00290 
00291 
00292 
00293 
00294 ATTRIBUTE_ALIGNED16(class) spuNodeCallback : public btNodeOverlapCallback
00295 {
00296         SpuCollisionPairInput* m_wuInput;
00297         SpuContactResult&               m_spuContacts;
00298         CollisionTask_LocalStoreMemory* m_lsMemPtr;
00299         ATTRIBUTE_ALIGNED16(btTriangleShape)    m_tmpTriangleShape;
00300 
00301         ATTRIBUTE_ALIGNED16(btVector3   spuTriangleVertices[3]);
00302         ATTRIBUTE_ALIGNED16(btScalar    spuUnscaledVertex[4]);
00303         
00304 
00305 
00306 public:
00307         spuNodeCallback(SpuCollisionPairInput* wuInput, CollisionTask_LocalStoreMemory* lsMemPtr,SpuContactResult& spuContacts)
00308                 :       m_wuInput(wuInput),
00309                 m_spuContacts(spuContacts),
00310                 m_lsMemPtr(lsMemPtr)
00311         {
00312         }
00313 
00314         virtual void processNode(int subPart, int triangleIndex)
00315         {
00318 
00319                 //              spu_printf("processNode with triangleIndex %d\n",triangleIndex);
00320 
00321                 if (m_lsMemPtr->bvhShapeData.gIndexMesh.m_indexType == PHY_SHORT)
00322                 {
00323                         unsigned short int* indexBasePtr = (unsigned short int*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexBase+triangleIndex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexStride);
00324                         ATTRIBUTE_ALIGNED16(unsigned short int tmpIndices[3]);
00325 
00326                         small_cache_read_triple(&tmpIndices[0],(ppu_address_t)&indexBasePtr[0],
00327                                                                         &tmpIndices[1],(ppu_address_t)&indexBasePtr[1],
00328                                                                         &tmpIndices[2],(ppu_address_t)&indexBasePtr[2],
00329                                                                         sizeof(unsigned short int));
00330 
00331                         m_lsMemPtr->spuIndices[0] = int(tmpIndices[0]);
00332                         m_lsMemPtr->spuIndices[1] = int(tmpIndices[1]);
00333                         m_lsMemPtr->spuIndices[2] = int(tmpIndices[2]);
00334                 } else
00335                 {
00336                         unsigned int* indexBasePtr = (unsigned int*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexBase+triangleIndex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexStride);
00337 
00338                         small_cache_read_triple(&m_lsMemPtr->spuIndices[0],(ppu_address_t)&indexBasePtr[0],
00339                                                                 &m_lsMemPtr->spuIndices[1],(ppu_address_t)&indexBasePtr[1],
00340                                                                 &m_lsMemPtr->spuIndices[2],(ppu_address_t)&indexBasePtr[2],
00341                                                                 sizeof(int));
00342                 }
00343                 
00344                 //              spu_printf("SPU index0=%d ,",spuIndices[0]);
00345                 //              spu_printf("SPU index1=%d ,",spuIndices[1]);
00346                 //              spu_printf("SPU index2=%d ,",spuIndices[2]);
00347                 //              spu_printf("SPU: indexBasePtr=%llx\n",indexBasePtr);
00348 
00349                 const btVector3& meshScaling = m_lsMemPtr->bvhShapeData.gTriangleMeshInterfacePtr->getScaling();
00350                 for (int j=2;btLikely( j>=0 );j--)
00351                 {
00352                         int graphicsindex = m_lsMemPtr->spuIndices[j];
00353 
00354                         //                      spu_printf("SPU index=%d ,",graphicsindex);
00355                         btScalar* graphicsbasePtr = (btScalar*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_vertexBase+graphicsindex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_vertexStride);
00356                         //                      spu_printf("SPU graphicsbasePtr=%llx\n",graphicsbasePtr);
00357 
00358 
00360 
00361                         //another DMA for each vertex
00362                         small_cache_read_triple(&spuUnscaledVertex[0],(ppu_address_t)&graphicsbasePtr[0],
00363                                                                         &spuUnscaledVertex[1],(ppu_address_t)&graphicsbasePtr[1],
00364                                                                         &spuUnscaledVertex[2],(ppu_address_t)&graphicsbasePtr[2],
00365                                                                         sizeof(btScalar));
00366                         
00367                         m_tmpTriangleShape.getVertexPtr(j).setValue(spuUnscaledVertex[0]*meshScaling.getX(),
00368                                 spuUnscaledVertex[1]*meshScaling.getY(),
00369                                 spuUnscaledVertex[2]*meshScaling.getZ());
00370 
00371                         //                      spu_printf("SPU:triangle vertices:%f,%f,%f\n",spuTriangleVertices[j].x(),spuTriangleVertices[j].y(),spuTriangleVertices[j].z());
00372                 }
00373 
00374 
00375                 SpuCollisionPairInput triangleConcaveInput(*m_wuInput);
00376 //              triangleConcaveInput.m_spuCollisionShapes[1] = &spuTriangleVertices[0];
00377                 triangleConcaveInput.m_spuCollisionShapes[1] = &m_tmpTriangleShape;
00378                 triangleConcaveInput.m_shapeType1 = TRIANGLE_SHAPE_PROXYTYPE;
00379 
00380                 m_spuContacts.setShapeIdentifiersB(subPart,triangleIndex);
00381 
00382                 //              m_spuContacts.flush();
00383 
00384                 ProcessSpuConvexConvexCollision(&triangleConcaveInput, m_lsMemPtr,m_spuContacts);
00386                 //      m_spuContacts.flush();
00387         }
00388 
00389 };
00390 
00391 
00392 
00393 void btConvexPlaneCollideSingleContact (SpuCollisionPairInput* wuInput,CollisionTask_LocalStoreMemory* lsMemPtr,SpuContactResult&  spuContacts)
00394 {
00395         
00396         btConvexShape* convexShape = (btConvexShape*) wuInput->m_spuCollisionShapes[0];
00397         btStaticPlaneShape* planeShape = (btStaticPlaneShape*) wuInput->m_spuCollisionShapes[1];
00398 
00399     bool hasCollision = false;
00400         const btVector3& planeNormal = planeShape->getPlaneNormal();
00401         const btScalar& planeConstant = planeShape->getPlaneConstant();
00402         
00403         
00404         btTransform convexWorldTransform = wuInput->m_worldTransform0;
00405         btTransform convexInPlaneTrans;
00406         convexInPlaneTrans= wuInput->m_worldTransform1.inverse() * convexWorldTransform;
00407         btTransform planeInConvex;
00408         planeInConvex= convexWorldTransform.inverse() * wuInput->m_worldTransform1;
00409         
00410         //btVector3 vtx = convexShape->localGetSupportVertexWithoutMarginNonVirtual(planeInConvex.getBasis()*-planeNormal);
00411         btVector3 vtx = convexShape->localGetSupportVertexNonVirtual(planeInConvex.getBasis()*-planeNormal);
00412 
00413         btVector3 vtxInPlane = convexInPlaneTrans(vtx);
00414         btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant);
00415 
00416         btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal;
00417         btVector3 vtxInPlaneWorld = wuInput->m_worldTransform1 * vtxInPlaneProjected;
00418 
00419         hasCollision = distance < lsMemPtr->getContactManifoldPtr()->getContactBreakingThreshold();
00420         //resultOut->setPersistentManifold(m_manifoldPtr);
00421         if (hasCollision)
00422         {
00424                 btVector3 normalOnSurfaceB =wuInput->m_worldTransform1.getBasis() * planeNormal;
00425                 btVector3 pOnB = vtxInPlaneWorld;
00426                 spuContacts.addContactPoint(normalOnSurfaceB,pOnB,distance);
00427         }
00428 }
00429 
00430 void    ProcessConvexPlaneSpuCollision(SpuCollisionPairInput* wuInput, CollisionTask_LocalStoreMemory* lsMemPtr, SpuContactResult& spuContacts)
00431 {
00432 
00433                 register        int dmaSize = 0;
00434                 register ppu_address_t  dmaPpuAddress2;
00435                 btPersistentManifold* manifold = (btPersistentManifold*)wuInput->m_persistentManifoldPtr;
00436 
00438                 ATTRIBUTE_ALIGNED16(char convexHullShape0[sizeof(btConvexHullShape)]);
00439                 ATTRIBUTE_ALIGNED16(char convexHullShape1[sizeof(btConvexHullShape)]);
00440 
00441                 if ( btLikely( wuInput->m_shapeType0== CONVEX_HULL_SHAPE_PROXYTYPE ) )
00442                 {
00443                         //      spu_printf("SPU: DMA btConvexHullShape\n");
00444                         
00445                         dmaSize = sizeof(btConvexHullShape);
00446                         dmaPpuAddress2 = wuInput->m_collisionShapes[0];
00447 
00448                         cellDmaGet(&convexHullShape0, dmaPpuAddress2  , dmaSize, DMA_TAG(1), 0, 0);
00449                         //cellDmaWaitTagStatusAll(DMA_MASK(1));
00450                 }
00451 
00452                 if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) )
00453                 {
00454                         //      spu_printf("SPU: DMA btConvexHullShape\n");
00455                         dmaSize = sizeof(btConvexHullShape);
00456                         dmaPpuAddress2 = wuInput->m_collisionShapes[1];
00457                         cellDmaGet(&convexHullShape1, dmaPpuAddress2  , dmaSize, DMA_TAG(1), 0, 0);
00458                         //cellDmaWaitTagStatusAll(DMA_MASK(1));
00459                 }
00460                 
00461                 if ( btLikely( wuInput->m_shapeType0 == CONVEX_HULL_SHAPE_PROXYTYPE ) )
00462                 {               
00463                         cellDmaWaitTagStatusAll(DMA_MASK(1));
00464                         dmaConvexVertexData (&lsMemPtr->convexVertexData[0], (btConvexHullShape*)&convexHullShape0);
00465                         lsMemPtr->convexVertexData[0].gSpuConvexShapePtr = wuInput->m_spuCollisionShapes[0];
00466                 }
00467 
00468                         
00469                 if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) )
00470                 {
00471                         cellDmaWaitTagStatusAll(DMA_MASK(1));
00472                         dmaConvexVertexData (&lsMemPtr->convexVertexData[1], (btConvexHullShape*)&convexHullShape1);
00473                         lsMemPtr->convexVertexData[1].gSpuConvexShapePtr = wuInput->m_spuCollisionShapes[1];
00474                 }
00475 
00476                 
00477                 btConvexPointCloudShape cpc0,cpc1;
00478 
00479                 if ( btLikely( wuInput->m_shapeType0 == CONVEX_HULL_SHAPE_PROXYTYPE ) )
00480                 {
00481                         cellDmaWaitTagStatusAll(DMA_MASK(2));
00482                         lsMemPtr->convexVertexData[0].gConvexPoints = &lsMemPtr->convexVertexData[0].g_convexPointBuffer[0];
00483                         btConvexHullShape* ch = (btConvexHullShape*)wuInput->m_spuCollisionShapes[0];
00484                         const btVector3& localScaling = ch->getLocalScalingNV();
00485                         cpc0.setPoints(lsMemPtr->convexVertexData[0].gConvexPoints,lsMemPtr->convexVertexData[0].gNumConvexPoints,false,localScaling);
00486                         wuInput->m_spuCollisionShapes[0] = &cpc0;
00487                 }
00488 
00489                 if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) )
00490                 {
00491                         cellDmaWaitTagStatusAll(DMA_MASK(2));           
00492                         lsMemPtr->convexVertexData[1].gConvexPoints = &lsMemPtr->convexVertexData[1].g_convexPointBuffer[0];
00493                         btConvexHullShape* ch = (btConvexHullShape*)wuInput->m_spuCollisionShapes[1];
00494                         const btVector3& localScaling = ch->getLocalScalingNV();
00495                         cpc1.setPoints(lsMemPtr->convexVertexData[1].gConvexPoints,lsMemPtr->convexVertexData[1].gNumConvexPoints,false,localScaling);
00496                         wuInput->m_spuCollisionShapes[1] = &cpc1;
00497 
00498                 }
00499 
00500 
00501 //              const btConvexShape* shape0Ptr = (const btConvexShape*)wuInput->m_spuCollisionShapes[0];
00502 //              const btConvexShape* shape1Ptr = (const btConvexShape*)wuInput->m_spuCollisionShapes[1];
00503 //              int shapeType0 = wuInput->m_shapeType0;
00504 //              int shapeType1 = wuInput->m_shapeType1;
00505                 float marginA = wuInput->m_collisionMargin0;
00506                 float marginB = wuInput->m_collisionMargin1;
00507 
00508                 SpuClosestPointInput    cpInput;
00509                 cpInput.m_convexVertexData[0] = &lsMemPtr->convexVertexData[0];
00510                 cpInput.m_convexVertexData[1] = &lsMemPtr->convexVertexData[1];
00511                 cpInput.m_transformA = wuInput->m_worldTransform0;
00512                 cpInput.m_transformB = wuInput->m_worldTransform1;
00513                 float sumMargin = (marginA+marginB+lsMemPtr->getContactManifoldPtr()->getContactBreakingThreshold());
00514                 cpInput.m_maximumDistanceSquared = sumMargin * sumMargin;
00515 
00516                 ppu_address_t manifoldAddress = (ppu_address_t)manifold;
00517 
00518                 btPersistentManifold* spuManifold=lsMemPtr->getContactManifoldPtr();
00519                 //spuContacts.setContactInfo(spuManifold,manifoldAddress,wuInput->m_worldTransform0,wuInput->m_worldTransform1,wuInput->m_isSwapped);
00520                 spuContacts.setContactInfo(spuManifold,manifoldAddress,lsMemPtr->getColObj0()->getWorldTransform(),
00521                         lsMemPtr->getColObj1()->getWorldTransform(),
00522                         lsMemPtr->getColObj0()->getRestitution(),lsMemPtr->getColObj1()->getRestitution(),
00523                         lsMemPtr->getColObj0()->getFriction(),lsMemPtr->getColObj1()->getFriction(),
00524                         wuInput->m_isSwapped);
00525 
00526 
00527                 btConvexPlaneCollideSingleContact(wuInput,lsMemPtr,spuContacts);
00528 
00529 
00530                 
00531         
00532 }
00533 
00534 
00535 
00536 
00540 void    ProcessConvexConcaveSpuCollision(SpuCollisionPairInput* wuInput, CollisionTask_LocalStoreMemory* lsMemPtr, SpuContactResult& spuContacts)
00541 {
00542         //order: first collision shape is convex, second concave. m_isSwapped is true, if the original order was opposite
00543         
00544         btBvhTriangleMeshShape* trimeshShape = (btBvhTriangleMeshShape*)wuInput->m_spuCollisionShapes[1];
00545         //need the mesh interface, for access to triangle vertices
00546         dmaBvhShapeData (&lsMemPtr->bvhShapeData, trimeshShape);
00547 
00548         btVector3 aabbMin(-1,-400,-1);
00549         btVector3 aabbMax(1,400,1);
00550 
00551 
00552         //recalc aabbs
00553         btTransform convexInTriangleSpace;
00554         convexInTriangleSpace = wuInput->m_worldTransform1.inverse() * wuInput->m_worldTransform0;
00555         btConvexInternalShape* convexShape = (btConvexInternalShape*)wuInput->m_spuCollisionShapes[0];
00556 
00557         computeAabb (aabbMin, aabbMax, convexShape, wuInput->m_collisionShapes[0], wuInput->m_shapeType0, convexInTriangleSpace);
00558 
00559 
00560         //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
00561         //convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax);
00562 
00563         //      btScalar extraMargin = collisionMarginTriangle;
00564         //      btVector3 extra(extraMargin,extraMargin,extraMargin);
00565         //      aabbMax += extra;
00566         //      aabbMin -= extra;
00567 
00569         unsigned short int quantizedQueryAabbMin[3];
00570         unsigned short int quantizedQueryAabbMax[3];
00571         lsMemPtr->bvhShapeData.getOptimizedBvh()->quantizeWithClamp(quantizedQueryAabbMin,aabbMin,0);
00572         lsMemPtr->bvhShapeData.getOptimizedBvh()->quantizeWithClamp(quantizedQueryAabbMax,aabbMax,1);
00573 
00574         QuantizedNodeArray&     nodeArray = lsMemPtr->bvhShapeData.getOptimizedBvh()->getQuantizedNodeArray();
00575         //spu_printf("SPU: numNodes = %d\n",nodeArray.size());
00576 
00577         BvhSubtreeInfoArray& subTrees = lsMemPtr->bvhShapeData.getOptimizedBvh()->getSubtreeInfoArray();
00578 
00579 
00580         spuNodeCallback nodeCallback(wuInput,lsMemPtr,spuContacts);
00581         IndexedMeshArray&       indexArray = lsMemPtr->bvhShapeData.gTriangleMeshInterfacePtr->getIndexedMeshArray();
00582         //spu_printf("SPU:indexArray.size() = %d\n",indexArray.size());
00583 
00584         //      spu_printf("SPU: numSubTrees = %d\n",subTrees.size());
00585         //not likely to happen
00586         if (subTrees.size() && indexArray.size() == 1)
00587         {
00589                 dmaBvhIndexedMesh (&lsMemPtr->bvhShapeData.gIndexMesh, indexArray, 0 /* index into indexArray */, 1 /* dmaTag */);
00590                 cellDmaWaitTagStatusAll(DMA_MASK(1));
00591                 
00592                 //display the headers
00593                 int numBatch = subTrees.size();
00594                 for (int i=0;i<numBatch;)
00595                 {
00596                         //@todo- can reorder DMA transfers for less stall
00597                         int remaining = subTrees.size() - i;
00598                         int nextBatch = remaining < MAX_SPU_SUBTREE_HEADERS ? remaining : MAX_SPU_SUBTREE_HEADERS;
00599                         
00600                         dmaBvhSubTreeHeaders (&lsMemPtr->bvhShapeData.gSubtreeHeaders[0], (ppu_address_t)(&subTrees[i]), nextBatch, 1);
00601                         cellDmaWaitTagStatusAll(DMA_MASK(1));
00602                         
00603 
00604                         //                      spu_printf("nextBatch = %d\n",nextBatch);
00605 
00606                         for (int j=0;j<nextBatch;j++)
00607                         {
00608                                 const btBvhSubtreeInfo& subtree = lsMemPtr->bvhShapeData.gSubtreeHeaders[j];
00609 
00610                                 unsigned int overlap = spuTestQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax);
00611                                 if (overlap)
00612                                 {
00613                                         btAssert(subtree.m_subtreeSize);
00614 
00615                                         //dma the actual nodes of this subtree
00616                                         dmaBvhSubTreeNodes (&lsMemPtr->bvhShapeData.gSubtreeNodes[0], subtree, nodeArray, 2);
00617                                         cellDmaWaitTagStatusAll(DMA_MASK(2));
00618 
00619                                         /* Walk this subtree */
00620                                         spuWalkStacklessQuantizedTree(&nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax,
00621                                                 &lsMemPtr->bvhShapeData.gSubtreeNodes[0],
00622                                                 0,
00623                                                 subtree.m_subtreeSize);
00624                                 }
00625                                 //                              spu_printf("subtreeSize = %d\n",gSubtreeHeaders[j].m_subtreeSize);
00626                         }
00627 
00628                         //      unsigned short int      m_quantizedAabbMin[3];
00629                         //      unsigned short int      m_quantizedAabbMax[3];
00630                         //      int                     m_rootNodeIndex;
00631                         //      int                     m_subtreeSize;
00632                         i+=nextBatch;
00633                 }
00634 
00635                 //pre-fetch first tree, then loop and double buffer
00636         }
00637 
00638 }
00639 
00640 
00641 #define MAX_DEGENERATE_STATS 15
00642 int stats[MAX_DEGENERATE_STATS]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
00643 int degenerateStats[MAX_DEGENERATE_STATS]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
00644 
00645 
00649 void    ProcessSpuConvexConvexCollision(SpuCollisionPairInput* wuInput, CollisionTask_LocalStoreMemory* lsMemPtr, SpuContactResult& spuContacts)
00650 {
00651         register int dmaSize;
00652         register ppu_address_t  dmaPpuAddress2;
00653         
00654 #ifdef DEBUG_SPU_COLLISION_DETECTION
00655         //spu_printf("SPU: ProcessSpuConvexConvexCollision\n");
00656 #endif //DEBUG_SPU_COLLISION_DETECTION
00657         //CollisionShape* shape0 = (CollisionShape*)wuInput->m_collisionShapes[0];
00658         //CollisionShape* shape1 = (CollisionShape*)wuInput->m_collisionShapes[1];
00659         btPersistentManifold* manifold = (btPersistentManifold*)wuInput->m_persistentManifoldPtr;
00660 
00661         bool genericGjk = true;
00662 
00663         if (genericGjk)
00664         {
00665                 //try generic GJK
00666 
00667                 
00668                 
00669                 //SpuConvexPenetrationDepthSolver* penetrationSolver=0;
00670                 btVoronoiSimplexSolver simplexSolver;
00671                 btGjkEpaPenetrationDepthSolver  epaPenetrationSolver2;
00672                 
00673                 btConvexPenetrationDepthSolver* penetrationSolver = &epaPenetrationSolver2;
00674                 
00675                 //SpuMinkowskiPenetrationDepthSolver    minkowskiPenetrationSolver;
00676 #ifdef ENABLE_EPA
00677                 if (gUseEpa)
00678                 {
00679                         penetrationSolver = &epaPenetrationSolver2;
00680                 } else
00681 #endif
00682                 {
00683                         //penetrationSolver = &minkowskiPenetrationSolver;
00684                 }
00685 
00686 
00688                 ATTRIBUTE_ALIGNED16(char convexHullShape0[sizeof(btConvexHullShape)]);
00689                 ATTRIBUTE_ALIGNED16(char convexHullShape1[sizeof(btConvexHullShape)]);
00690 
00691                 if ( btLikely( wuInput->m_shapeType0== CONVEX_HULL_SHAPE_PROXYTYPE ) )
00692                 {
00693                         //      spu_printf("SPU: DMA btConvexHullShape\n");
00694                         
00695                         dmaSize = sizeof(btConvexHullShape);
00696                         dmaPpuAddress2 = wuInput->m_collisionShapes[0];
00697 
00698                         cellDmaGet(&convexHullShape0, dmaPpuAddress2  , dmaSize, DMA_TAG(1), 0, 0);
00699                         //cellDmaWaitTagStatusAll(DMA_MASK(1));
00700                 }
00701 
00702                 if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) )
00703                 {
00704                         //      spu_printf("SPU: DMA btConvexHullShape\n");
00705                         dmaSize = sizeof(btConvexHullShape);
00706                         dmaPpuAddress2 = wuInput->m_collisionShapes[1];
00707                         cellDmaGet(&convexHullShape1, dmaPpuAddress2  , dmaSize, DMA_TAG(1), 0, 0);
00708                         //cellDmaWaitTagStatusAll(DMA_MASK(1));
00709                 }
00710                 
00711                 if ( btLikely( wuInput->m_shapeType0 == CONVEX_HULL_SHAPE_PROXYTYPE ) )
00712                 {               
00713                         cellDmaWaitTagStatusAll(DMA_MASK(1));
00714                         dmaConvexVertexData (&lsMemPtr->convexVertexData[0], (btConvexHullShape*)&convexHullShape0);
00715                         lsMemPtr->convexVertexData[0].gSpuConvexShapePtr = wuInput->m_spuCollisionShapes[0];
00716                 }
00717 
00718                         
00719                 if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) )
00720                 {
00721                         cellDmaWaitTagStatusAll(DMA_MASK(1));
00722                         dmaConvexVertexData (&lsMemPtr->convexVertexData[1], (btConvexHullShape*)&convexHullShape1);
00723                         lsMemPtr->convexVertexData[1].gSpuConvexShapePtr = wuInput->m_spuCollisionShapes[1];
00724                 }
00725 
00726                 
00727                 btConvexPointCloudShape cpc0,cpc1;
00728 
00729                 if ( btLikely( wuInput->m_shapeType0 == CONVEX_HULL_SHAPE_PROXYTYPE ) )
00730                 {
00731                         cellDmaWaitTagStatusAll(DMA_MASK(2));
00732                         lsMemPtr->convexVertexData[0].gConvexPoints = &lsMemPtr->convexVertexData[0].g_convexPointBuffer[0];
00733                         btConvexHullShape* ch = (btConvexHullShape*)wuInput->m_spuCollisionShapes[0];
00734                         const btVector3& localScaling = ch->getLocalScalingNV();
00735                         cpc0.setPoints(lsMemPtr->convexVertexData[0].gConvexPoints,lsMemPtr->convexVertexData[0].gNumConvexPoints,false,localScaling);
00736                         wuInput->m_spuCollisionShapes[0] = &cpc0;
00737                 }
00738 
00739                 if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) )
00740                 {
00741                         cellDmaWaitTagStatusAll(DMA_MASK(2));           
00742                         lsMemPtr->convexVertexData[1].gConvexPoints = &lsMemPtr->convexVertexData[1].g_convexPointBuffer[0];
00743                         btConvexHullShape* ch = (btConvexHullShape*)wuInput->m_spuCollisionShapes[1];
00744                         const btVector3& localScaling = ch->getLocalScalingNV();
00745                         cpc1.setPoints(lsMemPtr->convexVertexData[1].gConvexPoints,lsMemPtr->convexVertexData[1].gNumConvexPoints,false,localScaling);
00746                         wuInput->m_spuCollisionShapes[1] = &cpc1;
00747 
00748                 }
00749 
00750 
00751                 const btConvexShape* shape0Ptr = (const btConvexShape*)wuInput->m_spuCollisionShapes[0];
00752                 const btConvexShape* shape1Ptr = (const btConvexShape*)wuInput->m_spuCollisionShapes[1];
00753                 int shapeType0 = wuInput->m_shapeType0;
00754                 int shapeType1 = wuInput->m_shapeType1;
00755                 float marginA = wuInput->m_collisionMargin0;
00756                 float marginB = wuInput->m_collisionMargin1;
00757 
00758                 SpuClosestPointInput    cpInput;
00759                 cpInput.m_convexVertexData[0] = &lsMemPtr->convexVertexData[0];
00760                 cpInput.m_convexVertexData[1] = &lsMemPtr->convexVertexData[1];
00761                 cpInput.m_transformA = wuInput->m_worldTransform0;
00762                 cpInput.m_transformB = wuInput->m_worldTransform1;
00763                 float sumMargin = (marginA+marginB+lsMemPtr->getContactManifoldPtr()->getContactBreakingThreshold());
00764                 cpInput.m_maximumDistanceSquared = sumMargin * sumMargin;
00765 
00766                 ppu_address_t manifoldAddress = (ppu_address_t)manifold;
00767 
00768                 btPersistentManifold* spuManifold=lsMemPtr->getContactManifoldPtr();
00769                 //spuContacts.setContactInfo(spuManifold,manifoldAddress,wuInput->m_worldTransform0,wuInput->m_worldTransform1,wuInput->m_isSwapped);
00770                 spuContacts.setContactInfo(spuManifold,manifoldAddress,lsMemPtr->getColObj0()->getWorldTransform(),
00771                         lsMemPtr->getColObj1()->getWorldTransform(),
00772                         lsMemPtr->getColObj0()->getRestitution(),lsMemPtr->getColObj1()->getRestitution(),
00773                         lsMemPtr->getColObj0()->getFriction(),lsMemPtr->getColObj1()->getFriction(),
00774                         wuInput->m_isSwapped);
00775 
00776                 {
00777                         btGjkPairDetector gjk(shape0Ptr,shape1Ptr,shapeType0,shapeType1,marginA,marginB,&simplexSolver,penetrationSolver);//&vsSolver,penetrationSolver);
00778                         gjk.getClosestPoints(cpInput,spuContacts,0);//,debugDraw);
00779 
00780                         btAssert(gjk.m_lastUsedMethod <MAX_DEGENERATE_STATS);
00781                         stats[gjk.m_lastUsedMethod]++;
00782                         btAssert(gjk.m_degenerateSimplex <MAX_DEGENERATE_STATS);
00783                         degenerateStats[gjk.m_degenerateSimplex]++;
00784 
00785 #ifdef USE_SEPDISTANCE_UTIL                     
00786                         btScalar sepDist = gjk.getCachedSeparatingDistance()+spuManifold->getContactBreakingThreshold();
00787                         lsMemPtr->getlocalCollisionAlgorithm()->m_sepDistance.initSeparatingDistance(gjk.getCachedSeparatingAxis(),sepDist,wuInput->m_worldTransform0,wuInput->m_worldTransform1);
00788                         lsMemPtr->needsDmaPutContactManifoldAlgo = true;
00789 #endif //USE_SEPDISTANCE_UTIL
00790 
00791                 }
00792 
00793         }
00794 
00795 
00796 }
00797 
00798 
00799 template<typename T> void DoSwap(T& a, T& b)
00800 {
00801         char tmp[sizeof(T)];
00802         memcpy(tmp, &a, sizeof(T));
00803         memcpy(&a, &b, sizeof(T));
00804         memcpy(&b, tmp, sizeof(T));
00805 }
00806 
00807 SIMD_FORCE_INLINE void  dmaAndSetupCollisionObjects(SpuCollisionPairInput& collisionPairInput, CollisionTask_LocalStoreMemory& lsMem)
00808 {
00809         register int dmaSize;
00810         register ppu_address_t  dmaPpuAddress2;
00811                 
00812         dmaSize = sizeof(btCollisionObject);//btTransform);
00813         dmaPpuAddress2 = /*collisionPairInput.m_isSwapped ? (ppu_address_t)lsMem.gProxyPtr1->m_clientObject :*/ (ppu_address_t)lsMem.getlocalCollisionAlgorithm()->getCollisionObject0();
00814         lsMem.m_lsColObj0Ptr = (btCollisionObject*)cellDmaGetReadOnly(&lsMem.gColObj0Buffer, dmaPpuAddress2  , dmaSize, DMA_TAG(1), 0, 0);              
00815 
00816         dmaSize = sizeof(btCollisionObject);//btTransform);
00817         dmaPpuAddress2 = /*collisionPairInput.m_isSwapped ? (ppu_address_t)lsMem.gProxyPtr0->m_clientObject :*/ (ppu_address_t)lsMem.getlocalCollisionAlgorithm()->getCollisionObject1();
00818         lsMem.m_lsColObj1Ptr = (btCollisionObject*)cellDmaGetReadOnly(&lsMem.gColObj1Buffer, dmaPpuAddress2  , dmaSize, DMA_TAG(2), 0, 0);              
00819         
00820         cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));
00821 
00822         btCollisionObject* ob0 = lsMem.getColObj0();
00823         btCollisionObject* ob1 = lsMem.getColObj1();
00824 
00825         collisionPairInput.m_worldTransform0 = ob0->getWorldTransform();
00826         collisionPairInput.m_worldTransform1 = ob1->getWorldTransform();
00827 }
00828 
00829 
00830 
00831 void    handleCollisionPair(SpuCollisionPairInput& collisionPairInput, CollisionTask_LocalStoreMemory& lsMem,
00832                                                         SpuContactResult &spuContacts,
00833                                                         ppu_address_t collisionShape0Ptr, void* collisionShape0Loc,
00834                                                         ppu_address_t collisionShape1Ptr, void* collisionShape1Loc, bool dmaShapes = true)
00835 {
00836         
00837         if (btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType0) 
00838                 && btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType1))
00839         {
00840                 if (dmaShapes)
00841                 {
00842                         dmaCollisionShape (collisionShape0Loc, collisionShape0Ptr, 1, collisionPairInput.m_shapeType0);
00843                         dmaCollisionShape (collisionShape1Loc, collisionShape1Ptr, 2, collisionPairInput.m_shapeType1);
00844                         cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));
00845                 }
00846 
00847                 btConvexInternalShape* spuConvexShape0 = (btConvexInternalShape*)collisionShape0Loc;
00848                 btConvexInternalShape* spuConvexShape1 = (btConvexInternalShape*)collisionShape1Loc;
00849 
00850                 btVector3 dim0 = spuConvexShape0->getImplicitShapeDimensions();
00851                 btVector3 dim1 = spuConvexShape1->getImplicitShapeDimensions();
00852 
00853                 collisionPairInput.m_primitiveDimensions0 = dim0;
00854                 collisionPairInput.m_primitiveDimensions1 = dim1;
00855                 collisionPairInput.m_collisionShapes[0] = collisionShape0Ptr;
00856                 collisionPairInput.m_collisionShapes[1] = collisionShape1Ptr;
00857                 collisionPairInput.m_spuCollisionShapes[0] = spuConvexShape0;
00858                 collisionPairInput.m_spuCollisionShapes[1] = spuConvexShape1;
00859                 ProcessSpuConvexConvexCollision(&collisionPairInput,&lsMem,spuContacts);
00860         } 
00861         else if (btBroadphaseProxy::isCompound(collisionPairInput.m_shapeType0) && 
00862                         btBroadphaseProxy::isCompound(collisionPairInput.m_shapeType1))
00863         {
00864                 //snPause();
00865 
00866                 dmaCollisionShape (collisionShape0Loc, collisionShape0Ptr, 1, collisionPairInput.m_shapeType0);
00867                 dmaCollisionShape (collisionShape1Loc, collisionShape1Ptr, 2, collisionPairInput.m_shapeType1);
00868                 cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));
00869 
00870                 // Both are compounds, do N^2 CD for now
00872         
00873                 btCompoundShape* spuCompoundShape0 = (btCompoundShape*)collisionShape0Loc;
00874                 btCompoundShape* spuCompoundShape1 = (btCompoundShape*)collisionShape1Loc;
00875 
00876                 dmaCompoundShapeInfo (&lsMem.compoundShapeData[0], spuCompoundShape0, 1);
00877                 dmaCompoundShapeInfo (&lsMem.compoundShapeData[1], spuCompoundShape1, 2);
00878                 cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));
00879                 
00880 
00881                 dmaCompoundSubShapes (&lsMem.compoundShapeData[0], spuCompoundShape0, 1);
00882                 cellDmaWaitTagStatusAll(DMA_MASK(1));
00883                 dmaCompoundSubShapes (&lsMem.compoundShapeData[1], spuCompoundShape1, 1);
00884                 cellDmaWaitTagStatusAll(DMA_MASK(1));
00885 
00886                 int childShapeCount0 = spuCompoundShape0->getNumChildShapes();
00887                 btAssert(childShapeCount0< MAX_SPU_COMPOUND_SUBSHAPES);
00888                 int childShapeCount1 = spuCompoundShape1->getNumChildShapes();
00889                 btAssert(childShapeCount1< MAX_SPU_COMPOUND_SUBSHAPES);
00890 
00891                 // Start the N^2
00892                 for (int i = 0; i < childShapeCount0; ++i)
00893                 {
00894                         btCompoundShapeChild& childShape0 = lsMem.compoundShapeData[0].gSubshapes[i];
00895                         btAssert(!btBroadphaseProxy::isCompound(childShape0.m_childShapeType));
00896 
00897                         for (int j = 0; j < childShapeCount1; ++j)
00898                         {
00899                                 btCompoundShapeChild& childShape1 = lsMem.compoundShapeData[1].gSubshapes[j];
00900                                 btAssert(!btBroadphaseProxy::isCompound(childShape1.m_childShapeType));
00901 
00902 
00903                                 /* Create a new collision pair input struct using the two child shapes */
00904                                 SpuCollisionPairInput cinput (collisionPairInput);
00905 
00906                                 cinput.m_worldTransform0 = collisionPairInput.m_worldTransform0 * childShape0.m_transform;
00907                                 cinput.m_shapeType0 = childShape0.m_childShapeType;
00908                                 cinput.m_collisionMargin0 = childShape0.m_childMargin;
00909 
00910                                 cinput.m_worldTransform1 = collisionPairInput.m_worldTransform1 * childShape1.m_transform;
00911                                 cinput.m_shapeType1 = childShape1.m_childShapeType;
00912                                 cinput.m_collisionMargin1 = childShape1.m_childMargin;
00913                                 /* Recursively call handleCollisionPair () with new collision pair input */
00914                                 
00915                                 handleCollisionPair(cinput, lsMem, spuContacts,                 
00916                                         (ppu_address_t)childShape0.m_childShape, lsMem.compoundShapeData[0].gSubshapeShape[i], 
00917                                         (ppu_address_t)childShape1.m_childShape, lsMem.compoundShapeData[1].gSubshapeShape[j], false);
00918                         }
00919                 }
00920         }
00921         else if (btBroadphaseProxy::isCompound(collisionPairInput.m_shapeType0) )
00922         {
00923                 //snPause();
00924                 
00925                 dmaCollisionShape (collisionShape0Loc, collisionShape0Ptr, 1, collisionPairInput.m_shapeType0);
00926                 dmaCollisionShape (collisionShape1Loc, collisionShape1Ptr, 2, collisionPairInput.m_shapeType1);
00927                 cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));
00928 
00929                 // object 0 compound, object 1 non-compound
00930                 btCompoundShape* spuCompoundShape = (btCompoundShape*)collisionShape0Loc;
00931                 dmaCompoundShapeInfo (&lsMem.compoundShapeData[0], spuCompoundShape, 1);
00932                 cellDmaWaitTagStatusAll(DMA_MASK(1));
00933 
00934                 int childShapeCount = spuCompoundShape->getNumChildShapes();
00935                 btAssert(childShapeCount< MAX_SPU_COMPOUND_SUBSHAPES);
00936 
00937                 for (int i = 0; i < childShapeCount; ++i)
00938                 {
00939                         btCompoundShapeChild& childShape = lsMem.compoundShapeData[0].gSubshapes[i];
00940                         btAssert(!btBroadphaseProxy::isCompound(childShape.m_childShapeType));
00941                         // Dma the child shape
00942                         dmaCollisionShape (&lsMem.compoundShapeData[0].gSubshapeShape[i], (ppu_address_t)childShape.m_childShape, 1, childShape.m_childShapeType);
00943                         cellDmaWaitTagStatusAll(DMA_MASK(1));
00944                         
00945                         SpuCollisionPairInput cinput (collisionPairInput);
00946                         cinput.m_worldTransform0 = collisionPairInput.m_worldTransform0 * childShape.m_transform;
00947                         cinput.m_shapeType0 = childShape.m_childShapeType;
00948                         cinput.m_collisionMargin0 = childShape.m_childMargin;
00949 
00950                         handleCollisionPair(cinput, lsMem, spuContacts,                 
00951                                 (ppu_address_t)childShape.m_childShape, lsMem.compoundShapeData[0].gSubshapeShape[i], 
00952                                 collisionShape1Ptr, collisionShape1Loc, false);
00953                 }
00954         }
00955         else if (btBroadphaseProxy::isCompound(collisionPairInput.m_shapeType1) )
00956         {
00957                 //snPause();
00958                 
00959                 dmaCollisionShape (collisionShape0Loc, collisionShape0Ptr, 1, collisionPairInput.m_shapeType0);
00960                 dmaCollisionShape (collisionShape1Loc, collisionShape1Ptr, 2, collisionPairInput.m_shapeType1);
00961                 cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));
00962                 // object 0 non-compound, object 1 compound
00963                 btCompoundShape* spuCompoundShape = (btCompoundShape*)collisionShape1Loc;
00964                 dmaCompoundShapeInfo (&lsMem.compoundShapeData[0], spuCompoundShape, 1);
00965                 cellDmaWaitTagStatusAll(DMA_MASK(1));
00966                 
00967                 int childShapeCount = spuCompoundShape->getNumChildShapes();
00968                 btAssert(childShapeCount< MAX_SPU_COMPOUND_SUBSHAPES);
00969 
00970 
00971                 for (int i = 0; i < childShapeCount; ++i)
00972                 {
00973                         btCompoundShapeChild& childShape = lsMem.compoundShapeData[0].gSubshapes[i];
00974                         btAssert(!btBroadphaseProxy::isCompound(childShape.m_childShapeType));
00975                         // Dma the child shape
00976                         dmaCollisionShape (&lsMem.compoundShapeData[0].gSubshapeShape[i], (ppu_address_t)childShape.m_childShape, 1, childShape.m_childShapeType);
00977                         cellDmaWaitTagStatusAll(DMA_MASK(1));
00978 
00979                         SpuCollisionPairInput cinput (collisionPairInput);
00980                         cinput.m_worldTransform1 = collisionPairInput.m_worldTransform1 * childShape.m_transform;
00981                         cinput.m_shapeType1 = childShape.m_childShapeType;
00982                         cinput.m_collisionMargin1 = childShape.m_childMargin;
00983                         handleCollisionPair(cinput, lsMem, spuContacts,
00984                                 collisionShape0Ptr, collisionShape0Loc, 
00985                                 (ppu_address_t)childShape.m_childShape, lsMem.compoundShapeData[0].gSubshapeShape[i], false);
00986                 }
00987                 
00988         }
00989         else
00990         {
00991                 //a non-convex shape is involved                                                                        
00992                 bool handleConvexConcave = false;
00993 
00994                 //snPause();
00995 
00996                 if (btBroadphaseProxy::isConcave(collisionPairInput.m_shapeType0) &&
00997                         btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType1))
00998                 {
00999                         // Swap stuff
01000                         DoSwap(collisionShape0Ptr, collisionShape1Ptr);
01001                         DoSwap(collisionShape0Loc, collisionShape1Loc);
01002                         DoSwap(collisionPairInput.m_shapeType0, collisionPairInput.m_shapeType1);
01003                         DoSwap(collisionPairInput.m_worldTransform0, collisionPairInput.m_worldTransform1);
01004                         DoSwap(collisionPairInput.m_collisionMargin0, collisionPairInput.m_collisionMargin1);
01005                         
01006                         collisionPairInput.m_isSwapped = true;
01007                 }
01008                 
01009                 if (btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType0)&&
01010                         btBroadphaseProxy::isConcave(collisionPairInput.m_shapeType1))
01011                 {
01012                         handleConvexConcave = true;
01013                 }
01014                 if (handleConvexConcave)
01015                 {
01016                         if (dmaShapes)
01017                         {
01018                                 dmaCollisionShape (collisionShape0Loc, collisionShape0Ptr, 1, collisionPairInput.m_shapeType0);
01019                                 dmaCollisionShape (collisionShape1Loc, collisionShape1Ptr, 2, collisionPairInput.m_shapeType1);
01020                                 cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));
01021                         }
01022                         
01023                         if (collisionPairInput.m_shapeType1 == STATIC_PLANE_PROXYTYPE)
01024                         {
01025                                 btConvexInternalShape* spuConvexShape0 = (btConvexInternalShape*)collisionShape0Loc;
01026                                 btStaticPlaneShape* planeShape= (btStaticPlaneShape*)collisionShape1Loc;
01027 
01028                                 btVector3 dim0 = spuConvexShape0->getImplicitShapeDimensions();
01029                                 collisionPairInput.m_primitiveDimensions0 = dim0;
01030                                 collisionPairInput.m_collisionShapes[0] = collisionShape0Ptr;
01031                                 collisionPairInput.m_collisionShapes[1] = collisionShape1Ptr;
01032                                 collisionPairInput.m_spuCollisionShapes[0] = spuConvexShape0;
01033                                 collisionPairInput.m_spuCollisionShapes[1] = planeShape;
01034 
01035                                 ProcessConvexPlaneSpuCollision(&collisionPairInput,&lsMem,spuContacts);
01036                         } else
01037                         {
01038                                 btConvexInternalShape* spuConvexShape0 = (btConvexInternalShape*)collisionShape0Loc;
01039                                 btBvhTriangleMeshShape* trimeshShape = (btBvhTriangleMeshShape*)collisionShape1Loc;
01040 
01041                                 btVector3 dim0 = spuConvexShape0->getImplicitShapeDimensions();
01042                                 collisionPairInput.m_primitiveDimensions0 = dim0;
01043                                 collisionPairInput.m_collisionShapes[0] = collisionShape0Ptr;
01044                                 collisionPairInput.m_collisionShapes[1] = collisionShape1Ptr;
01045                                 collisionPairInput.m_spuCollisionShapes[0] = spuConvexShape0;
01046                                 collisionPairInput.m_spuCollisionShapes[1] = trimeshShape;
01047 
01048                                 ProcessConvexConcaveSpuCollision(&collisionPairInput,&lsMem,spuContacts);
01049                         }
01050                 }
01051 
01052         }
01053         
01054         spuContacts.flush();
01055 
01056 }
01057 
01058 
01059 void    processCollisionTask(void* userPtr, void* lsMemPtr)
01060 {
01061 
01062         SpuGatherAndProcessPairsTaskDesc* taskDescPtr = (SpuGatherAndProcessPairsTaskDesc*)userPtr;
01063         SpuGatherAndProcessPairsTaskDesc& taskDesc = *taskDescPtr;
01064         CollisionTask_LocalStoreMemory* colMemPtr = (CollisionTask_LocalStoreMemory*)lsMemPtr;
01065         CollisionTask_LocalStoreMemory& lsMem = *(colMemPtr);
01066 
01067         gUseEpa = taskDesc.m_useEpa;
01068 
01069         //      spu_printf("taskDescPtr=%llx\n",taskDescPtr);
01070 
01071         SpuContactResult spuContacts;
01072 
01074 
01075         ppu_address_t dmaInPtr = taskDesc.m_inPairPtr;
01076         unsigned int numPages = taskDesc.numPages;
01077         unsigned int numOnLastPage = taskDesc.numOnLastPage;
01078 
01079         // prefetch first set of inputs and wait
01080         lsMem.g_workUnitTaskBuffers.init();
01081 
01082         unsigned int nextNumOnPage = (numPages > 1)? MIDPHASE_NUM_WORKUNITS_PER_PAGE : numOnLastPage;
01083         lsMem.g_workUnitTaskBuffers.backBufferDmaGet(dmaInPtr, nextNumOnPage*sizeof(SpuGatherAndProcessWorkUnitInput), DMA_TAG(3));
01084         dmaInPtr += MIDPHASE_WORKUNIT_PAGE_SIZE;
01085 
01086         
01087         register unsigned char *inputPtr;
01088         register unsigned int numOnPage;
01089         register unsigned int j;
01090         SpuGatherAndProcessWorkUnitInput* wuInputs;     
01091         register int dmaSize;
01092         register ppu_address_t  dmaPpuAddress;
01093         register ppu_address_t  dmaPpuAddress2;
01094 
01095         int numPairs;
01096         register int p;
01097         SpuCollisionPairInput collisionPairInput;
01098         
01099         for (unsigned int i = 0; btLikely(i < numPages); i++)
01100         {
01101 
01102                 // wait for back buffer dma and swap buffers
01103                 inputPtr = lsMem.g_workUnitTaskBuffers.swapBuffers();
01104 
01105                 // number on current page is number prefetched last iteration
01106                 numOnPage = nextNumOnPage;
01107 
01108 
01109                 // prefetch next set of inputs
01110 #if MIDPHASE_NUM_WORKUNIT_PAGES > 2
01111                 if ( btLikely( i < numPages-1 ) )
01112 #else
01113                 if ( btUnlikely( i < numPages-1 ) )
01114 #endif
01115                 {
01116                         nextNumOnPage = (i == numPages-2)? numOnLastPage : MIDPHASE_NUM_WORKUNITS_PER_PAGE;
01117                         lsMem.g_workUnitTaskBuffers.backBufferDmaGet(dmaInPtr, nextNumOnPage*sizeof(SpuGatherAndProcessWorkUnitInput), DMA_TAG(3));
01118                         dmaInPtr += MIDPHASE_WORKUNIT_PAGE_SIZE;
01119                 }
01120 
01121                 wuInputs = reinterpret_cast<SpuGatherAndProcessWorkUnitInput *>(inputPtr);
01122                 
01123                 
01124                 for (j = 0; btLikely( j < numOnPage ); j++)
01125                 {
01126 #ifdef DEBUG_SPU_COLLISION_DETECTION
01127                 //      printMidphaseInput(&wuInputs[j]);
01128 #endif //DEBUG_SPU_COLLISION_DETECTION
01129 
01130 
01131                         numPairs = wuInputs[j].m_endIndex - wuInputs[j].m_startIndex;
01132                         
01133                         if ( btLikely( numPairs ) )
01134                         {
01135                                         dmaSize = numPairs*sizeof(btBroadphasePair);
01136                                         dmaPpuAddress = wuInputs[j].m_pairArrayPtr+wuInputs[j].m_startIndex * sizeof(btBroadphasePair);
01137                                         lsMem.m_pairsPointer = (btBroadphasePair*)cellDmaGetReadOnly(&lsMem.gBroadphasePairsBuffer, dmaPpuAddress  , dmaSize, DMA_TAG(1), 0, 0);
01138                                         cellDmaWaitTagStatusAll(DMA_MASK(1));
01139                                 
01140 
01141                                 for (p=0;p<numPairs;p++)
01142                                 {
01143 
01144                                         //for each broadphase pair, do something
01145 
01146                                         btBroadphasePair& pair = lsMem.getBroadphasePairPtr()[p];
01147 #ifdef DEBUG_SPU_COLLISION_DETECTION
01148                                         spu_printf("pair->m_userInfo = %d\n",pair.m_userInfo);
01149                                         spu_printf("pair->m_algorithm = %d\n",pair.m_algorithm);
01150                                         spu_printf("pair->m_pProxy0 = %d\n",pair.m_pProxy0);
01151                                         spu_printf("pair->m_pProxy1 = %d\n",pair.m_pProxy1);
01152 #endif //DEBUG_SPU_COLLISION_DETECTION
01153 
01154                                         if (pair.m_internalTmpValue == 2 && pair.m_algorithm && pair.m_pProxy0 && pair.m_pProxy1)
01155                                         {
01156                                                 dmaSize = sizeof(SpuContactManifoldCollisionAlgorithm);
01157                                                 dmaPpuAddress2 = (ppu_address_t)pair.m_algorithm;
01158                                                 lsMem.m_lsCollisionAlgorithmPtr = (SpuContactManifoldCollisionAlgorithm*)cellDmaGetReadOnly(&lsMem.gSpuContactManifoldAlgoBuffer, dmaPpuAddress2  , dmaSize, DMA_TAG(1), 0, 0);
01159 
01160                                                 cellDmaWaitTagStatusAll(DMA_MASK(1));
01161 
01162                                                 lsMem.needsDmaPutContactManifoldAlgo = false;
01163 
01164                                                 collisionPairInput.m_persistentManifoldPtr = (ppu_address_t) lsMem.getlocalCollisionAlgorithm()->getContactManifoldPtr();
01165                                                 collisionPairInput.m_isSwapped = false;
01166 
01167                                                 if (1)
01168                                                 {
01169 
01171 
01172 
01173 #ifdef DEBUG_SPU_COLLISION_DETECTION
01174                                         //              spu_printf("SPU collisionPairInput->m_shapeType0 = %d\n",collisionPairInput->m_shapeType0);
01175                                         //              spu_printf("SPU collisionPairInput->m_shapeType1 = %d\n",collisionPairInput->m_shapeType1);
01176 #endif //DEBUG_SPU_COLLISION_DETECTION
01177 
01178                                                         
01179                                                         dmaSize = sizeof(btPersistentManifold);
01180 
01181                                                         dmaPpuAddress2 = collisionPairInput.m_persistentManifoldPtr;
01182                                                         lsMem.m_lsManifoldPtr = (btPersistentManifold*)cellDmaGetReadOnly(&lsMem.gPersistentManifoldBuffer, dmaPpuAddress2  , dmaSize, DMA_TAG(1), 0, 0);
01183 
01184                                                         collisionPairInput.m_shapeType0 = lsMem.getlocalCollisionAlgorithm()->getShapeType0();
01185                                                         collisionPairInput.m_shapeType1 = lsMem.getlocalCollisionAlgorithm()->getShapeType1();
01186                                                         collisionPairInput.m_collisionMargin0 = lsMem.getlocalCollisionAlgorithm()->getCollisionMargin0();
01187                                                         collisionPairInput.m_collisionMargin1 = lsMem.getlocalCollisionAlgorithm()->getCollisionMargin1();
01188                                                         
01189                                                         
01190                                                         
01191                                                         //??cellDmaWaitTagStatusAll(DMA_MASK(1));
01192                                                         
01193 
01194                                                         if (1)
01195                                                         {
01196                                                                 //snPause();
01197 
01198                                                                 // Get the collision objects
01199                                                                 dmaAndSetupCollisionObjects(collisionPairInput, lsMem);
01200 
01201                                                                 if (lsMem.getColObj0()->isActive() || lsMem.getColObj1()->isActive())
01202                                                                 {
01203 
01204                                                                         lsMem.needsDmaPutContactManifoldAlgo = true;
01205 #ifdef USE_SEPDISTANCE_UTIL
01206                                                                         lsMem.getlocalCollisionAlgorithm()->m_sepDistance.updateSeparatingDistance(collisionPairInput.m_worldTransform0,collisionPairInput.m_worldTransform1);
01207 #endif //USE_SEPDISTANCE_UTIL
01208                                                         
01209 #define USE_DEDICATED_BOX_BOX 1
01210 #ifdef USE_DEDICATED_BOX_BOX
01211                                                                         bool boxbox = ((lsMem.getlocalCollisionAlgorithm()->getShapeType0()==BOX_SHAPE_PROXYTYPE)&&
01212                                                                                 (lsMem.getlocalCollisionAlgorithm()->getShapeType1()==BOX_SHAPE_PROXYTYPE));
01213                                                                         if (boxbox)
01214                                                                         {
01215                                                                                 //spu_printf("boxbox dist = %f\n",distance);
01216                                                                                 btPersistentManifold* spuManifold=lsMem.getContactManifoldPtr();
01217                                                                                 btPersistentManifold* manifold = (btPersistentManifold*)collisionPairInput.m_persistentManifoldPtr;
01218                                                                                 ppu_address_t manifoldAddress = (ppu_address_t)manifold;
01219 
01220                                                                                 spuContacts.setContactInfo(spuManifold,manifoldAddress,lsMem.getColObj0()->getWorldTransform(),
01221                                                                                         lsMem.getColObj1()->getWorldTransform(),
01222                                                                                         lsMem.getColObj0()->getRestitution(),lsMem.getColObj1()->getRestitution(),
01223                                                                                         lsMem.getColObj0()->getFriction(),lsMem.getColObj1()->getFriction(),
01224                                                                                         collisionPairInput.m_isSwapped);
01225 
01226                                                 
01227                                                                         //float distance=0.f;
01228                                                                         btVector3 normalInB;
01229 
01230 
01231                                                                         if (
01232 #ifdef USE_SEPDISTANCE_UTIL
01233                                                                                 lsMem.getlocalCollisionAlgorithm()->m_sepDistance.getConservativeSeparatingDistance()<=0.f
01234 #else
01235                                                                                 1
01236 #endif                                                                                  
01237                                                                                 )
01238                                                                                 {
01239 //#define USE_PE_BOX_BOX 1
01240 #ifdef USE_PE_BOX_BOX
01241                                                                                         {
01242 
01243                                                                                                 //getCollisionMargin0
01244                                                                                                 btScalar margin0 = lsMem.getlocalCollisionAlgorithm()->getCollisionMargin0();
01245                                                                                                 btScalar margin1 = lsMem.getlocalCollisionAlgorithm()->getCollisionMargin1();
01246                                                                                                 btVector3 shapeDim0 = lsMem.getlocalCollisionAlgorithm()->getShapeDimensions0()+btVector3(margin0,margin0,margin0);
01247                                                                                                 btVector3 shapeDim1 = lsMem.getlocalCollisionAlgorithm()->getShapeDimensions1()+btVector3(margin1,margin1,margin1);
01248 /*
01249                                                                                                 //Box boxA(shapeDim0.getX(),shapeDim0.getY(),shapeDim0.getZ());
01250                                                                                                 vmVector3 vmPos0 = getVmVector3(collisionPairInput.m_worldTransform0.getOrigin());
01251                                                                                                 vmVector3 vmPos1 = getVmVector3(collisionPairInput.m_worldTransform1.getOrigin());
01252                                                                                                 vmMatrix3 vmMatrix0 = getVmMatrix3(collisionPairInput.m_worldTransform0.getBasis());
01253                                                                                                 vmMatrix3 vmMatrix1 = getVmMatrix3(collisionPairInput.m_worldTransform1.getBasis());
01254 
01255                                                                                                 vmTransform3 transformA(vmMatrix0,vmPos0);
01256                                                                                                 Box boxB(shapeDim1.getX(),shapeDim1.getY(),shapeDim1.getZ());
01257                                                                                                 vmTransform3 transformB(vmMatrix1,vmPos1);
01258                                                                                                 BoxPoint resultClosestBoxPointA;
01259                                                                                                 BoxPoint resultClosestBoxPointB;
01260                                                                                                 vmVector3 resultNormal;
01261                                                                                                 */
01262 
01263 #ifdef USE_SEPDISTANCE_UTIL
01264                                                                                                 float distanceThreshold = FLT_MAX
01265 #else
01266                                                                                                 //float distanceThreshold = 0.f;
01267 #endif
01268 
01269 
01270                                                                                                 vmVector3 n;
01271                                                                                                 Box boxA;
01272                                                                                                 vmVector3 hA(shapeDim0.getX(),shapeDim0.getY(),shapeDim0.getZ());
01273                                                                                                 vmVector3 hB(shapeDim1.getX(),shapeDim1.getY(),shapeDim1.getZ());
01274                                                                                                 boxA.mHalf= hA;
01275                                                                                                 vmTransform3 trA;
01276                                                                                                 trA.setTranslation(getVmVector3(collisionPairInput.m_worldTransform0.getOrigin()));
01277                                                                                                 trA.setUpper3x3(getVmMatrix3(collisionPairInput.m_worldTransform0.getBasis()));
01278                                                                                                 Box boxB;
01279                                                                                                 boxB.mHalf = hB;
01280                                                                                                 vmTransform3 trB;
01281                                                                                                 trB.setTranslation(getVmVector3(collisionPairInput.m_worldTransform1.getOrigin()));
01282                                                                                                 trB.setUpper3x3(getVmMatrix3(collisionPairInput.m_worldTransform1.getBasis()));
01283                                                                                                 
01284                                                                                                 float distanceThreshold = spuManifold->getContactBreakingThreshold();//0.001f;
01285 
01286 
01287                                                                                                 BoxPoint ptA,ptB;
01288                                                                                                 float dist = boxBoxDistance(n, ptA, ptB,
01289                                                                                                                    boxA, trA, boxB,        trB,
01290                                                                                                                         distanceThreshold );
01291 
01292 
01293 //                                                                                              float distance = boxBoxDistance(resultNormal,resultClosestBoxPointA,resultClosestBoxPointB,  boxA, transformA, boxB,transformB,distanceThreshold);
01294                                                                                                 
01295                                                                                                 normalInB = -getBtVector3(n);//resultNormal);
01296 
01297                                                                                                 //if(dist < distanceThreshold)//spuManifold->getContactBreakingThreshold())
01298                                                                                                 if(dist < spuManifold->getContactBreakingThreshold())
01299                                                                                                 {
01300                                                                                                         btVector3 pointOnB = collisionPairInput.m_worldTransform1(getBtVector3(ptB.localPoint));
01301 
01302                                                                                                         spuContacts.addContactPoint(
01303                                                                                                                 normalInB,
01304                                                                                                                 pointOnB,
01305                                                                                                                 dist);
01306                                                                                                 }
01307                                                                                         } 
01308 #else                                                                   
01309                                                                                         {
01310 
01311                                                                                                 btScalar margin0 = lsMem.getlocalCollisionAlgorithm()->getCollisionMargin0();
01312                                                                                                 btScalar margin1 = lsMem.getlocalCollisionAlgorithm()->getCollisionMargin1();
01313                                                                                                 btVector3 shapeDim0 = lsMem.getlocalCollisionAlgorithm()->getShapeDimensions0()+btVector3(margin0,margin0,margin0);
01314                                                                                                 btVector3 shapeDim1 = lsMem.getlocalCollisionAlgorithm()->getShapeDimensions1()+btVector3(margin1,margin1,margin1);
01315 
01316 
01317                                                                                                 btBoxShape box0(shapeDim0);
01318                                                                                                 btBoxShape box1(shapeDim1);
01319 
01320                                                                                                 struct SpuBridgeContactCollector : public btDiscreteCollisionDetectorInterface::Result
01321                                                                                                 {
01322                                                                                                         SpuContactResult&       m_spuContacts;
01323 
01324                                                                                                         virtual void setShapeIdentifiersA(int partId0,int index0)
01325                                                                                                         {
01326                                                                                                                 m_spuContacts.setShapeIdentifiersA(partId0,index0);
01327                                                                                                         }
01328                                                                                                         virtual void setShapeIdentifiersB(int partId1,int index1)
01329                                                                                                         {
01330                                                                                                                 m_spuContacts.setShapeIdentifiersB(partId1,index1);
01331                                                                                                         }
01332                                                                                                         virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
01333                                                                                                         {
01334                                                                                                                 m_spuContacts.addContactPoint(normalOnBInWorld,pointInWorld,depth);
01335                                                                                                         }
01336 
01337                                                                                                         SpuBridgeContactCollector(SpuContactResult& spuContacts)
01338                                                                                                                 :m_spuContacts(spuContacts)
01339                                                                                                         {
01340 
01341                                                                                                         }
01342                                                                                                 };
01343                                                                                                 
01344                                                                                                 SpuBridgeContactCollector  bridgeOutput(spuContacts);
01345 
01346                                                                                                 btDiscreteCollisionDetectorInterface::ClosestPointInput input;
01347                                                                                                 input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
01348                                                                                                 input.m_transformA = collisionPairInput.m_worldTransform0;
01349                                                                                                 input.m_transformB = collisionPairInput.m_worldTransform1;
01350 
01351                                                                                                 btBoxBoxDetector detector(&box0,&box1);
01352                                                                                                 
01353                                                                                                 detector.getClosestPoints(input,bridgeOutput,0);
01354 
01355                                                                                         }
01356 #endif //USE_PE_BOX_BOX
01357                                                                                         
01358                                                                                         lsMem.needsDmaPutContactManifoldAlgo = true;
01359 #ifdef USE_SEPDISTANCE_UTIL
01360                                                                                         btScalar sepDist2 = distance+spuManifold->getContactBreakingThreshold();
01361                                                                                         lsMem.getlocalCollisionAlgorithm()->m_sepDistance.initSeparatingDistance(normalInB,sepDist2,collisionPairInput.m_worldTransform0,collisionPairInput.m_worldTransform1);
01362 #endif //USE_SEPDISTANCE_UTIL
01363                                                                                         gProcessedCol++;
01364                                                                                 } else
01365                                                                                 {
01366                                                                                         gSkippedCol++;
01367                                                                                 }
01368 
01369                                                                                 spuContacts.flush();
01370                                                                                         
01371 
01372                                                                         } else
01373 #endif //USE_DEDICATED_BOX_BOX
01374                                                                         {
01375                                                                                 if (
01376 #ifdef USE_SEPDISTANCE_UTIL
01377                                                                                         lsMem.getlocalCollisionAlgorithm()->m_sepDistance.getConservativeSeparatingDistance()<=0.f
01378 #else
01379                                                                                         1
01380 #endif //USE_SEPDISTANCE_UTIL
01381                                                                                         )
01382                                                                                 {
01383                                                                                         handleCollisionPair(collisionPairInput, lsMem, spuContacts,
01384                                                                                                 (ppu_address_t)lsMem.getColObj0()->getCollisionShape(), &lsMem.gCollisionShapes[0].collisionShape,
01385                                                                                                 (ppu_address_t)lsMem.getColObj1()->getCollisionShape(), &lsMem.gCollisionShapes[1].collisionShape);
01386                                                                                 } else
01387                                                                                 {
01388                                                                                                 //spu_printf("boxbox dist = %f\n",distance);
01389                                                                                         btPersistentManifold* spuManifold=lsMem.getContactManifoldPtr();
01390                                                                                         btPersistentManifold* manifold = (btPersistentManifold*)collisionPairInput.m_persistentManifoldPtr;
01391                                                                                         ppu_address_t manifoldAddress = (ppu_address_t)manifold;
01392 
01393                                                                                         spuContacts.setContactInfo(spuManifold,manifoldAddress,lsMem.getColObj0()->getWorldTransform(),
01394                                                                                                 lsMem.getColObj1()->getWorldTransform(),
01395                                                                                                 lsMem.getColObj0()->getRestitution(),lsMem.getColObj1()->getRestitution(),
01396                                                                                                 lsMem.getColObj0()->getFriction(),lsMem.getColObj1()->getFriction(),
01397                                                                                                 collisionPairInput.m_isSwapped);
01398 
01399                                                                                         spuContacts.flush();
01400                                                                                 }
01401                                                                         }
01402                                                                 
01403                                                                 }
01404 
01405                                                         }
01406                                                 }
01407 
01408 #ifdef USE_SEPDISTANCE_UTIL
01409 #if defined (__SPU__) || defined (USE_LIBSPE2)
01410                                                 if (lsMem.needsDmaPutContactManifoldAlgo)
01411                                                 {
01412                                                         dmaSize = sizeof(SpuContactManifoldCollisionAlgorithm);
01413                                                         dmaPpuAddress2 = (ppu_address_t)pair.m_algorithm;
01414                                                         cellDmaLargePut(&lsMem.gSpuContactManifoldAlgoBuffer, dmaPpuAddress2  , dmaSize, DMA_TAG(1), 0, 0);
01415                                                         cellDmaWaitTagStatusAll(DMA_MASK(1));
01416                                                 }
01417 #endif
01418 #endif //#ifdef USE_SEPDISTANCE_UTIL
01419 
01420                                         }
01421                                 }
01422                         }
01423                 } //end for (j = 0; j < numOnPage; j++)
01424 
01425         }//     for 
01426 
01427 
01428 
01429         return;
01430 }
01431 
01432