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 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 } 303
