00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062 #ifndef __VCGLIB_CLUSTERING
00063 #define __VCGLIB_CLUSTERING
00064
00065 #include<vcg/complex/trimesh/base.h>
00066 #include <vcg/complex/trimesh/clean.h>
00067 #include<vcg/space/triangle3.h>
00068 #include<vcg/complex/trimesh/update/topology.h>
00069 #include<vcg/space/index/grid_util.h>
00070
00071 #include <iostream>
00072 #include <math.h>
00073 #include <limits>
00074
00075
00076 #ifdef WIN32
00077 #ifndef __MINGW32__
00078 #include <hash_map>
00079 #include <hash_set>
00080 #define STDEXT stdext
00081 #else
00082 #include <ext/hash_map>
00083 #include <ext/hash_set>
00084 #define STDEXT __gnu_cxx
00085 #endif
00086 #else
00087 #include <ext/hash_map>
00088 #include <ext/hash_set>
00089 #define STDEXT __gnu_cxx
00090 #endif
00091
00092
00093
00094 namespace vcg{
00095 namespace tri{
00096 #define HASH_P0 73856093
00097 #define HASH_P1 19349663
00098 #define HASH_P2 83492791
00099
00100 class HashedPoint3i : public Point3i
00101 {
00102 public:
00103
00104 const size_t Hash() const
00105 {
00106 return (V(0)*HASH_P0 ^ V(1)*HASH_P1 ^ V(2)*HASH_P2);
00107 }
00108
00109 operator size_t () const
00110 {return Hash();}
00111 };
00112
00113
00114 #ifndef _MSC_VER
00115 }} namespace STDEXT {
00116 template <> struct hash<vcg::tri::HashedPoint3i>{
00117 inline size_t operator ()(const vcg::tri::HashedPoint3i &p) const {return size_t(p);}
00118 };
00119 } namespace vcg{ namespace tri{
00120 #endif
00121
00122
00123 template<class MeshType >
00124 class NearestToCenter
00125 {
00126 typedef typename MeshType::ScalarType ScalarType;
00127 typedef typename MeshType::CoordType CoordType;
00128 typedef typename MeshType::VertexType VertexType;
00129 typedef typename MeshType::FaceType FaceType;
00130 typedef BasicGrid<typename MeshType::ScalarType> GridType;
00131
00132 public:
00133 inline void AddVertex(MeshType &, GridType &g, Point3i &pi, VertexType &v)
00134 {
00135 CoordType c;
00136 g.IPiToBoxCenter(pi,c);
00137 ScalarType newDist = Distance(c,v.cP());
00138 if(!valid || newDist < bestDist)
00139 {
00140 valid=true;
00141 bestDist=newDist;
00142 bestPos=v.cP();
00143 bestN=v.cN();
00144 orig=&v;
00145 }
00146 }
00147 inline void AddFaceVertex(MeshType &m, FaceType &f, int i) { assert(0);}
00148 NearestToCenter(): valid(false){}
00149
00150 CoordType bestPos;
00151 CoordType bestN;
00152 ScalarType bestDist;
00153 bool valid;
00154 int id;
00155 VertexType *orig;
00156 CoordType Pos() const
00157 {
00158 assert(valid);
00159 return bestPos;
00160 }
00161 Color4b Col() const {return Color4b::White;}
00162 CoordType N() const {return bestN;}
00163 VertexType * Ptr() const {return orig;}
00164
00165 };
00166
00167
00168
00169 template<class MeshType>
00170 class AverageColorCell
00171 {
00172 typedef typename MeshType::CoordType CoordType;
00173 typedef typename MeshType::FaceType FaceType;
00174 typedef typename MeshType::VertexType VertexType;
00175
00176 typedef BasicGrid<typename MeshType::ScalarType> GridType;
00177
00178 public:
00179 inline void AddFaceVertex(MeshType &, FaceType &f, int i)
00180 {
00181 p+=f.cV(i)->cP();
00182 c+=CoordType(f.cV(i)->C()[0],f.cV(i)->C()[1],f.cV(i)->C()[2]);
00183
00184
00185
00186 n+=f.cN();
00187 cnt++;
00188 }
00189 inline void AddVertex(MeshType &, GridType &, Point3i &, VertexType &v)
00190 {
00191 p+=v.cP();
00192 n+=v.cN();
00193 c+=CoordType(v.C()[0],v.C()[1],v.C()[2]);
00194 cnt++;
00195 }
00196
00197 AverageColorCell(): p(0,0,0), n(0,0,0), c(0,0,0),cnt(0){}
00198 CoordType p;
00199 CoordType n;
00200 CoordType c;
00201 int cnt;
00202 int id;
00203 Color4b Col() const
00204 {
00205 return Color4b(c[0]/cnt,c[1]/cnt,c[2]/cnt,255);
00206 }
00207
00208 CoordType N() const {return n;}
00209 VertexType * Ptr() const {return 0;}
00210 CoordType Pos() const { return p/cnt; }
00211 };
00212
00213
00214
00215
00216
00217 template<class MeshType, class CellType>
00218 class Clustering
00219 {
00220 public:
00221 typedef typename MeshType::ScalarType ScalarType;
00222 typedef typename MeshType::CoordType CoordType;
00223 typedef typename MeshType::VertexType VertexType;
00224 typedef typename MeshType::FaceType FaceType;
00225 typedef typename MeshType::VertexPointer VertexPointer;
00226 typedef typename MeshType::VertexIterator VertexIterator;
00227 typedef typename MeshType::FaceIterator FaceIterator;
00228
00229
00230
00231
00232
00233
00234
00235 bool DuplicateFaceParam;
00236
00237
00238 class SimpleTri
00239 {
00240 public:
00241 CellType *v[3];
00242 const int ii(int i) const {return *((int *)(&(v[i])));}
00243 bool operator < ( const SimpleTri &p) const {
00244 return (v[2]!=p.v[2])?(v[2]<p.v[2]):
00245 (v[1]!=p.v[1])?(v[1]<p.v[1]):
00246 (v[0]<p.v[0]);
00247 }
00248
00249
00250 void sortOrient()
00251 {
00252 if(v[1] < v[0] && v[1] < v[2] ) { std::swap(v[0],v[1]); std::swap(v[1],v[2]); return; }
00253 if(v[2] < v[0] && v[2] < v[1] ) { std::swap(v[0],v[2]); std::swap(v[1],v[2]); return; }
00254 return;
00255 }
00256 void sort()
00257 {
00258 if(v[0] > v[1] ) std::swap(v[0],v[1]);
00259 if(v[0] > v[2] ) std::swap(v[0],v[2]);
00260 if(v[1] > v[2] ) std::swap(v[1],v[2]);
00261 }
00262
00263 operator size_t () const
00264 {
00265 return (ii(0)*HASH_P0 ^ ii(1)*HASH_P1 ^ ii(2)*HASH_P2);
00266 }
00267 };
00268
00269
00270
00271
00272
00273
00274
00275
00276
00277
00278
00279
00280
00281
00282 void Init(Box3<ScalarType> _mbb, int _size, ScalarType _cellsize=0)
00283 {
00284 GridCell.clear();
00285 TriSet.clear();
00286 Grid.bbox=_mbb;
00288 ScalarType infl = (_cellsize == (ScalarType)0) ? (Grid.bbox.Diag() / _size) : (_cellsize);
00289 Grid.bbox.min-=CoordType(infl,infl,infl);
00290 Grid.bbox.max+=CoordType(infl,infl,infl);
00291 Grid.dim = Grid.bbox.max - Grid.bbox.min;
00292 if( _cellsize==0)
00293 BestDim( _size, Grid.dim, Grid.siz );
00294 else
00295 Grid.siz = Point3i::Construct(Grid.dim / _cellsize);
00296
00297
00298 Grid.voxel[0] = Grid.dim[0]/Grid.siz[0];
00299 Grid.voxel[1] = Grid.dim[1]/Grid.siz[1];
00300 Grid.voxel[2] = Grid.dim[2]/Grid.siz[2];
00301 }
00302
00303
00304 BasicGrid<ScalarType> Grid;
00305
00306 #ifdef _MSC_VER
00307 STDEXT::hash_set<SimpleTri> TriSet;
00308 typedef typename STDEXT::hash_set<SimpleTri>::iterator TriHashSetIterator;
00309 #else
00310 struct SimpleTriHashFunc{
00311 inline size_t operator ()(const SimpleTri &p) const {return size_t(p);}
00312 };
00313 STDEXT::hash_set<SimpleTri,SimpleTriHashFunc> TriSet;
00314 typedef typename STDEXT::hash_set<SimpleTri,SimpleTriHashFunc>::iterator TriHashSetIterator;
00315 #endif
00316
00317 STDEXT::hash_map<HashedPoint3i,CellType> GridCell;
00318
00319
00320 void AddPointSet(MeshType &m, bool UseOnlySelected=false)
00321 {
00322 VertexIterator vi;
00323 for(vi=m.vert.begin();vi!=m.vert.end();++vi)
00324 if(!(*vi).IsD())
00325 if(!UseOnlySelected || (*vi).IsS())
00326 {
00327 HashedPoint3i pi;
00328 Grid.PToIP((*vi).cP(), pi );
00329 GridCell[pi].AddVertex(m,Grid,pi,*(vi));
00330 }
00331 }
00332
00333 void AddMesh(MeshType &m)
00334 {
00335 FaceIterator fi;
00336 for(fi=m.face.begin();fi!=m.face.end();++fi) if(!(*fi).IsD())
00337 {
00338 HashedPoint3i pi;
00339 SimpleTri st;
00340 for(int i=0;i<3;++i)
00341 {
00342 Grid.PToIP((*fi).cV(i)->cP(), pi );
00343 st.v[i]=&(GridCell[pi]);
00344 st.v[i]->AddFaceVertex(m,*(fi),i);
00345 }
00346 if( (st.v[0]!=st.v[1]) && (st.v[0]!=st.v[2]) && (st.v[1]!=st.v[2]) )
00347 {
00348 if(DuplicateFaceParam) st.sortOrient();
00349 else st.sort();
00350 TriSet.insert(st);
00351 }
00352
00353 }
00354 }
00355
00356 int CountPointSet() {return GridCell.size(); }
00357
00358 void SelectPointSet(MeshType &m)
00359 {
00360 typename STDEXT::hash_map<HashedPoint3i,CellType>::iterator gi;
00361 UpdateSelection<MeshType>::ClearVertex(m);
00362 for(gi=GridCell.begin();gi!=GridCell.end();++gi)
00363 {
00364 VertexType *ptr=(*gi).second.Ptr();
00365 if(ptr && ( ptr >= &*m.vert.begin() ) && ( ptr <= &*(m.vert.end() - 1) ) )
00366 ptr->SetS();
00367 }
00368 }
00369 void ExtractPointSet(MeshType &m)
00370 {
00371 m.Clear();
00372
00373 if (GridCell.empty()) return;
00374
00375 Allocator<MeshType>::AddVertices(m,GridCell.size());
00376 typename STDEXT::hash_map<HashedPoint3i,CellType>::iterator gi;
00377 int i=0;
00378 for(gi=GridCell.begin();gi!=GridCell.end();++gi)
00379 {
00380 m.vert[i].P()=(*gi).second.Pos();
00381 m.vert[i].N()=(*gi).second.N();
00382 m.vert[i].C()=(*gi).second.Col();
00383 ++i;
00384 }
00385
00386 }
00387
00388 void ExtractMesh(MeshType &m)
00389 {
00390 m.Clear();
00391
00392 if (TriSet.empty() || GridCell.empty())
00393 {
00394 return;
00395 }
00396
00397 Allocator<MeshType>::AddVertices(m,GridCell.size());
00398 typename STDEXT::hash_map<HashedPoint3i,CellType>::iterator gi;
00399 int i=0;
00400 for(gi=GridCell.begin();gi!=GridCell.end();++gi)
00401 {
00402 m.vert[i].P()=(*gi).second.Pos();
00403 if(m.vert[i].HasColor())
00404 m.vert[i].C()=(*gi).second.Col();
00405 (*gi).second.id=i;
00406 ++i;
00407 }
00408 Allocator<MeshType>::AddFaces(m,TriSet.size());
00409 TriHashSetIterator ti;
00410 i=0;
00411 for(ti=TriSet.begin();ti!=TriSet.end();++ti)
00412 {
00413 m.face[i].V(0)=&(m.vert[(*ti).v[0]->id]);
00414 m.face[i].V(1)=&(m.vert[(*ti).v[1]->id]);
00415 m.face[i].V(2)=&(m.vert[(*ti).v[2]->id]);
00416
00417
00418 if(!DuplicateFaceParam)
00419 {
00420 CoordType N=vcg::Normal(m.face[i]);
00421 int badOrient=0;
00422 if( N.dot((*ti).v[0]->N()) <0) ++badOrient;
00423 if( N.dot((*ti).v[1]->N()) <0) ++badOrient;
00424 if( N.dot((*ti).v[2]->N()) <0) ++badOrient;
00425 if(badOrient>2)
00426 std::swap(m.face[i].V(0),m.face[i].V(1));
00427 }
00428 i++;
00429 }
00430
00431 }
00432 };
00433 }
00434 }
00435
00436 #endif