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
  17 ///This file was written by Erwin Coumans
  18 ///Separating axis rest based on work from Pierre Terdiman, see
  19 ///And contact clipping based on work from Simon Hobbs
  20
  21 #include "btConvexPolyhedron.h"
  22 #include "LinearMath/btHashMap.h"
  23
  24 btConvexPolyhedron::btConvexPolyhedron()
  25 {
  26
  27 }
  28 btConvexPolyhedron::~btConvexPolyhedron()
  29 {
  30
  31 }
  32
  33
  34 inline bool IsAlmostZero(const btVector3& v)
  35 {
  36         if(fabsf(v.x())>1e-6 || fabsf(v.y())>1e-6 || fabsf(v.z())>1e-6) return false;
  37         return true;
  38 }
  39
  40 struct btInternalVertexPair
  41 {
  42         btInternalVertexPair(short int v0,short int v1)
  43                 :m_v0(v0),
  44                 m_v1(v1)
  45         {
  46                 if (m_v1>m_v0)
  47                         btSwap(m_v0,m_v1);
  48         }
  49         short int m_v0;
  50         short int m_v1;
  51         int getHash() const
  52         {
  53                 return m_v0+(m_v1<<16);
  54         }
  55         bool equals(const btInternalVertexPair& other) const
  56         {
  57                 return m_v0==other.m_v0 && m_v1==other.m_v1;
  58         }
  59 };
  60
  61 struct btInternalEdge
  62 {
  63         btInternalEdge()
  64                 :m_face0(-1),
  65                 m_face1(-1)
  66         {
  67         }
  68         short int m_face0;
  69         short int m_face1;
  70 };
  71
  72 //
  73
  74 #ifdef TEST_INTERNAL_OBJECTS
  75 bool btConvexPolyhedron::testContainment() const
  76 {
  77         for(int p=0;p<8;p++)
  78         {
  79                 btVector3 LocalPt;
  80                 if(p==0)                LocalPt = m_localCenter + btVector3(m_extents[0], m_extents[1], m_extents[2]);
  81                 else if(p==1)   LocalPt = m_localCenter + btVector3(m_extents[0], m_extents[1], -m_extents[2]);
  82                 else if(p==2)   LocalPt = m_localCenter + btVector3(m_extents[0], -m_extents[1], m_extents[2]);
  83                 else if(p==3)   LocalPt = m_localCenter + btVector3(m_extents[0], -m_extents[1], -m_extents[2]);
  84                 else if(p==4)   LocalPt = m_localCenter + btVector3(-m_extents[0], m_extents[1], m_extents[2]);
  85                 else if(p==5)   LocalPt = m_localCenter + btVector3(-m_extents[0], m_extents[1], -m_extents[2]);
  86                 else if(p==6)   LocalPt = m_localCenter + btVector3(-m_extents[0], -m_extents[1], m_extents[2]);
  87                 else if(p==7)   LocalPt = m_localCenter + btVector3(-m_extents[0], -m_extents[1], -m_extents[2]);
  88
  89                 for(int i=0;i<m_faces.size();i++)
  90                 {
  91                         const btVector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]);
  92                         const btScalar d = LocalPt.dot(Normal) + m_faces[i].m_plane[3];
  93                         if(d>0.0f)
  94                                 return false;
  95                 }
  96         }
  97         return true;
  98 }
  99 #endif
 100
 101 void    btConvexPolyhedron::initialize()
 102 {
 103
 104         btHashMap<btInternalVertexPair,btInternalEdge> edges;
 105
 106         btScalar TotalArea = 0.0f;
 107
 108         m_localCenter.setValue(0, 0, 0);
 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
 126                                 if (IsAlmostZero(m_uniqueEdges[p]-edge) ||
 127                                         IsAlmostZero(m_uniqueEdges[p]+edge))
 128                                 {
 129                                         found = true;
 130                                         break;
 131                                 }
 132                         }
 133
 134                         if (!found)
 135                         {
 136                                 m_uniqueEdges.push_back(edge);
 137                         }
 138
 139                         if (edptr)
 140                         {
 141                                 btAssert(edptr->m_face0>=0);
 142                                 btAssert(edptr->m_face1<0);
 143                                 edptr->m_face1 = i;
 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         for(int i=0;i<m_faces.size();i++)
 175         {
 176                 int numVertices = m_faces[i].m_indices.size();
 177                 int NbTris = numVertices-2;
 178
 179                 const btVector3& p0 = m_vertices[m_faces[i].m_indices[0]];
 180                 for(int j=1;j<=NbTris;j++)
 181                 {
 182                         int k = (j+1)%numVertices;
 183                         const btVector3& p1 = m_vertices[m_faces[i].m_indices[j]];
 184                         const btVector3& p2 = m_vertices[m_faces[i].m_indices[k]];
 185                         btScalar Area = ((p0 - p1).cross(p0 - p2)).length() * 0.5f;
 186                         btVector3 Center = (p0+p1+p2)/3.0f;
 187                         m_localCenter += Area * Center;
 188                         TotalArea += Area;
 189                 }
 190         }
 191         m_localCenter /= TotalArea;
 192
 193
 194
 195
 196 #ifdef TEST_INTERNAL_OBJECTS
 197         if(1)
 198         {
 199                 m_radius = FLT_MAX;
 200                 for(int i=0;i<m_faces.size();i++)
 201                 {
 202                         const btVector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]);
 203                         const btScalar dist = btFabs(m_localCenter.dot(Normal) + m_faces[i].m_plane[3]);
 204                         if(dist<m_radius)
 205                                 m_radius = dist;
 206                 }
 207
 208
 209                 btScalar MinX = FLT_MAX;
 210                 btScalar MinY = FLT_MAX;
 211                 btScalar MinZ = FLT_MAX;
 212                 btScalar MaxX = -FLT_MAX;
 213                 btScalar MaxY = -FLT_MAX;
 214                 btScalar MaxZ = -FLT_MAX;
 215                 for(int i=0; i<m_vertices.size(); i++)
 216                 {
 217                         const btVector3& pt = m_vertices[i];
 218                         if(pt.x()<MinX) MinX = pt.x();
 219                         if(pt.x()>MaxX) MaxX = pt.x();
 220                         if(pt.y()<MinY) MinY = pt.y();
 221                         if(pt.y()>MaxY) MaxY = pt.y();
 222                         if(pt.z()<MinZ) MinZ = pt.z();
 223                         if(pt.z()>MaxZ) MaxZ = pt.z();
 224                 }
 225                 mC.setValue(MaxX+MinX, MaxY+MinY, MaxZ+MinZ);
 226                 mE.setValue(MaxX-MinX, MaxY-MinY, MaxZ-MinZ);
 227
 228
 229
 230 //              const btScalar r = m_radius / sqrtf(2.0f);
 231                 const btScalar r = m_radius / sqrtf(3.0f);
 232                 const int LargestExtent = mE.maxAxis();
 233                 const btScalar Step = (mE[LargestExtent]*0.5f - r)/1024.0f;
 234                 m_extents[0] = m_extents[1] = m_extents[2] = r;
 235                 m_extents[LargestExtent] = mE[LargestExtent]*0.5f;
 236                 bool FoundBox = false;
 237                 for(int j=0;j<1024;j++)
 238                 {
 239                         if(testContainment())
 240                         {
 241                                 FoundBox = true;
 242                                 break;
 243                         }
 244
 245                         m_extents[LargestExtent] -= Step;
 246                 }
 247                 if(!FoundBox)
 248                 {
 249                         m_extents[0] = m_extents[1] = m_extents[2] = r;
 250                 }
 251                 else
 252                 {
 253                         // Refine the box
 254                         const btScalar Step = (m_radius - r)/1024.0f;
 255                         const int e0 = (1<<LargestExtent) & 3;
 256                         const int e1 = (1<<e0) & 3;
 257
 258                         for(int j=0;j<1024;j++)
 259                         {
 260                                 const btScalar Saved0 = m_extents[e0];
 261                                 const btScalar Saved1 = m_extents[e1];
 262                                 m_extents[e0] += Step;
 263                                 m_extents[e1] += Step;
 264
 265                                 if(!testContainment())
 266                                 {
 267                                         m_extents[e0] = Saved0;
 268                                         m_extents[e1] = Saved1;
 269                                         break;
 270                                 }
 271                         }
 272                 }
 273         }
 274 #endif
 275 }
 276
 277 void btConvexPolyhedron::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const\r
 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 }