dali-physics/third-party/bullet3/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 #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
  29 ///not supported on IBM SDK, until we fix the alignment of btVector3
  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 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
 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 #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                 }
 158                 case TRIANGLE_SHAPE_PROXYTYPE:
 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                 }
 167                 case CYLINDER_SHAPE_PROXYTYPE:
 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                 }
 228                 case CAPSULE_SHAPE_PROXYTYPE:
 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                 }
 280                 case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
 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                 }
 287                 case CONVEX_HULL_SHAPE_PROXYTYPE:
 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
 307 btVector3 btConvexShape::localGetSupportVertexNonVirtual(const btVector3& localDir) const
 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 () */
 320 btScalar btConvexShape::getMarginNonVirtual() const
 321 {
 322         switch (m_shapeType)
 323         {
 324                 case SPHERE_SHAPE_PROXYTYPE:
 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                 }
 334                 case TRIANGLE_SHAPE_PROXYTYPE:
 335                 {
 336                         btTriangleShape* triangleShape = (btTriangleShape*)this;
 337                         return triangleShape->getMarginNV();
 338                 }
 339                 case CYLINDER_SHAPE_PROXYTYPE:
 340                 {
 341                         btCylinderShape* cylShape = (btCylinderShape*)this;
 342                         return cylShape->getMarginNV();
 343                 }
 344                 case CONE_SHAPE_PROXYTYPE:
 345                 {
 346                         btConeShape* conShape = (btConeShape*)this;
 347                         return conShape->getMarginNV();
 348                 }
 349                 case CAPSULE_SHAPE_PROXYTYPE:
 350                 {
 351                         btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
 352                         return capsuleShape->getMarginNV();
 353                 }
 354                 case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
 355                 /* fall through */
 356                 case CONVEX_HULL_SHAPE_PROXYTYPE:
 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         {
 378                 case SPHERE_SHAPE_PROXYTYPE:
 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;
 389                 case CYLINDER_SHAPE_PROXYTYPE:
 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                 }
 405                 case TRIANGLE_SHAPE_PROXYTYPE:
 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
 414                                 btVector3 sv = localGetSupportVertexWithoutMarginNonVirtual(vec * t.getBasis());
 415
 416                                 btVector3 tmp = t(sv);
 417                                 aabbMax[i] = tmp[i] + margin;
 418                                 vec[i] = btScalar(-1.);
 419                                 tmp = t(localGetSupportVertexWithoutMarginNonVirtual(vec * t.getBasis()));
 420                                 aabbMin[i] = tmp[i] - margin;
 421                         }
 422                 }
 423                 break;
 424                 case CAPSULE_SHAPE_PROXYTYPE:
 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;
 437                 case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
 438                 case CONVEX_HULL_SHAPE_PROXYTYPE:
 439                 {
 440                         btPolyhedralConvexAabbCachingShape* convexHullShape = (btPolyhedralConvexAabbCachingShape*)this;
 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__