src/BulletCollision/CollisionShapes/btConvexShape.cpp

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