Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
Files
9f28b2bc121fb888fc1e5b148119fcf3162b0dd4
bullet3/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.cpp

1150 lines
42 KiB
C++
Raw Blame History

#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
#include "BulletCollision/CollisionShapes/btConvexShape.h"
#include "BulletCollision/CollisionShapes/btOptimizedBvh.h"
#include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h"
#include "BulletCollision/CollisionShapes/btTriangleShape.h"
#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
#include "BulletCollision/CollisionShapes/btConvexShape.h"
#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
#include "BulletCollision/CollisionShapes/btConvexHullShape.h"
#include "BulletCollision/CollisionShapes/btCompoundShape.h"
#include "BulletCollision/CollisionShapes/btConvexPointCloudShape.h"
#include "SpuGatheringCollisionTask.h"
//#define DEBUG_SPU_COLLISION_DETECTION 1
#include "../SpuDoubleBuffer.h"
#include "../SpuCollisionTaskProcess.h"
#include "../SpuGatheringCollisionDispatcher.h" //for SPU_BATCHSIZE_BROADPHASE_PAIRS
#include "../SpuContactManifoldCollisionAlgorithm.h"
#include "SpuContactResult.h"
#include "SpuCollisionShapes.h" //definition of SpuConvexPolyhedronVertexData
#ifdef __SPU__
///Software caching from the IBM Cell SDK, it reduces 25% SPU time for our test cases
#ifndef USE_LIBSPE2
#define USE_SOFTWARE_CACHE 1
#endif
#endif //__SPU__
////////////////////////////////////////////////
/// software caching
#if USE_SOFTWARE_CACHE
#include <spu_intrinsics.h>
#include <sys/spu_thread.h>
#include <sys/spu_event.h>
#include <stdint.h>
#define SPE_CACHE_NWAY 4
//#define SPE_CACHE_NSETS 32, 16
#define SPE_CACHE_NSETS 8
//#define SPE_CACHELINE_SIZE 512
#define SPE_CACHELINE_SIZE 128
#define SPE_CACHE_SET_TAGID(set) 15
///make sure that spe_cache.h is below those defines!
#include "spe_cache.h"
int g_CacheMisses=0;
int g_CacheHits=0;
#if 0 // Added to allow cache misses and hits to be tracked, change this to 1 to restore unmodified version
#define spe_cache_read(ea) _spe_cache_lookup_xfer_wait_(ea, 0, 1)
#else
#define spe_cache_read(ea) \
({ \
int set, idx, line, byte; \
_spe_cache_nway_lookup_(ea, set, idx); \
\
if (btUnlikely(idx < 0)) { \
++g_CacheMisses; \
idx = _spe_cache_miss_(ea, set, -1); \
spu_writech(22, SPE_CACHE_SET_TAGMASK(set)); \
spu_mfcstat(MFC_TAG_UPDATE_ALL); \
} \
else \
{ \
++g_CacheHits; \
} \
line = _spe_cacheline_num_(set, idx); \
byte = _spe_cacheline_byte_offset_(ea); \
(void *) &spe_cache_mem[line + byte]; \
})
#endif
#endif // USE_SOFTWARE_CACHE
bool gUseEpa = false;
#ifdef USE_SN_TUNER
#include <LibSN_SPU.h>
#endif //USE_SN_TUNER
#if defined (__CELLOS_LV2__) || defined (USE_LIBSPE2)
#else
#define IGNORE_ALIGNMENT 1
#define spu_printf printf
#include <stdio.h>
#endif
//int gNumConvexPoints0=0;
///Make sure no destructors are called on this memory
struct CollisionTask_LocalStoreMemory
{
ATTRIBUTE_ALIGNED16(char bufferProxy0[16]);
ATTRIBUTE_ALIGNED16(char bufferProxy1[16]);
ATTRIBUTE_ALIGNED16(btBroadphaseProxy* gProxyPtr0);
ATTRIBUTE_ALIGNED16(btBroadphaseProxy* gProxyPtr1);
btBroadphaseProxy* getProxyPtr0 ()
{
return (btBroadphaseProxy*)bufferProxy0;
}
btBroadphaseProxy* getProxyPtr1 ()
{
return (btBroadphaseProxy*)bufferProxy1;
}
ATTRIBUTE_ALIGNED16(char gColObj0 [sizeof(btCollisionObject)+16]);
ATTRIBUTE_ALIGNED16(char gColObj1 [sizeof(btCollisionObject)+16]);
btCollisionObject* getColObj0()
{
return (btCollisionObject*) gColObj0;
}
btCollisionObject* getColObj1()
{
return (btCollisionObject*) gColObj1;
}
DoubleBuffer<unsigned char, MIDPHASE_WORKUNIT_PAGE_SIZE> g_workUnitTaskBuffers;
ATTRIBUTE_ALIGNED16(btBroadphasePair gBroadphasePairs[SPU_BATCHSIZE_BROADPHASE_PAIRS]);
SpuContactManifoldCollisionAlgorithm gSpuContactManifoldAlgo;
SpuContactManifoldCollisionAlgorithm* getlocalCollisionAlgorithm()
{
return (SpuContactManifoldCollisionAlgorithm*)&gSpuContactManifoldAlgo;
}
btPersistentManifold gPersistentManifold;
SpuInternalShape gInternalShapes[3]; // the third is temporary storage
SpuInternalConvexHull gInternalConvexHull[3]; // the third is for temporary storage
///we reserve 32bit integer indices, even though they might be 16bit
ATTRIBUTE_ALIGNED16(int spuIndices[16]);
SpuBvhMeshShape gBvhMeshShape;
SpuCompoundShape compoundShapeData[2];
};
static void print_size_stats ()
{
spu_printf("sizeof(CollisionTask_LocalStoreMemory) = %d\n", sizeof(CollisionTask_LocalStoreMemory));
spu_printf("sizeof(SpuInternalShape) = %d\n", sizeof(SpuInternalShape));
spu_printf("sizeof(SpuInternalConvexHull) = %d\n", sizeof(SpuInternalConvexHull));
spu_printf("sizeof(SpuBvhMeshShape) = %d\n", sizeof(SpuBvhMeshShape));
}
#if defined(__CELLOS_LV2__) || defined(USE_LIBSPE2)
ATTRIBUTE_ALIGNED16(CollisionTask_LocalStoreMemory gLocalStoreMemory);
void* createCollisionLocalStoreMemory()
{
//print_size_stats ();
return &gLocalStoreMemory;
}
#else
void* createCollisionLocalStoreMemory()
{
return new CollisionTask_LocalStoreMemory;
};
#endif
void ProcessSpuConvexConvexCollision(SpuCollisionPairInput* wuInput, CollisionTask_LocalStoreMemory* lsMemPtr, SpuContactResult& spuContacts);
SIMD_FORCE_INLINE void small_cache_read(void* buffer, ppu_address_t ea, size_t size)
{
#if USE_SOFTWARE_CACHE
// Check for alignment requirements. We need to make sure the entire request fits within one cache line,
// so the first and last bytes should fall on the same cache line
btAssert((ea & ~SPE_CACHELINE_MASK) == ((ea + size - 1) & ~SPE_CACHELINE_MASK));
void* ls = spe_cache_read(ea);
memcpy(buffer, ls, size);
#else
stallingUnalignedDmaSmallGet(buffer,ea,size);
#endif
}
SIMD_FORCE_INLINE void small_cache_read_triple( void* ls0, ppu_address_t ea0,
void* ls1, ppu_address_t ea1,
void* ls2, ppu_address_t ea2,
size_t size)
{
btAssert(size<16);
ATTRIBUTE_ALIGNED16(char tmpBuffer0[32]);
ATTRIBUTE_ALIGNED16(char tmpBuffer1[32]);
ATTRIBUTE_ALIGNED16(char tmpBuffer2[32]);
uint32_t i;
///make sure last 4 bits are the same, for cellDmaSmallGet
char* localStore0 = (char*)ls0;
uint32_t last4BitsOffset = ea0 & 0x0f;
char* tmpTarget0 = tmpBuffer0 + last4BitsOffset;
#ifdef __SPU__
cellDmaSmallGet(tmpTarget0,ea0,size,DMA_TAG(1),0,0);
#else
tmpTarget0 = (char*)cellDmaSmallGetReadOnly(tmpTarget0,ea0,size,DMA_TAG(1),0,0);
#endif
char* localStore1 = (char*)ls1;
last4BitsOffset = ea1 & 0x0f;
char* tmpTarget1 = tmpBuffer1 + last4BitsOffset;
#ifdef __SPU__
cellDmaSmallGet(tmpTarget1,ea1,size,DMA_TAG(1),0,0);
#else
tmpTarget1 = (char*)cellDmaSmallGetReadOnly(tmpTarget1,ea1,size,DMA_TAG(1),0,0);
#endif
char* localStore2 = (char*)ls2;
last4BitsOffset = ea2 & 0x0f;
char* tmpTarget2 = tmpBuffer2 + last4BitsOffset;
#ifdef __SPU__
cellDmaSmallGet(tmpTarget2,ea2,size,DMA_TAG(1),0,0);
#else
tmpTarget2 = (char*)cellDmaSmallGetReadOnly(tmpTarget2,ea2,size,DMA_TAG(1),0,0);
#endif
cellDmaWaitTagStatusAll( DMA_MASK(1) );
//this is slowish, perhaps memcpy on SPU is smarter?
for (i=0; btLikely( i<size );i++)
{
localStore0[i] = tmpTarget0[i];
localStore1[i] = tmpTarget1[i];
localStore2[i] = tmpTarget2[i];
}
}
#if 0
////////////////////////
/// Convex versus Concave triangle mesh collision detection (handles concave triangle mesh versus sphere, box, cylinder, triangle, cone, convex polyhedron etc)
///////////////////
void ProcessConvexConcaveSpuCollision(SpuCollisionPairInput* wuInput, CollisionTask_LocalStoreMemory* lsMemPtr, SpuContactResult& spuContacts)
{
//order: first collision shape is convex, second concave. m_isSwapped is true, if the original order was opposite
register int dmaSize;
register ppu_address_t dmaPpuAddress2;
btBvhTriangleMeshShape* trimeshShape = (btBvhTriangleMeshShape*)wuInput->m_spuCollisionShapes[1];
//need the mesh interface, for access to triangle vertices
dmaBvhShapeData (&lsMemPtr->bvhShapeData, trimeshShape);
btVector3 aabbMin(-1,-400,-1);
btVector3 aabbMax(1,400,1);
//recalc aabbs
btTransform convexInTriangleSpace;
convexInTriangleSpace = wuInput->m_worldTransform1.inverse() * wuInput->m_worldTransform0;
btConvexInternalShape* convexShape = (btConvexInternalShape*)wuInput->m_spuCollisionShapes[0];
computeAabb (aabbMin, aabbMax, convexShape, wuInput->m_collisionShapes[0], wuInput->m_shapeType0, convexInTriangleSpace);
//CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
//convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax);
// btScalar extraMargin = collisionMarginTriangle;
// btVector3 extra(extraMargin,extraMargin,extraMargin);
// aabbMax += extra;
// aabbMin -= extra;
///quantize query AABB
unsigned short int quantizedQueryAabbMin[3];
unsigned short int quantizedQueryAabbMax[3];
lsMemPtr->bvhShapeData.getOptimizedBvh()->quantizeWithClamp(quantizedQueryAabbMin,aabbMin,0);
lsMemPtr->bvhShapeData.getOptimizedBvh()->quantizeWithClamp(quantizedQueryAabbMax,aabbMax,1);
QuantizedNodeArray& nodeArray = lsMemPtr->bvhShapeData.getOptimizedBvh()->getQuantizedNodeArray();
//spu_printf("SPU: numNodes = %d\n",nodeArray.size());
BvhSubtreeInfoArray& subTrees = lsMemPtr->bvhShapeData.getOptimizedBvh()->getSubtreeInfoArray();
spuNodeCallback nodeCallback(wuInput,lsMemPtr,spuContacts);
IndexedMeshArray& indexArray = lsMemPtr->bvhShapeData.gTriangleMeshInterfacePtr->getIndexedMeshArray();
//spu_printf("SPU:indexArray.size() = %d\n",indexArray.size());
// spu_printf("SPU: numSubTrees = %d\n",subTrees.size());
//not likely to happen
if (subTrees.size() && indexArray.size() == 1)
{
///DMA in the index info
dmaBvhIndexedMesh (&lsMemPtr->bvhShapeData.gIndexMesh, indexArray, 0 /* index into indexArray */, 1 /* dmaTag */);
cellDmaWaitTagStatusAll(DMA_MASK(1));
//display the headers
int numBatch = subTrees.size();
for (int i=0;i<numBatch;)
{
// BEN: TODO - can reorder DMA transfers for less stall
int remaining = subTrees.size() - i;
int nextBatch = remaining < MAX_SPU_SUBTREE_HEADERS ? remaining : MAX_SPU_SUBTREE_HEADERS;
dmaBvhSubTreeHeaders (&lsMemPtr->bvhShapeData.gSubtreeHeaders[0], (ppu_address_t)(&subTrees[i]), nextBatch, 1);
cellDmaWaitTagStatusAll(DMA_MASK(1));
// spu_printf("nextBatch = %d\n",nextBatch);
for (int j=0;j<nextBatch;j++)
{
const btBvhSubtreeInfo& subtree = lsMemPtr->bvhShapeData.gSubtreeHeaders[j];
unsigned int overlap = spuTestQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax);
if (overlap)
{
btAssert(subtree.m_subtreeSize);
//dma the actual nodes of this subtree
dmaBvhSubTreeNodes (&lsMemPtr->bvhShapeData.gSubtreeNodes[0], subtree, nodeArray, 2);
cellDmaWaitTagStatusAll(DMA_MASK(2));
/* Walk this subtree */
spuWalkStacklessQuantizedTree(&nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax,
&lsMemPtr->bvhShapeData.gSubtreeNodes[0],
0,
subtree.m_subtreeSize);
}
// spu_printf("subtreeSize = %d\n",gSubtreeHeaders[j].m_subtreeSize);
}
// unsigned short int m_quantizedAabbMin[3];
// unsigned short int m_quantizedAabbMax[3];
// int m_rootNodeIndex;
// int m_subtreeSize;
i+=nextBatch;
}
//pre-fetch first tree, then loop and double buffer
}
}
#endif
template<typename T> void DoSwap(T& a, T& b)
{
char tmp[sizeof(T)];
memcpy(tmp, &a, sizeof(T));
memcpy(&a, &b, sizeof(T));
memcpy(&b, tmp, sizeof(T));
}
SIMD_FORCE_INLINE void dmaAndSetupCollisionObjects(SpuCollisionPairInput& collisionPairInput, CollisionTask_LocalStoreMemory& lsMem)
{
register int dmaSize;
register ppu_address_t dmaPpuAddress2;
dmaSize = sizeof(btCollisionObject);
dmaPpuAddress2 = /*collisionPairInput.m_isSwapped ? (ppu_address_t)lsMem.gProxyPtr1->m_clientObject :*/ (ppu_address_t)lsMem.gProxyPtr0->m_clientObject;
cellDmaGet(&lsMem.gColObj0, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0);
dmaSize = sizeof(btCollisionObject);
dmaPpuAddress2 = /*collisionPairInput.m_isSwapped ? (ppu_address_t)lsMem.gProxyPtr0->m_clientObject :*/ (ppu_address_t)lsMem.gProxyPtr1->m_clientObject;
cellDmaGet(&lsMem.gColObj1, dmaPpuAddress2 , dmaSize, DMA_TAG(2), 0, 0);
cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));
collisionPairInput.m_worldTransform0 = lsMem.getColObj0()->getWorldTransform();
collisionPairInput.m_worldTransform1 = lsMem.getColObj1()->getWorldTransform();
}
void SpuConvexConvexCollisionAlgorithm (SpuCollisionPairInput& collisionPairInput,
CollisionTask_LocalStoreMemory& lsMem,
SpuContactResult &spuContacts,
SpuInternalShape* shape0Internal,
SpuInternalShape* shape1Internal,
SpuInternalConvexHull* shape0Points,
SpuInternalConvexHull* shape1Points)
{
btConvexPointCloudShape shape0ConvexHull (NULL, 0);
btConvexPointCloudShape shape1ConvexHull (NULL, 0);
btConvexShape* shape0;
btConvexShape* shape1;
btPersistentManifold* manifold = (btPersistentManifold*)collisionPairInput.m_persistentManifoldPtr;
#ifdef DEBUG_SPU_COLLISION_DETECTION
spu_printf("SpuConvexConvexCollisionAlgorithm ()\n");
#endif
if (collisionPairInput.m_shapeType0== CONVEX_HULL_SHAPE_PROXYTYPE )
{
shape0ConvexHull.setPoints (shape0Points->m_points, shape0Points->m_numPoints);
shape0 = &shape0ConvexHull;
} else {
shape0 = shape0Internal->m_convexShape;
}
if (collisionPairInput.m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE )
{
shape1ConvexHull.setPoints (shape1Points->m_points, shape1Points->m_numPoints);
shape1 = &shape1ConvexHull;
} else {
shape1 = shape1Internal->m_convexShape;
}
#ifdef DEBUG_SPU_COLLISION_DETECTION
spu_printf("shape0 = %p shape1 = %p\n", shape0, shape1);
#endif
{
btVoronoiSimplexSolver vsSolver;
btConvexPenetrationDepthSolver* penetrationSolver = NULL;
#define SPU_ENABLE_EPA 1
#ifdef SPU_ENABLE_EPA
btGjkEpaPenetrationDepthSolver epaPenetrationSolver;
btMinkowskiPenetrationDepthSolver minkowskiPenetrationSolver;
if (gUseEpa)
{
penetrationSolver = &epaPenetrationSolver;
} else {
penetrationSolver = &minkowskiPenetrationSolver;
}
#else
btMinkowskiPenetrationDepthSolver minkowskiPenetrationSolver;
penetrationSolver = &minkowskiPenetrationSolver;
#endif
btDiscreteCollisionDetectorInterface::ClosestPointInput cpInput;
cpInput.m_transformA = collisionPairInput.m_worldTransform0;
cpInput.m_transformB = collisionPairInput.m_worldTransform1;
btPersistentManifold* spuManifold= &lsMem.gPersistentManifold;
ppu_address_t manifoldAddress = (ppu_address_t)manifold;
spuContacts.setContactInfo(spuManifold,manifoldAddress,
lsMem.getColObj0()->getWorldTransform(),
lsMem.getColObj1()->getWorldTransform(),
lsMem.getColObj0()->getRestitution(),
lsMem.getColObj1()->getRestitution(),
lsMem.getColObj0()->getFriction(),
lsMem.getColObj1()->getFriction(),
collisionPairInput.m_isSwapped);
#ifdef DEBUG_SPU_COLLISION_DETECTION
spu_printf("start GJK\n");
#endif
btGjkPairDetector gjk(shape0,shape1,&vsSolver,penetrationSolver);
gjk.getClosestPoints(cpInput,spuContacts, NULL);//,debugDraw);
#ifdef DEBUG_SPU_COLLISION_DETECTION
spu_printf("stop GJK\n");
#endif
}
}
class spuNodeCallback : public btNodeOverlapCallback
{
const SpuCollisionPairInput& m_collisionPairInput;
CollisionTask_LocalStoreMemory& m_lsMem;
SpuContactResult& m_spuContacts;
ATTRIBUTE_ALIGNED16(btVector3 spuTriangleVertices[3]);
ATTRIBUTE_ALIGNED16(btScalar spuUnscaledVertex[4]);
public:
spuNodeCallback(const SpuCollisionPairInput& collisionPairInput,
CollisionTask_LocalStoreMemory& lsMem,
SpuContactResult& spuContacts)
: m_collisionPairInput(collisionPairInput),
m_lsMem(lsMem),
m_spuContacts(spuContacts)
{
}
virtual void processNode(int subPart, int triangleIndex)
{
///Create a triangle on the stack, call process collision, with GJK
///DMA the vertices, can benefit from software caching
//spu_printf("processNode with triangleIndex %d\n",triangleIndex);
///TODO: add switch between short int, and int indices, based on indexType
// ugly solution to support both 16bit and 32bit indices
if (m_lsMem.gBvhMeshShape.m_indexMesh.m_indexType == PHY_SHORT)
{
unsigned short int* indexBasePtr = (unsigned short int*)(m_lsMem.gBvhMeshShape.m_indexMesh.m_triangleIndexBase+triangleIndex*m_lsMem.gBvhMeshShape.m_indexMesh.m_triangleIndexStride);
ATTRIBUTE_ALIGNED16(unsigned short int tmpIndices[3]);
small_cache_read_triple(&tmpIndices[0],(ppu_address_t)&indexBasePtr[0],
&tmpIndices[1],(ppu_address_t)&indexBasePtr[1],
&tmpIndices[2],(ppu_address_t)&indexBasePtr[2],
sizeof(unsigned short int));
m_lsMem.spuIndices[0] = int(tmpIndices[0]);
m_lsMem.spuIndices[1] = int(tmpIndices[1]);
m_lsMem.spuIndices[2] = int(tmpIndices[2]);
} else
{
unsigned int* indexBasePtr = (unsigned int*)(m_lsMem.gBvhMeshShape.m_indexMesh.m_triangleIndexBase+triangleIndex*m_lsMem.gBvhMeshShape.m_indexMesh.m_triangleIndexStride);
small_cache_read_triple(&m_lsMem.spuIndices[0],(ppu_address_t)&indexBasePtr[0],
&m_lsMem.spuIndices[1],(ppu_address_t)&indexBasePtr[1],
&m_lsMem.spuIndices[2],(ppu_address_t)&indexBasePtr[2],
sizeof(int));
}
//spu_printf("SPU index0=%d ,",spuIndices[0]);
//spu_printf("SPU index1=%d ,",spuIndices[1]);
//spu_printf("SPU index2=%d ,",spuIndices[2]);
//spu_printf("SPU: indexBasePtr=%llx\n",indexBasePtr);
const btVector3& meshScaling = m_lsMem.gBvhMeshShape.m_triangleMeshInterface->getScaling();
for (int j=2;btLikely( j>=0 );j--)
{
int graphicsindex = m_lsMem.spuIndices[j];
//spu_printf("SPU index=%d ,",graphicsindex);
btScalar* graphicsbasePtr = (btScalar*)(m_lsMem.gBvhMeshShape.m_indexMesh.m_vertexBase+graphicsindex*m_lsMem.gBvhMeshShape.m_indexMesh.m_vertexStride);
//spu_printf("SPU graphicsbasePtr=%llx\n",graphicsbasePtr);
///handle un-aligned vertices...
//another DMA for each vertex
small_cache_read_triple(&spuUnscaledVertex[0],(ppu_address_t)&graphicsbasePtr[0],
&spuUnscaledVertex[1],(ppu_address_t)&graphicsbasePtr[1],
&spuUnscaledVertex[2],(ppu_address_t)&graphicsbasePtr[2],
sizeof(btScalar));
spuTriangleVertices[j] = btVector3(
spuUnscaledVertex[0]*meshScaling.getX(),
spuUnscaledVertex[1]*meshScaling.getY(),
spuUnscaledVertex[2]*meshScaling.getZ());
//spu_printf("SPU:triangle vertices:%f,%f,%f\n",spuTriangleVertices[j].x(),spuTriangleVertices[j].y(),spuTriangleVertices[j].z());
}
btTriangleShape tmpTriangleShape (spuTriangleVertices[0], spuTriangleVertices[1], spuTriangleVertices[2]);
SpuCollisionPairInput triangleConcaveInput(m_collisionPairInput);
triangleConcaveInput.m_shapeType1 = TRIANGLE_SHAPE_PROXYTYPE;
m_spuContacts.setShapeIdentifiers(-1,-1,subPart,triangleIndex);
//m_spuContacts.flush();
m_lsMem.gInternalShapes[2].m_collisionShape = &tmpTriangleShape;
m_lsMem.gInternalShapes[2].m_convexShape = &tmpTriangleShape;
SpuConvexConvexCollisionAlgorithm (triangleConcaveInput,
m_lsMem,
m_spuContacts,
&m_lsMem.gInternalShapes[0],
&m_lsMem.gInternalShapes[2],
&m_lsMem.gInternalConvexHull[0],
NULL);
///this flush should be automatic
//m_spuContacts.flush();
}
};
void SpuConvexConcaveCollisionAlgorithm (SpuCollisionPairInput& collisionPairInput,
CollisionTask_LocalStoreMemory& lsMem,
SpuContactResult &spuContacts,
SpuInternalShape* shape0Internal,
SpuInternalShape* shape1Internal,
SpuInternalConvexHull* shape0Points,
SpuInternalConvexHull* shape1Points)
{
btConvexPointCloudShape shape0ConvexHull (NULL, 0);
btConvexShape* shape0;
#ifdef DEBUG_SPU_COLLISION_DETECTION
spu_printf("SpuConvexConcaveCollisionAlgorithm ()\n");
#endif
if (collisionPairInput.m_shapeType0== CONVEX_HULL_SHAPE_PROXYTYPE )
{
shape0ConvexHull.setPoints (shape0Points->m_points, shape0Points->m_numPoints);
shape0 = &shape0ConvexHull;
} else {
shape0 = shape0Internal->m_convexShape;
}
SpuBvhMeshShape* bvhShape = &lsMem.gBvhMeshShape;
bvhShape->dmaMeshInterfaceAndOptimizedBvh (*shape1Internal, 1, 2);
cellDmaWaitTagStatusAll (DMA_MASK(1) | DMA_MASK(2));
// determine aabb of convex shape in triangle mesh
btVector3 aabbMin;
btVector3 aabbMax;
btTransform convexInTriangleSpace;
convexInTriangleSpace = collisionPairInput.m_worldTransform1.inverse() * collisionPairInput.m_worldTransform0;
shape0->getAabbNonVirtual (convexInTriangleSpace, aabbMin, aabbMax);
// btScalar extraMargin = collisionMarginTriangle;
// btVector3 extra(extraMargin,extraMargin,extraMargin);
// aabbMax += extra;
// aabbMin -= extra;
//quantize query AABB
unsigned short int quantizedQueryAabbMin[3];
unsigned short int quantizedQueryAabbMax[3];
bvhShape->m_optimizedBvh->quantizeWithClamp(quantizedQueryAabbMin,aabbMin,0);
bvhShape->m_optimizedBvh->quantizeWithClamp(quantizedQueryAabbMax,aabbMax,1);
QuantizedNodeArray& nodeArray = bvhShape->m_optimizedBvh->getQuantizedNodeArray();
//spu_printf("SPU: numNodes = %d\n",nodeArray.size());
BvhSubtreeInfoArray& subTrees = bvhShape->m_optimizedBvh->getSubtreeInfoArray();
spuNodeCallback nodeCallback(collisionPairInput,lsMem,spuContacts);
IndexedMeshArray& indexArray = bvhShape->m_triangleMeshInterface->getIndexedMeshArray();
//spu_printf("SPU:indexArray.size() = %d\n",indexArray.size());
//spu_printf("SPU: numSubTrees = %d\n",subTrees.size());
if (subTrees.size() && indexArray.size() == 1)
{
//DMA in the index info, we only support meshes with a single index (hence the '0')
bvhShape->dmaIndexedMesh (0, 1);
cellDmaWaitTagStatusAll(DMA_MASK(1));
//display the headers
int numBatch = subTrees.size();
for (int i=0;i<numBatch;)
{
// BEN: TODO - can reorder DMA transfers for less stall
int remaining = subTrees.size() - i;
int nextBatch = remaining < MAX_SPU_SUBTREE_HEADERS ? remaining : MAX_SPU_SUBTREE_HEADERS;
bvhShape->dmaSubTreeHeaders ((ppu_address_t)(&subTrees[i]), nextBatch, 1);
cellDmaWaitTagStatusAll(DMA_MASK(1));
// spu_printf("nextBatch = %d\n",nextBatch);
for (int j=0;j<nextBatch;j++)
{
const btBvhSubtreeInfo& subtree = bvhShape->m_subtreeHeaders[j];
unsigned int overlap = spuTestQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax);
if (overlap)
{
btAssert(subtree.m_subtreeSize);
//dma the actual nodes of this subtree
bvhShape->dmaSubTreeNodes (subtree, nodeArray, 2);
cellDmaWaitTagStatusAll(DMA_MASK(2));
/* Walk this subtree */
spuWalkStacklessQuantizedTree(&nodeCallback,
quantizedQueryAabbMin,
quantizedQueryAabbMax,
&bvhShape->m_subtreeNodes[0],
0,subtree.m_subtreeSize);
}
// spu_printf("subtreeSize = %d\n",gSubtreeHeaders[j].m_subtreeSize);
}
i+=nextBatch;
}
//pre-fetch first tree, then loop and double buffer
}
}
void handleCollisionPair(SpuCollisionPairInput& collisionPairInput, CollisionTask_LocalStoreMemory& lsMem,
SpuContactResult &spuContacts,
ppu_address_t collisionShape0Ptr,
ppu_address_t collisionShape1Ptr, bool dmaShapes = true)
{
#ifdef DEBUG_SPU_COLLISION_DETECTION
spu_printf("handleCollisionPair ()\n");
#endif
if (btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType0)
&& btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType1))
{
if (dmaShapes)
{
/* Don't load the same shape twice. */
if (collisionShape0Ptr != lsMem.gInternalShapes[0].m_ppuConvexShapePtr)
{
lsMem.gInternalShapes[0].dmaShapeData (collisionShape0Ptr, collisionPairInput.m_shapeType0, 1);
}
/* Don't load the same shape twice. */
if (collisionShape1Ptr != lsMem.gInternalShapes[1].m_ppuConvexShapePtr)
{
lsMem.gInternalShapes[1].dmaShapeData (collisionShape1Ptr, collisionPairInput.m_shapeType1, 2);
}
if (collisionPairInput.m_shapeType0 == CONVEX_HULL_SHAPE_PROXYTYPE)
{
btConvexHullShape* convexHull = (btConvexHullShape*)lsMem.gInternalShapes[0].m_convexShape;
cellDmaWaitTagStatusAll (DMA_MASK(1));
/* Don't load the same verts twice */
if ((ppu_address_t)convexHull->getPoints() != lsMem.gInternalConvexHull[0].m_ppuPointsPtr)
{
lsMem.gInternalConvexHull[0].dmaPointsData (lsMem.gInternalShapes[0], 1);
}
}
if (collisionPairInput.m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE)
{
btConvexHullShape* convexHull = (btConvexHullShape*)lsMem.gInternalShapes[1].m_convexShape;
cellDmaWaitTagStatusAll (DMA_MASK(2));
/* Don't load the same verts twice */
if ((ppu_address_t)convexHull->getPoints() != lsMem.gInternalConvexHull[1].m_ppuPointsPtr)
{
lsMem.gInternalConvexHull[1].dmaPointsData (lsMem.gInternalShapes[1], 2);
}
}
cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));
#if 0
if (collisionPairInput.m_shapeType0 == CONVEX_HULL_SHAPE_PROXYTYPE)
{
for (int i = 0; i < lsMem.gInternalConvexHull[0].m_numPoints; i++)
{
btVector3 vtx = lsMem.gInternalConvexHull[0].m_points[i];
spu_printf("%d %f %f %f\n", i, vtx.getX(), vtx.getY(), vtx.getZ());
}
}
if (collisionPairInput.m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE)
{
for (int i = 0; i < lsMem.gInternalConvexHull[1].m_numPoints; i++)
{
btVector3 vtx = lsMem.gInternalConvexHull[1].m_points[i];
spu_printf("%d %f %f %f\n", i, vtx.getX(), vtx.getY(), vtx.getZ());
}
}
#endif
}
SpuConvexConvexCollisionAlgorithm (collisionPairInput, lsMem, spuContacts,
&lsMem.gInternalShapes[0],
&lsMem.gInternalShapes[1],
&lsMem.gInternalConvexHull[0],
&lsMem.gInternalConvexHull[1]);
}
else if (btBroadphaseProxy::isCompound(collisionPairInput.m_shapeType0) &&
btBroadphaseProxy::isCompound(collisionPairInput.m_shapeType1))
{
//snPause();
/* Don't load the same shape twice. */
if (collisionShape0Ptr != lsMem.gInternalShapes[0].m_ppuConvexShapePtr)
{
lsMem.gInternalShapes[0].dmaShapeData (collisionShape0Ptr, collisionPairInput.m_shapeType0, 1);
}
/* Don't load the same shape twice. */
if (collisionShape1Ptr != lsMem.gInternalShapes[1].m_ppuConvexShapePtr)
{
lsMem.gInternalShapes[1].dmaShapeData (collisionShape1Ptr, collisionPairInput.m_shapeType1, 2);
}
cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));
// Both are compounds, do N^2 CD for now
// TODO: add some AABB-based pruning
btCompoundShape* spuCompoundShape0 = (btCompoundShape*)lsMem.gInternalShapes[0].m_collisionShape;
btCompoundShape* spuCompoundShape1 = (btCompoundShape*)lsMem.gInternalShapes[1].m_collisionShape;
lsMem.compoundShapeData[0].dmaChildShapeInfo (spuCompoundShape0, 1);
lsMem.compoundShapeData[1].dmaChildShapeInfo (spuCompoundShape1, 2);
cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));
int childShapeCount0 = spuCompoundShape0->getNumChildShapes();
int childShapeCount1 = spuCompoundShape1->getNumChildShapes();
// Start the N^2
for (int i = 0; i < childShapeCount0; ++i)
{
btCompoundShapeChild& childShape0 = lsMem.compoundShapeData[0].m_subshapes[i];
// dma childshape0 into gInternalShapes 0 (this overwrites the original compound shape)
lsMem.compoundShapeData[0].dmaChildShape (i, &lsMem.gInternalShapes[0], &lsMem.gInternalConvexHull[0], 1);
for (int j = 0; j < childShapeCount1; ++j)
{
btCompoundShapeChild& childShape1 = lsMem.compoundShapeData[1].m_subshapes[j];
lsMem.compoundShapeData[1].dmaChildShape (j, &lsMem.gInternalShapes[1], &lsMem.gInternalConvexHull[1], 2);
/* Create a new collision pair input struct using the two child shapes */
SpuCollisionPairInput cinput (collisionPairInput);
cinput.m_worldTransform0 = collisionPairInput.m_worldTransform0 * childShape0.m_transform;
cinput.m_shapeType0 = childShape0.m_childShapeType;
cinput.m_collisionMargin0 = childShape0.m_childMargin;
cinput.m_worldTransform1 = collisionPairInput.m_worldTransform1 * childShape1.m_transform;
cinput.m_shapeType1 = childShape1.m_childShapeType;
cinput.m_collisionMargin1 = childShape1.m_childMargin;
cellDmaWaitTagStatusAll (DMA_MASK(1) | DMA_MASK(2));
/* Recursively call handleCollisionPair () with new collision pair input */
handleCollisionPair(cinput, lsMem, spuContacts,
(ppu_address_t)childShape0.m_childShape,
(ppu_address_t)childShape1.m_childShape,false); // bug fix: changed index to j.
}
}
}
else if (btBroadphaseProxy::isCompound(collisionPairInput.m_shapeType0) )
{
/* Don't load the same shape twice. */
if (collisionShape0Ptr != lsMem.gInternalShapes[0].m_ppuConvexShapePtr)
{
lsMem.gInternalShapes[0].dmaShapeData (collisionShape0Ptr, collisionPairInput.m_shapeType0, 1);
}
/* Don't load the same shape twice. */
if (collisionShape1Ptr != lsMem.gInternalShapes[1].m_ppuConvexShapePtr)
{
lsMem.gInternalShapes[1].dmaShapeData (collisionShape1Ptr, collisionPairInput.m_shapeType1, 2);
}
if (collisionPairInput.m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE)
{
btConvexHullShape* convexHull = (btConvexHullShape*)lsMem.gInternalShapes[1].m_convexShape;
cellDmaWaitTagStatusAll (DMA_MASK(2));
/* Don't load the same verts twice */
if ((ppu_address_t)convexHull->getPoints() != lsMem.gInternalConvexHull[1].m_ppuPointsPtr)
{
lsMem.gInternalConvexHull[1].dmaPointsData (lsMem.gInternalShapes[1], 2);
}
}
cellDmaWaitTagStatusAll (DMA_MASK(1));
btCompoundShape* spuCompoundShape0 = (btCompoundShape*)lsMem.gInternalShapes[0].m_collisionShape;
lsMem.compoundShapeData[0].dmaChildShapeInfo (spuCompoundShape0, 1);
cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));
int childShapeCount0 = spuCompoundShape0->getNumChildShapes();
for (int i = 0; i < childShapeCount0; ++i)
{
btCompoundShapeChild& childShape = lsMem.compoundShapeData[0].m_subshapes[i];
lsMem.compoundShapeData[0].dmaChildShape (i, &lsMem.gInternalShapes[0], &lsMem.gInternalConvexHull[0], 1);
SpuCollisionPairInput cinput (collisionPairInput);
cinput.m_worldTransform0 = collisionPairInput.m_worldTransform0 * childShape.m_transform;
cinput.m_shapeType0 = childShape.m_childShapeType;
cinput.m_collisionMargin0 = childShape.m_childMargin;
cellDmaWaitTagStatusAll(DMA_MASK(1));
handleCollisionPair(cinput,
lsMem,
spuContacts,
(ppu_address_t)childShape.m_childShape,
collisionShape1Ptr, false);
}
}
else if (btBroadphaseProxy::isCompound(collisionPairInput.m_shapeType1) )
{
//snPause();
/* Don't load the same shape twice. */
if (collisionShape0Ptr != lsMem.gInternalShapes[0].m_ppuConvexShapePtr)
{
lsMem.gInternalShapes[0].dmaShapeData (collisionShape0Ptr, collisionPairInput.m_shapeType0, 1);
}
/* Don't load the same shape twice. */
if (collisionShape1Ptr != lsMem.gInternalShapes[1].m_ppuConvexShapePtr)
{
lsMem.gInternalShapes[1].dmaShapeData (collisionShape1Ptr, collisionPairInput.m_shapeType1, 2);
}
cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));
// object 0 non-compound, object 1 compound
btCompoundShape* spuCompoundShape = (btCompoundShape*)lsMem.gInternalShapes[1].m_collisionShape;
lsMem.compoundShapeData[1].dmaChildShapeInfo (spuCompoundShape, 1);
cellDmaWaitTagStatusAll(DMA_MASK(1));
int childShapeCount = spuCompoundShape->getNumChildShapes();
for (int i = 0; i < childShapeCount; ++i)
{
btCompoundShapeChild& childShape = lsMem.compoundShapeData[1].m_subshapes[i];
// Dma the child shape
lsMem.compoundShapeData[1].dmaChildShape (i, &lsMem.gInternalShapes[1], &lsMem.gInternalConvexHull[1], 1);
cellDmaWaitTagStatusAll(DMA_MASK(1));
SpuCollisionPairInput cinput (collisionPairInput);
cinput.m_worldTransform1 = collisionPairInput.m_worldTransform1 * childShape.m_transform;
cinput.m_shapeType1 = childShape.m_childShapeType;
cinput.m_collisionMargin1 = childShape.m_childMargin;
handleCollisionPair(cinput,
lsMem,
spuContacts,
collisionShape0Ptr,
(ppu_address_t)childShape.m_childShape, false);
}
}
else
{
//we only support convex v. concave
//a non-convex shape is involved
bool handleConvexConcave = false;
//concave v. convex
//swap into convex v. concave
if (btBroadphaseProxy::isConcave(collisionPairInput.m_shapeType0) &&
btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType1))
{
// Swap stuff
DoSwap(collisionShape0Ptr, collisionShape1Ptr);
DoSwap(collisionPairInput.m_shapeType0, collisionPairInput.m_shapeType1);
DoSwap(collisionPairInput.m_worldTransform0, collisionPairInput.m_worldTransform1);
DoSwap(collisionPairInput.m_collisionMargin0, collisionPairInput.m_collisionMargin1);
collisionPairInput.m_isSwapped = true;
}
if (btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType0)&&
btBroadphaseProxy::isConcave(collisionPairInput.m_shapeType1))
{
handleConvexConcave = true;
}
if (handleConvexConcave)
{
if (dmaShapes)
{
/* Don't load the same shape twice. */
if (collisionShape0Ptr != lsMem.gInternalShapes[0].m_ppuConvexShapePtr)
{
lsMem.gInternalShapes[0].dmaShapeData (collisionShape0Ptr, collisionPairInput.m_shapeType0, 1);
}
/* Don't load the same shape twice. */
if (collisionShape1Ptr != lsMem.gInternalShapes[1].m_ppuConvexShapePtr)
{
lsMem.gInternalShapes[1].dmaShapeData (collisionShape1Ptr, collisionPairInput.m_shapeType1, 2);
}
if (collisionPairInput.m_shapeType0 == CONVEX_HULL_SHAPE_PROXYTYPE)
{
btConvexHullShape* convexHull = (btConvexHullShape*)lsMem.gInternalShapes[0].m_convexShape;
cellDmaWaitTagStatusAll (DMA_MASK(1));
/* Don't load the same verts twice */
if ((ppu_address_t)convexHull->getPoints() != lsMem.gInternalConvexHull[0].m_ppuPointsPtr)
{
lsMem.gInternalConvexHull[0].dmaPointsData (lsMem.gInternalShapes[0], 1);
}
}
/* This can't happen because shape1 is the optimized Bvh shape */
#if 0
if (collisionPairInput.m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE)
{
btConvexHullShape* convexHull = (btConvexHullShape*)lsMem.gInternalShapes[1].m_convexShape;
cellDmaWaitTagStatusAll (DMA_MASK(2));
/* Don't load the same verts twice */
if ((ppu_address_t)convexHull->getPoints() != lsMem.gInternalConvexHull[1].m_ppuPointsPtr)
{
lsMem.gInternalConvexHull[1].dmaPointsData (lsMem.gInternalShapes[1], 2);
}
}
#endif
cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));
}
SpuConvexConcaveCollisionAlgorithm (collisionPairInput,
lsMem,
spuContacts,
&lsMem.gInternalShapes[0],
&lsMem.gInternalShapes[1],
&lsMem.gInternalConvexHull[0],
&lsMem.gInternalConvexHull[1]);
}
}
spuContacts.flush();
}
void processCollisionTask(void* userPtr, void* lsMemPtr)
{
SpuGatherAndProcessPairsTaskDesc* taskDescPtr = (SpuGatherAndProcessPairsTaskDesc*)userPtr;
SpuGatherAndProcessPairsTaskDesc& taskDesc = *taskDescPtr;
CollisionTask_LocalStoreMemory* colMemPtr = (CollisionTask_LocalStoreMemory*)lsMemPtr;
CollisionTask_LocalStoreMemory& lsMem = *(colMemPtr);
gUseEpa = taskDesc.m_useEpa;
// spu_printf("taskDescPtr=%llx\n",taskDescPtr);
SpuContactResult spuContacts;
////////////////////
ppu_address_t dmaInPtr = taskDesc.inPtr;
unsigned int numPages = taskDesc.numPages;
unsigned int numOnLastPage = taskDesc.numOnLastPage;
// prefetch first set of inputs and wait
lsMem.g_workUnitTaskBuffers.init();
unsigned int nextNumOnPage = (numPages > 1)? MIDPHASE_NUM_WORKUNITS_PER_PAGE : numOnLastPage;
lsMem.g_workUnitTaskBuffers.backBufferDmaGet(dmaInPtr, nextNumOnPage*sizeof(SpuGatherAndProcessWorkUnitInput), DMA_TAG(3));
dmaInPtr += MIDPHASE_WORKUNIT_PAGE_SIZE;
register unsigned char *inputPtr;
register unsigned int numOnPage;
register unsigned int j;
SpuGatherAndProcessWorkUnitInput* wuInputs;
register int dmaSize;
register ppu_address_t dmaPpuAddress;
register ppu_address_t dmaPpuAddress2;
int userInfo;
int numPairs;
register int p;
SpuCollisionPairInput collisionPairInput;
for (unsigned int i = 0; btLikely(i < numPages); i++)
{
// wait for back buffer dma and swap buffers
inputPtr = lsMem.g_workUnitTaskBuffers.swapBuffers();
// number on current page is number prefetched last iteration
numOnPage = nextNumOnPage;
// prefetch next set of inputs
#if MIDPHASE_NUM_WORKUNIT_PAGES > 2
if ( btLikely( i < numPages-1 ) )
#else
if ( btUnlikely( i < numPages-1 ) )
#endif
{
nextNumOnPage = (i == numPages-2)? numOnLastPage : MIDPHASE_NUM_WORKUNITS_PER_PAGE;
lsMem.g_workUnitTaskBuffers.backBufferDmaGet(dmaInPtr, nextNumOnPage*sizeof(SpuGatherAndProcessWorkUnitInput), DMA_TAG(3));
dmaInPtr += MIDPHASE_WORKUNIT_PAGE_SIZE;
}
wuInputs = reinterpret_cast<SpuGatherAndProcessWorkUnitInput *>(inputPtr);
for (j = 0; btLikely( j < numOnPage ); j++)
{
#ifdef DEBUG_SPU_COLLISION_DETECTION
// printMidphaseInput(&wuInputs[j]);
#endif //DEBUG_SPU_COLLISION_DETECTION
numPairs = wuInputs[j].m_endIndex - wuInputs[j].m_startIndex;
if ( btLikely( numPairs ) )
{
// DMA: broadphase pairs
dmaSize = numPairs*sizeof(btBroadphasePair);
dmaPpuAddress = wuInputs[j].m_pairArrayPtr+wuInputs[j].m_startIndex * sizeof(btBroadphasePair);
cellDmaGet(&lsMem.gBroadphasePairs, dmaPpuAddress , dmaSize, DMA_TAG(1), 0, 0);
cellDmaWaitTagStatusAll(DMA_MASK(1));
//for each broadphase pair, do something
for (p=0;p<numPairs;p++)
{
btBroadphasePair& pair = lsMem.gBroadphasePairs[p];
#ifdef DEBUG_SPU_COLLISION_DETECTION
spu_printf("pair->m_userInfo = %d\n",pair.m_userInfo);
spu_printf("pair->m_algorithm = %d\n",pair.m_algorithm);
spu_printf("pair->m_pProxy0 = %d\n",pair.m_pProxy0);
spu_printf("pair->m_pProxy1 = %d\n",pair.m_pProxy1);
#endif //DEBUG_SPU_COLLISION_DETECTION
userInfo = int(pair.m_userInfo);
// skip pairs we don't support
if (!pair.m_algorithm || !pair.m_pProxy0 ||
!pair.m_pProxy1 || userInfo != 2)
continue;
// DMA: SpuContactManifoldCollisionAlgorithm
// SpuContactManifoldCollisionAlgorithm:
// A dummy collision algorithm that gives us the
// collision types, and contact manifold pointers
dmaSize = sizeof(SpuContactManifoldCollisionAlgorithm);
dmaPpuAddress2 = (ppu_address_t)pair.m_algorithm;
cellDmaGet(&lsMem.gSpuContactManifoldAlgo, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0);
//cellDmaWaitTagStatusAll(DMA_MASK(1));
//snPause();
#ifdef DEBUG_SPU_COLLISION_DETECTION
spu_printf("SPU: manifoldPtr: %llx",collisionPairInput.m_persistentManifoldPtr);
#endif //DEBUG_SPU_COLLISION_DETECTION
// DMA: btBroadphaseProxy for object 0
dmaSize = sizeof(btBroadphaseProxy);
dmaPpuAddress2 = (ppu_address_t)pair.m_pProxy0;
lsMem.gProxyPtr0 = (btBroadphaseProxy*) lsMem.bufferProxy0;
stallingUnalignedDmaSmallGet(lsMem.gProxyPtr0, dmaPpuAddress2 , dmaSize);
// NOTE: The DMA initiated for SpuContactManifoldCollisionAlgorithm is synced in
// stallingUnalignedDmaSmallGet
collisionPairInput.m_persistentManifoldPtr = (ppu_address_t) lsMem.gSpuContactManifoldAlgo.getContactManifoldPtr();
collisionPairInput.m_isSwapped = false;
// DMA: btPersistentManifold
dmaSize = sizeof(btPersistentManifold);
dmaPpuAddress2 = collisionPairInput.m_persistentManifoldPtr;
cellDmaGet(&lsMem.gPersistentManifold, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0);
// DMA: btBroadphaseProxy for object 0
dmaSize = sizeof(btBroadphaseProxy);
dmaPpuAddress2 = (ppu_address_t)pair.m_pProxy1;
lsMem.gProxyPtr1 = (btBroadphaseProxy*) lsMem.bufferProxy1;
stallingUnalignedDmaSmallGet(lsMem.gProxyPtr1, dmaPpuAddress2 , dmaSize);
// NOTE: The DMA initiated for btPersistentManifold is synced in
// stallingUnalignedDmaSmallGet
#ifdef DEBUG_SPU_COLLISION_DETECTION
spu_printf("SPU collisionPairInput->m_shapeType0 = %d\n",collisionPairInput.m_shapeType0);
spu_printf("SPU collisionPairInput->m_shapeType1 = %d\n",collisionPairInput.m_shapeType1);
#endif //DEBUG_SPU_COLLISION_DETECTION
// Construct the collision Pair
collisionPairInput.m_shapeType0 = lsMem.gSpuContactManifoldAlgo.getShapeType0();
collisionPairInput.m_shapeType1 = lsMem.gSpuContactManifoldAlgo.getShapeType1();
collisionPairInput.m_collisionMargin0 = lsMem.gSpuContactManifoldAlgo.getCollisionMargin0();
collisionPairInput.m_collisionMargin1 = lsMem.gSpuContactManifoldAlgo.getCollisionMargin1();
cellDmaWaitTagStatusAll(DMA_MASK(1)); // might not be necessary
dmaAndSetupCollisionObjects(collisionPairInput, lsMem);
if (lsMem.getColObj0()->isActive() || lsMem.getColObj1()->isActive())
{
handleCollisionPair(collisionPairInput,
lsMem,
spuContacts,
(ppu_address_t)lsMem.getColObj0()->getCollisionShape(),
(ppu_address_t)lsMem.getColObj1()->getCollisionShape());
}
}
}
} //end for (j = 0; j < numOnPage; j++)
}// for
#ifdef DEBUG_SPU_COLLISION_DETECTION
spu_printf("processCollisionTask %d / %d\n",numPages, numOnLastPage);
#endif //DEBUG_SPU_COLLISION_DETECTION
return;
}
Reference in New Issue View Git Blame Copy Permalink