SpuGatheringCollisionTask.cpp

Go to the documentation of this file.

/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose, 
including commercial applications, and to alter it and redistribute it freely, 
subject to the following restrictions:

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.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/

#include "SpuGatheringCollisionTask.h"

//#define DEBUG_SPU_COLLISION_DETECTION 1
#include "../SpuDoubleBuffer.h"

#include "../SpuCollisionTaskProcess.h"
#include "../SpuGatheringCollisionDispatcher.h" //for SPU_BATCHSIZE_BROADPHASE_PAIRS

#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "../SpuContactManifoldCollisionAlgorithm.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "SpuContactResult.h"
#include "BulletCollision/CollisionShapes/btOptimizedBvh.h"
#include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h"
#include "BulletCollision/CollisionShapes/btConvexPointCloudShape.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 "SpuMinkowskiPenetrationDepthSolver.h"
//#include "SpuEpaPenetrationDepthSolver.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"


#include "boxBoxDistance.h"
#include "BulletMultiThreaded/vectormath2bullet.h"
#include "SpuCollisionShapes.h" //definition of SpuConvexPolyhedronVertexData
#include "BulletCollision/CollisionDispatch/btBoxBoxDetector.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
#include "BulletCollision/CollisionShapes/btTriangleShape.h"

#ifdef __SPU__
#ifndef USE_LIBSPE2
#define USE_SOFTWARE_CACHE 1
#endif
#endif //__SPU__

int gSkippedCol = 0;
int gProcessedCol = 0;

#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
#include "../Extras/software_cache/cache/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 (__SPU__) && !defined (USE_LIBSPE2)
#include <spu_printf.h>
#elif defined (USE_LIBSPE2)
#define spu_printf(a)
#else
#define IGNORE_ALIGNMENT 1
#include <stdio.h>
#include <stdlib.h>
#define spu_printf printf

#endif

//int gNumConvexPoints0=0;

struct  CollisionTask_LocalStoreMemory
{

        ATTRIBUTE_ALIGNED16(btBroadphasePair    gBroadphasePairsBuffer[SPU_BATCHSIZE_BROADPHASE_PAIRS]);
        DoubleBuffer<unsigned char, MIDPHASE_WORKUNIT_PAGE_SIZE> g_workUnitTaskBuffers;
        ATTRIBUTE_ALIGNED16(char gSpuContactManifoldAlgoBuffer [sizeof(SpuContactManifoldCollisionAlgorithm)+16]);
        ATTRIBUTE_ALIGNED16(char gColObj0Buffer [sizeof(btCollisionObject)+16]);
        ATTRIBUTE_ALIGNED16(char gColObj1Buffer [sizeof(btCollisionObject)+16]);
        ATTRIBUTE_ALIGNED16(int spuIndices[16]);
        btPersistentManifold    gPersistentManifoldBuffer;
        CollisionShape_LocalStoreMemory gCollisionShapes[2];
        bvhMeshShape_LocalStoreMemory bvhShapeData;
        SpuConvexPolyhedronVertexData convexVertexData[2];
        CompoundShape_LocalStoreMemory compoundShapeData[2];
                
        btCollisionObject*      m_lsColObj0Ptr;
        btCollisionObject*      m_lsColObj1Ptr;
        btBroadphasePair* m_pairsPointer;
        btPersistentManifold*   m_lsManifoldPtr;
        SpuContactManifoldCollisionAlgorithm*   m_lsCollisionAlgorithmPtr;

        bool    needsDmaPutContactManifoldAlgo;

        btCollisionObject* getColObj0()
        {
                return m_lsColObj0Ptr;
        }
        btCollisionObject* getColObj1()
        {
                return m_lsColObj1Ptr;
        }


        btBroadphasePair* getBroadphasePairPtr()
        {
                return m_pairsPointer;
        }

        SpuContactManifoldCollisionAlgorithm*   getlocalCollisionAlgorithm()
        {
                return m_lsCollisionAlgorithmPtr;
        }
        
        btPersistentManifold*   getContactManifoldPtr()
        {
                return m_lsManifoldPtr;
        }
};


#if defined(__CELLOS_LV2__) || defined(USE_LIBSPE2) 

ATTRIBUTE_ALIGNED16(CollisionTask_LocalStoreMemory      gLocalStoreMemory);

void* createCollisionLocalStoreMemory()
{
        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;
                

                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];
                }

                
}




class spuNodeCallback : public btNodeOverlapCallback
{
        SpuCollisionPairInput* m_wuInput;
        SpuContactResult&               m_spuContacts;
        CollisionTask_LocalStoreMemory* m_lsMemPtr;
        ATTRIBUTE_ALIGNED16(btTriangleShape)    m_tmpTriangleShape;

        ATTRIBUTE_ALIGNED16(btVector3   spuTriangleVertices[3]);
        ATTRIBUTE_ALIGNED16(btScalar    spuUnscaledVertex[4]);
        


public:
        spuNodeCallback(SpuCollisionPairInput* wuInput, CollisionTask_LocalStoreMemory* lsMemPtr,SpuContactResult& spuContacts)
                :       m_wuInput(wuInput),
                m_spuContacts(spuContacts),
                m_lsMemPtr(lsMemPtr)
        {
        }

        virtual void processNode(int subPart, int triangleIndex)
        {

                //              spu_printf("processNode with triangleIndex %d\n",triangleIndex);

                if (m_lsMemPtr->bvhShapeData.gIndexMesh.m_indexType == PHY_SHORT)
                {
                        unsigned short int* indexBasePtr = (unsigned short int*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexBase+triangleIndex*m_lsMemPtr->bvhShapeData.gIndexMesh.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_lsMemPtr->spuIndices[0] = int(tmpIndices[0]);
                        m_lsMemPtr->spuIndices[1] = int(tmpIndices[1]);
                        m_lsMemPtr->spuIndices[2] = int(tmpIndices[2]);
                } else
                {
                        unsigned int* indexBasePtr = (unsigned int*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexBase+triangleIndex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexStride);

                        small_cache_read_triple(&m_lsMemPtr->spuIndices[0],(ppu_address_t)&indexBasePtr[0],
                                                                &m_lsMemPtr->spuIndices[1],(ppu_address_t)&indexBasePtr[1],
                                                                &m_lsMemPtr->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_lsMemPtr->bvhShapeData.gTriangleMeshInterfacePtr->getScaling();
                for (int j=2;btLikely( j>=0 );j--)
                {
                        int graphicsindex = m_lsMemPtr->spuIndices[j];

                        //                      spu_printf("SPU index=%d ,",graphicsindex);
                        btScalar* graphicsbasePtr = (btScalar*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_vertexBase+graphicsindex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_vertexStride);
                        //                      spu_printf("SPU graphicsbasePtr=%llx\n",graphicsbasePtr);



                        //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));
                        
                        m_tmpTriangleShape.getVertexPtr(j).setValue(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());
                }


                SpuCollisionPairInput triangleConcaveInput(*m_wuInput);
//              triangleConcaveInput.m_spuCollisionShapes[1] = &spuTriangleVertices[0];
                triangleConcaveInput.m_spuCollisionShapes[1] = &m_tmpTriangleShape;
                triangleConcaveInput.m_shapeType1 = TRIANGLE_SHAPE_PROXYTYPE;

                m_spuContacts.setShapeIdentifiersB(subPart,triangleIndex);

                //              m_spuContacts.flush();

                ProcessSpuConvexConvexCollision(&triangleConcaveInput, m_lsMemPtr,m_spuContacts);
                //      m_spuContacts.flush();
        }

};



void btConvexPlaneCollideSingleContact (SpuCollisionPairInput* wuInput,CollisionTask_LocalStoreMemory* lsMemPtr,SpuContactResult&  spuContacts)
{
        
        btConvexShape* convexShape = (btConvexShape*) wuInput->m_spuCollisionShapes[0];
        btStaticPlaneShape* planeShape = (btStaticPlaneShape*) wuInput->m_spuCollisionShapes[1];

    bool hasCollision = false;
        const btVector3& planeNormal = planeShape->getPlaneNormal();
        const btScalar& planeConstant = planeShape->getPlaneConstant();
        
        
        btTransform convexWorldTransform = wuInput->m_worldTransform0;
        btTransform convexInPlaneTrans;
        convexInPlaneTrans= wuInput->m_worldTransform1.inverse() * convexWorldTransform;
        btTransform planeInConvex;
        planeInConvex= convexWorldTransform.inverse() * wuInput->m_worldTransform1;
        
        //btVector3 vtx = convexShape->localGetSupportVertexWithoutMarginNonVirtual(planeInConvex.getBasis()*-planeNormal);
        btVector3 vtx = convexShape->localGetSupportVertexNonVirtual(planeInConvex.getBasis()*-planeNormal);

        btVector3 vtxInPlane = convexInPlaneTrans(vtx);
        btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant);

        btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal;
        btVector3 vtxInPlaneWorld = wuInput->m_worldTransform1 * vtxInPlaneProjected;

        hasCollision = distance < lsMemPtr->getContactManifoldPtr()->getContactBreakingThreshold();
        //resultOut->setPersistentManifold(m_manifoldPtr);
        if (hasCollision)
        {
                btVector3 normalOnSurfaceB =wuInput->m_worldTransform1.getBasis() * planeNormal;
                btVector3 pOnB = vtxInPlaneWorld;
                spuContacts.addContactPoint(normalOnSurfaceB,pOnB,distance);
        }
}

void    ProcessConvexPlaneSpuCollision(SpuCollisionPairInput* wuInput, CollisionTask_LocalStoreMemory* lsMemPtr, SpuContactResult& spuContacts)
{

                register        int dmaSize = 0;
                register ppu_address_t  dmaPpuAddress2;
                btPersistentManifold* manifold = (btPersistentManifold*)wuInput->m_persistentManifoldPtr;

                ATTRIBUTE_ALIGNED16(char convexHullShape0[sizeof(btConvexHullShape)]);
                ATTRIBUTE_ALIGNED16(char convexHullShape1[sizeof(btConvexHullShape)]);

                if ( btLikely( wuInput->m_shapeType0== CONVEX_HULL_SHAPE_PROXYTYPE ) )
                {
                        //      spu_printf("SPU: DMA btConvexHullShape\n");
                        
                        dmaSize = sizeof(btConvexHullShape);
                        dmaPpuAddress2 = wuInput->m_collisionShapes[0];

                        cellDmaGet(&convexHullShape0, dmaPpuAddress2  , dmaSize, DMA_TAG(1), 0, 0);
                        //cellDmaWaitTagStatusAll(DMA_MASK(1));
                }

                if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) )
                {
                        //      spu_printf("SPU: DMA btConvexHullShape\n");
                        dmaSize = sizeof(btConvexHullShape);
                        dmaPpuAddress2 = wuInput->m_collisionShapes[1];
                        cellDmaGet(&convexHullShape1, dmaPpuAddress2  , dmaSize, DMA_TAG(1), 0, 0);
                        //cellDmaWaitTagStatusAll(DMA_MASK(1));
                }
                
                if ( btLikely( wuInput->m_shapeType0 == CONVEX_HULL_SHAPE_PROXYTYPE ) )
                {               
                        cellDmaWaitTagStatusAll(DMA_MASK(1));
                        dmaConvexVertexData (&lsMemPtr->convexVertexData[0], (btConvexHullShape*)&convexHullShape0);
                        lsMemPtr->convexVertexData[0].gSpuConvexShapePtr = wuInput->m_spuCollisionShapes[0];
                }

                        
                if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) )
                {
                        cellDmaWaitTagStatusAll(DMA_MASK(1));
                        dmaConvexVertexData (&lsMemPtr->convexVertexData[1], (btConvexHullShape*)&convexHullShape1);
                        lsMemPtr->convexVertexData[1].gSpuConvexShapePtr = wuInput->m_spuCollisionShapes[1];
                }

                
                btConvexPointCloudShape cpc0,cpc1;

                if ( btLikely( wuInput->m_shapeType0 == CONVEX_HULL_SHAPE_PROXYTYPE ) )
                {
                        cellDmaWaitTagStatusAll(DMA_MASK(2));
                        lsMemPtr->convexVertexData[0].gConvexPoints = &lsMemPtr->convexVertexData[0].g_convexPointBuffer[0];
                        btConvexHullShape* ch = (btConvexHullShape*)wuInput->m_spuCollisionShapes[0];
                        const btVector3& localScaling = ch->getLocalScalingNV();
                        cpc0.setPoints(lsMemPtr->convexVertexData[0].gConvexPoints,lsMemPtr->convexVertexData[0].gNumConvexPoints,false,localScaling);
                        wuInput->m_spuCollisionShapes[0] = &cpc0;
                }

                if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) )
                {
                        cellDmaWaitTagStatusAll(DMA_MASK(2));           
                        lsMemPtr->convexVertexData[1].gConvexPoints = &lsMemPtr->convexVertexData[1].g_convexPointBuffer[0];
                        btConvexHullShape* ch = (btConvexHullShape*)wuInput->m_spuCollisionShapes[1];
                        const btVector3& localScaling = ch->getLocalScalingNV();
                        cpc1.setPoints(lsMemPtr->convexVertexData[1].gConvexPoints,lsMemPtr->convexVertexData[1].gNumConvexPoints,false,localScaling);
                        wuInput->m_spuCollisionShapes[1] = &cpc1;

                }


                const btConvexShape* shape0Ptr = (const btConvexShape*)wuInput->m_spuCollisionShapes[0];
                const btConvexShape* shape1Ptr = (const btConvexShape*)wuInput->m_spuCollisionShapes[1];
                int shapeType0 = wuInput->m_shapeType0;
                int shapeType1 = wuInput->m_shapeType1;
                float marginA = wuInput->m_collisionMargin0;
                float marginB = wuInput->m_collisionMargin1;

                SpuClosestPointInput    cpInput;
                cpInput.m_convexVertexData[0] = &lsMemPtr->convexVertexData[0];
                cpInput.m_convexVertexData[1] = &lsMemPtr->convexVertexData[1];
                cpInput.m_transformA = wuInput->m_worldTransform0;
                cpInput.m_transformB = wuInput->m_worldTransform1;
                float sumMargin = (marginA+marginB+lsMemPtr->getContactManifoldPtr()->getContactBreakingThreshold());
                cpInput.m_maximumDistanceSquared = sumMargin * sumMargin;

                ppu_address_t manifoldAddress = (ppu_address_t)manifold;

                btPersistentManifold* spuManifold=lsMemPtr->getContactManifoldPtr();
                //spuContacts.setContactInfo(spuManifold,manifoldAddress,wuInput->m_worldTransform0,wuInput->m_worldTransform1,wuInput->m_isSwapped);
                spuContacts.setContactInfo(spuManifold,manifoldAddress,lsMemPtr->getColObj0()->getWorldTransform(),
                        lsMemPtr->getColObj1()->getWorldTransform(),
                        lsMemPtr->getColObj0()->getRestitution(),lsMemPtr->getColObj1()->getRestitution(),
                        lsMemPtr->getColObj0()->getFriction(),lsMemPtr->getColObj1()->getFriction(),
                        wuInput->m_isSwapped);


                btConvexPlaneCollideSingleContact(wuInput,lsMemPtr,spuContacts);


                
        
}




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
        
        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;

        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)
        {
                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;)
                {
                        //@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
        }

}


int stats[11]={0,0,0,0,0,0,0,0,0,0,0};
int degenerateStats[11]={0,0,0,0,0,0,0,0,0,0,0};


void    ProcessSpuConvexConvexCollision(SpuCollisionPairInput* wuInput, CollisionTask_LocalStoreMemory* lsMemPtr, SpuContactResult& spuContacts)
{
        register int dmaSize;
        register ppu_address_t  dmaPpuAddress2;
        
#ifdef DEBUG_SPU_COLLISION_DETECTION
        //spu_printf("SPU: ProcessSpuConvexConvexCollision\n");
#endif //DEBUG_SPU_COLLISION_DETECTION
        //CollisionShape* shape0 = (CollisionShape*)wuInput->m_collisionShapes[0];
        //CollisionShape* shape1 = (CollisionShape*)wuInput->m_collisionShapes[1];
        btPersistentManifold* manifold = (btPersistentManifold*)wuInput->m_persistentManifoldPtr;

        bool genericGjk = true;

        if (genericGjk)
        {
                //try generic GJK

                
                
                //SpuConvexPenetrationDepthSolver* penetrationSolver=0;
                btVoronoiSimplexSolver simplexSolver;
                btGjkEpaPenetrationDepthSolver  epaPenetrationSolver2;
                
                btConvexPenetrationDepthSolver* penetrationSolver = &epaPenetrationSolver2;
                
                //SpuMinkowskiPenetrationDepthSolver    minkowskiPenetrationSolver;
#ifdef ENABLE_EPA
                if (gUseEpa)
                {
                        penetrationSolver = &epaPenetrationSolver2;
                } else
#endif
                {
                        //penetrationSolver = &minkowskiPenetrationSolver;
                }


                ATTRIBUTE_ALIGNED16(char convexHullShape0[sizeof(btConvexHullShape)]);
                ATTRIBUTE_ALIGNED16(char convexHullShape1[sizeof(btConvexHullShape)]);

                if ( btLikely( wuInput->m_shapeType0== CONVEX_HULL_SHAPE_PROXYTYPE ) )
                {
                        //      spu_printf("SPU: DMA btConvexHullShape\n");
                        
                        dmaSize = sizeof(btConvexHullShape);
                        dmaPpuAddress2 = wuInput->m_collisionShapes[0];

                        cellDmaGet(&convexHullShape0, dmaPpuAddress2  , dmaSize, DMA_TAG(1), 0, 0);
                        //cellDmaWaitTagStatusAll(DMA_MASK(1));
                }

                if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) )
                {
                        //      spu_printf("SPU: DMA btConvexHullShape\n");
                        dmaSize = sizeof(btConvexHullShape);
                        dmaPpuAddress2 = wuInput->m_collisionShapes[1];
                        cellDmaGet(&convexHullShape1, dmaPpuAddress2  , dmaSize, DMA_TAG(1), 0, 0);
                        //cellDmaWaitTagStatusAll(DMA_MASK(1));
                }
                
                if ( btLikely( wuInput->m_shapeType0 == CONVEX_HULL_SHAPE_PROXYTYPE ) )
                {               
                        cellDmaWaitTagStatusAll(DMA_MASK(1));
                        dmaConvexVertexData (&lsMemPtr->convexVertexData[0], (btConvexHullShape*)&convexHullShape0);
                        lsMemPtr->convexVertexData[0].gSpuConvexShapePtr = wuInput->m_spuCollisionShapes[0];
                }

                        
                if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) )
                {
                        cellDmaWaitTagStatusAll(DMA_MASK(1));
                        dmaConvexVertexData (&lsMemPtr->convexVertexData[1], (btConvexHullShape*)&convexHullShape1);
                        lsMemPtr->convexVertexData[1].gSpuConvexShapePtr = wuInput->m_spuCollisionShapes[1];
                }

                
                btConvexPointCloudShape cpc0,cpc1;

                if ( btLikely( wuInput->m_shapeType0 == CONVEX_HULL_SHAPE_PROXYTYPE ) )
                {
                        cellDmaWaitTagStatusAll(DMA_MASK(2));
                        lsMemPtr->convexVertexData[0].gConvexPoints = &lsMemPtr->convexVertexData[0].g_convexPointBuffer[0];
                        btConvexHullShape* ch = (btConvexHullShape*)wuInput->m_spuCollisionShapes[0];
                        const btVector3& localScaling = ch->getLocalScalingNV();
                        cpc0.setPoints(lsMemPtr->convexVertexData[0].gConvexPoints,lsMemPtr->convexVertexData[0].gNumConvexPoints,false,localScaling);
                        wuInput->m_spuCollisionShapes[0] = &cpc0;
                }

                if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) )
                {
                        cellDmaWaitTagStatusAll(DMA_MASK(2));           
                        lsMemPtr->convexVertexData[1].gConvexPoints = &lsMemPtr->convexVertexData[1].g_convexPointBuffer[0];
                        btConvexHullShape* ch = (btConvexHullShape*)wuInput->m_spuCollisionShapes[1];
                        const btVector3& localScaling = ch->getLocalScalingNV();
                        cpc1.setPoints(lsMemPtr->convexVertexData[1].gConvexPoints,lsMemPtr->convexVertexData[1].gNumConvexPoints,false,localScaling);
                        wuInput->m_spuCollisionShapes[1] = &cpc1;

                }


                const btConvexShape* shape0Ptr = (const btConvexShape*)wuInput->m_spuCollisionShapes[0];
                const btConvexShape* shape1Ptr = (const btConvexShape*)wuInput->m_spuCollisionShapes[1];
                int shapeType0 = wuInput->m_shapeType0;
                int shapeType1 = wuInput->m_shapeType1;
                float marginA = wuInput->m_collisionMargin0;
                float marginB = wuInput->m_collisionMargin1;

                SpuClosestPointInput    cpInput;
                cpInput.m_convexVertexData[0] = &lsMemPtr->convexVertexData[0];
                cpInput.m_convexVertexData[1] = &lsMemPtr->convexVertexData[1];
                cpInput.m_transformA = wuInput->m_worldTransform0;
                cpInput.m_transformB = wuInput->m_worldTransform1;
                float sumMargin = (marginA+marginB+lsMemPtr->getContactManifoldPtr()->getContactBreakingThreshold());
                cpInput.m_maximumDistanceSquared = sumMargin * sumMargin;

                ppu_address_t manifoldAddress = (ppu_address_t)manifold;

                btPersistentManifold* spuManifold=lsMemPtr->getContactManifoldPtr();
                //spuContacts.setContactInfo(spuManifold,manifoldAddress,wuInput->m_worldTransform0,wuInput->m_worldTransform1,wuInput->m_isSwapped);
                spuContacts.setContactInfo(spuManifold,manifoldAddress,lsMemPtr->getColObj0()->getWorldTransform(),
                        lsMemPtr->getColObj1()->getWorldTransform(),
                        lsMemPtr->getColObj0()->getRestitution(),lsMemPtr->getColObj1()->getRestitution(),
                        lsMemPtr->getColObj0()->getFriction(),lsMemPtr->getColObj1()->getFriction(),
                        wuInput->m_isSwapped);

                {
                        btGjkPairDetector gjk(shape0Ptr,shape1Ptr,shapeType0,shapeType1,marginA,marginB,&simplexSolver,penetrationSolver);//&vsSolver,penetrationSolver);
                        gjk.getClosestPoints(cpInput,spuContacts,0);//,debugDraw);
                        
                        stats[gjk.m_lastUsedMethod]++;
                        degenerateStats[gjk.m_degenerateSimplex]++;

#ifdef USE_SEPDISTANCE_UTIL                     
                        btScalar sepDist = gjk.getCachedSeparatingDistance()+spuManifold->getContactBreakingThreshold();
                        lsMemPtr->getlocalCollisionAlgorithm()->m_sepDistance.initSeparatingDistance(gjk.getCachedSeparatingAxis(),sepDist,wuInput->m_worldTransform0,wuInput->m_worldTransform1);
                        lsMemPtr->needsDmaPutContactManifoldAlgo = true;
#endif //USE_SEPDISTANCE_UTIL

                }

        }


}


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);//btTransform);
        dmaPpuAddress2 = /*collisionPairInput.m_isSwapped ? (ppu_address_t)lsMem.gProxyPtr1->m_clientObject :*/ (ppu_address_t)lsMem.getlocalCollisionAlgorithm()->getCollisionObject0();
        lsMem.m_lsColObj0Ptr = (btCollisionObject*)cellDmaGetReadOnly(&lsMem.gColObj0Buffer, dmaPpuAddress2  , dmaSize, DMA_TAG(1), 0, 0);              

        dmaSize = sizeof(btCollisionObject);//btTransform);
        dmaPpuAddress2 = /*collisionPairInput.m_isSwapped ? (ppu_address_t)lsMem.gProxyPtr0->m_clientObject :*/ (ppu_address_t)lsMem.getlocalCollisionAlgorithm()->getCollisionObject1();
        lsMem.m_lsColObj1Ptr = (btCollisionObject*)cellDmaGetReadOnly(&lsMem.gColObj1Buffer, dmaPpuAddress2  , dmaSize, DMA_TAG(2), 0, 0);              
        
        cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));

        btCollisionObject* ob0 = lsMem.getColObj0();
        btCollisionObject* ob1 = lsMem.getColObj1();

        collisionPairInput.m_worldTransform0 = ob0->getWorldTransform();
        collisionPairInput.m_worldTransform1 = ob1->getWorldTransform();
}



void    handleCollisionPair(SpuCollisionPairInput& collisionPairInput, CollisionTask_LocalStoreMemory& lsMem,
                                                        SpuContactResult &spuContacts,
                                                        ppu_address_t collisionShape0Ptr, void* collisionShape0Loc,
                                                        ppu_address_t collisionShape1Ptr, void* collisionShape1Loc, bool dmaShapes = true)
{
        
        if (btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType0) 
                && btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType1))
        {
                if (dmaShapes)
                {
                        dmaCollisionShape (collisionShape0Loc, collisionShape0Ptr, 1, collisionPairInput.m_shapeType0);
                        dmaCollisionShape (collisionShape1Loc, collisionShape1Ptr, 2, collisionPairInput.m_shapeType1);
                        cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));
                }

                btConvexInternalShape* spuConvexShape0 = (btConvexInternalShape*)collisionShape0Loc;
                btConvexInternalShape* spuConvexShape1 = (btConvexInternalShape*)collisionShape1Loc;

                btVector3 dim0 = spuConvexShape0->getImplicitShapeDimensions();
                btVector3 dim1 = spuConvexShape1->getImplicitShapeDimensions();

                collisionPairInput.m_primitiveDimensions0 = dim0;
                collisionPairInput.m_primitiveDimensions1 = dim1;
                collisionPairInput.m_collisionShapes[0] = collisionShape0Ptr;
                collisionPairInput.m_collisionShapes[1] = collisionShape1Ptr;
                collisionPairInput.m_spuCollisionShapes[0] = spuConvexShape0;
                collisionPairInput.m_spuCollisionShapes[1] = spuConvexShape1;
                ProcessSpuConvexConvexCollision(&collisionPairInput,&lsMem,spuContacts);
        } 
        else if (btBroadphaseProxy::isCompound(collisionPairInput.m_shapeType0) && 
                        btBroadphaseProxy::isCompound(collisionPairInput.m_shapeType1))
        {
                //snPause();

                dmaCollisionShape (collisionShape0Loc, collisionShape0Ptr, 1, collisionPairInput.m_shapeType0);
                dmaCollisionShape (collisionShape1Loc, collisionShape1Ptr, 2, collisionPairInput.m_shapeType1);
                cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));

                // Both are compounds, do N^2 CD for now
        
                btCompoundShape* spuCompoundShape0 = (btCompoundShape*)collisionShape0Loc;
                btCompoundShape* spuCompoundShape1 = (btCompoundShape*)collisionShape1Loc;

                dmaCompoundShapeInfo (&lsMem.compoundShapeData[0], spuCompoundShape0, 1);
                dmaCompoundShapeInfo (&lsMem.compoundShapeData[1], spuCompoundShape1, 2);
                cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));
                

                dmaCompoundSubShapes (&lsMem.compoundShapeData[0], spuCompoundShape0, 1);
                cellDmaWaitTagStatusAll(DMA_MASK(1));
                dmaCompoundSubShapes (&lsMem.compoundShapeData[1], spuCompoundShape1, 1);
                cellDmaWaitTagStatusAll(DMA_MASK(1));

                int childShapeCount0 = spuCompoundShape0->getNumChildShapes();
                int childShapeCount1 = spuCompoundShape1->getNumChildShapes();

                // Start the N^2
                for (int i = 0; i < childShapeCount0; ++i)
                {
                        btCompoundShapeChild& childShape0 = lsMem.compoundShapeData[0].gSubshapes[i];

                        for (int j = 0; j < childShapeCount1; ++j)
                        {
                                btCompoundShapeChild& childShape1 = lsMem.compoundShapeData[1].gSubshapes[j];

                                /* 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;
                                /* Recursively call handleCollisionPair () with new collision pair input */
                                handleCollisionPair(cinput, lsMem, spuContacts,                 
                                        (ppu_address_t)childShape0.m_childShape, lsMem.compoundShapeData[0].gSubshapeShape[i], 
                                        (ppu_address_t)childShape1.m_childShape, lsMem.compoundShapeData[1].gSubshapeShape[j], false); // bug fix: changed index to j.
                        }
                }
        }
        else if (btBroadphaseProxy::isCompound(collisionPairInput.m_shapeType0) )
        {
                //snPause();
                
                dmaCollisionShape (collisionShape0Loc, collisionShape0Ptr, 1, collisionPairInput.m_shapeType0);
                dmaCollisionShape (collisionShape1Loc, collisionShape1Ptr, 2, collisionPairInput.m_shapeType1);
                cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));

                // object 0 compound, object 1 non-compound
                btCompoundShape* spuCompoundShape = (btCompoundShape*)collisionShape0Loc;
                dmaCompoundShapeInfo (&lsMem.compoundShapeData[0], spuCompoundShape, 1);
                cellDmaWaitTagStatusAll(DMA_MASK(1));

                int childShapeCount = spuCompoundShape->getNumChildShapes();

                for (int i = 0; i < childShapeCount; ++i)
                {
                        btCompoundShapeChild& childShape = lsMem.compoundShapeData[0].gSubshapes[i];

                        // Dma the child shape
                        dmaCollisionShape (&lsMem.compoundShapeData[0].gSubshapeShape[i], (ppu_address_t)childShape.m_childShape, 1, childShape.m_childShapeType);
                        cellDmaWaitTagStatusAll(DMA_MASK(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;

                        handleCollisionPair(cinput, lsMem, spuContacts,                 
                                (ppu_address_t)childShape.m_childShape, lsMem.compoundShapeData[0].gSubshapeShape[i], 
                                collisionShape1Ptr, collisionShape1Loc, false);
                }
        }
        else if (btBroadphaseProxy::isCompound(collisionPairInput.m_shapeType1) )
        {
                //snPause();
                
                dmaCollisionShape (collisionShape0Loc, collisionShape0Ptr, 1, collisionPairInput.m_shapeType0);
                dmaCollisionShape (collisionShape1Loc, collisionShape1Ptr, 2, collisionPairInput.m_shapeType1);
                cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));
                // object 0 non-compound, object 1 compound
                btCompoundShape* spuCompoundShape = (btCompoundShape*)collisionShape1Loc;
                dmaCompoundShapeInfo (&lsMem.compoundShapeData[0], spuCompoundShape, 1);
                cellDmaWaitTagStatusAll(DMA_MASK(1));
                
                int childShapeCount = spuCompoundShape->getNumChildShapes();

                for (int i = 0; i < childShapeCount; ++i)
                {
                        btCompoundShapeChild& childShape = lsMem.compoundShapeData[0].gSubshapes[i];
                        // Dma the child shape
                        dmaCollisionShape (&lsMem.compoundShapeData[0].gSubshapeShape[i], (ppu_address_t)childShape.m_childShape, 1, childShape.m_childShapeType);
                        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, collisionShape0Loc, 
                                (ppu_address_t)childShape.m_childShape, lsMem.compoundShapeData[0].gSubshapeShape[i], false);
                }
                
        }
        else
        {
                //a non-convex shape is involved                                                                        
                bool handleConvexConcave = false;

                //snPause();

                if (btBroadphaseProxy::isConcave(collisionPairInput.m_shapeType0) &&
                        btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType1))
                {
                        // Swap stuff
                        DoSwap(collisionShape0Ptr, collisionShape1Ptr);
                        DoSwap(collisionShape0Loc, collisionShape1Loc);
                        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)
                        {
                                dmaCollisionShape (collisionShape0Loc, collisionShape0Ptr, 1, collisionPairInput.m_shapeType0);
                                dmaCollisionShape (collisionShape1Loc, collisionShape1Ptr, 2, collisionPairInput.m_shapeType1);
                                cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));
                        }
                        
                        if (collisionPairInput.m_shapeType1 == STATIC_PLANE_PROXYTYPE)
                        {
                                btConvexInternalShape* spuConvexShape0 = (btConvexInternalShape*)collisionShape0Loc;
                                btStaticPlaneShape* planeShape= (btStaticPlaneShape*)collisionShape1Loc;

                                btVector3 dim0 = spuConvexShape0->getImplicitShapeDimensions();
                                collisionPairInput.m_primitiveDimensions0 = dim0;
                                collisionPairInput.m_collisionShapes[0] = collisionShape0Ptr;
                                collisionPairInput.m_collisionShapes[1] = collisionShape1Ptr;
                                collisionPairInput.m_spuCollisionShapes[0] = spuConvexShape0;
                                collisionPairInput.m_spuCollisionShapes[1] = planeShape;

                                ProcessConvexPlaneSpuCollision(&collisionPairInput,&lsMem,spuContacts);
                        } else
                        {
                                btConvexInternalShape* spuConvexShape0 = (btConvexInternalShape*)collisionShape0Loc;
                                btBvhTriangleMeshShape* trimeshShape = (btBvhTriangleMeshShape*)collisionShape1Loc;

                                btVector3 dim0 = spuConvexShape0->getImplicitShapeDimensions();
                                collisionPairInput.m_primitiveDimensions0 = dim0;
                                collisionPairInput.m_collisionShapes[0] = collisionShape0Ptr;
                                collisionPairInput.m_collisionShapes[1] = collisionShape1Ptr;
                                collisionPairInput.m_spuCollisionShapes[0] = spuConvexShape0;
                                collisionPairInput.m_spuCollisionShapes[1] = trimeshShape;

                                ProcessConvexConcaveSpuCollision(&collisionPairInput,&lsMem,spuContacts);
                        }
                }

        }
        
        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.m_inPairPtr;
        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 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 ) )
                        {
                                        dmaSize = numPairs*sizeof(btBroadphasePair);
                                        dmaPpuAddress = wuInputs[j].m_pairArrayPtr+wuInputs[j].m_startIndex * sizeof(btBroadphasePair);
                                        lsMem.m_pairsPointer = (btBroadphasePair*)cellDmaGetReadOnly(&lsMem.gBroadphasePairsBuffer, dmaPpuAddress  , dmaSize, DMA_TAG(1), 0, 0);
                                        cellDmaWaitTagStatusAll(DMA_MASK(1));
                                

                                for (p=0;p<numPairs;p++)
                                {

                                        //for each broadphase pair, do something

                                        btBroadphasePair& pair = lsMem.getBroadphasePairPtr()[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

                                        if (pair.m_internalTmpValue == 2 && pair.m_algorithm && pair.m_pProxy0 && pair.m_pProxy1)
                                        {
                                                dmaSize = sizeof(SpuContactManifoldCollisionAlgorithm);
                                                dmaPpuAddress2 = (ppu_address_t)pair.m_algorithm;
                                                lsMem.m_lsCollisionAlgorithmPtr = (SpuContactManifoldCollisionAlgorithm*)cellDmaGetReadOnly(&lsMem.gSpuContactManifoldAlgoBuffer, dmaPpuAddress2  , dmaSize, DMA_TAG(1), 0, 0);

                                                cellDmaWaitTagStatusAll(DMA_MASK(1));

                                                lsMem.needsDmaPutContactManifoldAlgo = false;

                                                collisionPairInput.m_persistentManifoldPtr = (ppu_address_t) lsMem.getlocalCollisionAlgorithm()->getContactManifoldPtr();
                                                collisionPairInput.m_isSwapped = false;

                                                if (1)
                                                {



#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

                                                        
                                                        dmaSize = sizeof(btPersistentManifold);

                                                        dmaPpuAddress2 = collisionPairInput.m_persistentManifoldPtr;
                                                        lsMem.m_lsManifoldPtr = (btPersistentManifold*)cellDmaGetReadOnly(&lsMem.gPersistentManifoldBuffer, dmaPpuAddress2  , dmaSize, DMA_TAG(1), 0, 0);

                                                        collisionPairInput.m_shapeType0 = lsMem.getlocalCollisionAlgorithm()->getShapeType0();
                                                        collisionPairInput.m_shapeType1 = lsMem.getlocalCollisionAlgorithm()->getShapeType1();
                                                        collisionPairInput.m_collisionMargin0 = lsMem.getlocalCollisionAlgorithm()->getCollisionMargin0();
                                                        collisionPairInput.m_collisionMargin1 = lsMem.getlocalCollisionAlgorithm()->getCollisionMargin1();
                                                        
                                                        
                                                        
                                                        //??cellDmaWaitTagStatusAll(DMA_MASK(1));
                                                        

                                                        if (1)
                                                        {
                                                                //snPause();

                                                                // Get the collision objects
                                                                dmaAndSetupCollisionObjects(collisionPairInput, lsMem);

                                                                if (lsMem.getColObj0()->isActive() || lsMem.getColObj1()->isActive())
                                                                {

                                                                        lsMem.needsDmaPutContactManifoldAlgo = true;
#ifdef USE_SEPDISTANCE_UTIL
                                                                        lsMem.getlocalCollisionAlgorithm()->m_sepDistance.updateSeparatingDistance(collisionPairInput.m_worldTransform0,collisionPairInput.m_worldTransform1);
#endif //USE_SEPDISTANCE_UTIL
                                                        
#define USE_DEDICATED_BOX_BOX 1
#ifdef USE_DEDICATED_BOX_BOX
                                                                        bool boxbox = ((lsMem.getlocalCollisionAlgorithm()->getShapeType0()==BOX_SHAPE_PROXYTYPE)&&
                                                                                (lsMem.getlocalCollisionAlgorithm()->getShapeType1()==BOX_SHAPE_PROXYTYPE));
                                                                        if (boxbox)
                                                                        {
                                                                                //spu_printf("boxbox dist = %f\n",distance);
                                                                                btPersistentManifold* spuManifold=lsMem.getContactManifoldPtr();
                                                                                btPersistentManifold* manifold = (btPersistentManifold*)collisionPairInput.m_persistentManifoldPtr;
                                                                                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);

                                                
                                                                        //float distance=0.f;
                                                                        btVector3 normalInB;


                                                                        if (
#ifdef USE_SEPDISTANCE_UTIL
                                                                                lsMem.getlocalCollisionAlgorithm()->m_sepDistance.getConservativeSeparatingDistance()<=0.f
#else
                                                                                1
#endif                                                                                  
                                                                                )
                                                                                {
//#define USE_PE_BOX_BOX 1
#ifdef USE_PE_BOX_BOX
                                                                                        {

                                                                                                //getCollisionMargin0
                                                                                                btScalar margin0 = lsMem.getlocalCollisionAlgorithm()->getCollisionMargin0();
                                                                                                btScalar margin1 = lsMem.getlocalCollisionAlgorithm()->getCollisionMargin1();
                                                                                                btVector3 shapeDim0 = lsMem.getlocalCollisionAlgorithm()->getShapeDimensions0()+btVector3(margin0,margin0,margin0);
                                                                                                btVector3 shapeDim1 = lsMem.getlocalCollisionAlgorithm()->getShapeDimensions1()+btVector3(margin1,margin1,margin1);

                                                                                                Box boxA(shapeDim0.getX(),shapeDim0.getY(),shapeDim0.getZ());
                                                                                                Vector3 vmPos0 = getVmVector3(collisionPairInput.m_worldTransform0.getOrigin());
                                                                                                Vector3 vmPos1 = getVmVector3(collisionPairInput.m_worldTransform1.getOrigin());
                                                                                                Matrix3 vmMatrix0 = getVmMatrix3(collisionPairInput.m_worldTransform0.getBasis());
                                                                                                Matrix3 vmMatrix1 = getVmMatrix3(collisionPairInput.m_worldTransform1.getBasis());

                                                                                                Transform3 transformA(vmMatrix0,vmPos0);
                                                                                                Box boxB(shapeDim1.getX(),shapeDim1.getY(),shapeDim1.getZ());
                                                                                                Transform3 transformB(vmMatrix1,vmPos1);
                                                                                                BoxPoint resultClosestBoxPointA;
                                                                                                BoxPoint resultClosestBoxPointB;
                                                                                                Vector3 resultNormal;
#ifdef USE_SEPDISTANCE_UTIL
                                                                                                float distanceThreshold = FLT_MAX
#else
                                                                                                float distanceThreshold = 0.f;
#endif


                                                                                                distance = boxBoxDistance(resultNormal,resultClosestBoxPointA,resultClosestBoxPointB,  boxA, transformA, boxB,transformB,distanceThreshold);
                                                                                                
                                                                                                normalInB = -getBtVector3(resultNormal);

                                                                                                if(distance < spuManifold->getContactBreakingThreshold())
                                                                                                {
                                                                                                        btVector3 pointOnB = collisionPairInput.m_worldTransform1(getBtVector3(resultClosestBoxPointB.localPoint));

                                                                                                        spuContacts.addContactPoint(
                                                                                                                normalInB,
                                                                                                                pointOnB,
                                                                                                                distance);
                                                                                                }
                                                                                        } 
#else                                                                   
                                                                                        {

                                                                                                btScalar margin0 = lsMem.getlocalCollisionAlgorithm()->getCollisionMargin0();
                                                                                                btScalar margin1 = lsMem.getlocalCollisionAlgorithm()->getCollisionMargin1();
                                                                                                btVector3 shapeDim0 = lsMem.getlocalCollisionAlgorithm()->getShapeDimensions0()+btVector3(margin0,margin0,margin0);
                                                                                                btVector3 shapeDim1 = lsMem.getlocalCollisionAlgorithm()->getShapeDimensions1()+btVector3(margin1,margin1,margin1);


                                                                                                btBoxShape box0(shapeDim0);
                                                                                                btBoxShape box1(shapeDim1);

                                                                                                struct SpuBridgeContactCollector : public btDiscreteCollisionDetectorInterface::Result
                                                                                                {
                                                                                                        SpuContactResult&       m_spuContacts;

                                                                                                        virtual void setShapeIdentifiersA(int partId0,int index0)
                                                                                                        {
                                                                                                                m_spuContacts.setShapeIdentifiersA(partId0,index0);
                                                                                                        }
                                                                                                        virtual void setShapeIdentifiersB(int partId1,int index1)
                                                                                                        {
                                                                                                                m_spuContacts.setShapeIdentifiersB(partId1,index1);
                                                                                                        }
                                                                                                        virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
                                                                                                        {
                                                                                                                m_spuContacts.addContactPoint(normalOnBInWorld,pointInWorld,depth);
                                                                                                        }

                                                                                                        SpuBridgeContactCollector(SpuContactResult& spuContacts)
                                                                                                                :m_spuContacts(spuContacts)
                                                                                                        {

                                                                                                        }
                                                                                                };
                                                                                                
                                                                                                SpuBridgeContactCollector  bridgeOutput(spuContacts);

                                                                                                btDiscreteCollisionDetectorInterface::ClosestPointInput input;
                                                                                                input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
                                                                                                input.m_transformA = collisionPairInput.m_worldTransform0;
                                                                                                input.m_transformB = collisionPairInput.m_worldTransform1;

                                                                                                btBoxBoxDetector detector(&box0,&box1);
                                                                                                
                                                                                                detector.getClosestPoints(input,bridgeOutput,0);

                                                                                        }
#endif //USE_PE_BOX_BOX
                                                                                        
                                                                                        lsMem.needsDmaPutContactManifoldAlgo = true;
#ifdef USE_SEPDISTANCE_UTIL
                                                                                        btScalar sepDist2 = distance+spuManifold->getContactBreakingThreshold();
                                                                                        lsMem.getlocalCollisionAlgorithm()->m_sepDistance.initSeparatingDistance(normalInB,sepDist2,collisionPairInput.m_worldTransform0,collisionPairInput.m_worldTransform1);
#endif //USE_SEPDISTANCE_UTIL
                                                                                        gProcessedCol++;
                                                                                } else
                                                                                {
                                                                                        gSkippedCol++;
                                                                                }

                                                                                spuContacts.flush();
                                                                                        

                                                                        } else
#endif //USE_DEDICATED_BOX_BOX
                                                                        {
                                                                                if (
#ifdef USE_SEPDISTANCE_UTIL
                                                                                        lsMem.getlocalCollisionAlgorithm()->m_sepDistance.getConservativeSeparatingDistance()<=0.f
#else
                                                                                        1
#endif //USE_SEPDISTANCE_UTIL
                                                                                        )
                                                                                {
                                                                                        handleCollisionPair(collisionPairInput, lsMem, spuContacts,
                                                                                                (ppu_address_t)lsMem.getColObj0()->getRootCollisionShape(), &lsMem.gCollisionShapes[0].collisionShape,
                                                                                                (ppu_address_t)lsMem.getColObj1()->getRootCollisionShape(), &lsMem.gCollisionShapes[1].collisionShape);
                                                                                } else
                                                                                {
                                                                                                //spu_printf("boxbox dist = %f\n",distance);
                                                                                        btPersistentManifold* spuManifold=lsMem.getContactManifoldPtr();
                                                                                        btPersistentManifold* manifold = (btPersistentManifold*)collisionPairInput.m_persistentManifoldPtr;
                                                                                        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);

                                                                                        spuContacts.flush();
                                                                                }
                                                                        }
                                                                
                                                                }

                                                        }
                                                }

#ifdef USE_SEPDISTANCE_UTIL
#if defined (__SPU__) || defined (USE_LIBSPE2)
                                                if (lsMem.needsDmaPutContactManifoldAlgo)
                                                {
                                                        dmaSize = sizeof(SpuContactManifoldCollisionAlgorithm);
                                                        dmaPpuAddress2 = (ppu_address_t)pair.m_algorithm;
                                                        cellDmaLargePut(&lsMem.gSpuContactManifoldAlgoBuffer, dmaPpuAddress2  , dmaSize, DMA_TAG(1), 0, 0);
                                                        cellDmaWaitTagStatusAll(DMA_MASK(1));
                                                }
#endif
#endif //#ifdef USE_SEPDISTANCE_UTIL

                                        }
                                }
                        }
                } //end for (j = 0; j < numOnPage; j++)

        }//     for 



        return;
}