btConvexPolyhedron.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2011 Advanced Micro Devices, Inc.  http://bulletphysics.org
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 
00016 
00020 
00021 #include "btConvexPolyhedron.h"
00022 #include "LinearMath/btHashMap.h"
00023 
00024 btConvexPolyhedron::btConvexPolyhedron()
00025 {
00026 
00027 }
00028 btConvexPolyhedron::~btConvexPolyhedron()
00029 {
00030 
00031 }
00032 
00033 
00034 inline bool IsAlmostZero(const btVector3& v)
00035 {
00036         if(fabsf(v.x())>1e-6 || fabsf(v.y())>1e-6 || fabsf(v.z())>1e-6) return false;
00037         return true;
00038 }
00039 
00040 struct btInternalVertexPair
00041 {
00042         btInternalVertexPair(short int v0,short int v1)
00043                 :m_v0(v0),
00044                 m_v1(v1)
00045         {
00046                 if (m_v1>m_v0)
00047                         btSwap(m_v0,m_v1);
00048         }
00049         short int m_v0;
00050         short int m_v1;
00051         int getHash() const
00052         {
00053                 return m_v0+(m_v1<<16);
00054         }
00055         bool equals(const btInternalVertexPair& other) const
00056         {
00057                 return m_v0==other.m_v0 && m_v1==other.m_v1;
00058         }
00059 };
00060 
00061 struct btInternalEdge
00062 {
00063         btInternalEdge()
00064                 :m_face0(-1),
00065                 m_face1(-1)
00066         {
00067         }
00068         short int m_face0;
00069         short int m_face1;
00070 };
00071 
00072 //
00073 
00074 #ifdef TEST_INTERNAL_OBJECTS
00075 bool btConvexPolyhedron::testContainment() const
00076 {
00077         for(int p=0;p<8;p++)
00078         {
00079                 btVector3 LocalPt;
00080                 if(p==0)                LocalPt = m_localCenter + btVector3(m_extents[0], m_extents[1], m_extents[2]);
00081                 else if(p==1)   LocalPt = m_localCenter + btVector3(m_extents[0], m_extents[1], -m_extents[2]);
00082                 else if(p==2)   LocalPt = m_localCenter + btVector3(m_extents[0], -m_extents[1], m_extents[2]);
00083                 else if(p==3)   LocalPt = m_localCenter + btVector3(m_extents[0], -m_extents[1], -m_extents[2]);
00084                 else if(p==4)   LocalPt = m_localCenter + btVector3(-m_extents[0], m_extents[1], m_extents[2]);
00085                 else if(p==5)   LocalPt = m_localCenter + btVector3(-m_extents[0], m_extents[1], -m_extents[2]);
00086                 else if(p==6)   LocalPt = m_localCenter + btVector3(-m_extents[0], -m_extents[1], m_extents[2]);
00087                 else if(p==7)   LocalPt = m_localCenter + btVector3(-m_extents[0], -m_extents[1], -m_extents[2]);
00088 
00089                 for(int i=0;i<m_faces.size();i++)
00090                 {
00091                         const btVector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]);
00092                         const btScalar d = LocalPt.dot(Normal) + m_faces[i].m_plane[3];
00093                         if(d>0.0f)
00094                                 return false;
00095                 }
00096         }
00097         return true;
00098 }
00099 #endif
00100 
00101 void    btConvexPolyhedron::initialize()
00102 {
00103 
00104         btHashMap<btInternalVertexPair,btInternalEdge> edges;
00105 
00106         btScalar TotalArea = 0.0f;
00107         
00108         m_localCenter.setValue(0, 0, 0);
00109         for(int i=0;i<m_faces.size();i++)
00110         {
00111                 int numVertices = m_faces[i].m_indices.size();
00112                 int NbTris = numVertices;
00113                 for(int j=0;j<NbTris;j++)
00114                 {
00115                         int k = (j+1)%numVertices;
00116                         btInternalVertexPair vp(m_faces[i].m_indices[j],m_faces[i].m_indices[k]);
00117                         btInternalEdge* edptr = edges.find(vp);
00118                         btVector3 edge = m_vertices[vp.m_v1]-m_vertices[vp.m_v0];
00119                         edge.normalize();
00120 
00121                         bool found = false;
00122 
00123                         for (int p=0;p<m_uniqueEdges.size();p++)
00124                         {
00125                                 
00126                                 if (IsAlmostZero(m_uniqueEdges[p]-edge) || 
00127                                         IsAlmostZero(m_uniqueEdges[p]+edge))
00128                                 {
00129                                         found = true;
00130                                         break;
00131                                 }
00132                         }
00133 
00134                         if (!found)
00135                         {
00136                                 m_uniqueEdges.push_back(edge);
00137                         }
00138 
00139                         if (edptr)
00140                         {
00141                                 btAssert(edptr->m_face0>=0);
00142                                 btAssert(edptr->m_face1<0);
00143                                 edptr->m_face1 = i;
00144                         } else
00145                         {
00146                                 btInternalEdge ed;
00147                                 ed.m_face0 = i;
00148                                 edges.insert(vp,ed);
00149                         }
00150                 }
00151         }
00152 
00153 #ifdef USE_CONNECTED_FACES
00154         for(int i=0;i<m_faces.size();i++)
00155         {
00156                 int numVertices = m_faces[i].m_indices.size();
00157                 m_faces[i].m_connectedFaces.resize(numVertices);
00158 
00159                 for(int j=0;j<numVertices;j++)
00160                 {
00161                         int k = (j+1)%numVertices;
00162                         btInternalVertexPair vp(m_faces[i].m_indices[j],m_faces[i].m_indices[k]);
00163                         btInternalEdge* edptr = edges.find(vp);
00164                         btAssert(edptr);
00165                         btAssert(edptr->m_face0>=0);
00166                         btAssert(edptr->m_face1>=0);
00167 
00168                         int connectedFace = (edptr->m_face0==i)?edptr->m_face1:edptr->m_face0;
00169                         m_faces[i].m_connectedFaces[j] = connectedFace;
00170                 }
00171         }
00172 #endif//USE_CONNECTED_FACES
00173 
00174         for(int i=0;i<m_faces.size();i++)
00175         {
00176                 int numVertices = m_faces[i].m_indices.size();
00177                 int NbTris = numVertices-2;
00178                 
00179                 const btVector3& p0 = m_vertices[m_faces[i].m_indices[0]];
00180                 for(int j=1;j<=NbTris;j++)
00181                 {
00182                         int k = (j+1)%numVertices;
00183                         const btVector3& p1 = m_vertices[m_faces[i].m_indices[j]];
00184                         const btVector3& p2 = m_vertices[m_faces[i].m_indices[k]];
00185                         btScalar Area = ((p0 - p1).cross(p0 - p2)).length() * 0.5f;
00186                         btVector3 Center = (p0+p1+p2)/3.0f;
00187                         m_localCenter += Area * Center;
00188                         TotalArea += Area;
00189                 }
00190         }
00191         m_localCenter /= TotalArea;
00192 
00193 
00194 
00195 
00196 #ifdef TEST_INTERNAL_OBJECTS
00197         if(1)
00198         {
00199                 m_radius = FLT_MAX;
00200                 for(int i=0;i<m_faces.size();i++)
00201                 {
00202                         const btVector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]);
00203                         const btScalar dist = btFabs(m_localCenter.dot(Normal) + m_faces[i].m_plane[3]);
00204                         if(dist<m_radius)
00205                                 m_radius = dist;
00206                 }
00207 
00208         
00209                 btScalar MinX = FLT_MAX;
00210                 btScalar MinY = FLT_MAX;
00211                 btScalar MinZ = FLT_MAX;
00212                 btScalar MaxX = -FLT_MAX;
00213                 btScalar MaxY = -FLT_MAX;
00214                 btScalar MaxZ = -FLT_MAX;
00215                 for(int i=0; i<m_vertices.size(); i++)
00216                 {
00217                         const btVector3& pt = m_vertices[i];
00218                         if(pt.x()<MinX) MinX = pt.x();
00219                         if(pt.x()>MaxX) MaxX = pt.x();
00220                         if(pt.y()<MinY) MinY = pt.y();
00221                         if(pt.y()>MaxY) MaxY = pt.y();
00222                         if(pt.z()<MinZ) MinZ = pt.z();
00223                         if(pt.z()>MaxZ) MaxZ = pt.z();
00224                 }
00225                 mC.setValue(MaxX+MinX, MaxY+MinY, MaxZ+MinZ);
00226                 mE.setValue(MaxX-MinX, MaxY-MinY, MaxZ-MinZ);
00227 
00228 
00229 
00230 //              const btScalar r = m_radius / sqrtf(2.0f);
00231                 const btScalar r = m_radius / sqrtf(3.0f);
00232                 const int LargestExtent = mE.maxAxis();
00233                 const btScalar Step = (mE[LargestExtent]*0.5f - r)/1024.0f;
00234                 m_extents[0] = m_extents[1] = m_extents[2] = r;
00235                 m_extents[LargestExtent] = mE[LargestExtent]*0.5f;
00236                 bool FoundBox = false;
00237                 for(int j=0;j<1024;j++)
00238                 {
00239                         if(testContainment())
00240                         {
00241                                 FoundBox = true;
00242                                 break;
00243                         }
00244 
00245                         m_extents[LargestExtent] -= Step;
00246                 }
00247                 if(!FoundBox)
00248                 {
00249                         m_extents[0] = m_extents[1] = m_extents[2] = r;
00250                 }
00251                 else
00252                 {
00253                         // Refine the box
00254                         const btScalar Step = (m_radius - r)/1024.0f;
00255                         const int e0 = (1<<LargestExtent) & 3;
00256                         const int e1 = (1<<e0) & 3;
00257 
00258                         for(int j=0;j<1024;j++)
00259                         {
00260                                 const btScalar Saved0 = m_extents[e0];
00261                                 const btScalar Saved1 = m_extents[e1];
00262                                 m_extents[e0] += Step;
00263                                 m_extents[e1] += Step;
00264 
00265                                 if(!testContainment())
00266                                 {
00267                                         m_extents[e0] = Saved0;
00268                                         m_extents[e1] = Saved1;
00269                                         break;
00270                                 }
00271                         }
00272                 }
00273         }
00274 #endif
00275 }
00276 
00277 void btConvexPolyhedron::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const
00278 {
00279         minProj = FLT_MAX;
00280         maxProj = -FLT_MAX;
00281         int numVerts = m_vertices.size();
00282         for(int i=0;i<numVerts;i++)
00283         {
00284                 btVector3 pt = trans * m_vertices[i];
00285                 btScalar dp = pt.dot(dir);
00286                 if(dp < minProj)
00287                 {
00288                         minProj = dp;
00289                         witnesPtMin = pt;
00290                 }
00291                 if(dp > maxProj)
00292                 {
00293                         maxProj = dp;
00294                         witnesPtMax = pt;
00295                 }
00296         }
00297         if(minProj>maxProj)
00298         {
00299                 btSwap(minProj,maxProj);
00300                 btSwap(witnesPtMin,witnesPtMax);
00301         }
00302 }