Bullet Collision Detection & Physics Library
btConvexShape.cpp
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. 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"
28
30#if defined (__CELLOS_LV2__) && defined (__SPU__)
31#include <spu_intrinsics.h>
32static 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
42{
43}
44
46{
47
48}
49
50
51void 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
73static 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
130{
131 switch (m_shapeType)
132 {
134 {
135 return btVector3(0,0,0);
136 }
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 }
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 }
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 }
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 btVector3 supVec(0,0,0);
234
236
237 btVector3 vec = vec0;
238 btScalar lenSqr = vec.length2();
239 if (lenSqr < SIMD_EPSILON*SIMD_EPSILON)
240 {
241 vec.setValue(1,0,0);
242 } else
243 {
244 btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
245 vec *= rlen;
246 }
247 btVector3 vtx;
248 btScalar newDot;
249 {
250 btVector3 pos(0,0,0);
251 pos[capsuleUpAxis] = halfHeight;
252
253 vtx = pos;
254 newDot = vec.dot(vtx);
255
256
257 if (newDot > maxDot)
258 {
259 maxDot = newDot;
260 supVec = vtx;
261 }
262 }
263 {
264 btVector3 pos(0,0,0);
265 pos[capsuleUpAxis] = -halfHeight;
266
267 vtx = pos;
268 newDot = vec.dot(vtx);
269 if (newDot > maxDot)
270 {
271 maxDot = newDot;
272 supVec = vtx;
273 }
274 }
275 return btVector3(supVec.getX(),supVec.getY(),supVec.getZ());
276 }
278 {
279 btConvexPointCloudShape* convexPointCloudShape = (btConvexPointCloudShape*)this;
280 btVector3* points = convexPointCloudShape->getUnscaledPoints ();
281 int numPoints = convexPointCloudShape->getNumPoints ();
282 return convexHullSupport (localDir, points, numPoints,convexPointCloudShape->getLocalScalingNV());
283 }
285 {
286 btConvexHullShape* convexHullShape = (btConvexHullShape*)this;
287 btVector3* points = convexHullShape->getUnscaledPoints();
288 int numPoints = convexHullShape->getNumPoints ();
289 return convexHullSupport (localDir, points, numPoints,convexHullShape->getLocalScalingNV());
290 }
291 default:
292#ifndef __SPU__
293 return this->localGetSupportingVertexWithoutMargin (localDir);
294#else
295 btAssert (0);
296#endif
297 }
298
299 // should never reach here
300 btAssert (0);
301 return btVector3 (btScalar(0.0f), btScalar(0.0f), btScalar(0.0f));
302}
303
305{
306 btVector3 localDirNorm = localDir;
307 if (localDirNorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
308 {
309 localDirNorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.));
310 }
311 localDirNorm.normalize ();
312
313 return localGetSupportVertexWithoutMarginNonVirtual(localDirNorm)+ getMarginNonVirtual() * localDirNorm;
314}
315
316/* TODO: This should be bumped up to btCollisionShape () */
318{
319 switch (m_shapeType)
320 {
322 {
323 btSphereShape* sphereShape = (btSphereShape*)this;
324 return sphereShape->getRadius ();
325 }
327 {
328 btBoxShape* convexShape = (btBoxShape*)this;
329 return convexShape->getMarginNV ();
330 }
332 {
333 btTriangleShape* triangleShape = (btTriangleShape*)this;
334 return triangleShape->getMarginNV ();
335 }
337 {
338 btCylinderShape* cylShape = (btCylinderShape*)this;
339 return cylShape->getMarginNV();
340 }
342 {
343 btConeShape* conShape = (btConeShape*)this;
344 return conShape->getMarginNV();
345 }
347 {
348 btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
349 return capsuleShape->getMarginNV();
350 }
352 /* fall through */
354 {
355 btPolyhedralConvexShape* convexHullShape = (btPolyhedralConvexShape*)this;
356 return convexHullShape->getMarginNV();
357 }
358 default:
359#ifndef __SPU__
360 return this->getMargin ();
361#else
362 btAssert (0);
363#endif
364 }
365
366 // should never reach here
367 btAssert (0);
368 return btScalar(0.0f);
369}
370#ifndef __SPU__
371void btConvexShape::getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
372{
373 switch (m_shapeType)
374 {
376 {
377 btSphereShape* sphereShape = (btSphereShape*)this;
378 btScalar radius = sphereShape->getImplicitShapeDimensions().getX();// * convexShape->getLocalScaling().getX();
379 btScalar margin = radius + sphereShape->getMarginNonVirtual();
380 const btVector3& center = t.getOrigin();
381 btVector3 extent(margin,margin,margin);
382 aabbMin = center - extent;
383 aabbMax = center + extent;
384 }
385 break;
387 /* fall through */
389 {
390 btBoxShape* convexShape = (btBoxShape*)this;
391 btScalar margin=convexShape->getMarginNonVirtual();
392 btVector3 halfExtents = convexShape->getImplicitShapeDimensions();
393 halfExtents += btVector3(margin,margin,margin);
394 btMatrix3x3 abs_b = t.getBasis().absolute();
395 btVector3 center = t.getOrigin();
396 btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
397
398 aabbMin = center - extent;
399 aabbMax = center + extent;
400 break;
401 }
403 {
404 btTriangleShape* triangleShape = (btTriangleShape*)this;
405 btScalar margin = triangleShape->getMarginNonVirtual();
406 for (int i=0;i<3;i++)
407 {
408 btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.));
409 vec[i] = btScalar(1.);
410
412
413 btVector3 tmp = t(sv);
414 aabbMax[i] = tmp[i]+margin;
415 vec[i] = btScalar(-1.);
417 aabbMin[i] = tmp[i]-margin;
418 }
419 }
420 break;
422 {
423 btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
424 btVector3 halfExtents(capsuleShape->getRadius(),capsuleShape->getRadius(),capsuleShape->getRadius());
425 int m_upAxis = capsuleShape->getUpAxis();
426 halfExtents[m_upAxis] = capsuleShape->getRadius() + capsuleShape->getHalfHeight();
427 btMatrix3x3 abs_b = t.getBasis().absolute();
428 btVector3 center = t.getOrigin();
429 btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
430 aabbMin = center - extent;
431 aabbMax = center + extent;
432 }
433 break;
436 {
438 btScalar margin = convexHullShape->getMarginNonVirtual();
439 convexHullShape->getNonvirtualAabb (t, aabbMin, aabbMax, margin);
440 }
441 break;
442 default:
443#ifndef __SPU__
444 this->getAabb (t, aabbMin, aabbMax);
445#else
446 btAssert (0);
447#endif
448 break;
449 }
450
451 // should never reach here
452 btAssert (0);
453}
454
455#endif //__SPU__
@ CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE
@ TRIANGLE_SHAPE_PROXYTYPE
@ SPHERE_SHAPE_PROXYTYPE
@ BOX_SHAPE_PROXYTYPE
@ CYLINDER_SHAPE_PROXYTYPE
@ CONE_SHAPE_PROXYTYPE
@ CAPSULE_SHAPE_PROXYTYPE
@ CONVEX_HULL_SHAPE_PROXYTYPE
static btVector3 convexHullSupport(const btVector3 &localDirOrg, const btVector3 *points, int numPoints, const btVector3 &localScaling)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
#define BT_LARGE_FLOAT
Definition: btScalar.h:294
btScalar btSqrt(btScalar y)
Definition: btScalar.h:444
#define btFsels(a, b, c)
Definition: btScalar.h:581
#define SIMD_EPSILON
Definition: btScalar.h:521
#define btAssert(x)
Definition: btScalar.h:131
The btBoxShape is a box primitive around the origin, its sides axis aligned with length specified by ...
Definition: btBoxShape.h:27
The btCapsuleShape represents a capsule around the Y axis, there is also the btCapsuleShapeX aligned ...
btScalar getRadius() const
int getUpAxis() const
btScalar getHalfHeight() const
The btConeShape implements a cone shape primitive, centered around the origin and aligned with the Y ...
Definition: btConeShape.h:25
The btConvexHullShape implements an implicit convex hull of an array of vertices.
btVector3 * getUnscaledPoints()
int getNumPoints() const
const btVector3 & getLocalScalingNV() const
const btVector3 & getImplicitShapeDimensions() const
The btConvexPointCloudShape implements an implicit convex hull of an array of vertices.
btScalar getMarginNonVirtual() const
virtual ~btConvexShape()
btConvexShape()
not supported on IBM SDK, until we fix the alignment of btVector3
virtual void project(const btTransform &trans, const btVector3 &dir, btScalar &minProj, btScalar &maxProj, btVector3 &witnesPtMin, btVector3 &witnesPtMax) const
virtual btVector3 localGetSupportingVertex(const btVector3 &vec) const =0
btVector3 localGetSupportVertexNonVirtual(const btVector3 &vec) const
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3 &vec) const =0
void getAabbNonVirtual(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const
virtual btScalar getMargin() const =0
void getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const =0
getAabb's default implementation is brute force, expected derived classes to implement a fast dedicat...
btVector3 localGetSupportVertexWithoutMarginNonVirtual(const btVector3 &vec) const
The btCylinderShape class implements a cylinder shape primitive, centered around the origin....
int getUpAxis() const
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:48
btMatrix3x3 absolute() const
Return the matrix with all values non negative.
Definition: btMatrix3x3.h:937
The btPolyhedralConvexAabbCachingShape adds aabb caching to the btPolyhedralConvexShape.
void getNonvirtualAabb(const btTransform &trans, btVector3 &aabbMin, btVector3 &aabbMax, btScalar margin) const
The btPolyhedralConvexShape is an internal interface class for polyhedral convex shapes.
The btSphereShape implements an implicit sphere, centered around a local origin with radius.
Definition: btSphereShape.h:24
btScalar getRadius() const
Definition: btSphereShape.h:50
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
btVector3 m_vertices1[3]
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:84
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:577
const btScalar & z() const
Return the z value.
Definition: btVector3.h:591
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
btVector3 dot3(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2) const
Definition: btVector3.h:731
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:652
long maxDot(const btVector3 *array, long array_count, btScalar &dotOut) const
returns index of maximum dot product between this and vectors in array[]
Definition: btVector3.h:1013
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:575
const btScalar & x() const
Return the x value.
Definition: btVector3.h:587
int maxAxis() const
Return the axis with the largest value Note return values are 0,1,2 for x, y, or z.
Definition: btVector3.h:487
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:309
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:573
const btScalar & y() const
Return the y value.
Definition: btVector3.h:589