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  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  }
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  }
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 
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 () */
321 {
322  switch (m_shapeType)
323  {
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  }
335  {
336  btTriangleShape* triangleShape = (btTriangleShape*)this;
337  return triangleShape->getMarginNV ();
338  }
340  {
341  btCylinderShape* cylShape = (btCylinderShape*)this;
342  return cylShape->getMarginNV();
343  }
345  {
346  btConeShape* conShape = (btConeShape*)this;
347  return conShape->getMarginNV();
348  }
350  {
351  btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
352  return capsuleShape->getMarginNV();
353  }
355  /* fall through */
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__
374 void btConvexShape::getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
375 {
376  switch (m_shapeType)
377  {
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;
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  }
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 
415 
416  btVector3 tmp = t(sv);
417  aabbMax[i] = tmp[i]+margin;
418  vec[i] = btScalar(-1.);
420  aabbMin[i] = tmp[i]-margin;
421  }
422  }
423  break;
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;
440  {
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__
#define SIMD_EPSILON
Definition: btScalar.h:494
const btVector3 & getLocalScalingNV() const
#define BT_LARGE_FLOAT
Definition: btScalar.h:280
virtual btVector3 localGetSupportingVertex(const btVector3 &vec) const =0
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
void getNonvirtualAabb(const btTransform &trans, btVector3 &aabbMin, btVector3 &aabbMax, btScalar margin) const
static btVector3 convexHullSupport(const btVector3 &localDirOrg, const btVector3 *points, int numPoints, const btVector3 &localScaling)
The btCapsuleShape represents a capsule around the Y axis, there is also the btCapsuleShapeX aligned ...
btScalar btSqrt(btScalar y)
Definition: btScalar.h:418
#define btAssert(x)
Definition: btScalar.h:113
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:1001
btVector3 localGetSupportVertexWithoutMarginNonVirtual(const btVector3 &vec) const
The btSphereShape implements an implicit sphere, centered around a local origin with radius...
Definition: btSphereShape.h:22
btScalar getRadius() const
int getNumPoints() const
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3 &vec) const =0
btScalar getMarginNonVirtual() const
int getUpAxis() const
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:297
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:565
btVector3 * getUnscaledPoints()
virtual ~btConvexShape()
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...
btMatrix3x3 absolute() const
Return the matrix with all values non negative.
Definition: btMatrix3x3.h:980
int getUpAxis() const
The btConeShape implements a cone shape primitive, centered around the origin and aligned with the Y ...
Definition: btConeShape.h:23
void getAabbNonVirtual(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:563
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:561
The btPolyhedralConvexShape is an internal interface class for polyhedral convex shapes.
btVector3 localGetSupportVertexNonVirtual(const btVector3 &vec) const
virtual btScalar getMargin() const =0
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
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:543
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
btScalar getMarginNV() 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.
const btVector3 & getImplicitShapeDimensions() const
The btCylinderShape class implements a cylinder shape primitive, centered around the origin...
btVector3 dot3(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2) const
Definition: btVector3.h:719
btScalar getRadius() const
Definition: btSphereShape.h:47
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. ...
The btConvexPointCloudShape implements an implicit convex hull of an array of vertices.
btScalar getHalfHeight() const
btVector3 m_vertices1[3]
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:278
btConvexShape()
not supported on IBM SDK, until we fix the alignment of btVector3
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:475
virtual void project(const btTransform &trans, const btVector3 &dir, btScalar &minProj, btScalar &maxProj, btVector3 &witnesPtMin, btVector3 &witnesPtMax) const
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579