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