dali-physics/third-party/bullet3/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2011 Advanced Micro Devices, Inc.  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 ///This file was written by Erwin Coumans
  17 ///Separating axis rest based on work from Pierre Terdiman, see
  18 ///And contact clipping based on work from Simon Hobbs
  19
  20 #include "btConvexPolyhedron.h"
  21 #include "LinearMath/btHashMap.h"
  22
  23 btConvexPolyhedron::btConvexPolyhedron()
  24 {
  25 }
  26 btConvexPolyhedron::~btConvexPolyhedron()
  27 {
  28 }
  29
  30 inline bool IsAlmostZero1(const btVector3& v)
  31 {
  32         if (btFabs(v.x()) > 1e-6 || btFabs(v.y()) > 1e-6 || btFabs(v.z()) > 1e-6) return false;
  33         return true;
  34 }
  35
  36 struct btInternalVertexPair
  37 {
  38         btInternalVertexPair(short int v0, short int v1)
  39                 : m_v0(v0),
  40                   m_v1(v1)
  41         {
  42                 if (m_v1 > m_v0)
  43                         btSwap(m_v0, m_v1);
  44         }
  45         short int m_v0;
  46         short int m_v1;
  47         int getHash() const
  48         {
  49                 return m_v0 + (m_v1 << 16);
  50         }
  51         bool equals(const btInternalVertexPair& other) const
  52         {
  53                 return m_v0 == other.m_v0 && m_v1 == other.m_v1;
  54         }
  55 };
  56
  57 struct btInternalEdge
  58 {
  59         btInternalEdge()
  60                 : m_face0(-1),
  61                   m_face1(-1)
  62         {
  63         }
  64         short int m_face0;
  65         short int m_face1;
  66 };
  67
  68 //
  69
  70 #ifdef TEST_INTERNAL_OBJECTS
  71 bool btConvexPolyhedron::testContainment() const
  72 {
  73         for (int p = 0; p < 8; p++)
  74         {
  75                 btVector3 LocalPt;
  76                 if (p == 0)
  77                         LocalPt = m_localCenter + btVector3(m_extents[0], m_extents[1], m_extents[2]);
  78                 else if (p == 1)
  79                         LocalPt = m_localCenter + btVector3(m_extents[0], m_extents[1], -m_extents[2]);
  80                 else if (p == 2)
  81                         LocalPt = m_localCenter + btVector3(m_extents[0], -m_extents[1], m_extents[2]);
  82                 else if (p == 3)
  83                         LocalPt = m_localCenter + btVector3(m_extents[0], -m_extents[1], -m_extents[2]);
  84                 else if (p == 4)
  85                         LocalPt = m_localCenter + btVector3(-m_extents[0], m_extents[1], m_extents[2]);
  86                 else if (p == 5)
  87                         LocalPt = m_localCenter + btVector3(-m_extents[0], m_extents[1], -m_extents[2]);
  88                 else if (p == 6)
  89                         LocalPt = m_localCenter + btVector3(-m_extents[0], -m_extents[1], m_extents[2]);
  90                 else if (p == 7)
  91                         LocalPt = m_localCenter + btVector3(-m_extents[0], -m_extents[1], -m_extents[2]);
  92
  93                 for (int i = 0; i < m_faces.size(); i++)
  94                 {
  95                         const btVector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]);
  96                         const btScalar d = LocalPt.dot(Normal) + m_faces[i].m_plane[3];
  97                         if (d > 0.0f)
  98                                 return false;
  99                 }
 100         }
 101         return true;
 102 }
 103 #endif
 104
 105 void btConvexPolyhedron::initialize()
 106 {
 107         btHashMap<btInternalVertexPair, btInternalEdge> edges;
 108
 109         for (int i = 0; i < m_faces.size(); i++)
 110         {
 111                 int numVertices = m_faces[i].m_indices.size();
 112                 int NbTris = numVertices;
 113                 for (int j = 0; j < NbTris; j++)
 114                 {
 115                         int k = (j + 1) % numVertices;
 116                         btInternalVertexPair vp(m_faces[i].m_indices[j], m_faces[i].m_indices[k]);
 117                         btInternalEdge* edptr = edges.find(vp);
 118                         btVector3 edge = m_vertices[vp.m_v1] - m_vertices[vp.m_v0];
 119                         edge.normalize();
 120
 121                         bool found = false;
 122
 123                         for (int p = 0; p < m_uniqueEdges.size(); p++)
 124                         {
 125                                 if (IsAlmostZero1(m_uniqueEdges[p] - edge) ||
 126                                         IsAlmostZero1(m_uniqueEdges[p] + edge))
 127                                 {
 128                                         found = true;
 129                                         break;
 130                                 }
 131                         }
 132
 133                         if (!found)
 134                         {
 135                                 m_uniqueEdges.push_back(edge);
 136                         }
 137
 138                         if (edptr)
 139                         {
 140                                 btAssert(edptr->m_face0 >= 0);
 141                                 btAssert(edptr->m_face1 < 0);
 142                                 edptr->m_face1 = i;
 143                         }
 144                         else
 145                         {
 146                                 btInternalEdge ed;
 147                                 ed.m_face0 = i;
 148                                 edges.insert(vp, ed);
 149                         }
 150                 }
 151         }
 152
 153 #ifdef USE_CONNECTED_FACES
 154         for (int i = 0; i < m_faces.size(); i++)
 155         {
 156                 int numVertices = m_faces[i].m_indices.size();
 157                 m_faces[i].m_connectedFaces.resize(numVertices);
 158
 159                 for (int j = 0; j < numVertices; j++)
 160                 {
 161                         int k = (j + 1) % numVertices;
 162                         btInternalVertexPair vp(m_faces[i].m_indices[j], m_faces[i].m_indices[k]);
 163                         btInternalEdge* edptr = edges.find(vp);
 164                         btAssert(edptr);
 165                         btAssert(edptr->m_face0 >= 0);
 166                         btAssert(edptr->m_face1 >= 0);
 167
 168                         int connectedFace = (edptr->m_face0 == i) ? edptr->m_face1 : edptr->m_face0;
 169                         m_faces[i].m_connectedFaces[j] = connectedFace;
 170                 }
 171         }
 172 #endif  //USE_CONNECTED_FACES
 173
 174         initialize2();
 175 }
 176
 177 void btConvexPolyhedron::initialize2()
 178 {
 179         m_localCenter.setValue(0, 0, 0);
 180         btScalar TotalArea = 0.0f;
 181         for (int i = 0; i < m_faces.size(); i++)
 182         {
 183                 int numVertices = m_faces[i].m_indices.size();
 184                 int NbTris = numVertices - 2;
 185
 186                 const btVector3& p0 = m_vertices[m_faces[i].m_indices[0]];
 187                 for (int j = 1; j <= NbTris; j++)
 188                 {
 189                         int k = (j + 1) % numVertices;
 190                         const btVector3& p1 = m_vertices[m_faces[i].m_indices[j]];
 191                         const btVector3& p2 = m_vertices[m_faces[i].m_indices[k]];
 192                         btScalar Area = ((p0 - p1).cross(p0 - p2)).length() * 0.5f;
 193                         btVector3 Center = (p0 + p1 + p2) / 3.0f;
 194                         m_localCenter += Area * Center;
 195                         TotalArea += Area;
 196                 }
 197         }
 198         m_localCenter /= TotalArea;
 199
 200 #ifdef TEST_INTERNAL_OBJECTS
 201         if (1)
 202         {
 203                 m_radius = FLT_MAX;
 204                 for (int i = 0; i < m_faces.size(); i++)
 205                 {
 206                         const btVector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]);
 207                         const btScalar dist = btFabs(m_localCenter.dot(Normal) + m_faces[i].m_plane[3]);
 208                         if (dist < m_radius)
 209                                 m_radius = dist;
 210                 }
 211
 212                 btScalar MinX = FLT_MAX;
 213                 btScalar MinY = FLT_MAX;
 214                 btScalar MinZ = FLT_MAX;
 215                 btScalar MaxX = -FLT_MAX;
 216                 btScalar MaxY = -FLT_MAX;
 217                 btScalar MaxZ = -FLT_MAX;
 218                 for (int i = 0; i < m_vertices.size(); i++)
 219                 {
 220                         const btVector3& pt = m_vertices[i];
 221                         if (pt.x() < MinX) MinX = pt.x();
 222                         if (pt.x() > MaxX) MaxX = pt.x();
 223                         if (pt.y() < MinY) MinY = pt.y();
 224                         if (pt.y() > MaxY) MaxY = pt.y();
 225                         if (pt.z() < MinZ) MinZ = pt.z();
 226                         if (pt.z() > MaxZ) MaxZ = pt.z();
 227                 }
 228                 mC.setValue(MaxX + MinX, MaxY + MinY, MaxZ + MinZ);
 229                 mE.setValue(MaxX - MinX, MaxY - MinY, MaxZ - MinZ);
 230
 231                 //              const btScalar r = m_radius / sqrtf(2.0f);
 232                 const btScalar r = m_radius / sqrtf(3.0f);
 233                 const int LargestExtent = mE.maxAxis();
 234                 const btScalar Step = (mE[LargestExtent] * 0.5f - r) / 1024.0f;
 235                 m_extents[0] = m_extents[1] = m_extents[2] = r;
 236                 m_extents[LargestExtent] = mE[LargestExtent] * 0.5f;
 237                 bool FoundBox = false;
 238                 for (int j = 0; j < 1024; j++)
 239                 {
 240                         if (testContainment())
 241                         {
 242                                 FoundBox = true;
 243                                 break;
 244                         }
 245
 246                         m_extents[LargestExtent] -= Step;
 247                 }
 248                 if (!FoundBox)
 249                 {
 250                         m_extents[0] = m_extents[1] = m_extents[2] = r;
 251                 }
 252                 else
 253                 {
 254                         // Refine the box
 255                         const btScalar Step = (m_radius - r) / 1024.0f;
 256                         const int e0 = (1 << LargestExtent) & 3;
 257                         const int e1 = (1 << e0) & 3;
 258
 259                         for (int j = 0; j < 1024; j++)
 260                         {
 261                                 const btScalar Saved0 = m_extents[e0];
 262                                 const btScalar Saved1 = m_extents[e1];
 263                                 m_extents[e0] += Step;
 264                                 m_extents[e1] += Step;
 265
 266                                 if (!testContainment())
 267                                 {
 268                                         m_extents[e0] = Saved0;
 269                                         m_extents[e1] = Saved1;
 270                                         break;
 271                                 }
 272                         }
 273                 }
 274         }
 275 #endif
 276 }
 277 void btConvexPolyhedron::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin, btVector3& witnesPtMax) const
 278 {
 279         minProj = FLT_MAX;
 280         maxProj = -FLT_MAX;
 281         int numVerts = m_vertices.size();
 282         for (int i = 0; i < numVerts; i++)
 283         {
 284                 btVector3 pt = trans * m_vertices[i];
 285                 btScalar dp = pt.dot(dir);
 286                 if (dp < minProj)
 287                 {
 288                         minProj = dp;
 289                         witnesPtMin = pt;
 290                 }
 291                 if (dp > maxProj)
 292                 {
 293                         maxProj = dp;
 294                         witnesPtMax = pt;
 295                 }
 296         }
 297         if (minProj > maxProj)
 298         {
 299                 btSwap(minProj, maxProj);
 300                 btSwap(witnesPtMin, witnesPtMax);
 301         }
 302 }