A.I.M.S algorithms


geodesic_algorithm_exact.h
Go to the documentation of this file.
1 
2 #ifndef GEODESIC_ALGORITHM_EXACT_20071231
3 #define GEODESIC_ALGORITHM_EXACT_20071231
4 
5 #include "geodesic_memory.h"
8 #include <vector>
9 #include <cmath>
10 #include <assert.h>
11 #include <set>
12 #include <iostream>
13 #include <memory>
14 #include <cstring>
15 
16 namespace geodesic
17 {
18 
20 {
21 public:
24  m_memory_allocator(mesh->edges().size(), mesh->edges().size()),
25  m_edge_interval_lists(mesh->edges().size())
26  {
27  m_type = EXACT;
28 
29  for(unsigned i=0; i<m_edge_interval_lists.size(); ++i)
30  {
31  m_edge_interval_lists[i].initialize(&mesh->edges()[i]);
32  }
33  };
34 
36 
37  void propagate(std::vector<SurfacePoint>& sources,
38  double max_propagation_distance = GEODESIC_INF, //propagation algorithm stops after reaching the certain distance from the source
39  std::vector<SurfacePoint>* stop_points = NULL); //or after ensuring that all the stop_points are covered
40 
41  void trace_back(SurfacePoint& destination, //trace back piecewise-linear path
42  std::vector<SurfacePoint>& path);
43 
44 
45  void trace_back_with_index(SurfacePoint& destination,
46  std::vector<SurfacePoint>& path,std::vector<unsigned>& indexVertex);
47 
48  unsigned best_source(SurfacePoint& point, //quickly find what source this point belongs to and what is the distance to this source
49  double& best_source_distance);
50 
51  void print_statistics();
52 
53 private:
54  typedef std::set<interval_pointer, Interval> IntervalQueue;
55 
56  void update_list_and_queue(list_pointer list,
57  IntervalWithStop* candidates, //up to two candidates
58  unsigned num_candidates);
59 
60  unsigned compute_propagated_parameters(double pseudo_x,
61  double pseudo_y,
62  double d, //parameters of the interval
63  double start,
64  double end, //start/end of the interval
65  double alpha, //corner angle
66  double L, //length of the new edge
67  bool first_interval, //if it is the first interval on the edge
68  bool last_interval,
69  bool turn_left,
70  bool turn_right,
71  IntervalWithStop* candidates); //if it is the last interval on the edge
72 
73  void construct_propagated_intervals(bool invert,
74  edge_pointer edge,
75  face_pointer face, //constructs iNew from the rest of the data
76  IntervalWithStop* candidates,
77  unsigned& num_candidates,
78  interval_pointer source_interval);
79 
80  double compute_positive_intersection(double start,
81  double pseudo_x,
82  double pseudo_y,
83  double sin_alpha,
84  double cos_alpha); //used in construct_propagated_intervals
85 
86  unsigned intersect_intervals(interval_pointer zero,
87  IntervalWithStop* one); //intersecting two intervals with up to three intervals in the end
88 
89  interval_pointer best_first_interval(SurfacePoint& point,
90  double& best_total_distance,
91  double& best_interval_position,
92  unsigned& best_source_index);
93 
94  bool check_stop_conditions(unsigned& index);
95 
96  void clear()
97  {
98  m_memory_allocator.clear();
99  m_queue.clear();
100  for(unsigned i=0; i<m_edge_interval_lists.size(); ++i)
101  {
102  m_edge_interval_lists[i].clear();
103  }
105  };
106 
107  list_pointer interval_list(edge_pointer e)
108  {
109  return &m_edge_interval_lists[e->id()];
110  };
111 
112  void set_sources(std::vector<SurfacePoint>& sources)
113  {
114  m_sources.initialize(sources);
115  }
116 
117  void initialize_propagation_data();
118 
119  void list_edges_visible_from_source(MeshElementBase* p,
120  std::vector<edge_pointer>& storage); //used in initialization
121 
122  long visible_from_source(SurfacePoint& point); //used in backtracing
123 
124  void best_point_on_the_edge_set(SurfacePoint& point,
125  std::vector<edge_pointer> const& storage,
126  interval_pointer& best_interval,
127  double& best_total_distance,
128  double& best_interval_position);
129 
130  void possible_traceback_edges(SurfacePoint& point,
131  std::vector<edge_pointer>& storage);
132 
133  bool erase_from_queue(interval_pointer p);
134 
135  IntervalQueue m_queue; //interval queue
136 
137  MemoryAllocator<Interval> m_memory_allocator; //quickly allocate and deallocate intervals
138  std::vector<IntervalList> m_edge_interval_lists; //every edge has its interval data
139 
140  enum MapType {OLD, NEW}; //used for interval intersection
141  MapType map[5];
142  double start[6];
143  interval_pointer i_new[5];
144 
145  size_t m_queue_max_size; //used for statistics
146  unsigned m_iterations; //used for statistics
147 
148  SortedSources m_sources;
149 };
150 
151 inline void GeodesicAlgorithmExact::best_point_on_the_edge_set(SurfacePoint& point,
152  std::vector<edge_pointer> const& storage,
153  interval_pointer& best_interval,
154  double& best_total_distance,
155  double& best_interval_position)
156 {
157  best_total_distance = 1e100;
158  for(unsigned i=0; i<storage.size(); ++i)
159  {
160  edge_pointer e = storage[i];
161  list_pointer list = interval_list(e);
162 
163  double offset;
164  double distance;
165  interval_pointer interval;
166 
167  list->find_closest_point(&point,
168  offset,
169  distance,
170  interval);
171 
172  if(distance < best_total_distance)
173  {
174  best_interval = interval;
175  best_total_distance = distance;
176  best_interval_position = offset;
177  }
178  }
179 }
180 
181 inline void GeodesicAlgorithmExact::possible_traceback_edges(SurfacePoint& point,
182  std::vector<edge_pointer>& storage)
183 {
184  storage.clear();
185 
186  if(point.type() == VERTEX)
187  {
188  vertex_pointer v = static_cast<vertex_pointer>(point.base_element());
189  for(unsigned i=0; i<v->adjacent_faces().size(); ++i)
190  {
191  face_pointer f = v->adjacent_faces()[i];
192  storage.push_back(f->opposite_edge(v));
193  }
194  }
195  else if(point.type() == EDGE)
196  {
197  edge_pointer e = static_cast<edge_pointer>(point.base_element());
198  for(unsigned i=0; i<e->adjacent_faces().size(); ++i)
199  {
200  face_pointer f = e->adjacent_faces()[i];
201 
202  storage.push_back(f->next_edge(e,e->v0()));
203  storage.push_back(f->next_edge(e,e->v1()));
204  }
205  }
206  else
207  {
208  face_pointer f = static_cast<face_pointer>(point.base_element());
209  storage.push_back(f->adjacent_edges()[0]);
210  storage.push_back(f->adjacent_edges()[1]);
211  storage.push_back(f->adjacent_edges()[2]);
212  }
213 }
214 
215 
216 inline long GeodesicAlgorithmExact::visible_from_source(SurfacePoint& point) //negative if not visible
217 {
218  assert(point.type() != UNDEFINED_POINT);
219 
220  if(point.type() == EDGE)
221  {
222  edge_pointer e = static_cast<edge_pointer>(point.base_element());
223  list_pointer list = interval_list(e);
224  double position = std::min(point.distance(e->v0()), e->length());
225  interval_pointer interval = list->covering_interval(position);
226  //assert(interval);
227  if(interval && interval->visible_from_source())
228  {
229  return (long)interval->source_index();
230  }
231  else
232  {
233  return -1;
234  }
235  }
236  else if(point.type() == FACE)
237  {
238  return -1;
239  }
240  else if(point.type() == VERTEX)
241  {
242  vertex_pointer v = static_cast<vertex_pointer>(point.base_element());
243  for(unsigned i=0; i<v->adjacent_edges().size(); ++i)
244  {
245  edge_pointer e = v->adjacent_edges()[i];
246  list_pointer list = interval_list(e);
247 
248  double position = e->v0()->id() == v->id() ? 0.0 : e->length();
249  interval_pointer interval = list->covering_interval(position);
250  if(interval && interval->visible_from_source())
251  {
252  return (long)interval->source_index();
253  }
254  }
255 
256  return -1;
257  }
258 
259  assert(0);
260  return 0;
261 }
262 
263 inline double GeodesicAlgorithmExact::compute_positive_intersection(double start,
264  double pseudo_x,
265  double pseudo_y,
266  double sin_alpha,
267  double cos_alpha)
268 {
269  assert(pseudo_y < 0);
270 
271  double denominator = sin_alpha*(pseudo_x - start) - cos_alpha*pseudo_y;
272  if(denominator<0.0)
273  {
274  return -1.0;
275  }
276 
277  double numerator = -pseudo_y*start;
278 
279  if(numerator < 1e-30)
280  {
281  return 0.0;
282  }
283 
284  if(denominator < 1e-30)
285  {
286  return -1.0;
287  }
288 
289  return numerator/denominator;
290 }
291 
292 inline void GeodesicAlgorithmExact::list_edges_visible_from_source(MeshElementBase* p,
293  std::vector<edge_pointer>& storage)
294 {
295  assert(p->type() != UNDEFINED_POINT);
296 
297  if(p->type() == FACE)
298  {
299  face_pointer f = static_cast<face_pointer>(p);
300  for(unsigned i=0; i<3; ++i)
301  {
302  storage.push_back(f->adjacent_edges()[i]);
303  }
304  }
305  else if(p->type() == EDGE)
306  {
307  edge_pointer e = static_cast<edge_pointer>(p);
308  storage.push_back(e);
309  }
310  else //VERTEX
311  {
312  vertex_pointer v = static_cast<vertex_pointer>(p);
313  for(unsigned i=0; i<v->adjacent_edges().size(); ++i)
314  {
315  storage.push_back(v->adjacent_edges()[i]);
316  }
317 
318  }
319 }
320 
321 inline bool GeodesicAlgorithmExact::erase_from_queue(interval_pointer p)
322 {
323  if(p->min() < GEODESIC_INF/10.0)// && p->min >= queue->begin()->first)
324  {
325  assert(m_queue.count(p)<=1); //the set is unique
326 
327  IntervalQueue::iterator it = m_queue.find(p);
328 
329  if(it != m_queue.end())
330  {
331  m_queue.erase(it);
332  return true;
333  }
334  }
335 
336  return false;
337 }
338 
339 inline unsigned GeodesicAlgorithmExact::intersect_intervals(interval_pointer zero,
340  IntervalWithStop* one) //intersecting two intervals with up to three intervals in the end
341 {
342  assert(zero->edge()->id() == one->edge()->id());
343  assert(zero->stop() > one->start() && zero->start() < one->stop());
344  assert(one->min() < GEODESIC_INF/10.0);
345 
346  double const local_epsilon = SMALLEST_INTERVAL_RATIO*one->edge()->length();
347 
348  unsigned N=0;
349  if(zero->min() > GEODESIC_INF/10.0)
350  {
351  start[0] = zero->start();
352  if(zero->start() < one->start() - local_epsilon)
353  {
354  map[0] = OLD;
355  start[1] = one->start();
356  map[1] = NEW;
357  N = 2;
358  }
359  else
360  {
361  map[0] = NEW;
362  N = 1;
363  }
364 
365  if(zero->stop() > one->stop() + local_epsilon)
366  {
367  map[N] = OLD; //"zero" interval
368  start[N++] = one->stop();
369  }
370 
371  start[N+1] = zero->stop();
372  return N;
373  }
374 
375  double const local_small_epsilon = 1e-8*one->edge()->length();
376 
377  double D = zero->d() - one->d();
378  double x0 = zero->pseudo_x();
379  double x1 = one->pseudo_x();
380  double R0 = x0*x0 + zero->pseudo_y()*zero->pseudo_y();
381  double R1 = x1*x1 + one->pseudo_y()*one->pseudo_y();
382 
383  double inter[2]; //points of intersection
384  char Ninter=0; //number of the points of the intersection
385 
386  if(std::abs(D)<local_epsilon) //if d1 == d0, equation is linear
387  {
388  double denom = x1 - x0;
389  if(std::abs(denom)>local_small_epsilon)
390  {
391  inter[0] = (R1 - R0)/(2.*denom); //one solution
392  Ninter = 1;
393  }
394  }
395  else
396  {
397  double D2 = D*D;
398  double Q = 0.5*(R1-R0-D2);
399  double X = x0 - x1;
400 
401  double A = X*X - D2;
402  double B = Q*X + D2*x0;
403  double C = Q*Q - D2*R0;
404 
405  if (std::abs(A)<local_small_epsilon) //if A == 0, linear equation
406  {
407  if(std::abs(B)>local_small_epsilon)
408  {
409  inter[0] = -C/B; //one solution
410  Ninter = 1;
411  }
412  }
413  else
414  {
415  double det = B*B-A*C;
416  if(det>local_small_epsilon*local_small_epsilon) //two roots
417  {
418  det = sqrt(det);
419  if(A>0.0) //make sure that the roots are ordered
420  {
421  inter[0] = (-B - det)/A;
422  inter[1] = (-B + det)/A;
423  }
424  else
425  {
426  inter[0] = (-B + det)/A;
427  inter[1] = (-B - det)/A;
428  }
429 
430  if(inter[1] - inter[0] > local_small_epsilon)
431  {
432  Ninter = 2;
433  }
434  else
435  {
436  Ninter = 1;
437  }
438  }
439  else if(det>=0.0) //single root
440  {
441  inter[0] = -B/A;
442  Ninter = 1;
443  }
444  }
445  }
446  //---------------------------find possible intervals---------------------------------------
447  double left = std::max(zero->start(), one->start()); //define left and right boundaries of the intersection of the intervals
448  double right = std::min(zero->stop(), one->stop());
449 
450  double good_start[4]; //points of intersection within the (left, right) limits +"left" + "right"
451  good_start[0] = left;
452  unsigned char Ngood_start=1; //number of the points of the intersection
453 
454  for(unsigned char i=0; i<Ninter; ++i) //for all points of intersection
455  {
456  double x = inter[i];
457  if(x > left + local_epsilon && x < right - local_epsilon)
458  {
459  good_start[Ngood_start++] = x;
460  }
461  }
462  good_start[Ngood_start++] = right;
463 
464  MapType mid_map[3];
465  for(unsigned char i=0; i<Ngood_start-1; ++i)
466  {
467  double mid = (good_start[i] + good_start[i+1])*0.5;
468  mid_map[i] = zero->signal(mid) <= one->signal(mid) ? OLD : NEW;
469  }
470 
471  //-----------------------------------output----------------------------------
472  N = 0;
473  if(zero->start() < left - local_epsilon) //additional "zero" interval
474  {
475  if(mid_map[0] == OLD) //first interval in the map is already the old one
476  {
477  good_start[0] = zero->start();
478  }
479  else
480  {
481  map[N] = OLD; //"zero" interval
482  start[N++] = zero->start();
483  }
484  }
485 
486  for(long i=0;i<Ngood_start-1;++i) //for all intervals
487  {
488  MapType current_map = mid_map[i];
489  if(N==0 || map[N-1] != current_map)
490  {
491  map[N] = current_map;
492  start[N++] = good_start[i];
493  }
494  }
495 
496  if(zero->stop() > one->stop() + local_epsilon)
497  {
498  if(N==0 || map[N-1] == NEW)
499  {
500  map[N] = OLD; //"zero" interval
501  start[N++] = one->stop();
502  }
503  }
504 
505  start[0] = zero->start(); // just to make sure that epsilons do not damage anything
506  //start[N] = zero->stop();
507 
508  return N;
509 }
510 
511 inline void GeodesicAlgorithmExact::initialize_propagation_data()
512 {
513  clear();
514 
515  IntervalWithStop candidate;
516  std::vector<edge_pointer> edges_visible_from_source;
517  for(unsigned i=0; i<m_sources.size(); ++i) //for all edges adjacent to the starting vertex
518  {
519  SurfacePoint* source = &m_sources[i];
520 
521  edges_visible_from_source.clear();
522  list_edges_visible_from_source(source->base_element(),
523  edges_visible_from_source);
524 
525  for(unsigned j=0; j<edges_visible_from_source.size(); ++j)
526  {
527  edge_pointer e = edges_visible_from_source[j];
528  candidate.initialize(e, source, i);
529  candidate.stop() = e->length();
530  candidate.compute_min_distance(candidate.stop());
531  candidate.direction() = Interval::FROM_SOURCE;
532 
533  update_list_and_queue(interval_list(e), &candidate, 1);
534  }
535  }
536 }
537 
538 inline void GeodesicAlgorithmExact::propagate(std::vector<SurfacePoint>& sources,
539  double max_propagation_distance, //propagation algorithm stops after reaching the certain distance from the source
540  std::vector<SurfacePoint>* stop_points)
541 {
542  set_stop_conditions(stop_points, max_propagation_distance);
543  set_sources(sources);
544  initialize_propagation_data();
545 
546  clock_t start = clock();
547 
548  unsigned satisfied_index = 0;
549 
550  m_iterations = 0; //for statistics
551  m_queue_max_size = 0;
552 
553  IntervalWithStop candidates[2];
554 
555  while(!m_queue.empty())
556  {
557  m_queue_max_size = std::max(m_queue.size(), m_queue_max_size);
558 
559  unsigned const check_period = 10;
560  if(++m_iterations % check_period == 0) //check if we covered all required vertices
561  {
562  if (check_stop_conditions(satisfied_index))
563  {
564  break;
565  }
566  }
567 
568  interval_pointer min_interval = *m_queue.begin();
569  m_queue.erase(m_queue.begin());
570  edge_pointer edge = min_interval->edge();
571  list_pointer list = interval_list(edge);
572 
573  assert(min_interval->d() < GEODESIC_INF);
574 
575  bool const first_interval = min_interval->start() == 0.0;
576  //bool const last_interval = min_interval->stop() == edge->length();
577  bool const last_interval = min_interval->next() == NULL;
578 
579  bool const turn_left = edge->v0()->saddle_or_boundary();
580  bool const turn_right = edge->v1()->saddle_or_boundary();
581 
582  for(unsigned i=0; i<edge->adjacent_faces().size(); ++i) //two possible faces to propagate
583  {
584  if(!edge->is_boundary()) //just in case, always propagate boundary edges
585  {
586  if((i == 0 && min_interval->direction() == Interval::FROM_FACE_0) ||
587  (i == 1 && min_interval->direction() == Interval::FROM_FACE_1))
588  {
589  continue;
590  }
591  }
592 
593  face_pointer face = edge->adjacent_faces()[i]; //if we come from 1, go to 2
594  edge_pointer next_edge = face->next_edge(edge,edge->v0());
595 
596  unsigned num_propagated = compute_propagated_parameters(min_interval->pseudo_x(),
597  min_interval->pseudo_y(),
598  min_interval->d(), //parameters of the interval
599  min_interval->start(),
600  min_interval->stop(), //start/end of the interval
601  face->vertex_angle(edge->v0()), //corner angle
602  next_edge->length(), //length of the new edge
603  first_interval, //if it is the first interval on the edge
604  last_interval,
605  turn_left,
606  turn_right,
607  candidates); //if it is the last interval on the edge
608  bool propagate_to_right = true;
609 
610  if(num_propagated)
611  {
612  if(candidates[num_propagated-1].stop() != next_edge->length())
613  {
614  propagate_to_right = false;
615  }
616 
617  bool const invert = next_edge->v0()->id() != edge->v0()->id(); //if the origins coinside, do not invert intervals
618 
619  construct_propagated_intervals(invert, //do not inverse
620  next_edge,
621  face,
622  candidates,
623  num_propagated,
624  min_interval);
625 
626  update_list_and_queue(interval_list(next_edge),
627  candidates,
628  num_propagated);
629  }
630 
631  if(propagate_to_right)
632  {
633  //propogation to the right edge
634  double length = edge->length();
635  next_edge = face->next_edge(edge,edge->v1());
636 
637  num_propagated = compute_propagated_parameters(length - min_interval->pseudo_x(),
638  min_interval->pseudo_y(),
639  min_interval->d(), //parameters of the interval
640  length - min_interval->stop(),
641  length - min_interval->start(), //start/end of the interval
642  face->vertex_angle(edge->v1()), //corner angle
643  next_edge->length(), //length of the new edge
644  last_interval, //if it is the first interval on the edge
645  first_interval,
646  turn_right,
647  turn_left,
648  candidates); //if it is the last interval on the edge
649 
650  if(num_propagated)
651  {
652  bool const invert = next_edge->v0()->id() != edge->v1()->id(); //if the origins coinside, do not invert intervals
653 
654  construct_propagated_intervals(invert, //do not inverse
655  next_edge,
656  face,
657  candidates,
658  num_propagated,
659  min_interval);
660 
661  update_list_and_queue(interval_list(next_edge),
662  candidates,
663  num_propagated);
664  }
665  }
666  }
667  }
668 
669  m_propagation_distance_stopped = m_queue.empty() ? GEODESIC_INF : (*m_queue.begin())->min();
670  clock_t stop = clock();
671  m_time_consumed = (static_cast<double>(stop)-static_cast<double>(start))/CLOCKS_PER_SEC;
672 
673 /* for(unsigned i=0; i<m_edge_interval_lists.size(); ++i)
674  {
675  list_pointer list = &m_edge_interval_lists[i];
676  interval_pointer p = list->first();
677  assert(p->start() == 0.0);
678  while(p->next())
679  {
680  assert(p->stop() == p->next()->start());
681  assert(p->d() < GEODESIC_INF);
682  p = p->next();
683  }
684  }*/
685 }
686 
687 
688 inline bool GeodesicAlgorithmExact::check_stop_conditions(unsigned& index)
689 {
690  double queue_distance = (*m_queue.begin())->min();
691  if(queue_distance < stop_distance())
692  {
693  return false;
694  }
695 
696  while(index < m_stop_vertices.size())
697  {
698  vertex_pointer v = m_stop_vertices[index].first;
699  edge_pointer edge = v->adjacent_edges()[0]; //take any edge
700 
701  double distance = edge->v0()->id() == v->id() ?
702  interval_list(edge)->signal(0.0) :
703  interval_list(edge)->signal(edge->length());
704 
705  if(queue_distance < distance + m_stop_vertices[index].second)
706  {
707  return false;
708  }
709 
710  ++index;
711  }
712  return true;
713 }
714 
715 
716 inline void GeodesicAlgorithmExact::update_list_and_queue(list_pointer list,
717  IntervalWithStop* candidates, //up to two candidates
718  unsigned num_candidates)
719 {
720  assert(num_candidates <= 2);
721  //assert(list->first() != NULL);
722  edge_pointer edge = list->edge();
723  double const local_epsilon = SMALLEST_INTERVAL_RATIO * edge->length();
724 
725  if(list->first() == NULL)
726  {
727  interval_pointer* p = &list->first();
728  IntervalWithStop* first;
729  IntervalWithStop* second;
730 
731  if(num_candidates == 1)
732  {
733  first = candidates;
734  second = candidates;
735  first->compute_min_distance(first->stop());
736  }
737  else
738  {
739  if(candidates->start() <= (candidates+1)->start())
740  {
741  first = candidates;
742  second = candidates+1;
743  }
744  else
745  {
746  first = candidates+1;
747  second = candidates;
748  }
749  assert(first->stop() == second->start());
750 
751  first->compute_min_distance(first->stop());
752  second->compute_min_distance(second->stop());
753  }
754 
755  if(first->start() > 0.0)
756  {
757  *p = m_memory_allocator.allocate();
758  (*p)->initialize(edge);
759  p = &(*p)->next();
760  }
761 
762  *p = m_memory_allocator.allocate();
763  memcpy(*p,first,sizeof(Interval));
764  m_queue.insert(*p);
765 
766  if(num_candidates == 2)
767  {
768  p = &(*p)->next();
769  *p = m_memory_allocator.allocate();
770  memcpy(*p,second,sizeof(Interval));
771  m_queue.insert(*p);
772  }
773 
774  if(second->stop() < edge->length())
775  {
776  p = &(*p)->next();
777  *p = m_memory_allocator.allocate();
778  (*p)->initialize(edge);
779  (*p)->start() = second->stop();
780  }
781  else
782  {
783  (*p)->next() = NULL;
784  }
785  return;
786  }
787 
788  bool propagate_flag;
789 
790  for(unsigned i=0; i<num_candidates; ++i) //for all new intervals
791  {
792  IntervalWithStop* q = &candidates[i];
793 
794  interval_pointer previous = NULL;
795 
796  interval_pointer p = list->first();
797  assert(p->start() == 0.0);
798 
799  while(p != NULL && p->stop() - local_epsilon < q->start())
800  {
801  p = p->next();
802  }
803 
804  while(p != NULL && p->start() < q->stop() - local_epsilon) //go through all old intervals
805  {
806  unsigned const N = intersect_intervals(p, q); //interset two intervals
807 
808  if(N == 1)
809  {
810  if(map[0]==OLD) //if "p" is always better, we do not need to update anything)
811  {
812  if(previous) //close previous interval and put in into the queue
813  {
814  previous->next() = p;
815  previous->compute_min_distance(p->start());
816  m_queue.insert(previous);
817  previous = NULL;
818  }
819 
820  p = p->next();
821 
822  }
823  else if(previous) //extend previous interval to cover everything; remove p
824  {
825  previous->next() = p->next();
826  erase_from_queue(p);
827  m_memory_allocator.deallocate(p);
828 
829  p = previous->next();
830  }
831  else //p becomes "previous"
832  {
833  previous = p;
834  interval_pointer next = p->next();
835  erase_from_queue(p);
836 
837  memcpy(previous,q,sizeof(Interval));
838 
839  previous->start() = start[0];
840  previous->next() = next;
841 
842  p = next;
843  }
844  continue;
845  }
846 
847  //update_flag = true;
848 
849  Interval swap(*p); //used for swapping information
850  propagate_flag = erase_from_queue(p);
851 
852  for(unsigned j=1; j<N; ++j) //no memory is needed for the first one
853  {
854  i_new[j] = m_memory_allocator.allocate(); //create new intervals
855  }
856 
857  if(map[0]==OLD) //finish previous, if any
858  {
859  if(previous)
860  {
861  previous->next() = p;
862  previous->compute_min_distance(previous->stop());
863  m_queue.insert(previous);
864  previous = NULL;
865  }
866  i_new[0] = p;
867  p->next() = i_new[1];
868  p->start() = start[0];
869  }
870  else if(previous) //extend previous interval to cover everything; remove p
871  {
872  i_new[0] = previous;
873  previous->next() = i_new[1];
874  m_memory_allocator.deallocate(p);
875  previous = NULL;
876  }
877  else //p becomes "previous"
878  {
879  i_new[0] = p;
880  memcpy(p,q,sizeof(Interval));
881 
882  p->next() = i_new[1];
883  p->start() = start[0];
884  }
885 
886  assert(!previous);
887 
888  for(unsigned j=1; j<N; ++j)
889  {
890  interval_pointer current_interval = i_new[j];
891 
892  if(map[j] == OLD)
893  {
894  memcpy(current_interval,&swap,sizeof(Interval));
895  }
896  else
897  {
898  memcpy(current_interval,q,sizeof(Interval));
899  }
900 
901  if(j == N-1)
902  {
903  current_interval->next() = swap.next();
904  }
905  else
906  {
907  current_interval->next() = i_new[j+1];
908  }
909 
910  current_interval->start() = start[j];
911  }
912 
913  for(unsigned j=0; j<N; ++j) //find "min" and add the intervals to the queue
914  {
915  if(j==N-1 && map[j]==NEW)
916  {
917  previous = i_new[j];
918  }
919  else
920  {
921  interval_pointer current_interval = i_new[j];
922 
923  current_interval->compute_min_distance(current_interval->stop()); //compute minimal distance
924 
925  if(map[j]==NEW || (map[j]==OLD && propagate_flag))
926  {
927  m_queue.insert(current_interval);
928  }
929  }
930  }
931 
932  p = swap.next();
933  }
934 
935  if(previous) //close previous interval and put in into the queue
936  {
937  previous->compute_min_distance(previous->stop());
938  m_queue.insert(previous);
939  previous = NULL;
940  }
941  }
942 }
943 
944 inline unsigned GeodesicAlgorithmExact::compute_propagated_parameters(double pseudo_x,
945  double pseudo_y,
946  double d, //parameters of the interval
947  double begin,
948  double end, //start/end of the interval
949  double alpha, //corner angle
950  double L, //length of the new edge
951  bool first_interval, //if it is the first interval on the edge
952  bool last_interval,
953  bool turn_left,
954  bool turn_right,
955  IntervalWithStop* candidates) //if it is the last interval on the edge
956 {
957  assert(pseudo_y<=0.0);
958  assert(d<GEODESIC_INF/10.0);
959  assert(begin<=end);
960  assert(first_interval ? (begin == 0.0) : true);
961 
962  IntervalWithStop* p = candidates;
963 
964  if(std::abs(pseudo_y) <= 1e-30) //pseudo-source is on the edge
965  {
966  if(first_interval && pseudo_x <= 0.0)
967  {
968  p->start() = 0.0;
969  p->stop() = L;
970  p->d() = d - pseudo_x;
971  p->pseudo_x() = 0.0;
972  p->pseudo_y() = 0.0;
973  return 1;
974  }
975  else if(last_interval && pseudo_x >= end)
976  {
977  p->start() = 0.0;
978  p->stop() = L;
979  p->d() = d + pseudo_x-end;
980  p->pseudo_x() = end*cos(alpha);
981  p->pseudo_y() = -end*sin(alpha);
982  return 1;
983  }
984  else if(pseudo_x >= begin && pseudo_x <= end)
985  {
986  p->start() = 0.0;
987  p->stop() = L;
988  p->d() = d;
989  p->pseudo_x() = pseudo_x*cos(alpha);
990  p->pseudo_y() = -pseudo_x*sin(alpha);
991  return 1;
992  }
993  else
994  {
995  return 0;
996  }
997  }
998 
999  double sin_alpha = sin(alpha);
1000  double cos_alpha = cos(alpha);
1001 
1002  //important: for the first_interval, this function returns zero only if the new edge is "visible" from the source
1003  //if the new edge can be covered only after turn_over, the value is negative (-1.0)
1004  double L1 = compute_positive_intersection(begin,
1005  pseudo_x,
1006  pseudo_y,
1007  sin_alpha,
1008  cos_alpha);
1009 
1010  if(L1 < 0 || L1 >= L)
1011  {
1012  if(first_interval && turn_left)
1013  {
1014  p->start() = 0.0;
1015  p->stop() = L;
1016  p->d() = d + sqrt(pseudo_x*pseudo_x + pseudo_y*pseudo_y);
1017  p->pseudo_y() = 0.0;
1018  p->pseudo_x() = 0.0;
1019  return 1;
1020  }
1021  else
1022  {
1023  return 0;
1024  }
1025  }
1026 
1027  double L2 = compute_positive_intersection(end,
1028  pseudo_x,
1029  pseudo_y,
1030  sin_alpha,
1031  cos_alpha);
1032 
1033  if(L2 < 0 || L2 >= L)
1034  {
1035  p->start() = L1;
1036  p->stop() = L;
1037  p->d() = d;
1038  p->pseudo_x() = cos_alpha*pseudo_x + sin_alpha*pseudo_y;
1039  p->pseudo_y() = -sin_alpha*pseudo_x + cos_alpha*pseudo_y;
1040 
1041  return 1;
1042  }
1043 
1044  p->start() = L1;
1045  p->stop() = L2;
1046  p->d() = d;
1047  p->pseudo_x() = cos_alpha*pseudo_x + sin_alpha*pseudo_y;
1048  p->pseudo_y() = -sin_alpha*pseudo_x + cos_alpha*pseudo_y;
1049  assert(p->pseudo_y() <= 0.0);
1050 
1051  if(!(last_interval && turn_right))
1052  {
1053  return 1;
1054  }
1055  else
1056  {
1057  p = candidates + 1;
1058 
1059  p->start() = L2;
1060  p->stop() = L;
1061  double dx = pseudo_x - end;
1062  p->d() = d + sqrt(dx*dx + pseudo_y*pseudo_y);
1063  p->pseudo_x() = end*cos_alpha;
1064  p->pseudo_y() = -end*sin_alpha;
1065 
1066  return 2;
1067  }
1068 }
1069 
1070 inline void GeodesicAlgorithmExact::construct_propagated_intervals(bool invert,
1071  edge_pointer edge,
1072  face_pointer face, //constructs iNew from the rest of the data
1073  IntervalWithStop* candidates,
1074  unsigned& num_candidates,
1075  interval_pointer source_interval) //up to two candidates
1076 {
1077  double edge_length = edge->length();
1078  double local_epsilon = SMALLEST_INTERVAL_RATIO * edge_length;
1079 
1080  //kill very small intervals in order to avoid precision problems
1081  if(num_candidates == 2)
1082  {
1083  double start = std::min(candidates->start(), (candidates+1)->start());
1084  double stop = std::max(candidates->stop(), (candidates+1)->stop());
1085  if(candidates->stop()-candidates->start() < local_epsilon) // kill interval 0
1086  {
1087  *candidates = *(candidates+1);
1088  num_candidates = 1;
1089  candidates->start() = start;
1090  candidates->stop() = stop;
1091  }
1092  else if ((candidates+1)->stop() - (candidates+1)->start() < local_epsilon)
1093  {
1094  num_candidates = 1;
1095  candidates->start() = start;
1096  candidates->stop() = stop;
1097  }
1098  }
1099 
1100  IntervalWithStop* first;
1101  IntervalWithStop* second;
1102  if(num_candidates == 1)
1103  {
1104  first = candidates;
1105  second = candidates;
1106  }
1107  else
1108  {
1109  if(candidates->start() <= (candidates+1)->start())
1110  {
1111  first = candidates;
1112  second = candidates+1;
1113  }
1114  else
1115  {
1116  first = candidates+1;
1117  second = candidates;
1118  }
1119  assert(first->stop() == second->start());
1120  }
1121 
1122  if(first->start() < local_epsilon)
1123  {
1124  first->start() = 0.0;
1125  }
1126  if(edge_length - second->stop() < local_epsilon)
1127  {
1128  second->stop() = edge_length;
1129  }
1130 
1131  //invert intervals if necessary; fill missing data and set pointers correctly
1132  Interval::DirectionType direction = edge->adjacent_faces()[0]->id() == face->id() ?
1135 
1136  if(!invert) //in this case everything is straighforward, we do not have to invert the intervals
1137  {
1138  for(unsigned i=0; i<num_candidates; ++i)
1139  {
1140  IntervalWithStop* p = candidates + i;
1141 
1142  p->next() = (i == num_candidates - 1) ? NULL : candidates + i + 1;
1143  p->edge() = edge;
1144  p->direction() = direction;
1145  p->source_index() = source_interval->source_index();
1146 
1147  p->min() = 0.0; //it will be changed later on
1148 
1149  assert(p->start() < p->stop());
1150  }
1151  }
1152  else //now we have to invert the intervals
1153  {
1154  for(unsigned i=0; i<num_candidates; ++i)
1155  {
1156  IntervalWithStop* p = candidates + i;
1157 
1158  p->next() = (i == 0) ? NULL : candidates + i - 1;
1159  p->edge() = edge;
1160  p->direction() = direction;
1161  p->source_index() = source_interval->source_index();
1162 
1163  double length = edge_length;
1164  p->pseudo_x() = length - p->pseudo_x();
1165 
1166  double start = length - p->stop();
1167  p->stop() = length - p->start();
1168  p->start() = start;
1169 
1170  p->min() = 0;
1171 
1172  //assert(p->start() < p->stop());
1173  assert(p->start() >= 0.0);
1174  assert(p->stop() <= edge->length());
1175  }
1176  }
1177 }
1178 
1179 
1180 inline unsigned GeodesicAlgorithmExact::best_source(SurfacePoint& point, //quickly find what source this point belongs to and what is the distance to this source
1181  double& best_source_distance)
1182 {
1183  double best_interval_position;
1184  unsigned best_source_index;
1185 
1186  best_first_interval(point,
1187  best_source_distance,
1188  best_interval_position,
1189  best_source_index);
1190 
1191  return best_source_index;
1192 }
1193 
1194 inline interval_pointer GeodesicAlgorithmExact::best_first_interval(SurfacePoint& point,
1195  double& best_total_distance,
1196  double& best_interval_position,
1197  unsigned& best_source_index)
1198 {
1199  assert(point.type() != UNDEFINED_POINT);
1200 
1201  interval_pointer best_interval = NULL;
1202  best_total_distance = GEODESIC_INF;
1203 
1204  if(point.type() == EDGE)
1205  {
1206  edge_pointer e = static_cast<edge_pointer>(point.base_element());
1207  list_pointer list = interval_list(e);
1208 
1209  best_interval_position = point.distance(e->v0());
1210  best_interval = list->covering_interval(best_interval_position);
1211  if(best_interval)
1212  {
1213  //assert(best_interval && best_interval->d() < GEODESIC_INF);
1214  best_total_distance = best_interval->signal(best_interval_position);
1215  best_source_index = best_interval->source_index();
1216  }
1217  }
1218  else if(point.type() == FACE)
1219  {
1220  face_pointer f = static_cast<face_pointer>(point.base_element());
1221  for(unsigned i=0; i<3; ++i)
1222  {
1223  edge_pointer e = f->adjacent_edges()[i];
1224  list_pointer list = interval_list(e);
1225 
1226  double offset;
1227  double distance;
1228  interval_pointer interval;
1229 
1230  list->find_closest_point(&point,
1231  offset,
1232  distance,
1233  interval);
1234 
1235  if(interval && distance < best_total_distance)
1236  {
1237  best_interval = interval;
1238  best_total_distance = distance;
1239  best_interval_position = offset;
1240  best_source_index = interval->source_index();
1241  }
1242  }
1243 
1244  //check for all sources that might be located inside this face
1245  SortedSources::sorted_iterator_pair local_sources = m_sources.sources(f);
1246  for(SortedSources::sorted_iterator it=local_sources.first; it != local_sources.second; ++it)
1247  {
1248  SurfacePointWithIndex* source = *it;
1249  double distance = point.distance(source);
1250  if(distance < best_total_distance)
1251  {
1252  best_interval = NULL;
1253  best_total_distance = distance;
1254  best_interval_position = 0.0;
1255  best_source_index = source->index();
1256  }
1257  }
1258  }
1259  else if(point.type() == VERTEX)
1260  {
1261  vertex_pointer v = static_cast<vertex_pointer>(point.base_element());
1262  for(unsigned i=0; i<v->adjacent_edges().size(); ++i)
1263  {
1264  edge_pointer e = v->adjacent_edges()[i];
1265  list_pointer list = interval_list(e);
1266 
1267  double position = e->v0()->id() == v->id() ? 0.0 : e->length();
1268  interval_pointer interval = list->covering_interval(position);
1269  if(interval)
1270  {
1271  double distance = interval->signal(position);
1272 
1273  if(distance < best_total_distance)
1274  {
1275  best_interval = interval;
1276  best_total_distance = distance;
1277  best_interval_position = position;
1278  best_source_index = interval->source_index();
1279  }
1280  }
1281  }
1282  }
1283 
1284  if(best_total_distance > m_propagation_distance_stopped) //result is unreliable
1285  {
1286  best_total_distance = GEODESIC_INF;
1287  return NULL;
1288  }
1289  else
1290  {
1291  return best_interval;
1292  }
1293 }
1294 
1296  std::vector<SurfacePoint>& path,std::vector<unsigned>& indexVertex)
1297 {
1298  trace_back(destination,path);
1299 }
1300 
1301 inline void GeodesicAlgorithmExact::trace_back(SurfacePoint& destination, //trace back piecewise-linear path
1302  std::vector<SurfacePoint>& path)
1303 {
1304  path.clear();
1305  double best_total_distance;
1306  double best_interval_position;
1307  unsigned source_index = std::numeric_limits<unsigned>::max();
1308  interval_pointer best_interval = best_first_interval(destination,
1309  best_total_distance,
1310  best_interval_position,
1311  source_index);
1312 
1313  if(best_total_distance >= GEODESIC_INF/2.0) //unable to find the right path
1314  {
1315  return;
1316  }
1317 
1318  path.push_back(destination);
1319 
1320  if(best_interval) //if we did not hit the face source immediately
1321  {
1322  std::vector<edge_pointer> possible_edges;
1323  possible_edges.reserve(10);
1324 
1325  while(visible_from_source(path.back()) < 0) //while this point is not in the direct visibility of some source (if we are inside the FACE, we obviously hit the source)
1326  {
1327  SurfacePoint& q = path.back();
1328 
1329  possible_traceback_edges(q, possible_edges);
1330 
1331  interval_pointer interval;
1332  double total_distance;
1333  double position;
1334 
1335  best_point_on_the_edge_set(q,
1336  possible_edges,
1337  interval,
1338  total_distance,
1339  position);
1340 
1341  //std::cout << total_distance + length(path) << std::endl;
1342  assert(total_distance<GEODESIC_INF);
1343  source_index = interval->source_index();
1344 
1345  edge_pointer e = interval->edge();
1346  double local_epsilon = SMALLEST_INTERVAL_RATIO*e->length();
1347  if(position < local_epsilon)
1348  {
1349  path.push_back(SurfacePoint(e->v0()));
1350  }
1351  else if(position > e->length()-local_epsilon)
1352  {
1353  path.push_back(SurfacePoint(e->v1()));
1354  }
1355  else
1356  {
1357  double normalized_position = position/e->length();
1358  path.push_back(SurfacePoint(e, normalized_position));
1359  }
1360  }
1361  }
1362 
1363  SurfacePoint& source = static_cast<SurfacePoint&>(m_sources[source_index]);
1364  if(path.back().distance(&source) > 0)
1365  {
1366  path.push_back(source);
1367  }
1368 }
1369 
1371 {
1373 
1374  unsigned interval_counter = 0;
1375  for(unsigned i=0; i<m_edge_interval_lists.size(); ++i)
1376  {
1377  interval_counter += m_edge_interval_lists[i].number_of_intervals();
1378  }
1379  double intervals_per_edge = (double)interval_counter/(double)m_edge_interval_lists.size();
1380 
1381  double memory = m_edge_interval_lists.size()*sizeof(IntervalList) +
1382  interval_counter*sizeof(Interval);
1383 
1384  std::cout << "uses about " << memory/1e6 << "Mb of memory" <<std::endl;
1385  std::cout << interval_counter << " total intervals, or "
1386  << intervals_per_edge << " intervals per edge"
1387  << std::endl;
1388  std::cout << "maximum interval queue size is " << m_queue_max_size << std::endl;
1389  std::cout << "number of interval propagations is " << m_iterations << std::endl;
1390 }
1391 
1392 } //geodesic
1393 
1394 #endif //GEODESIC_ALGORITHM_EXACT_20071231
unsigned best_source(SurfacePoint &point, double &best_source_distance)
float min(float x, float y)
Definition: thickness.h:105
float max(float x, float y)
Definition: thickness.h:97
edge_pointer_vector & adjacent_edges()
std::pair< sorted_iterator, sorted_iterator > sorted_iterator_pair
void set_stop_conditions(std::vector< SurfacePoint > *stop_points, double stop_distance)
std::vector< stop_vertex_with_distace_type > m_stop_vertices
std::vector< Edge > & edges()
Definition: geodesic_mesh.h:44
Vertex * vertex_pointer
vertex_pointer v0()
void propagate(std::vector< SurfacePoint > &sources, double max_propagation_distance=GEODESIC_INF, std::vector< SurfacePoint > *stop_points=NULL)
sorted_vector_type::iterator sorted_iterator
double vertex_angle(vertex_pointer v)
double length(std::vector< SurfacePoint > &path)
interval_pointer covering_interval(double offset)
void find_closest_point(double const x, double const y, double &offset, double &distance)
vertex_pointer v1()
void trace_back(SurfacePoint &destination, std::vector< SurfacePoint > &path)
void initialize(std::vector< SurfacePoint > &sources)
face_pointer_vector & adjacent_faces()
double distance(double *v)
sorted_iterator_pair sources(base_pointer mesh_element)
void swap(scoped_ptr< T > &x, scoped_ptr< T > &y)
edge_pointer next_edge(edge_pointer e, vertex_pointer v)
void trace_back_with_index(SurfacePoint &destination, std::vector< SurfacePoint > &path, std::vector< unsigned > &indexVertex)
static _Tp max()