dali-physics/third-party/bullet3/src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.cpp

   1 /*
   2 Copyright (c) 2012 Advanced Micro Devices, Inc.
   3
   4 This software is provided 'as-is', without any express or implied warranty.
   5 In no event will the authors be held liable for any damages arising from the use of this software.
   6 Permission is granted to anyone to use this software for any purpose,
   7 including commercial applications, and to alter it and redistribute it freely,
   8 subject to the following restrictions:
   9
  10 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.
  11 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  12 3. This notice may not be removed or altered from any source distribution.
  13 */
  14 //Originally written by Erwin Coumans
  15
  16 #include "b3ConvexUtility.h"
  17 #include "Bullet3Geometry/b3ConvexHullComputer.h"
  18 #include "Bullet3Geometry/b3GrahamScan2dConvexHull.h"
  19 #include "Bullet3Common/b3Quaternion.h"
  20 #include "Bullet3Common/b3HashMap.h"
  21
  22 b3ConvexUtility::~b3ConvexUtility()
  23 {
  24 }
  25
  26 bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices, int numPoints, bool mergeCoplanarTriangles)
  27 {
  28         b3ConvexHullComputer conv;
  29         conv.compute(&orgVertices[0].getX(), sizeof(b3Vector3), numPoints, 0.f, 0.f);
  30
  31         b3AlignedObjectArray<b3Vector3> faceNormals;
  32         int numFaces = conv.faces.size();
  33         faceNormals.resize(numFaces);
  34         b3ConvexHullComputer* convexUtil = &conv;
  35
  36         b3AlignedObjectArray<b3MyFace> tmpFaces;
  37         tmpFaces.resize(numFaces);
  38
  39         int numVertices = convexUtil->vertices.size();
  40         m_vertices.resize(numVertices);
  41         for (int p = 0; p < numVertices; p++)
  42         {
  43                 m_vertices[p] = convexUtil->vertices[p];
  44         }
  45
  46         for (int i = 0; i < numFaces; i++)
  47         {
  48                 int face = convexUtil->faces[i];
  49                 //printf("face=%d\n",face);
  50                 const b3ConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face];
  51                 const b3ConvexHullComputer::Edge* edge = firstEdge;
  52
  53                 b3Vector3 edges[3];
  54                 int numEdges = 0;
  55                 //compute face normals
  56
  57                 do
  58                 {
  59                         int src = edge->getSourceVertex();
  60                         tmpFaces[i].m_indices.push_back(src);
  61                         int targ = edge->getTargetVertex();
  62                         b3Vector3 wa = convexUtil->vertices[src];
  63
  64                         b3Vector3 wb = convexUtil->vertices[targ];
  65                         b3Vector3 newEdge = wb - wa;
  66                         newEdge.normalize();
  67                         if (numEdges < 2)
  68                                 edges[numEdges++] = newEdge;
  69
  70                         edge = edge->getNextEdgeOfFace();
  71                 } while (edge != firstEdge);
  72
  73                 b3Scalar planeEq = 1e30f;
  74
  75                 if (numEdges == 2)
  76                 {
  77                         faceNormals[i] = edges[0].cross(edges[1]);
  78                         faceNormals[i].normalize();
  79                         tmpFaces[i].m_plane[0] = faceNormals[i].getX();
  80                         tmpFaces[i].m_plane[1] = faceNormals[i].getY();
  81                         tmpFaces[i].m_plane[2] = faceNormals[i].getZ();
  82                         tmpFaces[i].m_plane[3] = planeEq;
  83                 }
  84                 else
  85                 {
  86                         b3Assert(0);  //degenerate?
  87                         faceNormals[i].setZero();
  88                 }
  89
  90                 for (int v = 0; v < tmpFaces[i].m_indices.size(); v++)
  91                 {
  92                         b3Scalar eq = m_vertices[tmpFaces[i].m_indices[v]].dot(faceNormals[i]);
  93                         if (planeEq > eq)
  94                         {
  95                                 planeEq = eq;
  96                         }
  97                 }
  98                 tmpFaces[i].m_plane[3] = -planeEq;
  99         }
 100
 101         //merge coplanar faces and copy them to m_polyhedron
 102
 103         b3Scalar faceWeldThreshold = 0.999f;
 104         b3AlignedObjectArray<int> todoFaces;
 105         for (int i = 0; i < tmpFaces.size(); i++)
 106                 todoFaces.push_back(i);
 107
 108         while (todoFaces.size())
 109         {
 110                 b3AlignedObjectArray<int> coplanarFaceGroup;
 111                 int refFace = todoFaces[todoFaces.size() - 1];
 112
 113                 coplanarFaceGroup.push_back(refFace);
 114                 b3MyFace& faceA = tmpFaces[refFace];
 115                 todoFaces.pop_back();
 116
 117                 b3Vector3 faceNormalA = b3MakeVector3(faceA.m_plane[0], faceA.m_plane[1], faceA.m_plane[2]);
 118                 for (int j = todoFaces.size() - 1; j >= 0; j--)
 119                 {
 120                         int i = todoFaces[j];
 121                         b3MyFace& faceB = tmpFaces[i];
 122                         b3Vector3 faceNormalB = b3MakeVector3(faceB.m_plane[0], faceB.m_plane[1], faceB.m_plane[2]);
 123                         if (faceNormalA.dot(faceNormalB) > faceWeldThreshold)
 124                         {
 125                                 coplanarFaceGroup.push_back(i);
 126                                 todoFaces.remove(i);
 127                         }
 128                 }
 129
 130                 bool did_merge = false;
 131                 if (coplanarFaceGroup.size() > 1)
 132                 {
 133                         //do the merge: use Graham Scan 2d convex hull
 134
 135                         b3AlignedObjectArray<b3GrahamVector3> orgpoints;
 136                         b3Vector3 averageFaceNormal = b3MakeVector3(0, 0, 0);
 137
 138                         for (int i = 0; i < coplanarFaceGroup.size(); i++)
 139                         {
 140                                 //                              m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]);
 141
 142                                 b3MyFace& face = tmpFaces[coplanarFaceGroup[i]];
 143                                 b3Vector3 faceNormal = b3MakeVector3(face.m_plane[0], face.m_plane[1], face.m_plane[2]);
 144                                 averageFaceNormal += faceNormal;
 145                                 for (int f = 0; f < face.m_indices.size(); f++)
 146                                 {
 147                                         int orgIndex = face.m_indices[f];
 148                                         b3Vector3 pt = m_vertices[orgIndex];
 149
 150                                         bool found = false;
 151
 152                                         for (int i = 0; i < orgpoints.size(); i++)
 153                                         {
 154                                                 //if ((orgpoints[i].m_orgIndex == orgIndex) || ((rotatedPt-orgpoints[i]).length2()<0.0001))
 155                                                 if (orgpoints[i].m_orgIndex == orgIndex)
 156                                                 {
 157                                                         found = true;
 158                                                         break;
 159                                                 }
 160                                         }
 161                                         if (!found)
 162                                                 orgpoints.push_back(b3GrahamVector3(pt, orgIndex));
 163                                 }
 164                         }
 165
 166                         b3MyFace combinedFace;
 167                         for (int i = 0; i < 4; i++)
 168                                 combinedFace.m_plane[i] = tmpFaces[coplanarFaceGroup[0]].m_plane[i];
 169
 170                         b3AlignedObjectArray<b3GrahamVector3> hull;
 171
 172                         averageFaceNormal.normalize();
 173                         b3GrahamScanConvexHull2D(orgpoints, hull, averageFaceNormal);
 174
 175                         for (int i = 0; i < hull.size(); i++)
 176                         {
 177                                 combinedFace.m_indices.push_back(hull[i].m_orgIndex);
 178                                 for (int k = 0; k < orgpoints.size(); k++)
 179                                 {
 180                                         if (orgpoints[k].m_orgIndex == hull[i].m_orgIndex)
 181                                         {
 182                                                 orgpoints[k].m_orgIndex = -1;  // invalidate...
 183                                                 break;
 184                                         }
 185                                 }
 186                         }
 187
 188                         // are there rejected vertices?
 189                         bool reject_merge = false;
 190
 191                         for (int i = 0; i < orgpoints.size(); i++)
 192                         {
 193                                 if (orgpoints[i].m_orgIndex == -1)
 194                                         continue;  // this is in the hull...
 195                                 // this vertex is rejected -- is anybody else using this vertex?
 196                                 for (int j = 0; j < tmpFaces.size(); j++)
 197                                 {
 198                                         b3MyFace& face = tmpFaces[j];
 199                                         // is this a face of the current coplanar group?
 200                                         bool is_in_current_group = false;
 201                                         for (int k = 0; k < coplanarFaceGroup.size(); k++)
 202                                         {
 203                                                 if (coplanarFaceGroup[k] == j)
 204                                                 {
 205                                                         is_in_current_group = true;
 206                                                         break;
 207                                                 }
 208                                         }
 209                                         if (is_in_current_group)  // ignore this face...
 210                                                 continue;
 211                                         // does this face use this rejected vertex?
 212                                         for (int v = 0; v < face.m_indices.size(); v++)
 213                                         {
 214                                                 if (face.m_indices[v] == orgpoints[i].m_orgIndex)
 215                                                 {
 216                                                         // this rejected vertex is used in another face -- reject merge
 217                                                         reject_merge = true;
 218                                                         break;
 219                                                 }
 220                                         }
 221                                         if (reject_merge)
 222                                                 break;
 223                                 }
 224                                 if (reject_merge)
 225                                         break;
 226                         }
 227
 228                         if (!reject_merge)
 229                         {
 230                                 // do this merge!
 231                                 did_merge = true;
 232                                 m_faces.push_back(combinedFace);
 233                         }
 234                 }
 235                 if (!did_merge)
 236                 {
 237                         for (int i = 0; i < coplanarFaceGroup.size(); i++)
 238                         {
 239                                 b3MyFace face = tmpFaces[coplanarFaceGroup[i]];
 240                                 m_faces.push_back(face);
 241                         }
 242                 }
 243         }
 244
 245         initialize();
 246
 247         return true;
 248 }
 249
 250 inline bool IsAlmostZero(const b3Vector3& v)
 251 {
 252         if (fabsf(v.getX()) > 1e-6 || fabsf(v.getY()) > 1e-6 || fabsf(v.getZ()) > 1e-6) return false;
 253         return true;
 254 }
 255
 256 struct b3InternalVertexPair
 257 {
 258         b3InternalVertexPair(short int v0, short int v1)
 259                 : m_v0(v0),
 260                   m_v1(v1)
 261         {
 262                 if (m_v1 > m_v0)
 263                         b3Swap(m_v0, m_v1);
 264         }
 265         short int m_v0;
 266         short int m_v1;
 267         int getHash() const
 268         {
 269                 return m_v0 + (m_v1 << 16);
 270         }
 271         bool equals(const b3InternalVertexPair& other) const
 272         {
 273                 return m_v0 == other.m_v0 && m_v1 == other.m_v1;
 274         }
 275 };
 276
 277 struct b3InternalEdge
 278 {
 279         b3InternalEdge()
 280                 : m_face0(-1),
 281                   m_face1(-1)
 282         {
 283         }
 284         short int m_face0;
 285         short int m_face1;
 286 };
 287
 288 //
 289
 290 #ifdef TEST_INTERNAL_OBJECTS
 291 bool b3ConvexUtility::testContainment() const
 292 {
 293         for (int p = 0; p < 8; p++)
 294         {
 295                 b3Vector3 LocalPt;
 296                 if (p == 0)
 297                         LocalPt = m_localCenter + b3Vector3(m_extents[0], m_extents[1], m_extents[2]);
 298                 else if (p == 1)
 299                         LocalPt = m_localCenter + b3Vector3(m_extents[0], m_extents[1], -m_extents[2]);
 300                 else if (p == 2)
 301                         LocalPt = m_localCenter + b3Vector3(m_extents[0], -m_extents[1], m_extents[2]);
 302                 else if (p == 3)
 303                         LocalPt = m_localCenter + b3Vector3(m_extents[0], -m_extents[1], -m_extents[2]);
 304                 else if (p == 4)
 305                         LocalPt = m_localCenter + b3Vector3(-m_extents[0], m_extents[1], m_extents[2]);
 306                 else if (p == 5)
 307                         LocalPt = m_localCenter + b3Vector3(-m_extents[0], m_extents[1], -m_extents[2]);
 308                 else if (p == 6)
 309                         LocalPt = m_localCenter + b3Vector3(-m_extents[0], -m_extents[1], m_extents[2]);
 310                 else if (p == 7)
 311                         LocalPt = m_localCenter + b3Vector3(-m_extents[0], -m_extents[1], -m_extents[2]);
 312
 313                 for (int i = 0; i < m_faces.size(); i++)
 314                 {
 315                         const b3Vector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]);
 316                         const b3Scalar d = LocalPt.dot(Normal) + m_faces[i].m_plane[3];
 317                         if (d > 0.0f)
 318                                 return false;
 319                 }
 320         }
 321         return true;
 322 }
 323 #endif
 324
 325 void b3ConvexUtility::initialize()
 326 {
 327         b3HashMap<b3InternalVertexPair, b3InternalEdge> edges;
 328
 329         b3Scalar TotalArea = 0.0f;
 330
 331         m_localCenter.setValue(0, 0, 0);
 332         for (int i = 0; i < m_faces.size(); i++)
 333         {
 334                 int numVertices = m_faces[i].m_indices.size();
 335                 int NbTris = numVertices;
 336                 for (int j = 0; j < NbTris; j++)
 337                 {
 338                         int k = (j + 1) % numVertices;
 339                         b3InternalVertexPair vp(m_faces[i].m_indices[j], m_faces[i].m_indices[k]);
 340                         b3InternalEdge* edptr = edges.find(vp);
 341                         b3Vector3 edge = m_vertices[vp.m_v1] - m_vertices[vp.m_v0];
 342                         edge.normalize();
 343
 344                         bool found = false;
 345                         b3Vector3 diff, diff2;
 346
 347                         for (int p = 0; p < m_uniqueEdges.size(); p++)
 348                         {
 349                                 diff = m_uniqueEdges[p] - edge;
 350                                 diff2 = m_uniqueEdges[p] + edge;
 351
 352                                 //      if ((diff.length2()==0.f) ||
 353                                 //      (diff2.length2()==0.f))
 354
 355                                 if (IsAlmostZero(diff) ||
 356                                         IsAlmostZero(diff2))
 357                                 {
 358                                         found = true;
 359                                         break;
 360                                 }
 361                         }
 362
 363                         if (!found)
 364                         {
 365                                 m_uniqueEdges.push_back(edge);
 366                         }
 367
 368                         if (edptr)
 369                         {
 370                                 //TBD: figure out why I added this assert
 371                                 //                              b3Assert(edptr->m_face0>=0);
 372                                 //                      b3Assert(edptr->m_face1<0);
 373                                 edptr->m_face1 = i;
 374                         }
 375                         else
 376                         {
 377                                 b3InternalEdge ed;
 378                                 ed.m_face0 = i;
 379                                 edges.insert(vp, ed);
 380                         }
 381                 }
 382         }
 383
 384 #ifdef USE_CONNECTED_FACES
 385         for (int i = 0; i < m_faces.size(); i++)
 386         {
 387                 int numVertices = m_faces[i].m_indices.size();
 388                 m_faces[i].m_connectedFaces.resize(numVertices);
 389
 390                 for (int j = 0; j < numVertices; j++)
 391                 {
 392                         int k = (j + 1) % numVertices;
 393                         b3InternalVertexPair vp(m_faces[i].m_indices[j], m_faces[i].m_indices[k]);
 394                         b3InternalEdge* edptr = edges.find(vp);
 395                         b3Assert(edptr);
 396                         b3Assert(edptr->m_face0 >= 0);
 397                         b3Assert(edptr->m_face1 >= 0);
 398
 399                         int connectedFace = (edptr->m_face0 == i) ? edptr->m_face1 : edptr->m_face0;
 400                         m_faces[i].m_connectedFaces[j] = connectedFace;
 401                 }
 402         }
 403 #endif  //USE_CONNECTED_FACES
 404
 405         for (int i = 0; i < m_faces.size(); i++)
 406         {
 407                 int numVertices = m_faces[i].m_indices.size();
 408                 int NbTris = numVertices - 2;
 409
 410                 const b3Vector3& p0 = m_vertices[m_faces[i].m_indices[0]];
 411                 for (int j = 1; j <= NbTris; j++)
 412                 {
 413                         int k = (j + 1) % numVertices;
 414                         const b3Vector3& p1 = m_vertices[m_faces[i].m_indices[j]];
 415                         const b3Vector3& p2 = m_vertices[m_faces[i].m_indices[k]];
 416                         b3Scalar Area = ((p0 - p1).cross(p0 - p2)).length() * 0.5f;
 417                         b3Vector3 Center = (p0 + p1 + p2) / 3.0f;
 418                         m_localCenter += Area * Center;
 419                         TotalArea += Area;
 420                 }
 421         }
 422         m_localCenter /= TotalArea;
 423
 424 #ifdef TEST_INTERNAL_OBJECTS
 425         if (1)
 426         {
 427                 m_radius = FLT_MAX;
 428                 for (int i = 0; i < m_faces.size(); i++)
 429                 {
 430                         const b3Vector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]);
 431                         const b3Scalar dist = b3Fabs(m_localCenter.dot(Normal) + m_faces[i].m_plane[3]);
 432                         if (dist < m_radius)
 433                                 m_radius = dist;
 434                 }
 435
 436                 b3Scalar MinX = FLT_MAX;
 437                 b3Scalar MinY = FLT_MAX;
 438                 b3Scalar MinZ = FLT_MAX;
 439                 b3Scalar MaxX = -FLT_MAX;
 440                 b3Scalar MaxY = -FLT_MAX;
 441                 b3Scalar MaxZ = -FLT_MAX;
 442                 for (int i = 0; i < m_vertices.size(); i++)
 443                 {
 444                         const b3Vector3& pt = m_vertices[i];
 445                         if (pt.getX() < MinX) MinX = pt.getX();
 446                         if (pt.getX() > MaxX) MaxX = pt.getX();
 447                         if (pt.getY() < MinY) MinY = pt.getY();
 448                         if (pt.getY() > MaxY) MaxY = pt.getY();
 449                         if (pt.getZ() < MinZ) MinZ = pt.getZ();
 450                         if (pt.getZ() > MaxZ) MaxZ = pt.getZ();
 451                 }
 452                 mC.setValue(MaxX + MinX, MaxY + MinY, MaxZ + MinZ);
 453                 mE.setValue(MaxX - MinX, MaxY - MinY, MaxZ - MinZ);
 454
 455                 //              const b3Scalar r = m_radius / sqrtf(2.0f);
 456                 const b3Scalar r = m_radius / sqrtf(3.0f);
 457                 const int LargestExtent = mE.maxAxis();
 458                 const b3Scalar Step = (mE[LargestExtent] * 0.5f - r) / 1024.0f;
 459                 m_extents[0] = m_extents[1] = m_extents[2] = r;
 460                 m_extents[LargestExtent] = mE[LargestExtent] * 0.5f;
 461                 bool FoundBox = false;
 462                 for (int j = 0; j < 1024; j++)
 463                 {
 464                         if (testContainment())
 465                         {
 466                                 FoundBox = true;
 467                                 break;
 468                         }
 469
 470                         m_extents[LargestExtent] -= Step;
 471                 }
 472                 if (!FoundBox)
 473                 {
 474                         m_extents[0] = m_extents[1] = m_extents[2] = r;
 475                 }
 476                 else
 477                 {
 478                         // Refine the box
 479                         const b3Scalar Step = (m_radius - r) / 1024.0f;
 480                         const int e0 = (1 << LargestExtent) & 3;
 481                         const int e1 = (1 << e0) & 3;
 482
 483                         for (int j = 0; j < 1024; j++)
 484                         {
 485                                 const b3Scalar Saved0 = m_extents[e0];
 486                                 const b3Scalar Saved1 = m_extents[e1];
 487                                 m_extents[e0] += Step;
 488                                 m_extents[e1] += Step;
 489
 490                                 if (!testContainment())
 491                                 {
 492                                         m_extents[e0] = Saved0;
 493                                         m_extents[e1] = Saved1;
 494                                         break;
 495                                 }
 496                         }
 497                 }
 498         }
 499 #endif
 500 }