geodesic_cpp_03_02_2008/geodesic_algorithm_dijkstra_alternative.h

//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