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), q(_v->Q()){}
180  VertexPointer v;
181  ScalarType q;
182 
183  bool operator < (const DIJKDist &o) const
184  {
185  if( q != o.q)
186  return q > o.q;
187  return v<o.v;
188  }
189  };
190 
191  /* Auxiliary class for keeping the heap of vertices to visit and their estimated distance */
192  struct FaceDist{
193  FaceDist(FacePointer _f):f(_f){}
194  FacePointer f;
195  bool operator < (const FaceDist &o) const
196  {
197  if( f->Q() != o.f->Q())
198  return f->Q() > o.f->Q();
199  return f<o.f;
200  }
201  };
202 
203  /* Temporary data to associate to all the vertices: estimated distance and boolean flag */
204  struct TempData{
205  TempData(){}
206  TempData(const ScalarType & _d):d(_d),source(0),parent(0){}
207 
208  ScalarType d;
209  VertexPointer source;//closest source
210  VertexPointer parent;
211  };
212 
213  typedef SimpleTempData<std::vector<VertexType>, TempData > TempDataType;
214 
215 
216  struct pred: public std::binary_function<VertDist,VertDist,bool>{
217  pred(){}
218  bool operator()(const VertDist& v0, const VertDist& v1) const
219  {return (v0.d > v1.d);}
220  };
221 
222  /*
223  *
224  curr: vertex for which distance should be estimated
225  d_pw1: distance of pw1 from the source
226  d_curr: distance of curr from the source
227 
228 The function estimates the distance of pw from the source
229 in the assumption the mesh is developable (and without holes)
230 along the path, so that (source,pw1,curr) from a triangle.
231 All the math is to comput the angles at pw1 and curr with the Erone formula.
232 
233 The if cases take care of the cases where the angles are obtuse.
234 
235  curr
236  d_pw1 +
237  | +pw
238 source+ |
239  d_curr +
240  pw1
241 
242  */
243  template <class DistanceFunctor>
244  static ScalarType Distance(DistanceFunctor &distFunc,
245  const VertexPointer &pw,
246  const VertexPointer &pw1,
247  const VertexPointer &curr,
248  const ScalarType &d_pw1,
249  const ScalarType &d_curr)
250  {
251  ScalarType curr_d=0;
252 
253  ScalarType ew_c = distFunc(pw,curr);
254  ScalarType ew_w1 = distFunc(pw,pw1);
255  ScalarType ec_w1 = distFunc(pw1,curr);
256  CoordType w_c = (pw->cP()-curr->cP()).Normalize() * ew_c;
257  CoordType w_w1 = (pw->cP() - pw1->cP()).Normalize() * ew_w1;
258  CoordType w1_c = (pw1->cP() - curr->cP()).Normalize() * ec_w1;
259 
260  ScalarType alpha,alpha_, beta,beta_,theta,h,delta,s,a,b;
261 
262  alpha = acos((w_c.dot(w1_c))/(ew_c*ec_w1));
263  s = (d_curr + d_pw1+ec_w1)/2;
264  a = s/ec_w1;
265  b = a*s;
266  alpha_ = 2*acos ( std::min<ScalarType>(1.0,sqrt( (b- a* d_pw1)/d_curr)));
267 
268  if ( alpha+alpha_ > M_PI){
269  curr_d = d_curr + ew_c;
270  }else
271  {
272  beta_ = 2*acos ( std::min<ScalarType>(1.0,sqrt( (b- a* d_curr)/d_pw1)));
273  beta = acos((w_w1).dot(-w1_c)/(ew_w1*ec_w1));
274 
275  if ( beta+beta_ > M_PI)
276  curr_d = d_pw1 + ew_w1;
277  else
278  {
279  theta = ScalarType(M_PI)-alpha-alpha_;
280  delta = cos(theta)* ew_c;
281  h = sin(theta)* ew_c;
282  curr_d = sqrt( pow(h,2)+ pow(d_curr + delta,2));
283  }
284  }
285  return (curr_d);
286  }
287 
288 
289 
290 
291 /*
292 This is the low level version of the geodesic computation framework.
293 Starting from the seeds, it assign a distance value to each vertex. The distance of a vertex is its
294 approximated geodesic distance to the closest seeds.
295 This is function is not meant to be called (although is not prevented). Instead, it is invoked by
296 wrapping function.
297 */
298 
299  template <class DistanceFunctor>
300  static VertexPointer Visit(
301  MeshType & m,
302  std::vector<VertDist> & seedVec, // the set of seeds to start from
303  DistanceFunctor &distFunc,
304  ScalarType distance_threshold = std::numeric_limits<ScalarType>::max(), // cut off distance (do no compute anything farther than this value)
305  typename MeshType::template PerVertexAttributeHandle<VertexPointer> * vertSource = NULL, // if present we put in this attribute the closest source for each vertex
306  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
307  std::vector<VertexPointer> *InInterval=NULL)
308  {
309  VertexPointer farthest=0;
310 // int t0=clock();
311  //Requirements
312  tri::RequireVFAdjacency(m);
313  tri::RequirePerVertexQuality(m);
314 
315  assert(!seedVec.empty());
316 
317  TempDataType TD(m.vert, std::numeric_limits<ScalarType>::max());
318 
319  // initialize Heap
320  std::vector<VertDist> frontierHeap;
321  typename std::vector <VertDist >::iterator ifr;
322  for(ifr = seedVec.begin(); ifr != seedVec.end(); ++ifr){
323  TD[(*ifr).v].d = (*ifr).d;
324  TD[(*ifr).v].source = (*ifr).v;
325  TD[(*ifr).v].parent = (*ifr).v;
326  frontierHeap.push_back(*ifr);
327  }
328  make_heap(frontierHeap.begin(),frontierHeap.end(),pred());
329 
330  ScalarType curr_d,d_curr = 0.0,d_heap;
331  ScalarType max_distance=0.0;
332 // int t1=clock();
333  while(!frontierHeap.empty() && max_distance < distance_threshold)
334  {
335  pop_heap(frontierHeap.begin(),frontierHeap.end(),pred());
336  VertexPointer curr = (frontierHeap.back()).v;
337  if (InInterval!=NULL) InInterval->push_back(curr);
338 
339  if(vertSource!=NULL) (*vertSource)[curr] = TD[curr].source;
340  if(vertParent!=NULL) (*vertParent)[curr] = TD[curr].parent;
341 
342  d_heap = (frontierHeap.back()).d;
343  frontierHeap.pop_back();
344 
345  assert(TD[curr].d <= d_heap);
346  if(TD[curr].d < d_heap ) // a vertex whose distance has been improved after it was inserted in the queue
347  continue;
348  assert(TD[curr].d == d_heap);
349 
350  d_curr = TD[curr].d;
351 
352  for(face::VFIterator<FaceType> vfi(curr) ; vfi.f!=0; ++vfi )
353  {
354  for(int k=0;k<2;++k)
355  {
356  VertexPointer pw,pw1;
357  if(k==0) {
358  pw = vfi.f->V1(vfi.z);
359  pw1= vfi.f->V2(vfi.z);
360  }
361  else {
362  pw = vfi.f->V2(vfi.z);
363  pw1= vfi.f->V1(vfi.z);
364  }
365 
366  const ScalarType & d_pw1 = TD[pw1].d;
367  {
368  const ScalarType inter = distFunc(curr,pw1);//(curr->P() - pw1->P()).Norm();
369  const ScalarType tol = (inter + d_curr + d_pw1)*.0001f;
370 
371  if ( (TD[pw1].source != TD[curr].source)||// not the same source
372  (inter + d_curr < d_pw1 +tol ) ||
373  (inter + d_pw1 < d_curr +tol ) ||
374  (d_curr + d_pw1 < inter +tol ) // triangular inequality
375  )
376  curr_d = d_curr + distFunc(pw,curr);//(pw->P()-curr->P()).Norm();
377  else
378  curr_d = Distance(distFunc,pw,pw1,curr,d_pw1,d_curr);
379  }
380 
381  if(TD[pw].d > curr_d){
382  TD[pw].d = curr_d;
383  TD[pw].source = TD[curr].source;
384  TD[pw].parent = curr;
385  frontierHeap.push_back(VertDist(pw,curr_d));
386  push_heap(frontierHeap.begin(),frontierHeap.end(),pred());
387  }
388 // if(isLeaf){
389  if(d_curr > max_distance){
390  max_distance = d_curr;
391  farthest = curr;
392  }
393 // }
394  }
395  } // end for VFIterator
396  }// end while
397 // int t2=clock();
398 
399  // Copy found distance onto the Quality (\todo parametric!)
400  if (InInterval==NULL)
401  {
402  for(VertexIterator vi = m.vert.begin(); vi != m.vert.end(); ++vi) if(!(*vi).IsD())
403  (*vi).Q() = TD[&(*vi)].d;
404  }
405  else
406  {
407  assert(InInterval->size()>0);
408  for(size_t i=0;i<InInterval->size();i++)
409  (*InInterval)[i]->Q() = TD[(*InInterval)[i]].d;
410  }
411 // int t3=clock();
412 // 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);
413  return farthest;
414  }
415 
416 public:
448  static bool Compute( MeshType & m,
449  const std::vector<VertexPointer> & seedVec)
450  {
451  EuclideanDistance<MeshType> dd;
452  return Compute(m,seedVec,dd);
453  }
454 
455  template <class DistanceFunctor>
456  static bool Compute( MeshType & m,
457  const std::vector<VertexPointer> & seedVec,
458  DistanceFunctor &distFunc,
459  ScalarType maxDistanceThr = std::numeric_limits<ScalarType>::max(),
460  std::vector<VertexPointer> *withinDistanceVec=NULL,
461  typename MeshType::template PerVertexAttributeHandle<VertexPointer> * sourceSeed = NULL,
462  typename MeshType::template PerVertexAttributeHandle<VertexPointer> * parentSeed = NULL
463  )
464  {
465  if(seedVec.empty()) return false;
466  std::vector<VertDist> vdSeedVec;
467  typename std::vector<VertexPointer>::const_iterator fi;
468  for( fi = seedVec.begin(); fi != seedVec.end() ; ++fi)
469  vdSeedVec.push_back(VertDist(*fi,0.0));
470  Visit(m, vdSeedVec, distFunc, maxDistanceThr, sourceSeed, parentSeed, withinDistanceVec);
471  return true;
472  }
473 
474  /* \brief Assigns to each vertex of the mesh its distance to the closest vertex on the boundary
475 
476 It is just a simple wrapper of the basic Compute()
477 
478  Note: update the field Q() of the vertices
479  Note: it needs the border bit set.
480  */
481  static bool DistanceFromBorder( MeshType & m, typename MeshType::template PerVertexAttributeHandle<VertexPointer> * sources = NULL)
482  {
483  std::vector<VertexPointer> fro;
484  for(VertexIterator vi = m.vert.begin(); vi != m.vert.end(); ++vi)
485  if( (*vi).IsB())
486  fro.push_back(&(*vi));
487  if(fro.empty()) return false;
488  EuclideanDistance<MeshType> dd;
490  return Compute(m,fro,dd,std::numeric_limits<ScalarType>::max(),0,sources);
491  }
492 
493 
494  static bool ConvertPerVertexSeedToPerFaceSeed(MeshType &m, const std::vector<VertexPointer> &vertexSeedVec,
495  std::vector<FacePointer> &faceSeedVec)
496  {
497  tri::RequireVFAdjacency(m);
498  tri::RequirePerFaceMark(m);
499 
500  faceSeedVec.clear();
501  tri::UnMarkAll(m);
502  for(size_t i=0;i<vertexSeedVec.size();++i)
503  {
504  for(face::VFIterator<FaceType> vfi(vertexSeedVec[i]);!vfi.End();++vfi)
505  {
506  if(tri::IsMarked(m,vfi.F())) return false;
507  faceSeedVec.push_back(vfi.F());
508  tri::Mark(m,vfi.F());
509  }
510  }
511  return true;
512  }
513 
514  static inline std::string sourcesAttributeName(void) { return "sources"; }
515  static inline std::string parentsAttributeName(void) { return "parent"; }
516 
517  template <class DistanceFunctor>
518  static void PerFaceDijkstraCompute(MeshType &m, const std::vector<FacePointer> &seedVec,
519  DistanceFunctor &distFunc,
520  ScalarType maxDistanceThr = std::numeric_limits<ScalarType>::max(),
521  std::vector<FacePointer> *InInterval=NULL,
522  FacePointer FaceTarget=NULL,
523  bool avoid_selected=false)
524  {
525  tri::RequireFFAdjacency(m);
526  tri::RequirePerFaceMark(m);
527  tri::RequirePerFaceQuality(m);
528 
529  typename MeshType::template PerFaceAttributeHandle<FacePointer> sourceHandle
530  = tri::Allocator<MeshType>::template GetPerFaceAttribute<FacePointer>(m, sourcesAttributeName());
531 
532  typename MeshType::template PerFaceAttributeHandle<FacePointer> parentHandle
533  = tri::Allocator<MeshType>::template GetPerFaceAttribute<FacePointer>(m, parentsAttributeName());
534 
535  std::vector<FaceDist> Heap;
536  tri::UnMarkAll(m);
537  for(size_t i=0;i<seedVec.size();++i)
538  {
539  tri::Mark(m,seedVec[i]);
540  seedVec[i]->Q()=0;
541  sourceHandle[seedVec[i]]=seedVec[i];
542  parentHandle[seedVec[i]]=seedVec[i];
543  Heap.push_back(FaceDist(seedVec[i]));
544  if (InInterval!=NULL) InInterval->push_back(seedVec[i]);
545  }
546 
547  std::make_heap(Heap.begin(),Heap.end());
548  while(!Heap.empty())
549  {
550  pop_heap(Heap.begin(),Heap.end());
551  FacePointer curr = (Heap.back()).f;
552  if ((FaceTarget!=NULL)&&(curr==FaceTarget))return;
553  Heap.pop_back();
554 
555  for(int i=0;i<3;++i)
556  {
557  if(!face::IsBorder(*curr,i) )
558  {
559  FacePointer nextF = curr->FFp(i);
560  ScalarType nextDist = curr->Q() + distFunc(curr,nextF);
561  if( (nextDist < maxDistanceThr) &&
562  (!tri::IsMarked(m,nextF) || nextDist < nextF->Q()) )
563  {
564  nextF->Q() = nextDist;
565  if ((avoid_selected)&&(nextF->IsS()))continue;
566  tri::Mark(m,nextF);
567  Heap.push_back(FaceDist(nextF));
568  push_heap(Heap.begin(),Heap.end());
569  if (InInterval!=NULL) InInterval->push_back(nextF);
570  sourceHandle[nextF] = sourceHandle[curr];
571  parentHandle[nextF] = curr;
572 // printf("Heapsize %i nextDist = %f curr face %i next face %i \n",Heap.size(), nextDist, tri::Index(m,curr), tri::Index(m,nextF));
573  }
574  }
575  }
576  }
577  }
578 
579 
580 
581 
582  template <class DistanceFunctor>
583  static void PerVertexDijkstraCompute(MeshType &m, const std::vector<VertexPointer> &seedVec,
584  DistanceFunctor &distFunc,
585  ScalarType maxDistanceThr = std::numeric_limits<ScalarType>::max(),
586  std::vector<VertexPointer> *InInterval=NULL,
587  typename MeshType::template PerVertexAttributeHandle<VertexPointer> * sourceHandle= NULL,
588  typename MeshType::template PerVertexAttributeHandle<VertexPointer> * parentHandle=NULL,
589  bool avoid_selected=false,
590  VertexPointer target=NULL)
591  {
592  tri::RequireVFAdjacency(m);
593  tri::RequirePerVertexMark(m);
594  tri::RequirePerVertexQuality(m);
595 
596 // typename MeshType::template PerVertexAttributeHandle<VertexPointer> sourceHandle
597 // = tri::Allocator<MeshType>::template GetPerVertexAttribute<VertexPointer>(m, sourcesAttributeName());
598 
599 // typename MeshType::template PerVertexAttributeHandle<VertexPointer> parentHandle
600 // = tri::Allocator<MeshType>::template GetPerVertexAttribute<VertexPointer> (m, parentsAttributeName());
601 
602  std::vector<DIJKDist> Heap;
603  tri::UnMarkAll(m);
604  tri::UnMarkAll(m);
605 
606  for(size_t i=0;i<seedVec.size();++i)
607  {
608  assert(!tri::IsMarked(m,seedVec[i]));
609  tri::Mark(m,seedVec[i]);
610  seedVec[i]->Q()=0;
611  if (sourceHandle!=NULL)
612  (*sourceHandle)[seedVec[i]]=seedVec[i];
613  if (parentHandle!=NULL)
614  (*parentHandle)[seedVec[i]]=seedVec[i];
615  Heap.push_back(DIJKDist(seedVec[i]));
616  if (InInterval!=NULL) InInterval->push_back(seedVec[i]);
617  }
618 
619  std::make_heap(Heap.begin(),Heap.end());
620  while(!Heap.empty())
621  {
622  pop_heap(Heap.begin(),Heap.end());
623  VertexPointer curr = (Heap.back()).v;
624  if ((target!=NULL)&&(target==curr))return;
625  Heap.pop_back();
626  std::vector<VertexPointer> vertVec;
627  face::VVStarVF<FaceType>(curr,vertVec);
628  for(size_t i=0;i<vertVec.size();++i)
629  {
630  VertexPointer nextV = vertVec[i];
631  if ((avoid_selected)&&(nextV->IsS()))continue;
632  ScalarType nextDist = curr->Q() + distFunc(curr,nextV);
633  if( (nextDist < maxDistanceThr) &&
634  (!tri::IsMarked(m,nextV) || nextDist < nextV->Q()) )
635  {
636  nextV->Q() = nextDist;
637  tri::Mark(m,nextV);
638  Heap.push_back(DIJKDist(nextV));
639  push_heap(Heap.begin(),Heap.end());
640  if (InInterval!=NULL) InInterval->push_back(nextV);
641  if (sourceHandle!=NULL)
642  (*sourceHandle)[nextV] = (*sourceHandle)[curr];
643  if (parentHandle!=NULL)
644  (*parentHandle)[nextV] = curr;
645 // printf("Heapsize %i nextDist = %f curr vert %i next vert %i \n",Heap.size(), nextDist, tri::Index(m,curr), tri::Index(m,nextV));
646  }
647  }
648  }
649  }
650 
651 
652 };// end class
653 }// end namespace tri
654 }// end namespace vcg
655 #endif