Bullet Collision Detection & Physics Library
btConvexShape.cpp
Go to the documentation of this file.
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"
28 
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 
42 {
43 }
44 
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 
130 {
131  switch (m_shapeType)
132  {
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  }
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 
235  btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
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  }
326  case BOX_SHAPE_PROXYTYPE:
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__
371 void 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 */
388  case BOX_SHAPE_PROXYTYPE:
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__
btVector3 localGetSupportVertexNonVirtual(const btVector3 &vec) const
#define SIMD_EPSILON
Definition: btScalar.h:521
#define BT_LARGE_FLOAT
Definition: btScalar.h:294
virtual btVector3 localGetSupportingVertex(const btVector3 &vec) const =0
btScalar getRadius() const
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:652
btVector3 dot3(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2) const
Definition: btVector3.h:731
static btVector3 convexHullSupport(const btVector3 &localDirOrg, const btVector3 *points, int numPoints, const btVector3 &localScaling)
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
virtual void project(const btTransform &trans, const btVector3 &dir, btScalar &minProj, btScalar &maxProj, btVector3 &witnesPtMin, btVector3 &witnesPtMax) const
The btCapsuleShape represents a capsule around the Y axis, there is also the btCapsuleShapeX aligned ...
btScalar btSqrt(btScalar y)
Definition: btScalar.h:444
#define btAssert(x)
Definition: btScalar.h:131
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:575
The btSphereShape implements an implicit sphere, centered around a local origin with radius.
Definition: btSphereShape.h:22
int getNumPoints() const
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3 &vec) const =0
int getUpAxis() const
int getUpAxis() const
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:309
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:577
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
btVector3 * getUnscaledPoints()
virtual ~btConvexShape()
btScalar getHalfHeight() const
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
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...
const btScalar & x() const
Return the x value.
Definition: btVector3.h:587
void getAabbNonVirtual(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const
The btConeShape implements a cone shape primitive, centered around the origin and aligned with the Y ...
Definition: btConeShape.h:23
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
btScalar getRadius() const
Definition: btSphereShape.h:50
const btScalar & y() const
Return the y value.
Definition: btVector3.h:589
const btScalar & z() const
Return the z value.
Definition: btVector3.h:591
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
The btPolyhedralConvexShape is an internal interface class for polyhedral convex shapes.
virtual btScalar getMargin() const =0
The btBoxShape is a box primitive around the origin, its sides axis aligned with length specified by ...
Definition: btBoxShape.h:26
#define btFsels(a, b, c)
Definition: btScalar.h:581
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
btScalar getMarginNonVirtual() const
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
The btPolyhedralConvexAabbCachingShape adds aabb caching to the btPolyhedralConvexShape.
btMatrix3x3 absolute() const
Return the matrix with all values non negative.
Definition: btMatrix3x3.h:937
The btCylinderShape class implements a cylinder shape primitive, centered around the origin....
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
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:48
The btConvexHullShape implements an implicit convex hull of an array of vertices.
const btVector3 & getLocalScalingNV() const
void getNonvirtualAabb(const btTransform &trans, btVector3 &aabbMin, btVector3 &aabbMax, btScalar margin) const
The btConvexPointCloudShape implements an implicit convex hull of an array of vertices.
const btVector3 & getImplicitShapeDimensions() const
btVector3 localGetSupportVertexWithoutMarginNonVirtual(const btVector3 &vec) const
btVector3 m_vertices1[3]
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:573
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
btConvexShape()
not supported on IBM SDK, until we fix the alignment of btVector3