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