00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016 #include "SpuGatheringCollisionTask.h"
00017
00018
00019 #include "../SpuDoubleBuffer.h"
00020
00021 #include "../SpuCollisionTaskProcess.h"
00022 #include "../SpuGatheringCollisionDispatcher.h"
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
00042 #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
00043
00044
00045 #include "boxBoxDistance.h"
00046 #include "BulletMultiThreaded/vectormath2bullet.h"
00047 #include "SpuCollisionShapes.h"
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
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
00071 #define SPE_CACHE_NSETS 8
00072
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
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
00225
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
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
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
00345
00346
00347
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
00355 btScalar* graphicsbasePtr = (btScalar*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_vertexBase+graphicsindex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_vertexStride);
00356
00357
00358
00360
00361
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
00372 }
00373
00374
00375 SpuCollisionPairInput triangleConcaveInput(*m_wuInput);
00376
00377 triangleConcaveInput.m_spuCollisionShapes[1] = &m_tmpTriangleShape;
00378 triangleConcaveInput.m_shapeType1 = TRIANGLE_SHAPE_PROXYTYPE;
00379
00380 m_spuContacts.setShapeIdentifiersB(subPart,triangleIndex);
00381
00382
00383
00384 ProcessSpuConvexConvexCollision(&triangleConcaveInput, m_lsMemPtr,m_spuContacts);
00386
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
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
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
00444
00445 dmaSize = sizeof(btConvexHullShape);
00446 dmaPpuAddress2 = wuInput->m_collisionShapes[0];
00447
00448 cellDmaGet(&convexHullShape0, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0);
00449
00450 }
00451
00452 if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) )
00453 {
00454
00455 dmaSize = sizeof(btConvexHullShape);
00456 dmaPpuAddress2 = wuInput->m_collisionShapes[1];
00457 cellDmaGet(&convexHullShape1, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0);
00458
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
00502
00503
00504
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
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
00543
00544 btBvhTriangleMeshShape* trimeshShape = (btBvhTriangleMeshShape*)wuInput->m_spuCollisionShapes[1];
00545
00546 dmaBvhShapeData (&lsMemPtr->bvhShapeData, trimeshShape);
00547
00548 btVector3 aabbMin(-1,-400,-1);
00549 btVector3 aabbMax(1,400,1);
00550
00551
00552
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
00561
00562
00563
00564
00565
00566
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
00576
00577 BvhSubtreeInfoArray& subTrees = lsMemPtr->bvhShapeData.getOptimizedBvh()->getSubtreeInfoArray();
00578
00579
00580 spuNodeCallback nodeCallback(wuInput,lsMemPtr,spuContacts);
00581 IndexedMeshArray& indexArray = lsMemPtr->bvhShapeData.gTriangleMeshInterfacePtr->getIndexedMeshArray();
00582
00583
00584
00585
00586 if (subTrees.size() && indexArray.size() == 1)
00587 {
00589 dmaBvhIndexedMesh (&lsMemPtr->bvhShapeData.gIndexMesh, indexArray, 0 , 1 );
00590 cellDmaWaitTagStatusAll(DMA_MASK(1));
00591
00592
00593 int numBatch = subTrees.size();
00594 for (int i=0;i<numBatch;)
00595 {
00596
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
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
00616 dmaBvhSubTreeNodes (&lsMemPtr->bvhShapeData.gSubtreeNodes[0], subtree, nodeArray, 2);
00617 cellDmaWaitTagStatusAll(DMA_MASK(2));
00618
00619
00620 spuWalkStacklessQuantizedTree(&nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax,
00621 &lsMemPtr->bvhShapeData.gSubtreeNodes[0],
00622 0,
00623 subtree.m_subtreeSize);
00624 }
00625
00626 }
00627
00628
00629
00630
00631
00632 i+=nextBatch;
00633 }
00634
00635
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
00656 #endif //DEBUG_SPU_COLLISION_DETECTION
00657
00658
00659 btPersistentManifold* manifold = (btPersistentManifold*)wuInput->m_persistentManifoldPtr;
00660
00661 bool genericGjk = true;
00662
00663 if (genericGjk)
00664 {
00665
00666
00667
00668
00669
00670 btVoronoiSimplexSolver simplexSolver;
00671 btGjkEpaPenetrationDepthSolver epaPenetrationSolver2;
00672
00673 btConvexPenetrationDepthSolver* penetrationSolver = &epaPenetrationSolver2;
00674
00675
00676 #ifdef ENABLE_EPA
00677 if (gUseEpa)
00678 {
00679 penetrationSolver = &epaPenetrationSolver2;
00680 } else
00681 #endif
00682 {
00683
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
00694
00695 dmaSize = sizeof(btConvexHullShape);
00696 dmaPpuAddress2 = wuInput->m_collisionShapes[0];
00697
00698 cellDmaGet(&convexHullShape0, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0);
00699
00700 }
00701
00702 if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) )
00703 {
00704
00705 dmaSize = sizeof(btConvexHullShape);
00706 dmaPpuAddress2 = wuInput->m_collisionShapes[1];
00707 cellDmaGet(&convexHullShape1, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0);
00708
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
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);
00778 gjk.getClosestPoints(cpInput,spuContacts,0);
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);
00813 dmaPpuAddress2 = (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);
00817 dmaPpuAddress2 = (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
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
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
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
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
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
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
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
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
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
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
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
00992 bool handleConvexConcave = false;
00993
00994
00995
00996 if (btBroadphaseProxy::isConcave(collisionPairInput.m_shapeType0) &&
00997 btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType1))
00998 {
00999
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
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
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
01103 inputPtr = lsMem.g_workUnitTaskBuffers.swapBuffers();
01104
01105
01106 numOnPage = nextNumOnPage;
01107
01108
01109
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
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
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
01175
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
01192
01193
01194 if (1)
01195 {
01196
01197
01198
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
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
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
01240 #ifdef USE_PE_BOX_BOX
01241 {
01242
01243
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
01250
01251
01252
01253
01254
01255
01256
01257
01258
01259
01260
01261
01262
01263 #ifdef USE_SEPDISTANCE_UTIL
01264 float distanceThreshold = FLT_MAX
01265 #else
01266
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();
01285
01286
01287 BoxPoint ptA,ptB;
01288 float dist = boxBoxDistance(n, ptA, ptB,
01289 boxA, trA, boxB, trB,
01290 distanceThreshold );
01291
01292
01293
01294
01295 normalInB = -getBtVector3(n);
01296
01297
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
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 }
01424
01425 }
01426
01427
01428
01429 return;
01430 }
01431
01432