VCG Library
geodesic.h
1 /****************************************************************************
2 * VCGLib o o *
3 * Visual and Computer Graphics Library o o *
4 * _ O _ *
5 * Copyright(C) 2004-2016 \/)\/ *
6 * Visual Computing Lab /\/| *
7 * ISTI - Italian National Research Council | *
8 * \ *
9 * All rights reserved. *
10 * *
11 * This program is free software; you can redistribute it and/or modify *
12 * it under the terms of the GNU General Public License as published by *
13 * the Free Software Foundation; either version 2 of the License, or *
14 * (at your option) any later version. *
15 * *
16 * This program is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
19 * GNU General Public License (http://www.gnu.org/licenses/gpl.txt) *
20 * for more details. *
21 * *
22 ****************************************************************************/
23 #include <vcg/simplex/face/pos.h>
24 #include <vcg/simplex/face/topology.h>
25 #include <vcg/complex/algorithms/update/quality.h>
26 #include <deque>
27 #include <functional>
28 #ifndef __VCGLIB_GEODESIC
29 #define __VCGLIB_GEODESIC
30 
31 namespace vcg{
32 namespace tri{
33 
34 template <class MeshType>
35 struct EuclideanDistance{
36  typedef typename MeshType::VertexType VertexType;
37  typedef typename MeshType::ScalarType ScalarType;
38  typedef typename MeshType::FacePointer FacePointer;
39 
40  EuclideanDistance(){}
41 
42  ScalarType operator()(const VertexType * v0, const VertexType * v1) const
43  {return vcg::Distance(v0->cP(),v1->cP());}
44 
45  ScalarType operator()(const FacePointer f0, const FacePointer f1) const
46  {return vcg::Distance(Barycenter(*f0),Barycenter(*f1));}
47 };
48 
49 
50 template <class MeshType>
51 class IsotropicDistance{
52 private:
53  // The only member of this class. The attribute handle used to
54  typename MeshType::template PerVertexAttributeHandle<float> wH;
55 public:
56  typedef typename MeshType::VertexType VertexType;
57  typedef typename MeshType::ScalarType ScalarType;
58  typedef typename MeshType::FacePointer FacePointer;
59 
65 
66  IsotropicDistance(MeshType &m, float variance)
67  {
68  // the wH attribute store the scaling factor to be applied to the distance
69  wH = tri::Allocator<MeshType>:: template GetPerVertexAttribute<float> (m,"distW");
70  float qmin = 1.0f/variance;
71  float qmax = variance;
72  float qrange = qmax-qmin;
73  std::pair<float,float> minmax = Stat<MeshType>::ComputePerVertexQualityMinMax(m);
74  float range = minmax.second-minmax.first;
75  for(size_t i=0;i<m.vert.size();++i)
76  wH[i]=qmin+((m.vert[i].Q()-minmax.first)/range)*qrange;
77 
78 // qDebug("Range %f %f %f",minmax.first,minmax.second,range);
79  }
80 
81  ScalarType operator()( VertexType * v0, VertexType * v1)
82  {
83  float scale = (wH[v0]+wH[v1])/2.0f;
84  return (1.0f/scale)*vcg::Distance(v0->cP(),v1->cP());
85  }
86 };
87 
88 
89 template <class MeshType>
90 struct BasicCrossFunctor
91 {
92  BasicCrossFunctor(MeshType &m) { tri::RequirePerVertexCurvatureDir(m); }
93  typedef typename MeshType::VertexType VertexType;
94 
95  typename MeshType::CoordType D1(VertexType &v) { return v.PD1(); }
96  typename MeshType::CoordType D2(VertexType &v) { return v.PD2(); }
97 };
98 
107 template <class MeshType>
109  typedef typename MeshType::VertexType VertexType;
110  typedef typename MeshType::ScalarType ScalarType;
111  typedef typename MeshType::CoordType CoordType;
112  typedef typename MeshType::VertexIterator VertexIterator;
113 
114  typename MeshType::template PerVertexAttributeHandle<CoordType> wxH,wyH;
115 public:
116  template <class CrossFunctor > AnisotropicDistance(MeshType &m, CrossFunctor &cf)
117  {
118  wxH = tri::Allocator<MeshType>:: template GetPerVertexAttribute<CoordType> (m,"distDirX");
119  wyH = tri::Allocator<MeshType>:: template GetPerVertexAttribute<CoordType> (m,"distDirY");
120 
121  for(VertexIterator vi=m.vert.begin();vi!=m.vert.end();++vi)
122  {
123  wxH[vi]=cf.D1(*vi);
124  wyH[vi]=cf.D2(*vi);
125  }
126  }
127 
128  ScalarType operator()( VertexType * v0, VertexType * v1)
129  {
130  CoordType dd = v0->cP()-v1->cP();
131  float x = (fabs(dd * wxH[v0])+fabs(dd *wxH[v1]))/2.0f;
132  float y = (fabs(dd * wyH[v0])+fabs(dd *wyH[v1]))/2.0f;
133 
134  return sqrt(x*x+y*y);
135  }
136 };
137 
138 
139 
140 
141 
142 
143 
144 
145 
146 
153 template <class MeshType>
154 class Geodesic{
155 
156 public:
157 
158  typedef typename MeshType::VertexType VertexType;
159  typedef typename MeshType::VertexIterator VertexIterator;
160  typedef typename MeshType::VertexPointer VertexPointer;
161  typedef typename MeshType::FacePointer FacePointer;
162  typedef typename MeshType::FaceType FaceType;
163  typedef typename MeshType::CoordType CoordType;
164  typedef typename MeshType::ScalarType ScalarType;
165 
166 
167 
168 /* Auxiliary class for keeping the heap of vertices to visit and their estimated distance */
169  struct VertDist{
170  VertDist(){}
171  VertDist(VertexPointer _v, ScalarType _d):v(_v),d(_d){}
172 
173  VertexPointer v;
174  ScalarType d;
175  };
176 
177 
178  struct DIJKDist{
179  DIJKDist(VertexPointer _v):v(_v){}
180  VertexPointer v;
181 
182  bool operator < (const DIJKDist &o) const
183  {
184  if( v->Q() != o.v->Q())
185  return v->Q() > o.v->Q();
186  return v<o.v;
187  }
188  };
189 
190  /* Auxiliary class for keeping the heap of vertices to visit and their estimated distance */
191  struct FaceDist{
192  FaceDist(FacePointer _f):f(_f){}
193  FacePointer f;
194  bool operator < (const FaceDist &o) const
195  {
196  if( f->Q() != o.f->Q())
197  return f->Q() > o.f->Q();
198  return f<o.f;
199  }
200  };
201 
202  /* Temporary data to associate to all the vertices: estimated distance and boolean flag */
203  struct TempData{
204  TempData(){}
205  TempData(const ScalarType & _d):d(_d),source(0),parent(0){}
206 
207  ScalarType d;
208  VertexPointer source;//closest source
209  VertexPointer parent;
210  };
211 
212  typedef SimpleTempData<std::vector<VertexType>, TempData > TempDataType;
213 
214 
215  struct pred: public std::binary_function<VertDist,VertDist,bool>{
216  pred(){}
217  bool operator()(const VertDist& v0, const VertDist& v1) const
218  {return (v0.d > v1.d);}
219  };
220 
221  /*
222  *
223  curr: vertex for which distance should be estimated
224  d_pw1: distance of pw1 from the source
225  d_curr: distance of curr from the source
226 
227 The function estimates the distance of pw from the source
228 in the assumption the mesh is developable (and without holes)
229 along the path, so that (source,pw1,curr) from a triangle.
230 All the math is to comput the angles at pw1 and curr with the Erone formula.
231 
232 The if cases take care of the cases where the angles are obtuse.
233 
234  curr
235  d_pw1 +
236  | +pw
237 source+ |
238  d_curr +
239  pw1
240 
241  */
242  template <class DistanceFunctor>
243  static ScalarType Distance(DistanceFunctor &distFunc,
244  const VertexPointer &pw,
245  const VertexPointer &pw1,
246  const VertexPointer &curr,
247  const ScalarType &d_pw1,
248  const ScalarType &d_curr)
249  {
250  ScalarType curr_d=0;
251 
252  ScalarType ew_c = distFunc(pw,curr);
253  ScalarType ew_w1 = distFunc(pw,pw1);
254  ScalarType ec_w1 = distFunc(pw1,curr);
255  CoordType w_c = (pw->cP()-curr->cP()).Normalize() * ew_c;
256  CoordType w_w1 = (pw->cP() - pw1->cP()).Normalize() * ew_w1;
257  CoordType w1_c = (pw1->cP() - curr->cP()).Normalize() * ec_w1;
258 
259  ScalarType alpha,alpha_, beta,beta_,theta,h,delta,s,a,b;
260 
261  alpha = acos((w_c.dot(w1_c))/(ew_c*ec_w1));
262  s = (d_curr + d_pw1+ec_w1)/2;
263  a = s/ec_w1;
264  b = a*s;
265  alpha_ = 2*acos ( std::min<ScalarType>(1.0,sqrt( (b- a* d_pw1)/d_curr)));
266 
267  if ( alpha+alpha_ > M_PI){
268  curr_d = d_curr + ew_c;
269  }else
270  {
271  beta_ = 2*acos ( std::min<ScalarType>(1.0,sqrt( (b- a* d_curr)/d_pw1)));
272  beta = acos((w_w1).dot(-w1_c)/(ew_w1*ec_w1));
273 
274  if ( beta+beta_ > M_PI)
275  curr_d = d_pw1 + ew_w1;
276  else
277  {
278  theta = ScalarType(M_PI)-alpha-alpha_;
279  delta = cos(theta)* ew_c;
280  h = sin(theta)* ew_c;
281  curr_d = sqrt( pow(h,2)+ pow(d_curr + delta,2));
282  }
283  }
284  return (curr_d);
285  }
286 
287 
288 
289 
290 /*
291 This is the low level version of the geodesic computation framework.
292 Starting from the seeds, it assign a distance value to each vertex. The distance of a vertex is its
293 approximated geodesic distance to the closest seeds.
294 This is function is not meant to be called (although is not prevented). Instead, it is invoked by
295 wrapping function.
296 */
297 
298  template <class DistanceFunctor>
299  static VertexPointer Visit(
300  MeshType & m,
301  std::vector<VertDist> & seedVec, // the set of seeds to start from
302  DistanceFunctor &distFunc,
303  ScalarType distance_threshold = std::numeric_limits<ScalarType>::max(), // cut off distance (do no compute anything farther than this value)
304  typename MeshType::template PerVertexAttributeHandle<VertexPointer> * vertSource = NULL, // if present we put in this attribute the closest source for each vertex
305  typename MeshType::template PerVertexAttributeHandle<VertexPointer> * vertParent = NULL, // if present we put in this attribute the parent in the path that goes from the vertex to the closest source
306  std::vector<VertexPointer> *InInterval=NULL)
307  {
308  VertexPointer farthest=0;
309 // int t0=clock();
310  //Requirements
311  tri::RequireVFAdjacency(m);
312  tri::RequirePerVertexQuality(m);
313 
314  assert(!seedVec.empty());
315 
316  TempDataType TD(m.vert, std::numeric_limits<ScalarType>::max());
317 
318  // initialize Heap
319  std::vector<VertDist> frontierHeap;
320  typename std::vector <VertDist >::iterator ifr;
321  for(ifr = seedVec.begin(); ifr != seedVec.end(); ++ifr){
322  TD[(*ifr).v].d = (*ifr).d;
323  TD[(*ifr).v].source = (*ifr).v;
324  TD[(*ifr).v].parent = (*ifr).v;
325  frontierHeap.push_back(*ifr);
326  }
327  make_heap(frontierHeap.begin(),frontierHeap.end(),pred());
328 
329  ScalarType curr_d,d_curr = 0.0,d_heap;
330  ScalarType max_distance=0.0;
331 // int t1=clock();
332  while(!frontierHeap.empty() && max_distance < distance_threshold)
333  {
334  pop_heap(frontierHeap.begin(),frontierHeap.end(),pred());
335  VertexPointer curr = (frontierHeap.back()).v;
336  if (InInterval!=NULL) InInterval->push_back(curr);
337 
338  if(vertSource!=NULL) (*vertSource)[curr] = TD[curr].source;
339  if(vertParent!=NULL) (*vertParent)[curr] = TD[curr].parent;
340 
341  d_heap = (frontierHeap.back()).d;
342  frontierHeap.pop_back();
343 
344  assert(TD[curr].d <= d_heap);
345  if(TD[curr].d < d_heap ) // a vertex whose distance has been improved after it was inserted in the queue
346  continue;
347  assert(TD[curr].d == d_heap);
348 
349  d_curr = TD[curr].d;
350 
351  for(face::VFIterator<FaceType> vfi(curr) ; vfi.f!=0; ++vfi )
352  {
353  for(int k=0;k<2;++k)
354  {
355  VertexPointer pw,pw1;
356  if(k==0) {
357  pw = vfi.f->V1(vfi.z);
358  pw1= vfi.f->V2(vfi.z);
359  }
360  else {
361  pw = vfi.f->V2(vfi.z);
362  pw1= vfi.f->V1(vfi.z);
363  }
364 
365  const ScalarType & d_pw1 = TD[pw1].d;
366  {
367  const ScalarType inter = distFunc(curr,pw1);//(curr->P() - pw1->P()).Norm();
368  const ScalarType tol = (inter + d_curr + d_pw1)*.0001f;
369 
370  if ( (TD[pw1].source != TD[curr].source)||// not the same source
371  (inter + d_curr < d_pw1 +tol ) ||
372  (inter + d_pw1 < d_curr +tol ) ||
373  (d_curr + d_pw1 < inter +tol ) // triangular inequality
374  )
375  curr_d = d_curr + distFunc(pw,curr);//(pw->P()-curr->P()).Norm();
376  else
377  curr_d = Distance(distFunc,pw,pw1,curr,d_pw1,d_curr);
378  }
379 
380  if(TD[pw].d > curr_d){
381  TD[pw].d = curr_d;
382  TD[pw].source = TD[curr].source;
383  TD[pw].parent = curr;
384  frontierHeap.push_back(VertDist(pw,curr_d));
385  push_heap(frontierHeap.begin(),frontierHeap.end(),pred());
386  }
387 // if(isLeaf){
388  if(d_curr > max_distance){
389  max_distance = d_curr;
390  farthest = curr;
391  }
392 // }
393  }
394  } // end for VFIterator
395  }// end while
396 // int t2=clock();
397 
398  // Copy found distance onto the Quality (\todo parametric!)
399  if (InInterval==NULL)
400  {
401  for(VertexIterator vi = m.vert.begin(); vi != m.vert.end(); ++vi) if(!(*vi).IsD())
402  (*vi).Q() = TD[&(*vi)].d;
403  }
404  else
405  {
406  assert(InInterval->size()>0);
407  for(size_t i=0;i<InInterval->size();i++)
408  (*InInterval)[i]->Q() = TD[(*InInterval)[i]].d;
409  }
410 // int t3=clock();
411 // printf("Init %6.3f\nVisit %6.3f\nFinal %6.3f\n",float(t1-t0)/CLOCKS_PER_SEC,float(t2-t1)/CLOCKS_PER_SEC,float(t3-t2)/CLOCKS_PER_SEC);
412  return farthest;
413  }
414 
415 public:
447  static bool Compute( MeshType & m,
448  const std::vector<VertexPointer> & seedVec)
449  {
450  EuclideanDistance<MeshType> dd;
451  return Compute(m,seedVec,dd);
452  }
453 
454  template <class DistanceFunctor>
455  static bool Compute( MeshType & m,
456  const std::vector<VertexPointer> & seedVec,
457  DistanceFunctor &distFunc,
458  ScalarType maxDistanceThr = std::numeric_limits<ScalarType>::max(),
459  std::vector<VertexPointer> *withinDistanceVec=NULL,
460  typename MeshType::template PerVertexAttributeHandle<VertexPointer> * sourceSeed = NULL,
461  typename MeshType::template PerVertexAttributeHandle<VertexPointer> * parentSeed = NULL
462  )
463  {
464  if(seedVec.empty()) return false;
465  std::vector<VertDist> vdSeedVec;
466  typename std::vector<VertexPointer>::const_iterator fi;
467  for( fi = seedVec.begin(); fi != seedVec.end() ; ++fi)
468  vdSeedVec.push_back(VertDist(*fi,0.0));
469  Visit(m, vdSeedVec, distFunc, maxDistanceThr, sourceSeed, parentSeed, withinDistanceVec);
470  return true;
471  }
472 
473  /* \brief Assigns to each vertex of the mesh its distance to the closest vertex on the boundary
474 
475 It is just a simple wrapper of the basic Compute()
476 
477  Note: update the field Q() of the vertices
478  Note: it needs the border bit set.
479  */
480  static bool DistanceFromBorder( MeshType & m, typename MeshType::template PerVertexAttributeHandle<VertexPointer> * sources = NULL)
481  {
482  std::vector<VertexPointer> fro;
483  for(VertexIterator vi = m.vert.begin(); vi != m.vert.end(); ++vi)
484  if( (*vi).IsB())
485  fro.push_back(&(*vi));
486  if(fro.empty()) return false;
487  EuclideanDistance<MeshType> dd;
489  return Compute(m,fro,dd,std::numeric_limits<ScalarType>::max(),0,sources);
490  }
491 
492 
493  static bool ConvertPerVertexSeedToPerFaceSeed(MeshType &m, const std::vector<VertexPointer> &vertexSeedVec,
494  std::vector<FacePointer> &faceSeedVec)
495  {
496  tri::RequireVFAdjacency(m);
497  tri::RequirePerFaceMark(m);
498 
499  faceSeedVec.clear();
500  tri::UnMarkAll(m);
501  for(size_t i=0;i<vertexSeedVec.size();++i)
502  {
503  for(face::VFIterator<FaceType> vfi(vertexSeedVec[i]);!vfi.End();++vfi)
504  {
505  if(tri::IsMarked(m,vfi.F())) return false;
506  faceSeedVec.push_back(vfi.F());
507  tri::Mark(m,vfi.F());
508  }
509  }
510  return true;
511  }
512 
513  static inline std::string sourcesAttributeName(void) { return "sources"; }
514  static inline std::string parentsAttributeName(void) { return "parent"; }
515 
516  template <class DistanceFunctor>
517  static void PerFaceDijsktraCompute(MeshType &m, const std::vector<FacePointer> &seedVec,
518  DistanceFunctor &distFunc,
519  ScalarType maxDistanceThr = std::numeric_limits<ScalarType>::max(),
520  std::vector<FacePointer> *InInterval=NULL,
521  FacePointer FaceTarget=NULL,
522  bool avoid_selected=false)
523  {
524  tri::RequireFFAdjacency(m);
525  tri::RequirePerFaceMark(m);
526  tri::RequirePerFaceQuality(m);
527 
528  typename MeshType::template PerFaceAttributeHandle<FacePointer> sourceHandle
529  = tri::Allocator<MeshType>::template GetPerFaceAttribute<FacePointer>(m, sourcesAttributeName());
530 
531  typename MeshType::template PerFaceAttributeHandle<FacePointer> parentHandle
532  = tri::Allocator<MeshType>::template GetPerFaceAttribute<FacePointer>(m, parentsAttributeName());
533 
534  std::vector<FaceDist> Heap;
535  tri::UnMarkAll(m);
536  for(size_t i=0;i<seedVec.size();++i)
537  {
538  tri::Mark(m,seedVec[i]);
539  seedVec[i]->Q()=0;
540  sourceHandle[seedVec[i]]=seedVec[i];
541  parentHandle[seedVec[i]]=seedVec[i];
542  Heap.push_back(FaceDist(seedVec[i]));
543  if (InInterval!=NULL) InInterval->push_back(seedVec[i]);
544  }
545 
546  std::make_heap(Heap.begin(),Heap.end());
547  while(!Heap.empty())
548  {
549  pop_heap(Heap.begin(),Heap.end());
550  FacePointer curr = (Heap.back()).f;
551  if ((FaceTarget!=NULL)&&(curr==FaceTarget))return;
552  Heap.pop_back();
553 
554  for(int i=0;i<3;++i)
555  {
556  if(!face::IsBorder(*curr,i) )
557  {
558  FacePointer nextF = curr->FFp(i);
559  ScalarType nextDist = curr->Q() + distFunc(curr,nextF);
560  if( (nextDist < maxDistanceThr) &&
561  (!tri::IsMarked(m,nextF) || nextDist < nextF->Q()) )
562  {
563  nextF->Q() = nextDist;
564  if ((avoid_selected)&&(nextF->IsS()))continue;
565  tri::Mark(m,nextF);
566  Heap.push_back(FaceDist(nextF));
567  push_heap(Heap.begin(),Heap.end());
568  if (InInterval!=NULL) InInterval->push_back(nextF);
569  sourceHandle[nextF] = sourceHandle[curr];
570  parentHandle[nextF] = curr;
571 // printf("Heapsize %i nextDist = %f curr face %i next face %i \n",Heap.size(), nextDist, tri::Index(m,curr), tri::Index(m,nextF));
572  }
573  }
574  }
575  }
576  }
577 
578 
579 
580 
581  template <class DistanceFunctor>
582  static void PerVertexDijsktraCompute(MeshType &m, const std::vector<VertexPointer> &seedVec,
583  DistanceFunctor &distFunc,
584  ScalarType maxDistanceThr = std::numeric_limits<ScalarType>::max(),
585  std::vector<VertexPointer> *InInterval=NULL,
586  typename MeshType::template PerVertexAttributeHandle<VertexPointer> * sourceHandle= NULL,
587  typename MeshType::template PerVertexAttributeHandle<VertexPointer> * parentHandle=NULL,
588  bool avoid_selected=false,
589  VertexPointer target=NULL)
590  {
591  tri::RequireVFAdjacency(m);
592  tri::RequirePerVertexMark(m);
593  tri::RequirePerVertexQuality(m);
594 
595 // typename MeshType::template PerVertexAttributeHandle<VertexPointer> sourceHandle
596 // = tri::Allocator<MeshType>::template GetPerVertexAttribute<VertexPointer>(m, sourcesAttributeName());
597 
598 // typename MeshType::template PerVertexAttributeHandle<VertexPointer> parentHandle
599 // = tri::Allocator<MeshType>::template GetPerVertexAttribute<VertexPointer> (m, parentsAttributeName());
600 
601  std::vector<DIJKDist> Heap;
602  tri::UnMarkAll(m);
603 
604  for(size_t i=0;i<seedVec.size();++i)
605  {
606  assert(!tri::IsMarked(m,seedVec[i]));
607  tri::Mark(m,seedVec[i]);
608  seedVec[i]->Q()=0;
609  if (sourceHandle!=NULL)
610  (*sourceHandle)[seedVec[i]]=seedVec[i];
611  if (parentHandle!=NULL)
612  (*parentHandle)[seedVec[i]]=seedVec[i];
613  Heap.push_back(DIJKDist(seedVec[i]));
614  if (InInterval!=NULL) InInterval->push_back(seedVec[i]);
615  }
616 
617  std::make_heap(Heap.begin(),Heap.end());
618  while(!Heap.empty())
619  {
620  pop_heap(Heap.begin(),Heap.end());
621  VertexPointer curr = (Heap.back()).v;
622  if ((target!=NULL)&&(target==curr))return;
623  Heap.pop_back();
624  std::vector<VertexPointer> vertVec;
625  face::VVStarVF<FaceType>(curr,vertVec);
626  for(size_t i=0;i<vertVec.size();++i)
627  {
628  VertexPointer nextV = vertVec[i];
629  if ((avoid_selected)&&(nextV->IsS()))continue;
630  ScalarType nextDist = curr->Q() + distFunc(curr,nextV);
631  if( (nextDist < maxDistanceThr) &&
632  (!tri::IsMarked(m,nextV) || nextDist < nextV->Q()) )
633  {
634  nextV->Q() = nextDist;
635  tri::Mark(m,nextV);
636  Heap.push_back(DIJKDist(nextV));
637  push_heap(Heap.begin(),Heap.end());
638  if (InInterval!=NULL) InInterval->push_back(nextV);
639  if (sourceHandle!=NULL)
640  (*sourceHandle)[nextV] = (*sourceHandle)[curr];
641  if (parentHandle!=NULL)
642  (*parentHandle)[nextV] = curr;
643 // printf("Heapsize %i nextDist = %f curr vert %i next vert %i \n",Heap.size(), nextDist, tri::Index(m,curr), tri::Index(m,nextV));
644  }
645  }
646  }
647  }
648 
649 
650 };// end class
651 }// end namespace tri
652 }// end namespace vcg
653 #endif