SpuCollisionShapes.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 
00016 
00017 #include "SpuCollisionShapes.h"
00018 
00020 #if defined (__CELLOS_LV2__) && defined (__SPU__)
00021 #include <spu_intrinsics.h>
00022 static inline vec_float4 vec_dot3( vec_float4 vec0, vec_float4 vec1 )
00023 {
00024     vec_float4 result;
00025     result = spu_mul( vec0, vec1 );
00026     result = spu_madd( spu_rlqwbyte( vec0, 4 ), spu_rlqwbyte( vec1, 4 ), result );
00027     return spu_madd( spu_rlqwbyte( vec0, 8 ), spu_rlqwbyte( vec1, 8 ), result );
00028 }
00029 #endif //__SPU__
00030 
00031 
00032 void computeAabb (btVector3& aabbMin, btVector3& aabbMax, btConvexInternalShape* convexShape, ppu_address_t convexShapePtr, int shapeType, const btTransform& xform)
00033 {
00034         //calculate the aabb, given the types...
00035         switch (shapeType)
00036         {
00037         case CYLINDER_SHAPE_PROXYTYPE:
00038                 /* fall through */
00039         case BOX_SHAPE_PROXYTYPE:
00040         {
00041                 btScalar margin=convexShape->getMarginNV();
00042                 btVector3 halfExtents = convexShape->getImplicitShapeDimensions();
00043                 halfExtents += btVector3(margin,margin,margin);
00044                 const btTransform& t = xform;
00045                 btMatrix3x3 abs_b = t.getBasis().absolute();  
00046                 btVector3 center = t.getOrigin();
00047         btVector3 extent = halfExtents.dot3( abs_b[0], abs_b[1], abs_b[2] );
00048                 
00049                 aabbMin = center - extent;
00050                 aabbMax = center + extent;
00051                 break;
00052         }
00053         case CAPSULE_SHAPE_PROXYTYPE:
00054         {
00055                 btScalar margin=convexShape->getMarginNV();
00056                 btVector3 halfExtents = convexShape->getImplicitShapeDimensions();
00057                 //add the radius to y-axis to get full height
00058                 btScalar radius = halfExtents[0];
00059                 halfExtents[1] += radius;
00060                 halfExtents += btVector3(margin,margin,margin);
00061 #if 0
00062                 int capsuleUpAxis = convexShape->getUpAxis();
00063                 btScalar halfHeight = convexShape->getHalfHeight();
00064                 btScalar radius = convexShape->getRadius();
00065                 halfExtents[capsuleUpAxis] = radius + halfHeight;
00066 #endif
00067                 const btTransform& t = xform;
00068                 btMatrix3x3 abs_b = t.getBasis().absolute();  
00069                 btVector3 center = t.getOrigin();
00070         btVector3 extent = halfExtents.dot3( abs_b[0], abs_b[1], abs_b[2] );
00071                 
00072                 aabbMin = center - extent;
00073                 aabbMax = center + extent;
00074                 break;
00075         }
00076         case SPHERE_SHAPE_PROXYTYPE:
00077         {
00078                 btScalar radius = convexShape->getImplicitShapeDimensions().getX();// * convexShape->getLocalScaling().getX();
00079                 btScalar margin = radius + convexShape->getMarginNV();
00080                 const btTransform& t = xform;
00081                 const btVector3& center = t.getOrigin();
00082                 btVector3 extent(margin,margin,margin);
00083                 aabbMin = center - extent;
00084                 aabbMax = center + extent;
00085                 break;
00086         }
00087         case CONVEX_HULL_SHAPE_PROXYTYPE:
00088         {
00089                 ATTRIBUTE_ALIGNED16(char convexHullShape0[sizeof(btConvexHullShape)]);
00090                 cellDmaGet(&convexHullShape0, convexShapePtr  , sizeof(btConvexHullShape), DMA_TAG(1), 0, 0);
00091                 cellDmaWaitTagStatusAll(DMA_MASK(1));
00092                 btConvexHullShape* localPtr = (btConvexHullShape*)&convexHullShape0;
00093                 const btTransform& t = xform;
00094                 btScalar margin = convexShape->getMarginNV();
00095                 localPtr->getNonvirtualAabb(t,aabbMin,aabbMax,margin);
00096                 //spu_printf("SPU convex aabbMin=%f,%f,%f=\n",aabbMin.getX(),aabbMin.getY(),aabbMin.getZ());
00097                 //spu_printf("SPU convex aabbMax=%f,%f,%f=\n",aabbMax.getX(),aabbMax.getY(),aabbMax.getZ());
00098                 break;
00099         }
00100         default:
00101                 {
00102         //      spu_printf("SPU: unsupported shapetype %d in AABB calculation\n");
00103                 }
00104         };
00105 }
00106 
00107 void dmaBvhShapeData (bvhMeshShape_LocalStoreMemory* bvhMeshShape, btBvhTriangleMeshShape* triMeshShape)
00108 {
00109         register int dmaSize;
00110         register ppu_address_t  dmaPpuAddress2;
00111 
00112         dmaSize = sizeof(btTriangleIndexVertexArray);
00113         dmaPpuAddress2 = reinterpret_cast<ppu_address_t>(triMeshShape->getMeshInterface());
00114         //      spu_printf("trimeshShape->getMeshInterface() == %llx\n",dmaPpuAddress2);
00115 #ifdef __SPU__
00116         cellDmaGet(&bvhMeshShape->gTriangleMeshInterfaceStorage, dmaPpuAddress2  , dmaSize, DMA_TAG(1), 0, 0);
00117         bvhMeshShape->gTriangleMeshInterfacePtr = &bvhMeshShape->gTriangleMeshInterfaceStorage;
00118 #else
00119         bvhMeshShape->gTriangleMeshInterfacePtr = (btTriangleIndexVertexArray*)cellDmaGetReadOnly(&bvhMeshShape->gTriangleMeshInterfaceStorage, dmaPpuAddress2  , dmaSize, DMA_TAG(1), 0, 0);
00120 #endif
00121 
00122         //cellDmaWaitTagStatusAll(DMA_MASK(1));
00123         
00125         
00126         dmaSize = sizeof(btOptimizedBvh);
00127         dmaPpuAddress2 = reinterpret_cast<ppu_address_t>(triMeshShape->getOptimizedBvh());
00128         //spu_printf("trimeshShape->getOptimizedBvh() == %llx\n",dmaPpuAddress2);
00129         cellDmaGet(&bvhMeshShape->gOptimizedBvh, dmaPpuAddress2  , dmaSize, DMA_TAG(2), 0, 0);
00130         //cellDmaWaitTagStatusAll(DMA_MASK(2));
00131         cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));
00132 }
00133 
00134 void dmaBvhIndexedMesh (btIndexedMesh* IndexMesh, IndexedMeshArray& indexArray, int index, uint32_t dmaTag)
00135 {               
00136         cellDmaGet(IndexMesh, (ppu_address_t)&indexArray[index]  , sizeof(btIndexedMesh), DMA_TAG(dmaTag), 0, 0);
00137         
00138 }
00139 
00140 void dmaBvhSubTreeHeaders (btBvhSubtreeInfo* subTreeHeaders, ppu_address_t subTreePtr, int batchSize, uint32_t dmaTag)
00141 {
00142         cellDmaGet(subTreeHeaders, subTreePtr, batchSize * sizeof(btBvhSubtreeInfo), DMA_TAG(dmaTag), 0, 0);
00143 }
00144 
00145 void dmaBvhSubTreeNodes (btQuantizedBvhNode* nodes, const btBvhSubtreeInfo& subtree, QuantizedNodeArray&        nodeArray, int dmaTag)
00146 {
00147         cellDmaGet(nodes, reinterpret_cast<ppu_address_t>(&nodeArray[subtree.m_rootNodeIndex]) , subtree.m_subtreeSize* sizeof(btQuantizedBvhNode), DMA_TAG(2), 0, 0);
00148 }
00149 
00151 int             getShapeTypeSize(int shapeType)
00152 {
00153 
00154 
00155         switch (shapeType)
00156         {
00157         case CYLINDER_SHAPE_PROXYTYPE:
00158                 {
00159                         int shapeSize = sizeof(btCylinderShape);
00160                         btAssert(shapeSize < MAX_SHAPE_SIZE);
00161                         return shapeSize;
00162                 }
00163         case BOX_SHAPE_PROXYTYPE:
00164                 {
00165                         int shapeSize = sizeof(btBoxShape);
00166                         btAssert(shapeSize < MAX_SHAPE_SIZE);
00167                         return shapeSize;
00168                 }
00169         case SPHERE_SHAPE_PROXYTYPE:
00170                 {
00171                         int shapeSize = sizeof(btSphereShape);
00172                         btAssert(shapeSize < MAX_SHAPE_SIZE);
00173                         return shapeSize;
00174                 }
00175         case TRIANGLE_MESH_SHAPE_PROXYTYPE:
00176                 {
00177                         int shapeSize = sizeof(btBvhTriangleMeshShape);
00178                         btAssert(shapeSize < MAX_SHAPE_SIZE);
00179                         return shapeSize;
00180                 }
00181         case CAPSULE_SHAPE_PROXYTYPE:
00182                 {
00183                         int shapeSize = sizeof(btCapsuleShape);
00184                         btAssert(shapeSize < MAX_SHAPE_SIZE);
00185                         return shapeSize;
00186                 }
00187 
00188         case CONVEX_HULL_SHAPE_PROXYTYPE:
00189                 {
00190                         int shapeSize = sizeof(btConvexHullShape);
00191                         btAssert(shapeSize < MAX_SHAPE_SIZE);
00192                         return shapeSize;
00193                 }
00194 
00195         case COMPOUND_SHAPE_PROXYTYPE:
00196                 {
00197                         int shapeSize = sizeof(btCompoundShape);
00198                         btAssert(shapeSize < MAX_SHAPE_SIZE);
00199                         return shapeSize;
00200                 }
00201         case STATIC_PLANE_PROXYTYPE:
00202                 {
00203                         int shapeSize = sizeof(btStaticPlaneShape);
00204                         btAssert(shapeSize < MAX_SHAPE_SIZE);
00205                         return shapeSize;
00206                 }
00207 
00208         default:
00209                 btAssert(0);
00210                 //unsupported shapetype, please add here
00211                 return 0;
00212         }
00213 }
00214 
00215 void dmaConvexVertexData (SpuConvexPolyhedronVertexData* convexVertexData, btConvexHullShape* convexShapeSPU)
00216 {
00217         convexVertexData->gNumConvexPoints = convexShapeSPU->getNumPoints();
00218         if (convexVertexData->gNumConvexPoints>MAX_NUM_SPU_CONVEX_POINTS)
00219         {
00220                 btAssert(0);
00221         //      spu_printf("SPU: Error: MAX_NUM_SPU_CONVEX_POINTS(%d) exceeded: %d\n",MAX_NUM_SPU_CONVEX_POINTS,convexVertexData->gNumConvexPoints);
00222                 return;
00223         }
00224                         
00225         register int dmaSize = convexVertexData->gNumConvexPoints*sizeof(btVector3);
00226         ppu_address_t pointsPPU = (ppu_address_t) convexShapeSPU->getUnscaledPoints();
00227         cellDmaGet(&convexVertexData->g_convexPointBuffer[0], pointsPPU  , dmaSize, DMA_TAG(2), 0, 0);
00228 }
00229 
00230 void dmaCollisionShape (void* collisionShapeLocation, ppu_address_t collisionShapePtr, uint32_t dmaTag, int shapeType)
00231 {
00232         register int dmaSize = getShapeTypeSize(shapeType);
00233         cellDmaGet(collisionShapeLocation, collisionShapePtr  , dmaSize, DMA_TAG(dmaTag), 0, 0);
00234         //cellDmaGetReadOnly(collisionShapeLocation, collisionShapePtr  , dmaSize, DMA_TAG(dmaTag), 0, 0);
00235         //cellDmaWaitTagStatusAll(DMA_MASK(dmaTag));
00236 }
00237 
00238 void dmaCompoundShapeInfo (CompoundShape_LocalStoreMemory* compoundShapeLocation, btCompoundShape* spuCompoundShape, uint32_t dmaTag)
00239 {
00240         register int dmaSize;
00241         register        ppu_address_t   dmaPpuAddress2;
00242         int childShapeCount = spuCompoundShape->getNumChildShapes();
00243         dmaSize = childShapeCount * sizeof(btCompoundShapeChild);
00244         dmaPpuAddress2 = (ppu_address_t)spuCompoundShape->getChildList();
00245         cellDmaGet(&compoundShapeLocation->gSubshapes[0], dmaPpuAddress2, dmaSize, DMA_TAG(dmaTag), 0, 0);
00246 }
00247 
00248 void dmaCompoundSubShapes (CompoundShape_LocalStoreMemory* compoundShapeLocation, btCompoundShape* spuCompoundShape, uint32_t dmaTag)
00249 {
00250         int childShapeCount = spuCompoundShape->getNumChildShapes();
00251         int i;
00252         // DMA all the subshapes 
00253         for ( i = 0; i < childShapeCount; ++i)
00254         {
00255                 btCompoundShapeChild& childShape = compoundShapeLocation->gSubshapes[i];
00256                 dmaCollisionShape (&compoundShapeLocation->gSubshapeShape[i],(ppu_address_t)childShape.m_childShape, dmaTag, childShape.m_childShapeType);
00257         }
00258 }
00259 
00260 
00261 void    spuWalkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,const btQuantizedBvhNode* rootNode,int startNodeIndex,int endNodeIndex)
00262 {
00263 
00264         int curIndex = startNodeIndex;
00265         int walkIterations = 0;
00266 #ifdef BT_DEBUG
00267         int subTreeSize = endNodeIndex - startNodeIndex;
00268 #endif
00269 
00270         int escapeIndex;
00271 
00272         unsigned int aabbOverlap, isLeafNode;
00273 
00274         while (curIndex < endNodeIndex)
00275         {
00276                 //catch bugs in tree data
00277                 btAssert (walkIterations < subTreeSize);
00278 
00279                 walkIterations++;
00280                 aabbOverlap = spuTestQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax);
00281                 isLeafNode = rootNode->isLeafNode();
00282 
00283                 if (isLeafNode && aabbOverlap)
00284                 {
00285                         //printf("overlap with node %d\n",rootNode->getTriangleIndex());
00286                         nodeCallback->processNode(0,rootNode->getTriangleIndex());
00287                         //                      spu_printf("SPU: overlap detected with triangleIndex:%d\n",rootNode->getTriangleIndex());
00288                 } 
00289 
00290                 if (aabbOverlap || isLeafNode)
00291                 {
00292                         rootNode++;
00293                         curIndex++;
00294                 } else
00295                 {
00296                         escapeIndex = rootNode->getEscapeIndex();
00297                         rootNode += escapeIndex;
00298                         curIndex += escapeIndex;
00299                 }
00300         }
00301 
00302 }