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>
vec_dot3(vec_float4 vec0,vec_float4 vec1)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
btConvexShape()41 btConvexShape::btConvexShape ()
42 {
43 }
44
~btConvexShape()45 btConvexShape::~btConvexShape()
46 {
47
48 }
49
50
project(const btTransform & trans,const btVector3 & dir,btScalar & min,btScalar & max,btVector3 & witnesPtMin,btVector3 & witnesPtMax) const51 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
convexHullSupport(const btVector3 & localDirOrg,const btVector3 * points,int numPoints,const btVector3 & localScaling)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
localGetSupportVertexWithoutMarginNonVirtual(const btVector3 & localDir) const129 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
localGetSupportVertexNonVirtual(const btVector3 & localDir) const307 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 () */
getMarginNonVirtual() const320 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__
getAabbNonVirtual(const btTransform & t,btVector3 & aabbMin,btVector3 & aabbMax) const374 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__
460