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
project(const btTransform & trans,const btVector3 & dir,btScalar & min,btScalar & max,btVector3 & witnesPtMin,btVector3 & witnesPtMax) const49 void btConvexShape::project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max, btVector3& witnesPtMin, btVector3& witnesPtMax) const
50 {
51 btVector3 localAxis = dir * trans.getBasis();
52 btVector3 vtx1 = trans(localGetSupportingVertex(localAxis));
53 btVector3 vtx2 = trans(localGetSupportingVertex(-localAxis));
54
55 min = vtx1.dot(dir);
56 max = vtx2.dot(dir);
57 witnesPtMax = vtx2;
58 witnesPtMin = vtx1;
59
60 if (min > max)
61 {
62 btScalar tmp = min;
63 min = max;
64 max = tmp;
65 witnesPtMax = vtx1;
66 witnesPtMin = vtx2;
67 }
68 }
69
convexHullSupport(const btVector3 & localDirOrg,const btVector3 * points,int numPoints,const btVector3 & localScaling)70 static btVector3 convexHullSupport(const btVector3& localDirOrg, const btVector3* points, int numPoints, const btVector3& localScaling)
71 {
72 btVector3 vec = localDirOrg * localScaling;
73
74 #if defined(__CELLOS_LV2__) && defined(__SPU__)
75
76 btVector3 localDir = vec;
77
78 vec_float4 v_distMax = {-FLT_MAX, 0, 0, 0};
79 vec_int4 v_idxMax = {-999, 0, 0, 0};
80 int v = 0;
81 int numverts = numPoints;
82
83 for (; v < (int)numverts - 4; v += 4)
84 {
85 vec_float4 p0 = vec_dot3(points[v].get128(), localDir.get128());
86 vec_float4 p1 = vec_dot3(points[v + 1].get128(), localDir.get128());
87 vec_float4 p2 = vec_dot3(points[v + 2].get128(), localDir.get128());
88 vec_float4 p3 = vec_dot3(points[v + 3].get128(), localDir.get128());
89 const vec_int4 i0 = {v, 0, 0, 0};
90 const vec_int4 i1 = {v + 1, 0, 0, 0};
91 const vec_int4 i2 = {v + 2, 0, 0, 0};
92 const vec_int4 i3 = {v + 3, 0, 0, 0};
93 vec_uint4 retGt01 = spu_cmpgt(p0, p1);
94 vec_float4 pmax01 = spu_sel(p1, p0, retGt01);
95 vec_int4 imax01 = spu_sel(i1, i0, retGt01);
96 vec_uint4 retGt23 = spu_cmpgt(p2, p3);
97 vec_float4 pmax23 = spu_sel(p3, p2, retGt23);
98 vec_int4 imax23 = spu_sel(i3, i2, retGt23);
99 vec_uint4 retGt0123 = spu_cmpgt(pmax01, pmax23);
100 vec_float4 pmax0123 = spu_sel(pmax23, pmax01, retGt0123);
101 vec_int4 imax0123 = spu_sel(imax23, imax01, retGt0123);
102 vec_uint4 retGtMax = spu_cmpgt(v_distMax, pmax0123);
103 v_distMax = spu_sel(pmax0123, v_distMax, retGtMax);
104 v_idxMax = spu_sel(imax0123, v_idxMax, retGtMax);
105 }
106 for (; v < (int)numverts; v++)
107 {
108 vec_float4 p = vec_dot3(points[v].get128(), localDir.get128());
109 const vec_int4 i = {v, 0, 0, 0};
110 vec_uint4 retGtMax = spu_cmpgt(v_distMax, p);
111 v_distMax = spu_sel(p, v_distMax, retGtMax);
112 v_idxMax = spu_sel(i, v_idxMax, retGtMax);
113 }
114 int ptIndex = spu_extract(v_idxMax, 0);
115 const btVector3& supVec = points[ptIndex] * localScaling;
116 return supVec;
117 #else
118
119 btScalar maxDot;
120 long ptIndex = vec.maxDot(points, numPoints, maxDot);
121 btAssert(ptIndex >= 0);
122 if (ptIndex < 0)
123 {
124 ptIndex = 0;
125 }
126 btVector3 supVec = points[ptIndex] * localScaling;
127 return supVec;
128 #endif //__SPU__
129 }
130
localGetSupportVertexWithoutMarginNonVirtual(const btVector3 & localDir) const131 btVector3 btConvexShape::localGetSupportVertexWithoutMarginNonVirtual(const btVector3& localDir) const
132 {
133 switch (m_shapeType)
134 {
135 case SPHERE_SHAPE_PROXYTYPE:
136 {
137 return btVector3(0, 0, 0);
138 }
139 case BOX_SHAPE_PROXYTYPE:
140 {
141 btBoxShape* convexShape = (btBoxShape*)this;
142 const btVector3& halfExtents = convexShape->getImplicitShapeDimensions();
143
144 #if defined(__APPLE__) && (defined(BT_USE_SSE) || defined(BT_USE_NEON))
145 #if defined(BT_USE_SSE)
146 return btVector3(_mm_xor_ps(_mm_and_ps(localDir.mVec128, (__m128){-0.0f, -0.0f, -0.0f, -0.0f}), halfExtents.mVec128));
147 #elif defined(BT_USE_NEON)
148 return btVector3((float32x4_t)(((uint32x4_t)localDir.mVec128 & (uint32x4_t){0x80000000, 0x80000000, 0x80000000, 0x80000000}) ^ (uint32x4_t)halfExtents.mVec128));
149 #else
150 #error unknown vector arch
151 #endif
152 #else
153 return btVector3(btFsels(localDir.x(), halfExtents.x(), -halfExtents.x()),
154 btFsels(localDir.y(), halfExtents.y(), -halfExtents.y()),
155 btFsels(localDir.z(), halfExtents.z(), -halfExtents.z()));
156 #endif
157 }
158 case TRIANGLE_SHAPE_PROXYTYPE:
159 {
160 btTriangleShape* triangleShape = (btTriangleShape*)this;
161 btVector3 dir(localDir.getX(), localDir.getY(), localDir.getZ());
162 btVector3* vertices = &triangleShape->m_vertices1[0];
163 btVector3 dots = dir.dot3(vertices[0], vertices[1], vertices[2]);
164 btVector3 sup = vertices[dots.maxAxis()];
165 return btVector3(sup.getX(), sup.getY(), sup.getZ());
166 }
167 case CYLINDER_SHAPE_PROXYTYPE:
168 {
169 btCylinderShape* cylShape = (btCylinderShape*)this;
170 //mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis)
171
172 btVector3 halfExtents = cylShape->getImplicitShapeDimensions();
173 btVector3 v(localDir.getX(), localDir.getY(), localDir.getZ());
174 int cylinderUpAxis = cylShape->getUpAxis();
175 int XX(1), YY(0), ZZ(2);
176
177 switch (cylinderUpAxis)
178 {
179 case 0:
180 {
181 XX = 1;
182 YY = 0;
183 ZZ = 2;
184 }
185 break;
186 case 1:
187 {
188 XX = 0;
189 YY = 1;
190 ZZ = 2;
191 }
192 break;
193 case 2:
194 {
195 XX = 0;
196 YY = 2;
197 ZZ = 1;
198 }
199 break;
200 default:
201 btAssert(0);
202 break;
203 };
204
205 btScalar radius = halfExtents[XX];
206 btScalar halfHeight = halfExtents[cylinderUpAxis];
207
208 btVector3 tmp;
209 btScalar d;
210
211 btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
212 if (s != btScalar(0.0))
213 {
214 d = radius / s;
215 tmp[XX] = v[XX] * d;
216 tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
217 tmp[ZZ] = v[ZZ] * d;
218 return btVector3(tmp.getX(), tmp.getY(), tmp.getZ());
219 }
220 else
221 {
222 tmp[XX] = radius;
223 tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
224 tmp[ZZ] = btScalar(0.0);
225 return btVector3(tmp.getX(), tmp.getY(), tmp.getZ());
226 }
227 }
228 case CAPSULE_SHAPE_PROXYTYPE:
229 {
230 btVector3 vec0(localDir.getX(), localDir.getY(), localDir.getZ());
231
232 btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
233 btScalar halfHeight = capsuleShape->getHalfHeight();
234 int capsuleUpAxis = capsuleShape->getUpAxis();
235
236 btVector3 supVec(0, 0, 0);
237
238 btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
239
240 btVector3 vec = vec0;
241 btScalar lenSqr = vec.length2();
242 if (lenSqr < SIMD_EPSILON * SIMD_EPSILON)
243 {
244 vec.setValue(1, 0, 0);
245 }
246 else
247 {
248 btScalar rlen = btScalar(1.) / btSqrt(lenSqr);
249 vec *= rlen;
250 }
251 btVector3 vtx;
252 btScalar newDot;
253 {
254 btVector3 pos(0, 0, 0);
255 pos[capsuleUpAxis] = halfHeight;
256
257 vtx = pos;
258 newDot = vec.dot(vtx);
259
260 if (newDot > maxDot)
261 {
262 maxDot = newDot;
263 supVec = vtx;
264 }
265 }
266 {
267 btVector3 pos(0, 0, 0);
268 pos[capsuleUpAxis] = -halfHeight;
269
270 vtx = pos;
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 btMatrix3x3 abs_b = t.getBasis().absolute();
431 btVector3 center = t.getOrigin();
432 btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
433 aabbMin = center - extent;
434 aabbMax = center + extent;
435 }
436 break;
437 case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
438 case CONVEX_HULL_SHAPE_PROXYTYPE:
439 {
440 btPolyhedralConvexAabbCachingShape* convexHullShape = (btPolyhedralConvexAabbCachingShape*)this;
441 btScalar margin = convexHullShape->getMarginNonVirtual();
442 convexHullShape->getNonvirtualAabb(t, aabbMin, aabbMax, margin);
443 }
444 break;
445 default:
446 #ifndef __SPU__
447 this->getAabb(t, aabbMin, aabbMax);
448 #else
449 btAssert(0);
450 #endif
451 break;
452 }
453
454 // should never reach here
455 btAssert(0);
456 }
457
458 #endif //__SPU__
459