src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp

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