== SoC Bullet - Bullet Upgrade to 2.76 ==
[blender.git] / extern / bullet2 / 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 = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents));
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 = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents));
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 }