dali-physics/third-party/bullet3/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.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 #if defined(_WIN32) || defined(__i386__)
  16 #define BT_USE_SSE_IN_API
  17 #endif
  18
  19 #include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
  20 #include "btConvexPolyhedron.h"
  21 #include "LinearMath/btConvexHullComputer.h"
  22 #include <new>
  23 #include "LinearMath/btGeometryUtil.h"
  24 #include "LinearMath/btGrahamScan2dConvexHull.h"
  25
  26 btPolyhedralConvexShape::btPolyhedralConvexShape() : btConvexInternalShape(),
  27                                                                                                          m_polyhedron(0)
  28 {
  29 }
  30
  31 btPolyhedralConvexShape::~btPolyhedralConvexShape()
  32 {
  33         if (m_polyhedron)
  34         {
  35                 m_polyhedron->~btConvexPolyhedron();
  36                 btAlignedFree(m_polyhedron);
  37         }
  38 }
  39
  40 void btPolyhedralConvexShape::setPolyhedralFeatures(btConvexPolyhedron& polyhedron)
  41 {
  42         if (m_polyhedron)
  43         {
  44                 *m_polyhedron = polyhedron;
  45         }
  46         else
  47         {
  48                 void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron), 16);
  49                 m_polyhedron = new (mem) btConvexPolyhedron(polyhedron);
  50         }
  51 }
  52
  53 bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMargin)
  54 {
  55         if (m_polyhedron)
  56         {
  57                 m_polyhedron->~btConvexPolyhedron();
  58                 btAlignedFree(m_polyhedron);
  59         }
  60
  61         void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron), 16);
  62         m_polyhedron = new (mem) btConvexPolyhedron;
  63
  64         btAlignedObjectArray<btVector3> orgVertices;
  65
  66         for (int i = 0; i < getNumVertices(); i++)
  67         {
  68                 btVector3& newVertex = orgVertices.expand();
  69                 getVertex(i, newVertex);
  70         }
  71
  72         btConvexHullComputer conv;
  73
  74         if (shiftVerticesByMargin)
  75         {
  76                 btAlignedObjectArray<btVector3> planeEquations;
  77                 btGeometryUtil::getPlaneEquationsFromVertices(orgVertices, planeEquations);
  78
  79                 btAlignedObjectArray<btVector3> shiftedPlaneEquations;
  80                 for (int p = 0; p < planeEquations.size(); p++)
  81                 {
  82                         btVector3 plane = planeEquations[p];
  83                         //         btScalar margin = getMargin();
  84                         plane[3] -= getMargin();
  85                         shiftedPlaneEquations.push_back(plane);
  86                 }
  87
  88                 btAlignedObjectArray<btVector3> tmpVertices;
  89
  90                 btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations, tmpVertices);
  91
  92                 conv.compute(&tmpVertices[0].getX(), sizeof(btVector3), tmpVertices.size(), 0.f, 0.f);
  93         }
  94         else
  95         {
  96                 conv.compute(&orgVertices[0].getX(), sizeof(btVector3), orgVertices.size(), 0.f, 0.f);
  97         }
  98
  99 #ifndef BT_RECONSTRUCT_FACES
 100
 101         int numVertices = conv.vertices.size();
 102         m_polyhedron->m_vertices.resize(numVertices);
 103         for (int p = 0; p < numVertices; p++)
 104         {
 105                 m_polyhedron->m_vertices[p] = conv.vertices[p];
 106         }
 107
 108         int v0, v1;
 109         for (int j = 0; j < conv.faces.size(); j++)
 110         {
 111                 btVector3 edges[3];
 112                 int numEdges = 0;
 113                 btFace combinedFace;
 114                 const btConvexHullComputer::Edge* edge = &conv.edges[conv.faces[j]];
 115                 v0 = edge->getSourceVertex();
 116                 int prevVertex = v0;
 117                 combinedFace.m_indices.push_back(v0);
 118                 v1 = edge->getTargetVertex();
 119                 while (v1 != v0)
 120                 {
 121                         btVector3 wa = conv.vertices[prevVertex];
 122                         btVector3 wb = conv.vertices[v1];
 123                         btVector3 newEdge = wb - wa;
 124                         newEdge.normalize();
 125                         if (numEdges < 2)
 126                                 edges[numEdges++] = newEdge;
 127
 128                         //face->addIndex(v1);
 129                         combinedFace.m_indices.push_back(v1);
 130                         edge = edge->getNextEdgeOfFace();
 131                         prevVertex = v1;
 132                         int v01 = edge->getSourceVertex();
 133                         v1 = edge->getTargetVertex();
 134                 }
 135
 136                 btAssert(combinedFace.m_indices.size() > 2);
 137
 138                 btVector3 faceNormal = edges[0].cross(edges[1]);
 139                 faceNormal.normalize();
 140
 141                 btScalar planeEq = 1e30f;
 142
 143                 for (int v = 0; v < combinedFace.m_indices.size(); v++)
 144                 {
 145                         btScalar eq = m_polyhedron->m_vertices[combinedFace.m_indices[v]].dot(faceNormal);
 146                         if (planeEq > eq)
 147                         {
 148                                 planeEq = eq;
 149                         }
 150                 }
 151                 combinedFace.m_plane[0] = faceNormal.getX();
 152                 combinedFace.m_plane[1] = faceNormal.getY();
 153                 combinedFace.m_plane[2] = faceNormal.getZ();
 154                 combinedFace.m_plane[3] = -planeEq;
 155
 156                 m_polyhedron->m_faces.push_back(combinedFace);
 157         }
 158
 159 #else  //BT_RECONSTRUCT_FACES
 160
 161         btAlignedObjectArray<btVector3> faceNormals;
 162         int numFaces = conv.faces.size();
 163         faceNormals.resize(numFaces);
 164         btConvexHullComputer* convexUtil = &conv;
 165
 166         btAlignedObjectArray<btFace> tmpFaces;
 167         tmpFaces.resize(numFaces);
 168
 169         int numVertices = convexUtil->vertices.size();
 170         m_polyhedron->m_vertices.resize(numVertices);
 171         for (int p = 0; p < numVertices; p++)
 172         {
 173                 m_polyhedron->m_vertices[p] = convexUtil->vertices[p];
 174         }
 175
 176         for (int i = 0; i < numFaces; i++)
 177         {
 178                 int face = convexUtil->faces[i];
 179                 //printf("face=%d\n",face);
 180                 const btConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face];
 181                 const btConvexHullComputer::Edge* edge = firstEdge;
 182
 183                 btVector3 edges[3];
 184                 int numEdges = 0;
 185                 //compute face normals
 186
 187                 do
 188                 {
 189                         int src = edge->getSourceVertex();
 190                         tmpFaces[i].m_indices.push_back(src);
 191                         int targ = edge->getTargetVertex();
 192                         btVector3 wa = convexUtil->vertices[src];
 193
 194                         btVector3 wb = convexUtil->vertices[targ];
 195                         btVector3 newEdge = wb - wa;
 196                         newEdge.normalize();
 197                         if (numEdges < 2)
 198                                 edges[numEdges++] = newEdge;
 199
 200                         edge = edge->getNextEdgeOfFace();
 201                 } while (edge != firstEdge);
 202
 203                 btScalar planeEq = 1e30f;
 204
 205                 if (numEdges == 2)
 206                 {
 207                         faceNormals[i] = edges[0].cross(edges[1]);
 208                         faceNormals[i].normalize();
 209                         tmpFaces[i].m_plane[0] = faceNormals[i].getX();
 210                         tmpFaces[i].m_plane[1] = faceNormals[i].getY();
 211                         tmpFaces[i].m_plane[2] = faceNormals[i].getZ();
 212                         tmpFaces[i].m_plane[3] = planeEq;
 213                 }
 214                 else
 215                 {
 216                         btAssert(0);  //degenerate?
 217                         faceNormals[i].setZero();
 218                 }
 219
 220                 for (int v = 0; v < tmpFaces[i].m_indices.size(); v++)
 221                 {
 222                         btScalar eq = m_polyhedron->m_vertices[tmpFaces[i].m_indices[v]].dot(faceNormals[i]);
 223                         if (planeEq > eq)
 224                         {
 225                                 planeEq = eq;
 226                         }
 227                 }
 228                 tmpFaces[i].m_plane[3] = -planeEq;
 229         }
 230
 231         //merge coplanar faces and copy them to m_polyhedron
 232
 233         btScalar faceWeldThreshold = 0.999f;
 234         btAlignedObjectArray<int> todoFaces;
 235         for (int i = 0; i < tmpFaces.size(); i++)
 236                 todoFaces.push_back(i);
 237
 238         while (todoFaces.size())
 239         {
 240                 btAlignedObjectArray<int> coplanarFaceGroup;
 241                 int refFace = todoFaces[todoFaces.size() - 1];
 242
 243                 coplanarFaceGroup.push_back(refFace);
 244                 btFace& faceA = tmpFaces[refFace];
 245                 todoFaces.pop_back();
 246
 247                 btVector3 faceNormalA(faceA.m_plane[0], faceA.m_plane[1], faceA.m_plane[2]);
 248                 for (int j = todoFaces.size() - 1; j >= 0; j--)
 249                 {
 250                         int i = todoFaces[j];
 251                         btFace& faceB = tmpFaces[i];
 252                         btVector3 faceNormalB(faceB.m_plane[0], faceB.m_plane[1], faceB.m_plane[2]);
 253                         if (faceNormalA.dot(faceNormalB) > faceWeldThreshold)
 254                         {
 255                                 coplanarFaceGroup.push_back(i);
 256                                 todoFaces.remove(i);
 257                         }
 258                 }
 259
 260                 bool did_merge = false;
 261                 if (coplanarFaceGroup.size() > 1)
 262                 {
 263                         //do the merge: use Graham Scan 2d convex hull
 264
 265                         btAlignedObjectArray<GrahamVector3> orgpoints;
 266                         btVector3 averageFaceNormal(0, 0, 0);
 267
 268                         for (int i = 0; i < coplanarFaceGroup.size(); i++)
 269                         {
 270                                 //                              m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]);
 271
 272                                 btFace& face = tmpFaces[coplanarFaceGroup[i]];
 273                                 btVector3 faceNormal(face.m_plane[0], face.m_plane[1], face.m_plane[2]);
 274                                 averageFaceNormal += faceNormal;
 275                                 for (int f = 0; f < face.m_indices.size(); f++)
 276                                 {
 277                                         int orgIndex = face.m_indices[f];
 278                                         btVector3 pt = m_polyhedron->m_vertices[orgIndex];
 279
 280                                         bool found = false;
 281
 282                                         for (int i = 0; i < orgpoints.size(); i++)
 283                                         {
 284                                                 //if ((orgpoints[i].m_orgIndex == orgIndex) || ((rotatedPt-orgpoints[i]).length2()<0.0001))
 285                                                 if (orgpoints[i].m_orgIndex == orgIndex)
 286                                                 {
 287                                                         found = true;
 288                                                         break;
 289                                                 }
 290                                         }
 291                                         if (!found)
 292                                                 orgpoints.push_back(GrahamVector3(pt, orgIndex));
 293                                 }
 294                         }
 295
 296                         btFace combinedFace;
 297                         for (int i = 0; i < 4; i++)
 298                                 combinedFace.m_plane[i] = tmpFaces[coplanarFaceGroup[0]].m_plane[i];
 299
 300                         btAlignedObjectArray<GrahamVector3> hull;
 301
 302                         averageFaceNormal.normalize();
 303                         GrahamScanConvexHull2D(orgpoints, hull, averageFaceNormal);
 304
 305                         for (int i = 0; i < hull.size(); i++)
 306                         {
 307                                 combinedFace.m_indices.push_back(hull[i].m_orgIndex);
 308                                 for (int k = 0; k < orgpoints.size(); k++)
 309                                 {
 310                                         if (orgpoints[k].m_orgIndex == hull[i].m_orgIndex)
 311                                         {
 312                                                 orgpoints[k].m_orgIndex = -1;  // invalidate...
 313                                                 break;
 314                                         }
 315                                 }
 316                         }
 317
 318                         // are there rejected vertices?
 319                         bool reject_merge = false;
 320
 321                         for (int i = 0; i < orgpoints.size(); i++)
 322                         {
 323                                 if (orgpoints[i].m_orgIndex == -1)
 324                                         continue;  // this is in the hull...
 325                                 // this vertex is rejected -- is anybody else using this vertex?
 326                                 for (int j = 0; j < tmpFaces.size(); j++)
 327                                 {
 328                                         btFace& face = tmpFaces[j];
 329                                         // is this a face of the current coplanar group?
 330                                         bool is_in_current_group = false;
 331                                         for (int k = 0; k < coplanarFaceGroup.size(); k++)
 332                                         {
 333                                                 if (coplanarFaceGroup[k] == j)
 334                                                 {
 335                                                         is_in_current_group = true;
 336                                                         break;
 337                                                 }
 338                                         }
 339                                         if (is_in_current_group)  // ignore this face...
 340                                                 continue;
 341                                         // does this face use this rejected vertex?
 342                                         for (int v = 0; v < face.m_indices.size(); v++)
 343                                         {
 344                                                 if (face.m_indices[v] == orgpoints[i].m_orgIndex)
 345                                                 {
 346                                                         // this rejected vertex is used in another face -- reject merge
 347                                                         reject_merge = true;
 348                                                         break;
 349                                                 }
 350                                         }
 351                                         if (reject_merge)
 352                                                 break;
 353                                 }
 354                                 if (reject_merge)
 355                                         break;
 356                         }
 357
 358                         if (!reject_merge)
 359                         {
 360                                 // do this merge!
 361                                 did_merge = true;
 362                                 m_polyhedron->m_faces.push_back(combinedFace);
 363                         }
 364                 }
 365                 if (!did_merge)
 366                 {
 367                         for (int i = 0; i < coplanarFaceGroup.size(); i++)
 368                         {
 369                                 btFace face = tmpFaces[coplanarFaceGroup[i]];
 370                                 m_polyhedron->m_faces.push_back(face);
 371                         }
 372                 }
 373         }
 374
 375 #endif  //BT_RECONSTRUCT_FACES
 376
 377         m_polyhedron->initialize();
 378
 379         return true;
 380 }
 381
 382 #ifndef MIN
 383 #define MIN(_a, _b) ((_a) < (_b) ? (_a) : (_b))
 384 #endif
 385
 386 btVector3 btPolyhedralConvexShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0) const
 387 {
 388         btVector3 supVec(0, 0, 0);
 389 #ifndef __SPU__
 390         int i;
 391         btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
 392
 393         btVector3 vec = vec0;
 394         btScalar lenSqr = vec.length2();
 395         if (lenSqr < btScalar(0.0001))
 396         {
 397                 vec.setValue(1, 0, 0);
 398         }
 399         else
 400         {
 401                 btScalar rlen = btScalar(1.) / btSqrt(lenSqr);
 402                 vec *= rlen;
 403         }
 404
 405         btVector3 vtx;
 406         btScalar newDot;
 407
 408         for (int k = 0; k < getNumVertices(); k += 128)
 409         {
 410                 btVector3 temp[128];
 411                 int inner_count = MIN(getNumVertices() - k, 128);
 412                 for (i = 0; i < inner_count; i++)
 413                         getVertex(i, temp[i]);
 414                 i = (int)vec.maxDot(temp, inner_count, newDot);
 415                 if (newDot > maxDot)
 416                 {
 417                         maxDot = newDot;
 418                         supVec = temp[i];
 419                 }
 420         }
 421
 422 #endif  //__SPU__
 423         return supVec;
 424 }
 425
 426 void btPolyhedralConvexShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
 427 {
 428 #ifndef __SPU__
 429         int i;
 430
 431         btVector3 vtx;
 432         btScalar newDot;
 433
 434         for (i = 0; i < numVectors; i++)
 435         {
 436                 supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT);
 437         }
 438
 439         for (int j = 0; j < numVectors; j++)
 440         {
 441                 const btVector3& vec = vectors[j];
 442
 443                 for (int k = 0; k < getNumVertices(); k += 128)
 444                 {
 445                         btVector3 temp[128];
 446                         int inner_count = MIN(getNumVertices() - k, 128);
 447                         for (i = 0; i < inner_count; i++)
 448                                 getVertex(i, temp[i]);
 449                         i = (int)vec.maxDot(temp, inner_count, newDot);
 450                         if (newDot > supportVerticesOut[j][3])
 451                         {
 452                                 supportVerticesOut[j] = temp[i];
 453                                 supportVerticesOut[j][3] = newDot;
 454                         }
 455                 }
 456         }
 457
 458 #endif  //__SPU__
 459 }
 460
 461 void btPolyhedralConvexShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const
 462 {
 463 #ifndef __SPU__
 464         //not yet, return box inertia
 465
 466         btScalar margin = getMargin();
 467
 468         btTransform ident;
 469         ident.setIdentity();
 470         btVector3 aabbMin, aabbMax;
 471         getAabb(ident, aabbMin, aabbMax);
 472         btVector3 halfExtents = (aabbMax - aabbMin) * btScalar(0.5);
 473
 474         btScalar lx = btScalar(2.) * (halfExtents.x() + margin);
 475         btScalar ly = btScalar(2.) * (halfExtents.y() + margin);
 476         btScalar lz = btScalar(2.) * (halfExtents.z() + margin);
 477         const btScalar x2 = lx * lx;
 478         const btScalar y2 = ly * ly;
 479         const btScalar z2 = lz * lz;
 480         const btScalar scaledmass = mass * btScalar(0.08333333);
 481
 482         inertia = scaledmass * (btVector3(y2 + z2, x2 + z2, x2 + y2));
 483 #endif  //__SPU__
 484 }
 485
 486 void btPolyhedralConvexAabbCachingShape::setLocalScaling(const btVector3& scaling)
 487 {
 488         btConvexInternalShape::setLocalScaling(scaling);
 489         recalcLocalAabb();
 490 }
 491
 492 btPolyhedralConvexAabbCachingShape::btPolyhedralConvexAabbCachingShape()
 493         : btPolyhedralConvexShape(),
 494           m_localAabbMin(1, 1, 1),
 495           m_localAabbMax(-1, -1, -1),
 496           m_isLocalAabbValid(false)
 497 {
 498 }
 499
 500 void btPolyhedralConvexAabbCachingShape::getAabb(const btTransform& trans, btVector3& aabbMin, btVector3& aabbMax) const
 501 {
 502         getNonvirtualAabb(trans, aabbMin, aabbMax, getMargin());
 503 }
 504
 505 void btPolyhedralConvexAabbCachingShape::recalcLocalAabb()
 506 {
 507         m_isLocalAabbValid = true;
 508
 509 #if 1
 510         static const btVector3 _directions[] =
 511                 {
 512                         btVector3(1., 0., 0.),
 513                         btVector3(0., 1., 0.),
 514                         btVector3(0., 0., 1.),
 515                         btVector3(-1., 0., 0.),
 516                         btVector3(0., -1., 0.),
 517                         btVector3(0., 0., -1.)};
 518
 519         btVector3 _supporting[] =
 520                 {
 521                         btVector3(0., 0., 0.),
 522                         btVector3(0., 0., 0.),
 523                         btVector3(0., 0., 0.),
 524                         btVector3(0., 0., 0.),
 525                         btVector3(0., 0., 0.),
 526                         btVector3(0., 0., 0.)};
 527
 528         batchedUnitVectorGetSupportingVertexWithoutMargin(_directions, _supporting, 6);
 529
 530         for (int i = 0; i < 3; ++i)
 531         {
 532                 m_localAabbMax[i] = _supporting[i][i] + m_collisionMargin;
 533                 m_localAabbMin[i] = _supporting[i + 3][i] - m_collisionMargin;
 534         }
 535
 536 #else
 537
 538         for (int i = 0; i < 3; i++)
 539         {
 540                 btVector3 vec(btScalar(0.), btScalar(0.), btScalar(0.));
 541                 vec[i] = btScalar(1.);
 542                 btVector3 tmp = localGetSupportingVertex(vec);
 543                 m_localAabbMax[i] = tmp[i];
 544                 vec[i] = btScalar(-1.);
 545                 tmp = localGetSupportingVertex(vec);
 546                 m_localAabbMin[i] = tmp[i];
 547         }
 548 #endif
 549 }