bullet: Update to current svn, r2636
[blender.git] / extern / bullet2 / src / BulletCollision / CollisionShapes / btConvexShape.cpp
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
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 #if defined (_WIN32) || defined (__i386__)
17 #define BT_USE_SSE_IN_API
18 #endif
19
20 #include "btConvexShape.h"
21 #include "btTriangleShape.h"
22 #include "btSphereShape.h"
23 #include "btCylinderShape.h"
24 #include "btCapsuleShape.h"
25 #include "btConvexHullShape.h"
26 #include "btConvexPointCloudShape.h"
27
28 ///not supported on IBM SDK, until we fix the alignment of btVector3
29 #if defined (__CELLOS_LV2__) && defined (__SPU__)
30 #include <spu_intrinsics.h>
31 static inline vec_float4 vec_dot3( vec_float4 vec0, vec_float4 vec1 )
32 {
33     vec_float4 result;
34     result = spu_mul( vec0, vec1 );
35     result = spu_madd( spu_rlqwbyte( vec0, 4 ), spu_rlqwbyte( vec1, 4 ), result );
36     return spu_madd( spu_rlqwbyte( vec0, 8 ), spu_rlqwbyte( vec1, 8 ), result );
37 }
38 #endif //__SPU__
39
40 btConvexShape::btConvexShape ()
41 {
42 }
43
44 btConvexShape::~btConvexShape()
45 {
46
47 }
48
49
50 void btConvexShape::project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max) const
51 {
52         btVector3 localAxis = dir*trans.getBasis();
53         btVector3 vtx1 = trans(localGetSupportingVertex(localAxis));
54         btVector3 vtx2 = trans(localGetSupportingVertex(-localAxis));
55
56         min = vtx1.dot(dir);
57         max = vtx2.dot(dir);
58
59         if(min>max)
60         {
61                 btScalar tmp = min;
62                 min = max;
63                 max = tmp;
64         }
65 }
66
67
68 static btVector3 convexHullSupport (const btVector3& localDirOrg, const btVector3* points, int numPoints, const btVector3& localScaling)
69 {       
70
71         btVector3 vec = localDirOrg * localScaling;
72
73 #if defined (__CELLOS_LV2__) && defined (__SPU__)
74
75         btVector3 localDir = vec;
76
77         vec_float4 v_distMax = {-FLT_MAX,0,0,0};
78         vec_int4 v_idxMax = {-999,0,0,0};
79         int v=0;
80         int numverts = numPoints;
81
82         for(;v<(int)numverts-4;v+=4) {
83                 vec_float4 p0 = vec_dot3(points[v  ].get128(),localDir.get128());
84                 vec_float4 p1 = vec_dot3(points[v+1].get128(),localDir.get128());
85                 vec_float4 p2 = vec_dot3(points[v+2].get128(),localDir.get128());
86                 vec_float4 p3 = vec_dot3(points[v+3].get128(),localDir.get128());
87                 const vec_int4 i0 = {v  ,0,0,0};
88                 const vec_int4 i1 = {v+1,0,0,0};
89                 const vec_int4 i2 = {v+2,0,0,0};
90                 const vec_int4 i3 = {v+3,0,0,0};
91                 vec_uint4  retGt01 = spu_cmpgt(p0,p1);
92                 vec_float4 pmax01 = spu_sel(p1,p0,retGt01);
93                 vec_int4   imax01 = spu_sel(i1,i0,retGt01);
94                 vec_uint4  retGt23 = spu_cmpgt(p2,p3);
95                 vec_float4 pmax23 = spu_sel(p3,p2,retGt23);
96                 vec_int4   imax23 = spu_sel(i3,i2,retGt23);
97                 vec_uint4  retGt0123 = spu_cmpgt(pmax01,pmax23);
98                 vec_float4 pmax0123 = spu_sel(pmax23,pmax01,retGt0123);
99                 vec_int4   imax0123 = spu_sel(imax23,imax01,retGt0123);
100                 vec_uint4  retGtMax = spu_cmpgt(v_distMax,pmax0123);
101                 v_distMax = spu_sel(pmax0123,v_distMax,retGtMax);
102                 v_idxMax = spu_sel(imax0123,v_idxMax,retGtMax);
103         }
104         for(;v<(int)numverts;v++) {
105                 vec_float4 p = vec_dot3(points[v].get128(),localDir.get128());
106                 const vec_int4 i = {v,0,0,0};
107                 vec_uint4  retGtMax = spu_cmpgt(v_distMax,p);
108                 v_distMax = spu_sel(p,v_distMax,retGtMax);
109                 v_idxMax = spu_sel(i,v_idxMax,retGtMax);
110         }
111         int ptIndex = spu_extract(v_idxMax,0);
112         const btVector3& supVec= points[ptIndex] * localScaling;
113         return supVec;
114 #else
115
116     btScalar maxDot;
117     long ptIndex = vec.maxDot( points, numPoints, maxDot);
118         btAssert(ptIndex >= 0);
119         btVector3 supVec = points[ptIndex] * localScaling;
120         return supVec;
121 #endif //__SPU__
122 }
123
124 btVector3 btConvexShape::localGetSupportVertexWithoutMarginNonVirtual (const btVector3& localDir) const
125 {
126         switch (m_shapeType)
127         {
128     case SPHERE_SHAPE_PROXYTYPE:
129         {
130                 return btVector3(0,0,0);
131     }
132         case BOX_SHAPE_PROXYTYPE:
133         {
134                 btBoxShape* convexShape = (btBoxShape*)this;
135                 const btVector3& halfExtents = convexShape->getImplicitShapeDimensions();
136
137 #if defined( __APPLE__ ) && (defined( BT_USE_SSE )||defined( BT_USE_NEON ))
138     #if defined( BT_USE_SSE )
139             return btVector3( _mm_xor_ps( _mm_and_ps( localDir.mVec128, (__m128){-0.0f, -0.0f, -0.0f, -0.0f }), halfExtents.mVec128 ));
140     #elif defined( BT_USE_NEON )
141             return btVector3( (float32x4_t) (((uint32x4_t) localDir.mVec128 & (uint32x4_t){ 0x80000000, 0x80000000, 0x80000000, 0x80000000}) ^ (uint32x4_t) halfExtents.mVec128 ));
142     #else
143         #error unknown vector arch
144     #endif
145 #else
146                 return btVector3(btFsels(localDir.x(), halfExtents.x(), -halfExtents.x()),
147                         btFsels(localDir.y(), halfExtents.y(), -halfExtents.y()),
148                         btFsels(localDir.z(), halfExtents.z(), -halfExtents.z()));
149 #endif
150         }
151         case TRIANGLE_SHAPE_PROXYTYPE:
152         {
153                 btTriangleShape* triangleShape = (btTriangleShape*)this;
154                 btVector3 dir(localDir.getX(),localDir.getY(),localDir.getZ());
155                 btVector3* vertices = &triangleShape->m_vertices1[0];
156         btVector3 dots = dir.dot3(vertices[0], vertices[1], vertices[2]);
157                 btVector3 sup = vertices[dots.maxAxis()];
158                 return btVector3(sup.getX(),sup.getY(),sup.getZ());
159         }
160         case CYLINDER_SHAPE_PROXYTYPE:
161         {
162                 btCylinderShape* cylShape = (btCylinderShape*)this;
163                 //mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis)
164
165                 btVector3 halfExtents = cylShape->getImplicitShapeDimensions();
166                 btVector3 v(localDir.getX(),localDir.getY(),localDir.getZ());
167                 int cylinderUpAxis = cylShape->getUpAxis();
168                 int XX(1),YY(0),ZZ(2);
169
170                 switch (cylinderUpAxis)
171                 {
172                 case 0:
173                 {
174                         XX = 1;
175                         YY = 0;
176                         ZZ = 2;
177                 }
178                 break;
179                 case 1:
180                 {
181                         XX = 0;
182                         YY = 1;
183                         ZZ = 2; 
184                 }
185                 break;
186                 case 2:
187                 {
188                         XX = 0;
189                         YY = 2;
190                         ZZ = 1;
191                         
192                 }
193                 break;
194                 default:
195                         btAssert(0);
196                 break;
197                 };
198
199                 btScalar radius = halfExtents[XX];
200                 btScalar halfHeight = halfExtents[cylinderUpAxis];
201
202                 btVector3 tmp;
203                 btScalar d ;
204
205                 btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
206                 if (s != btScalar(0.0))
207                 {
208                         d = radius / s;  
209                         tmp[XX] = v[XX] * d;
210                         tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
211                         tmp[ZZ] = v[ZZ] * d;
212                         return btVector3(tmp.getX(),tmp.getY(),tmp.getZ());
213                 } else {
214                         tmp[XX] = radius;
215                         tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
216                         tmp[ZZ] = btScalar(0.0);
217                         return btVector3(tmp.getX(),tmp.getY(),tmp.getZ());
218                 }
219         }
220         case CAPSULE_SHAPE_PROXYTYPE:
221         {
222                 btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ());
223
224                 btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
225                 btScalar halfHeight = capsuleShape->getHalfHeight();
226                 int capsuleUpAxis = capsuleShape->getUpAxis();
227
228                 btScalar radius = capsuleShape->getRadius();
229                 btVector3 supVec(0,0,0);
230
231                 btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
232
233                 btVector3 vec = vec0;
234                 btScalar lenSqr = vec.length2();
235                 if (lenSqr < btScalar(0.0001))
236                 {
237                         vec.setValue(1,0,0);
238                 } else
239                 {
240                         btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
241                         vec *= rlen;
242                 }
243                 btVector3 vtx;
244                 btScalar newDot;
245                 {
246                         btVector3 pos(0,0,0);
247                         pos[capsuleUpAxis] = halfHeight;
248
249                         //vtx = pos +vec*(radius);
250                         vtx = pos +vec*(radius) - vec * capsuleShape->getMarginNV();
251                         newDot = vec.dot(vtx);
252                         
253
254                         if (newDot > maxDot)
255                         {
256                                 maxDot = newDot;
257                                 supVec = vtx;
258                         }
259                 }
260                 {
261                         btVector3 pos(0,0,0);
262                         pos[capsuleUpAxis] = -halfHeight;
263
264                         //vtx = pos +vec*(radius);
265                         vtx = pos +vec*(radius) - vec * capsuleShape->getMarginNV();
266                         newDot = vec.dot(vtx);
267                         if (newDot > maxDot)
268                         {
269                                 maxDot = newDot;
270                                 supVec = vtx;
271                         }
272                 }
273                 return btVector3(supVec.getX(),supVec.getY(),supVec.getZ());    
274         }
275         case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
276         {
277                 btConvexPointCloudShape* convexPointCloudShape = (btConvexPointCloudShape*)this;
278                 btVector3* points = convexPointCloudShape->getUnscaledPoints ();
279                 int numPoints = convexPointCloudShape->getNumPoints ();
280                 return convexHullSupport (localDir, points, numPoints,convexPointCloudShape->getLocalScalingNV());
281         }
282         case CONVEX_HULL_SHAPE_PROXYTYPE:
283         {
284                 btConvexHullShape* convexHullShape = (btConvexHullShape*)this;
285                 btVector3* points = convexHullShape->getUnscaledPoints();
286                 int numPoints = convexHullShape->getNumPoints ();
287                 return convexHullSupport (localDir, points, numPoints,convexHullShape->getLocalScalingNV());
288         }
289     default:
290 #ifndef __SPU__
291                 return this->localGetSupportingVertexWithoutMargin (localDir);
292 #else
293                 btAssert (0);
294 #endif
295         }
296
297         // should never reach here
298         btAssert (0);
299         return btVector3 (btScalar(0.0f), btScalar(0.0f), btScalar(0.0f));
300 }
301
302 btVector3 btConvexShape::localGetSupportVertexNonVirtual (const btVector3& localDir) const
303 {
304         btVector3 localDirNorm = localDir;
305         if (localDirNorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
306         {
307                 localDirNorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.));
308         }
309         localDirNorm.normalize ();
310
311         return localGetSupportVertexWithoutMarginNonVirtual(localDirNorm)+ getMarginNonVirtual() * localDirNorm;
312 }
313
314 /* TODO: This should be bumped up to btCollisionShape () */
315 btScalar btConvexShape::getMarginNonVirtual () const
316 {
317         switch (m_shapeType)
318         {
319     case SPHERE_SHAPE_PROXYTYPE:
320         {
321                 btSphereShape* sphereShape = (btSphereShape*)this;
322                 return sphereShape->getRadius ();
323         }
324         case BOX_SHAPE_PROXYTYPE:
325         {
326                 btBoxShape* convexShape = (btBoxShape*)this;
327                 return convexShape->getMarginNV ();
328         }
329         case TRIANGLE_SHAPE_PROXYTYPE:
330         {
331                 btTriangleShape* triangleShape = (btTriangleShape*)this;
332                 return triangleShape->getMarginNV ();
333         }
334         case CYLINDER_SHAPE_PROXYTYPE:
335         {
336                 btCylinderShape* cylShape = (btCylinderShape*)this;
337                 return cylShape->getMarginNV();
338         }
339         case CAPSULE_SHAPE_PROXYTYPE:
340         {
341                 btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
342                 return capsuleShape->getMarginNV();
343         }
344         case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
345         /* fall through */
346         case CONVEX_HULL_SHAPE_PROXYTYPE:
347         {
348                 btPolyhedralConvexShape* convexHullShape = (btPolyhedralConvexShape*)this;
349                 return convexHullShape->getMarginNV();
350         }
351     default:
352 #ifndef __SPU__
353                 return this->getMargin ();
354 #else
355                 btAssert (0);
356 #endif
357         }
358
359         // should never reach here
360         btAssert (0);
361         return btScalar(0.0f);
362 }
363 #ifndef __SPU__
364 void btConvexShape::getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
365 {
366         switch (m_shapeType)
367         {
368     case SPHERE_SHAPE_PROXYTYPE:
369         {
370                 btSphereShape* sphereShape = (btSphereShape*)this;
371                 btScalar radius = sphereShape->getImplicitShapeDimensions().getX();// * convexShape->getLocalScaling().getX();
372                 btScalar margin = radius + sphereShape->getMarginNonVirtual();
373                 const btVector3& center = t.getOrigin();
374                 btVector3 extent(margin,margin,margin);
375                 aabbMin = center - extent;
376                 aabbMax = center + extent;
377     }
378         break;
379         case CYLINDER_SHAPE_PROXYTYPE:
380         /* fall through */
381         case BOX_SHAPE_PROXYTYPE:
382         {
383                 btBoxShape* convexShape = (btBoxShape*)this;
384                 btScalar margin=convexShape->getMarginNonVirtual();
385                 btVector3 halfExtents = convexShape->getImplicitShapeDimensions();
386                 halfExtents += btVector3(margin,margin,margin);
387                 btMatrix3x3 abs_b = t.getBasis().absolute();  
388                 btVector3 center = t.getOrigin();
389         btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);    
390         
391                 aabbMin = center - extent;
392                 aabbMax = center + extent;
393                 break;
394         }
395         case TRIANGLE_SHAPE_PROXYTYPE:
396         {
397                 btTriangleShape* triangleShape = (btTriangleShape*)this;
398                 btScalar margin = triangleShape->getMarginNonVirtual();
399                 for (int i=0;i<3;i++)
400                 {
401                         btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.));
402                         vec[i] = btScalar(1.);
403
404                         btVector3 sv = localGetSupportVertexWithoutMarginNonVirtual(vec*t.getBasis());
405
406                         btVector3 tmp = t(sv);
407                         aabbMax[i] = tmp[i]+margin;
408                         vec[i] = btScalar(-1.);
409                         tmp = t(localGetSupportVertexWithoutMarginNonVirtual(vec*t.getBasis()));
410                         aabbMin[i] = tmp[i]-margin;
411                 }       
412         }
413         break;
414         case CAPSULE_SHAPE_PROXYTYPE:
415         {
416                 btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
417                 btVector3 halfExtents(capsuleShape->getRadius(),capsuleShape->getRadius(),capsuleShape->getRadius());
418                 int m_upAxis = capsuleShape->getUpAxis();
419                 halfExtents[m_upAxis] = capsuleShape->getRadius() + capsuleShape->getHalfHeight();
420                 halfExtents += btVector3(capsuleShape->getMarginNonVirtual(),capsuleShape->getMarginNonVirtual(),capsuleShape->getMarginNonVirtual());
421                 btMatrix3x3 abs_b = t.getBasis().absolute();  
422                 btVector3 center = t.getOrigin();
423         btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);    
424                 aabbMin = center - extent;
425                 aabbMax = center + extent;
426         }
427         break;
428         case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
429         case CONVEX_HULL_SHAPE_PROXYTYPE:
430         {
431                 btPolyhedralConvexAabbCachingShape* convexHullShape = (btPolyhedralConvexAabbCachingShape*)this;
432                 btScalar margin = convexHullShape->getMarginNonVirtual();
433                 convexHullShape->getNonvirtualAabb (t, aabbMin, aabbMax, margin);
434         }
435         break;
436     default:
437 #ifndef __SPU__
438                 this->getAabb (t, aabbMin, aabbMax);
439 #else
440                 btAssert (0);
441 #endif
442         break;
443         }
444
445         // should never reach here
446         btAssert (0);
447 }
448
449 #endif //__SPU__