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 newDot,maxDot = btScalar(-BT_LARGE_FLOAT);
 113         int ptIndex = -1;
 114
 115         for (int i=0;i<numPoints;i++)
 116         {
 117
 118                 newDot = vec.dot(points[i]);
 119                 if (newDot > maxDot)
 120                 {
 121                         maxDot = newDot;
 122                         ptIndex = i;
 123                 }
 124         }
 125         btAssert(ptIndex >= 0);
 126         btVector3 supVec = points[ptIndex] * localScaling;
 127         return supVec;
 128 #endif //__SPU__
 129 }
 130
 131 btVector3 btConvexShape::localGetSupportVertexWithoutMarginNonVirtual (const btVector3& localDir) const
 132 {
 133         switch (m_shapeType)
 134         {
 135     case SPHERE_SHAPE_PROXYTYPE:
 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                 return btVector3(btFsels(localDir.x(), halfExtents.x(), -halfExtents.x()),
 145                         btFsels(localDir.y(), halfExtents.y(), -halfExtents.y()),
 146                         btFsels(localDir.z(), halfExtents.z(), -halfExtents.z()));
 147         }
 148         case TRIANGLE_SHAPE_PROXYTYPE:
 149         {
 150                 btTriangleShape* triangleShape = (btTriangleShape*)this;
 151                 btVector3 dir(localDir.getX(),localDir.getY(),localDir.getZ());
 152                 btVector3* vertices = &triangleShape->m_vertices1[0];
 153                 btVector3 dots(dir.dot(vertices[0]), dir.dot(vertices[1]), dir.dot(vertices[2]));
 154                 btVector3 sup = vertices[dots.maxAxis()];
 155                 return btVector3(sup.getX(),sup.getY(),sup.getZ());
 156         }
 157         case CYLINDER_SHAPE_PROXYTYPE:
 158         {
 159                 btCylinderShape* cylShape = (btCylinderShape*)this;
 160                 //mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis)
 161
 162                 btVector3 halfExtents = cylShape->getImplicitShapeDimensions();
 163                 btVector3 v(localDir.getX(),localDir.getY(),localDir.getZ());
 164                 int cylinderUpAxis = cylShape->getUpAxis();
 165                 int XX(1),YY(0),ZZ(2);
 166
 167                 switch (cylinderUpAxis)
 168                 {
 169                 case 0:
 170                 {
 171                         XX = 1;
 172                         YY = 0;
 173                         ZZ = 2;
 174                 }
 175                 break;
 176                 case 1:
 177                 {
 178                         XX = 0;
 179                         YY = 1;
 180                         ZZ = 2;
 181                 }
 182                 break;
 183                 case 2:
 184                 {
 185                         XX = 0;
 186                         YY = 2;
 187                         ZZ = 1;
 188
 189                 }
 190                 break;
 191                 default:
 192                         btAssert(0);
 193                 break;
 194                 };
 195
 196                 btScalar radius = halfExtents[XX];
 197                 btScalar halfHeight = halfExtents[cylinderUpAxis];
 198
 199                 btVector3 tmp;
 200                 btScalar d ;
 201
 202                 btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
 203                 if (s != btScalar(0.0))
 204                 {
 205                         d = radius / s;
 206                         tmp[XX] = v[XX] * d;
 207                         tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
 208                         tmp[ZZ] = v[ZZ] * d;
 209                         return btVector3(tmp.getX(),tmp.getY(),tmp.getZ());
 210                 } else {
 211                         tmp[XX] = radius;
 212                         tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
 213                         tmp[ZZ] = btScalar(0.0);
 214                         return btVector3(tmp.getX(),tmp.getY(),tmp.getZ());
 215                 }
 216         }
 217         case CAPSULE_SHAPE_PROXYTYPE:
 218         {
 219                 btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ());
 220
 221                 btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
 222                 btScalar halfHeight = capsuleShape->getHalfHeight();
 223                 int capsuleUpAxis = capsuleShape->getUpAxis();
 224
 225                 btScalar radius = capsuleShape->getRadius();
 226                 btVector3 supVec(0,0,0);
 227
 228                 btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
 229
 230                 btVector3 vec = vec0;
 231                 btScalar lenSqr = vec.length2();
 232                 if (lenSqr < btScalar(0.0001))
 233                 {
 234                         vec.setValue(1,0,0);
 235                 } else
 236                 {
 237                         btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
 238                         vec *= rlen;
 239                 }
 240                 btVector3 vtx;
 241                 btScalar newDot;
 242                 {
 243                         btVector3 pos(0,0,0);
 244                         pos[capsuleUpAxis] = halfHeight;
 245
 246                         //vtx = pos +vec*(radius);
 247                         vtx = pos +vec*(radius) - vec * capsuleShape->getMarginNV();
 248                         newDot = vec.dot(vtx);
 249
 250
 251                         if (newDot > maxDot)
 252                         {
 253                                 maxDot = newDot;
 254                                 supVec = vtx;
 255                         }
 256                 }
 257                 {
 258                         btVector3 pos(0,0,0);
 259                         pos[capsuleUpAxis] = -halfHeight;
 260
 261                         //vtx = pos +vec*(radius);
 262                         vtx = pos +vec*(radius) - vec * capsuleShape->getMarginNV();
 263                         newDot = vec.dot(vtx);
 264                         if (newDot > maxDot)
 265                         {
 266                                 maxDot = newDot;
 267                                 supVec = vtx;
 268                         }
 269                 }
 270                 return btVector3(supVec.getX(),supVec.getY(),supVec.getZ());
 271         }
 272         case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
 273         {
 274                 btConvexPointCloudShape* convexPointCloudShape = (btConvexPointCloudShape*)this;
 275                 btVector3* points = convexPointCloudShape->getUnscaledPoints ();
 276                 int numPoints = convexPointCloudShape->getNumPoints ();
 277                 return convexHullSupport (localDir, points, numPoints,convexPointCloudShape->getLocalScalingNV());
 278         }
 279         case CONVEX_HULL_SHAPE_PROXYTYPE:
 280         {
 281                 btConvexHullShape* convexHullShape = (btConvexHullShape*)this;
 282                 btVector3* points = convexHullShape->getUnscaledPoints();
 283                 int numPoints = convexHullShape->getNumPoints ();
 284                 return convexHullSupport (localDir, points, numPoints,convexHullShape->getLocalScalingNV());
 285         }
 286     default:
 287 #ifndef __SPU__
 288                 return this->localGetSupportingVertexWithoutMargin (localDir);
 289 #else
 290                 btAssert (0);
 291 #endif
 292         }
 293
 294         // should never reach here
 295         btAssert (0);
 296         return btVector3 (btScalar(0.0f), btScalar(0.0f), btScalar(0.0f));
 297 }
 298
 299 btVector3 btConvexShape::localGetSupportVertexNonVirtual (const btVector3& localDir) const
 300 {
 301         btVector3 localDirNorm = localDir;
 302         if (localDirNorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
 303         {
 304                 localDirNorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.));
 305         }
 306         localDirNorm.normalize ();
 307
 308         return localGetSupportVertexWithoutMarginNonVirtual(localDirNorm)+ getMarginNonVirtual() * localDirNorm;
 309 }
 310
 311 /* TODO: This should be bumped up to btCollisionShape () */
 312 btScalar btConvexShape::getMarginNonVirtual () const
 313 {
 314         switch (m_shapeType)
 315         {
 316     case SPHERE_SHAPE_PROXYTYPE:
 317         {
 318                 btSphereShape* sphereShape = (btSphereShape*)this;
 319                 return sphereShape->getRadius ();
 320         }
 321         case BOX_SHAPE_PROXYTYPE:
 322         {
 323                 btBoxShape* convexShape = (btBoxShape*)this;
 324                 return convexShape->getMarginNV ();
 325         }
 326         case TRIANGLE_SHAPE_PROXYTYPE:
 327         {
 328                 btTriangleShape* triangleShape = (btTriangleShape*)this;
 329                 return triangleShape->getMarginNV ();
 330         }
 331         case CYLINDER_SHAPE_PROXYTYPE:
 332         {
 333                 btCylinderShape* cylShape = (btCylinderShape*)this;
 334                 return cylShape->getMarginNV();
 335         }
 336         case CAPSULE_SHAPE_PROXYTYPE:
 337         {
 338                 btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
 339                 return capsuleShape->getMarginNV();
 340         }
 341         case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
 342         /* fall through */
 343         case CONVEX_HULL_SHAPE_PROXYTYPE:
 344         {
 345                 btPolyhedralConvexShape* convexHullShape = (btPolyhedralConvexShape*)this;
 346                 return convexHullShape->getMarginNV();
 347         }
 348     default:
 349 #ifndef __SPU__
 350                 return this->getMargin ();
 351 #else
 352                 btAssert (0);
 353 #endif
 354         }
 355
 356         // should never reach here
 357         btAssert (0);
 358         return btScalar(0.0f);
 359 }
 360 #ifndef __SPU__
 361 void btConvexShape::getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
 362 {
 363         switch (m_shapeType)
 364         {
 365     case SPHERE_SHAPE_PROXYTYPE:
 366         {
 367                 btSphereShape* sphereShape = (btSphereShape*)this;
 368                 btScalar radius = sphereShape->getImplicitShapeDimensions().getX();// * convexShape->getLocalScaling().getX();
 369                 btScalar margin = radius + sphereShape->getMarginNonVirtual();
 370                 const btVector3& center = t.getOrigin();
 371                 btVector3 extent(margin,margin,margin);
 372                 aabbMin = center - extent;
 373                 aabbMax = center + extent;
 374     }
 375         break;
 376         case CYLINDER_SHAPE_PROXYTYPE:
 377         /* fall through */
 378         case BOX_SHAPE_PROXYTYPE:
 379         {
 380                 btBoxShape* convexShape = (btBoxShape*)this;
 381                 btScalar margin=convexShape->getMarginNonVirtual();
 382                 btVector3 halfExtents = convexShape->getImplicitShapeDimensions();
 383                 halfExtents += btVector3(margin,margin,margin);
 384                 btMatrix3x3 abs_b = t.getBasis().absolute();
 385                 btVector3 center = t.getOrigin();
 386                 btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents));
 387
 388                 aabbMin = center - extent;
 389                 aabbMax = center + extent;
 390                 break;
 391         }
 392         case TRIANGLE_SHAPE_PROXYTYPE:
 393         {
 394                 btTriangleShape* triangleShape = (btTriangleShape*)this;
 395                 btScalar margin = triangleShape->getMarginNonVirtual();
 396                 for (int i=0;i<3;i++)
 397                 {
 398                         btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.));
 399                         vec[i] = btScalar(1.);
 400
 401                         btVector3 sv = localGetSupportVertexWithoutMarginNonVirtual(vec*t.getBasis());
 402
 403                         btVector3 tmp = t(sv);
 404                         aabbMax[i] = tmp[i]+margin;
 405                         vec[i] = btScalar(-1.);
 406                         tmp = t(localGetSupportVertexWithoutMarginNonVirtual(vec*t.getBasis()));
 407                         aabbMin[i] = tmp[i]-margin;
 408                 }
 409         }
 410         break;
 411         case CAPSULE_SHAPE_PROXYTYPE:
 412         {
 413                 btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
 414                 btVector3 halfExtents(capsuleShape->getRadius(),capsuleShape->getRadius(),capsuleShape->getRadius());
 415                 int m_upAxis = capsuleShape->getUpAxis();
 416                 halfExtents[m_upAxis] = capsuleShape->getRadius() + capsuleShape->getHalfHeight();
 417                 halfExtents += btVector3(capsuleShape->getMarginNonVirtual(),capsuleShape->getMarginNonVirtual(),capsuleShape->getMarginNonVirtual());
 418                 btMatrix3x3 abs_b = t.getBasis().absolute();
 419                 btVector3 center = t.getOrigin();
 420                 btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents));
 421                 aabbMin = center - extent;
 422                 aabbMax = center + extent;
 423         }
 424         break;
 425         case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
 426         case CONVEX_HULL_SHAPE_PROXYTYPE:
 427         {
 428                 btPolyhedralConvexAabbCachingShape* convexHullShape = (btPolyhedralConvexAabbCachingShape*)this;
 429                 btScalar margin = convexHullShape->getMarginNonVirtual();
 430                 convexHullShape->getNonvirtualAabb (t, aabbMin, aabbMax, margin);
 431         }
 432         break;
 433     default:
 434 #ifndef __SPU__
 435                 this->getAabb (t, aabbMin, aabbMax);
 436 #else
 437                 btAssert (0);
 438 #endif
 439         break;
 440         }
 441
 442         // should never reach here
 443         btAssert (0);
 444 }
 445
 446 #endif //__SPU__