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