geodesic_algorithm_dijkstra_alternative.h

Go to the documentation of this file.

//Copyright (C) 2008 Danil Kirsanov, MIT License
#ifndef GEODESIC_ALGORITHM_DIJKSTRA_ALTERNATIVE_010506
#define GEODESIC_ALGORITHM_DIJKSTRA_ALTERNATIVE_010506

#include "geodesic_algorithm_base.h"
#include "geodesic_mesh_elements.h"
#include <vector>
#include <set>
#include <assert.h>

namespace geodesic{

class DijkstraNode1
{
  typedef DijkstraNode1* node_pointer;
public: 
  DijkstraNode1(){};
  ~DijkstraNode1(){};

  double& distance_from_source(){return m_distance;};
  node_pointer& previous(){return m_previous;};
  unsigned& source_index(){return m_source_index;};
  vertex_pointer& vertex(){return m_vertex;};

  void clear()
  {
    m_distance = GEODESIC_INF;
    m_previous = NULL;
  }

  bool operator()(node_pointer const s1, node_pointer const s2) const
  {
    return s1->distance_from_source() != s2->distance_from_source() ?
         s1->distance_from_source() < s2->distance_from_source() :
         s1->vertex()->id() < s2->vertex()->id();
  };

private: 
  double m_distance;          //distance to the closest source
  unsigned m_source_index;      //closest source index
  node_pointer m_previous;      //previous node in the geodesic path
  vertex_pointer m_vertex;      //correspoding vertex
};

class GeodesicAlgorithmDijkstraAlternative: public GeodesicAlgorithmBase
{
public:
  typedef DijkstraNode1 Node;
  typedef Node* node_pointer;

  GeodesicAlgorithmDijkstraAlternative(geodesic::Mesh* mesh = NULL):
    GeodesicAlgorithmBase(mesh),
    m_nodes(mesh->vertices().size())
  {
    m_type = DIJKSTRA;
    for(unsigned i=0; i<m_nodes.size(); ++i)
    {
      m_nodes[i].vertex() = &m_mesh->vertices()[i];
    }
  };

  ~GeodesicAlgorithmDijkstraAlternative(){};

  virtual void propagate(std::vector<SurfacePoint>& sources,
                 double max_propagation_distance  = GEODESIC_INF,     //propagation algorithm stops after reaching the certain distance from the source
               std::vector<SurfacePoint>* stop_points = NULL); //or after ensuring that all the stop_points are covered

  virtual void trace_back(SurfacePoint& destination,    //trace back piecewise-linear path
              std::vector<SurfacePoint>& path);

  virtual unsigned best_source(SurfacePoint& point,     //quickly find what source this point belongs to and what is the distance to this source
                  double& best_source_distance);
private:

  void set_sources(std::vector<SurfacePoint>& sources)
  {
    m_sources = sources;
  }

  bool check_stop_conditions(unsigned& index);

  std::vector<Node> m_nodes;  //nodes of the subdivision graph located on the vertices

  std::vector<SurfacePoint> m_sources;    //for simplicity, we keep sources as they are

  typedef std::set<node_pointer, Node> queue_type;
  queue_type m_queue;
};

inline unsigned GeodesicAlgorithmDijkstraAlternative::best_source(SurfacePoint& point,      //quickly find what source this point belongs to and what is the distance to this source
                                     double& best_source_distance)
{
  if(point.type() == VERTEX)
  {
    vertex_pointer v = (vertex_pointer)point.base_element();
    node_pointer node = &m_nodes[v->id()];
    best_source_distance = node->distance_from_source();
    return node->source_index();
  }
  else
  {
    std::vector<vertex_pointer> possible_vertices;    //initialize vertices directly visible from sources
    m_mesh->closest_vertices(&point, &possible_vertices);

    best_source_distance = GEODESIC_INF;
    vertex_pointer min_vertex = NULL;
    for(unsigned i=0; i<possible_vertices.size(); ++i)
    {
      vertex_pointer v = possible_vertices[i];

      double distance_from_source = m_nodes[v->id()].distance_from_source();
      double distance_from_dest = v->distance(&point);
      if(distance_from_source + distance_from_dest < best_source_distance)
      {
        best_source_distance = distance_from_source + distance_from_dest;
        min_vertex = v;
      }
    }
    assert(min_vertex);
    node_pointer node = &m_nodes[min_vertex->id()];
    return node->source_index();
  }
}

inline void GeodesicAlgorithmDijkstraAlternative::trace_back(SurfacePoint& destination,   //trace back piecewise-linear path
                               std::vector<SurfacePoint>& path)
{
  path.clear();
  path.push_back(destination);

  if(destination.type() != VERTEX)    //snap to the closest vertex
  {
    std::vector<vertex_pointer> possible_vertices;    //initialize vertices directly visible from sources
    m_mesh->closest_vertices(&destination, &possible_vertices);

    double min_distance = GEODESIC_INF;
    vertex_pointer min_vertex = NULL;
    for(unsigned i=0; i<possible_vertices.size(); ++i)
    {
      vertex_pointer v = possible_vertices[i];

      double distance_from_source = m_nodes[v->id()].distance_from_source();
      double distance_from_dest = v->distance(&destination);
      if(distance_from_source + distance_from_dest < min_distance)
      {
        min_distance = distance_from_source + distance_from_dest;
        min_vertex = v;
      }
    }
    assert(min_vertex);
    path.push_back(SurfacePoint(min_vertex));
  }

  node_pointer node = &m_nodes[path.back().base_element()->id()];
  while(node->previous())     //follow the path
  {
    node = node->previous();
    path.push_back(SurfacePoint(node->vertex()));
  }

  SurfacePoint& source = m_sources[node->source_index()];   //add source to the path if it is not already there
  if(source.type() != VERTEX ||
     source.base_element()->id() != node->vertex()->id())
  {
    path.push_back(source);
  }
}

inline void GeodesicAlgorithmDijkstraAlternative::propagate(std::vector<SurfacePoint>& sources,
                                double max_propagation_distance,      //propagation algorithm stops after reaching the certain distance from the source
                              std::vector<SurfacePoint>* stop_points)   //or after ensuring that all the stop_points are covered
{
  set_stop_conditions(stop_points, max_propagation_distance);
  set_sources(sources);

  m_queue.clear();                  //clear everything
  for(unsigned i=0; i<m_nodes.size(); ++i)
  {
    m_nodes[i].clear();
  }

  clock_t start = clock();

  std::vector<vertex_pointer> visible_vertices;   //initialize vertices directly visible from sources
  for(unsigned i=0; i<m_sources.size(); ++i)
  {
    SurfacePoint* s = &m_sources[i];
    m_mesh->closest_vertices(s, &visible_vertices);
    for(unsigned j=0; j<visible_vertices.size(); ++j)
    {
      vertex_pointer v = visible_vertices[j];
      double distance = s->distance(v);

      node_pointer node = &m_nodes[v->id()];
      if(distance < node->distance_from_source())
      {
        node->distance_from_source() = distance;
        node->source_index() = i;
        node->previous() = NULL;
      }
    }
    visible_vertices.clear();
  }

  for(unsigned i=0; i<m_nodes.size(); ++i)    //initialize the queue
  {
    if(m_nodes[i].distance_from_source() < GEODESIC_INF)
    {
      m_queue.insert(&m_nodes[i]);
    }
  }

  unsigned counter = 0;
  unsigned satisfied_index = 0;

  while(!m_queue.empty())         //main cycle
  {
    if(counter++ % 10 == 0)   //check if we covered all required vertices
    {
      if (check_stop_conditions(satisfied_index))
      {
        break;
      }
      //std::cout << counter << " " << m_queue.size() << " " << (*m_queue.begin())->distance_from_source()<< std::endl;
    }

    node_pointer min_node = *m_queue.begin();
    m_queue.erase(m_queue.begin());

    vertex_pointer v = min_node->vertex();

    for(unsigned i=0; i<v->adjacent_edges().size(); ++i)    //update all the adgecent vertices
    {
      edge_pointer e = v->adjacent_edges()[i];
      vertex_pointer next_v = e->opposite_vertex(v);
      node_pointer next_node = &m_nodes[next_v->id()];
      //double current_distance =
      if(next_node->distance_from_source() > min_node->distance_from_source() + e->length())
      {
        if(next_node->distance_from_source() < GEODESIC_INF)    //remove it from the queue
        {
          queue_type::iterator iter = m_queue.find(next_node);
          assert(iter != m_queue.end());
          m_queue.erase(iter);
        }
        next_node->distance_from_source() = min_node->distance_from_source() + e->length();
        next_node->source_index() = min_node->source_index();
        next_node->previous() = min_node;
        m_queue.insert(next_node);
      }
    }
  }
  //std::cout << std::endl;

  clock_t finish = clock();
  m_time_consumed = (static_cast<double>(finish)-static_cast<double>(start))/CLOCKS_PER_SEC;
}

inline bool GeodesicAlgorithmDijkstraAlternative::check_stop_conditions(unsigned& index)
{
  double queue_min_distance = (*m_queue.begin())->distance_from_source();

  if(queue_min_distance < m_max_propagation_distance)
  {
    return false;
  }

  while(index < m_stop_vertices.size())
  {
    vertex_pointer v = m_stop_vertices[index].first;
    Node& node = m_nodes[v->id()];
    if(queue_min_distance < node.distance_from_source() + m_stop_vertices[index].second)
    {
      return false;
    }
    ++index;
  }
  return true;
}

}   //geodesic

#endif //GEODESIC_ALGORITHM_DIJKSTRA_ALTERNATIVE_010506