Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
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
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
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 }