src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuCollisionShapes.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 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.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15
  16
  17 #include "SpuCollisionShapes.h"
  18
  19 ///not supported on IBM SDK, until we fix the alignment of btVector3
  20 #if defined (__CELLOS_LV2__) && defined (__SPU__)
  21 #include <spu_intrinsics.h>
  22 static inline vec_float4 vec_dot3( vec_float4 vec0, vec_float4 vec1 )
  23 {
  24     vec_float4 result;
  25     result = spu_mul( vec0, vec1 );
  26     result = spu_madd( spu_rlqwbyte( vec0, 4 ), spu_rlqwbyte( vec1, 4 ), result );
  27     return spu_madd( spu_rlqwbyte( vec0, 8 ), spu_rlqwbyte( vec1, 8 ), result );
  28 }
  29 #endif //__SPU__
  30
  31
  32 void computeAabb (btVector3& aabbMin, btVector3& aabbMax, btConvexInternalShape* convexShape, ppu_address_t convexShapePtr, int shapeType, const btTransform& xform)
  33 {
  34         //calculate the aabb, given the types...
  35         switch (shapeType)
  36         {
  37         case CYLINDER_SHAPE_PROXYTYPE:
  38                 /* fall through */
  39         case BOX_SHAPE_PROXYTYPE:
  40         {
  41                 btScalar margin=convexShape->getMarginNV();
  42                 btVector3 halfExtents = convexShape->getImplicitShapeDimensions();
  43                 halfExtents += btVector3(margin,margin,margin);
  44                 const btTransform& t = xform;
  45                 btMatrix3x3 abs_b = t.getBasis().absolute();
  46                 btVector3 center = t.getOrigin();
  47         btVector3 extent = halfExtents.dot3( abs_b[0], abs_b[1], abs_b[2] );
  48
  49                 aabbMin = center - extent;
  50                 aabbMax = center + extent;
  51                 break;
  52         }
  53         case CAPSULE_SHAPE_PROXYTYPE:
  54         {
  55                 btScalar margin=convexShape->getMarginNV();
  56                 btVector3 halfExtents = convexShape->getImplicitShapeDimensions();
  57                 //add the radius to y-axis to get full height
  58                 btScalar radius = halfExtents[0];
  59                 halfExtents[1] += radius;
  60                 halfExtents += btVector3(margin,margin,margin);
  61 #if 0
  62                 int capsuleUpAxis = convexShape->getUpAxis();
  63                 btScalar halfHeight = convexShape->getHalfHeight();
  64                 btScalar radius = convexShape->getRadius();
  65                 halfExtents[capsuleUpAxis] = radius + halfHeight;
  66 #endif
  67                 const btTransform& t = xform;
  68                 btMatrix3x3 abs_b = t.getBasis().absolute();
  69                 btVector3 center = t.getOrigin();
  70         btVector3 extent = halfExtents.dot3( abs_b[0], abs_b[1], abs_b[2] );
  71
  72                 aabbMin = center - extent;
  73                 aabbMax = center + extent;
  74                 break;
  75         }
  76         case SPHERE_SHAPE_PROXYTYPE:
  77         {
  78                 btScalar radius = convexShape->getImplicitShapeDimensions().getX();// * convexShape->getLocalScaling().getX();
  79                 btScalar margin = radius + convexShape->getMarginNV();
  80                 const btTransform& t = xform;
  81                 const btVector3& center = t.getOrigin();
  82                 btVector3 extent(margin,margin,margin);
  83                 aabbMin = center - extent;
  84                 aabbMax = center + extent;
  85                 break;
  86         }
  87         case CONVEX_HULL_SHAPE_PROXYTYPE:
  88         {
  89                 ATTRIBUTE_ALIGNED16(char convexHullShape0[sizeof(btConvexHullShape)]);
  90                 cellDmaGet(&convexHullShape0, convexShapePtr  , sizeof(btConvexHullShape), DMA_TAG(1), 0, 0);
  91                 cellDmaWaitTagStatusAll(DMA_MASK(1));
  92                 btConvexHullShape* localPtr = (btConvexHullShape*)&convexHullShape0;
  93                 const btTransform& t = xform;
  94                 btScalar margin = convexShape->getMarginNV();
  95                 localPtr->getNonvirtualAabb(t,aabbMin,aabbMax,margin);
  96                 //spu_printf("SPU convex aabbMin=%f,%f,%f=\n",aabbMin.getX(),aabbMin.getY(),aabbMin.getZ());
  97                 //spu_printf("SPU convex aabbMax=%f,%f,%f=\n",aabbMax.getX(),aabbMax.getY(),aabbMax.getZ());
  98                 break;
  99         }
 100         default:
 101                 {
 102         //      spu_printf("SPU: unsupported shapetype %d in AABB calculation\n");
 103                 }
 104         };
 105 }
 106
 107 void dmaBvhShapeData (bvhMeshShape_LocalStoreMemory* bvhMeshShape, btBvhTriangleMeshShape* triMeshShape)
 108 {
 109         register int dmaSize;
 110         register ppu_address_t  dmaPpuAddress2;
 111
 112         dmaSize = sizeof(btTriangleIndexVertexArray);
 113         dmaPpuAddress2 = reinterpret_cast<ppu_address_t>(triMeshShape->getMeshInterface());
 114         //      spu_printf("trimeshShape->getMeshInterface() == %llx\n",dmaPpuAddress2);
 115 #ifdef __SPU__
 116         cellDmaGet(&bvhMeshShape->gTriangleMeshInterfaceStorage, dmaPpuAddress2  , dmaSize, DMA_TAG(1), 0, 0);
 117         bvhMeshShape->gTriangleMeshInterfacePtr = &bvhMeshShape->gTriangleMeshInterfaceStorage;
 118 #else
 119         bvhMeshShape->gTriangleMeshInterfacePtr = (btTriangleIndexVertexArray*)cellDmaGetReadOnly(&bvhMeshShape->gTriangleMeshInterfaceStorage, dmaPpuAddress2  , dmaSize, DMA_TAG(1), 0, 0);
 120 #endif
 121
 122         //cellDmaWaitTagStatusAll(DMA_MASK(1));
 123
 124         ///now DMA over the BVH
 125
 126         dmaSize = sizeof(btOptimizedBvh);
 127         dmaPpuAddress2 = reinterpret_cast<ppu_address_t>(triMeshShape->getOptimizedBvh());
 128         //spu_printf("trimeshShape->getOptimizedBvh() == %llx\n",dmaPpuAddress2);
 129         cellDmaGet(&bvhMeshShape->gOptimizedBvh, dmaPpuAddress2  , dmaSize, DMA_TAG(2), 0, 0);
 130         //cellDmaWaitTagStatusAll(DMA_MASK(2));
 131         cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));
 132 }
 133
 134 void dmaBvhIndexedMesh (btIndexedMesh* IndexMesh, IndexedMeshArray& indexArray, int index, uint32_t dmaTag)
 135 {
 136         cellDmaGet(IndexMesh, (ppu_address_t)&indexArray[index]  , sizeof(btIndexedMesh), DMA_TAG(dmaTag), 0, 0);
 137
 138 }
 139
 140 void dmaBvhSubTreeHeaders (btBvhSubtreeInfo* subTreeHeaders, ppu_address_t subTreePtr, int batchSize, uint32_t dmaTag)
 141 {
 142         cellDmaGet(subTreeHeaders, subTreePtr, batchSize * sizeof(btBvhSubtreeInfo), DMA_TAG(dmaTag), 0, 0);
 143 }
 144
 145 void dmaBvhSubTreeNodes (btQuantizedBvhNode* nodes, const btBvhSubtreeInfo& subtree, QuantizedNodeArray&        nodeArray, int dmaTag)
 146 {
 147         cellDmaGet(nodes, reinterpret_cast<ppu_address_t>(&nodeArray[subtree.m_rootNodeIndex]) , subtree.m_subtreeSize* sizeof(btQuantizedBvhNode), DMA_TAG(2), 0, 0);
 148 }
 149
 150 ///getShapeTypeSize could easily be optimized, but it is not likely a bottleneck
 151 int             getShapeTypeSize(int shapeType)
 152 {
 153
 154
 155         switch (shapeType)
 156         {
 157         case CYLINDER_SHAPE_PROXYTYPE:
 158                 {
 159                         int shapeSize = sizeof(btCylinderShape);
 160                         btAssert(shapeSize < MAX_SHAPE_SIZE);
 161                         return shapeSize;
 162                 }
 163         case BOX_SHAPE_PROXYTYPE:
 164                 {
 165                         int shapeSize = sizeof(btBoxShape);
 166                         btAssert(shapeSize < MAX_SHAPE_SIZE);
 167                         return shapeSize;
 168                 }
 169         case SPHERE_SHAPE_PROXYTYPE:
 170                 {
 171                         int shapeSize = sizeof(btSphereShape);
 172                         btAssert(shapeSize < MAX_SHAPE_SIZE);
 173                         return shapeSize;
 174                 }
 175         case TRIANGLE_MESH_SHAPE_PROXYTYPE:
 176                 {
 177                         int shapeSize = sizeof(btBvhTriangleMeshShape);
 178                         btAssert(shapeSize < MAX_SHAPE_SIZE);
 179                         return shapeSize;
 180                 }
 181         case CAPSULE_SHAPE_PROXYTYPE:
 182                 {
 183                         int shapeSize = sizeof(btCapsuleShape);
 184                         btAssert(shapeSize < MAX_SHAPE_SIZE);
 185                         return shapeSize;
 186                 }
 187
 188         case CONVEX_HULL_SHAPE_PROXYTYPE:
 189                 {
 190                         int shapeSize = sizeof(btConvexHullShape);
 191                         btAssert(shapeSize < MAX_SHAPE_SIZE);
 192                         return shapeSize;
 193                 }
 194
 195         case COMPOUND_SHAPE_PROXYTYPE:
 196                 {
 197                         int shapeSize = sizeof(btCompoundShape);
 198                         btAssert(shapeSize < MAX_SHAPE_SIZE);
 199                         return shapeSize;
 200                 }
 201         case STATIC_PLANE_PROXYTYPE:
 202                 {
 203                         int shapeSize = sizeof(btStaticPlaneShape);
 204                         btAssert(shapeSize < MAX_SHAPE_SIZE);
 205                         return shapeSize;
 206                 }
 207
 208         default:
 209                 btAssert(0);
 210                 //unsupported shapetype, please add here
 211                 return 0;
 212         }
 213 }
 214
 215 void dmaConvexVertexData (SpuConvexPolyhedronVertexData* convexVertexData, btConvexHullShape* convexShapeSPU)
 216 {
 217         convexVertexData->gNumConvexPoints = convexShapeSPU->getNumPoints();
 218         if (convexVertexData->gNumConvexPoints>MAX_NUM_SPU_CONVEX_POINTS)
 219         {
 220                 btAssert(0);
 221         //      spu_printf("SPU: Error: MAX_NUM_SPU_CONVEX_POINTS(%d) exceeded: %d\n",MAX_NUM_SPU_CONVEX_POINTS,convexVertexData->gNumConvexPoints);
 222                 return;
 223         }
 224
 225         register int dmaSize = convexVertexData->gNumConvexPoints*sizeof(btVector3);
 226         ppu_address_t pointsPPU = (ppu_address_t) convexShapeSPU->getUnscaledPoints();
 227         cellDmaGet(&convexVertexData->g_convexPointBuffer[0], pointsPPU  , dmaSize, DMA_TAG(2), 0, 0);
 228 }
 229
 230 void dmaCollisionShape (void* collisionShapeLocation, ppu_address_t collisionShapePtr, uint32_t dmaTag, int shapeType)
 231 {
 232         register int dmaSize = getShapeTypeSize(shapeType);
 233         cellDmaGet(collisionShapeLocation, collisionShapePtr  , dmaSize, DMA_TAG(dmaTag), 0, 0);
 234         //cellDmaGetReadOnly(collisionShapeLocation, collisionShapePtr  , dmaSize, DMA_TAG(dmaTag), 0, 0);
 235         //cellDmaWaitTagStatusAll(DMA_MASK(dmaTag));
 236 }
 237
 238 void dmaCompoundShapeInfo (CompoundShape_LocalStoreMemory* compoundShapeLocation, btCompoundShape* spuCompoundShape, uint32_t dmaTag)
 239 {
 240         register int dmaSize;
 241         register        ppu_address_t   dmaPpuAddress2;
 242         int childShapeCount = spuCompoundShape->getNumChildShapes();
 243         dmaSize = childShapeCount * sizeof(btCompoundShapeChild);
 244         dmaPpuAddress2 = (ppu_address_t)spuCompoundShape->getChildList();
 245         cellDmaGet(&compoundShapeLocation->gSubshapes[0], dmaPpuAddress2, dmaSize, DMA_TAG(dmaTag), 0, 0);
 246 }
 247
 248 void dmaCompoundSubShapes (CompoundShape_LocalStoreMemory* compoundShapeLocation, btCompoundShape* spuCompoundShape, uint32_t dmaTag)
 249 {
 250         int childShapeCount = spuCompoundShape->getNumChildShapes();
 251         int i;
 252         // DMA all the subshapes
 253         for ( i = 0; i < childShapeCount; ++i)
 254         {
 255                 btCompoundShapeChild& childShape = compoundShapeLocation->gSubshapes[i];
 256                 dmaCollisionShape (&compoundShapeLocation->gSubshapeShape[i],(ppu_address_t)childShape.m_childShape, dmaTag, childShape.m_childShapeType);
 257         }
 258 }
 259
 260
 261 void    spuWalkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,const btQuantizedBvhNode* rootNode,int startNodeIndex,int endNodeIndex)
 262 {
 263
 264         int curIndex = startNodeIndex;
 265         int walkIterations = 0;
 266 #ifdef BT_DEBUG
 267         int subTreeSize = endNodeIndex - startNodeIndex;
 268 #endif
 269
 270         int escapeIndex;
 271
 272         unsigned int aabbOverlap, isLeafNode;
 273
 274         while (curIndex < endNodeIndex)
 275         {
 276                 //catch bugs in tree data
 277                 btAssert (walkIterations < subTreeSize);
 278
 279                 walkIterations++;
 280                 aabbOverlap = spuTestQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax);
 281                 isLeafNode = rootNode->isLeafNode();
 282
 283                 if (isLeafNode && aabbOverlap)
 284                 {
 285                         //printf("overlap with node %d\n",rootNode->getTriangleIndex());
 286                         nodeCallback->processNode(0,rootNode->getTriangleIndex());
 287                         //                      spu_printf("SPU: overlap detected with triangleIndex:%d\n",rootNode->getTriangleIndex());
 288                 }
 289
 290                 if (aabbOverlap || isLeafNode)
 291                 {
 292                         rootNode++;
 293                         curIndex++;
 294                 } else
 295                 {
 296                         escapeIndex = rootNode->getEscapeIndex();
 297                         rootNode += escapeIndex;
 298                         curIndex += escapeIndex;
 299                 }
 300         }
 301
 302 }