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 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 
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 
132 {
133  switch (m_shapeType)
134  {
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  }
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  }
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  }
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  }
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  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;
439  {
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__
btVector3 localGetSupportVertexNonVirtual(const btVector3 &vec) const
#define SIMD_EPSILON
Definition: btScalar.h:523
#define BT_LARGE_FLOAT
Definition: btScalar.h:296
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:640
btVector3 dot3(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2) const
Definition: btVector3.h:720
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:251
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:446
#define btAssert(x)
Definition: btScalar.h:133
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:563
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:303
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:565
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:998
btVector3 * getUnscaledPoints()
virtual ~btConvexShape()
btScalar getHalfHeight() const
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
void getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const =0
getAabb&#39;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:575
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:229
btScalar getRadius() const
Definition: btSphereShape.h:48
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:108
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:583
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
btScalar getMarginNonVirtual() const
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:28
The btPolyhedralConvexAabbCachingShape adds aabb caching to the btPolyhedralConvexShape.
btMatrix3x3 absolute() const
Return the matrix with all values non negative.
Definition: btMatrix3x3.h:1005
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:477
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:46
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:561
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:294
btConvexShape()
not supported on IBM SDK, until we fix the alignment of btVector3