geodesic_cpp_03_02_2008/geodesic_algorithm_graph_base.h

#ifndef GEODESIC_ALGORITHM_GRAPH_BASE_010907
#define GEODESIC_ALGORITHM_GRAPH_BASE_010907

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

namespace geodesic{

template<class Node>
class GeodesicAlgorithmGraphBase: public GeodesicAlgorithmBase
{
public:
    typedef Node* node_pointer;

    GeodesicAlgorithmGraphBase(geodesic::Mesh* mesh):
        GeodesicAlgorithmBase(mesh)
    {};

    ~GeodesicAlgorithmGraphBase(){};

    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

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

    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); 

    void print_statistics()
    {
        GeodesicAlgorithmBase::print_statistics();

        double memory = m_nodes.size()*sizeof(Node);
        std::cout << "uses about " << memory/1e6 << "Mb of memory" <<std::endl;
    }

protected:
    unsigned node_index(vertex_pointer v)       //gives index of the node that corresponds to this vertex
    {
        return v->id();
    };

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

    node_pointer best_first_node(SurfacePoint& point, double& best_total_distance)
    {
        node_pointer best_node = NULL;  
        if(point.type() == VERTEX)      
        {
            vertex_pointer v = (vertex_pointer)point.base_element();
            best_node = &m_nodes[node_index(v)];
            best_total_distance = best_node->distance_from_source(); 
        }
        else
        {
            std::vector<node_pointer> possible_nodes;
            list_nodes_visible_from_source(point.base_element(), possible_nodes);

            best_total_distance = GEODESIC_INF;
            for(unsigned i=0; i<possible_nodes.size(); ++i)
            {
                node_pointer node = possible_nodes[i];

                double distance_from_dest = node->distance(&point);
                if(node->distance_from_source() + distance_from_dest < best_total_distance)
                {
                    best_total_distance = node->distance_from_source() + distance_from_dest;
                    best_node = node;
                }
            }
        }

        //assert(best_node);
        //assert(best_total_distance<GEODESIC_INF);
        if(best_total_distance > m_propagation_distance_stopped)        //result is unreliable
        {
            best_total_distance = GEODESIC_INF;
            return NULL;
        }
        else
        {
            return best_node;
        }
    };  //quickly find what node will be the next one in geodesic path

    bool check_stop_conditions(unsigned& index);        //check when propagation should stop

    virtual void list_nodes_visible_from_source(MeshElementBase* p, 
                                                std::vector<node_pointer>& storage) = 0;        //list all nodes that belong to this mesh element

    virtual void list_nodes_visible_from_node(node_pointer node,            //list all nodes that belong to this mesh element
                                              std::vector<node_pointer>& storage,
                                              std::vector<double>& distances, 
                                              double threshold_distance) = 0;   //list only the nodes whose current distance is larger than the threshold

    std::vector<Node> m_nodes;  //nodes of the graph 

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

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

template<class Node>
void GeodesicAlgorithmGraphBase<Node>::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();
    m_propagation_distance_stopped = GEODESIC_INF;
    for(unsigned i=0; i<m_nodes.size(); ++i)
    {
        m_nodes[i].clear();
    }

    clock_t start = clock();

    std::vector<node_pointer> visible_nodes;        //initialize vertices directly visible from sources
    for(unsigned i=0; i<m_sources.size(); ++i)
    {
        SurfacePoint* source = &m_sources[i];
        list_nodes_visible_from_source(source->base_element(), 
                                       visible_nodes);          

        for(unsigned j=0; j<visible_nodes.size(); ++j)
        {
            node_pointer node = visible_nodes[j];
            double distance = node->distance(source);
            if(distance < node->distance_from_source())
            {
                node->distance_from_source() = distance;
                node->source_index() = i;
                node->previous() = NULL;
            }
        }
        visible_nodes.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;
    
    std::vector<double> distances_between_nodes;
    while(!m_queue.empty())                 //main cycle
    {
        if(counter++ % 10 == 0)     //check if we covered all required vertices
        {
            if (check_stop_conditions(satisfied_index))
            {
                break;
            }
        }

        node_pointer min_node = *m_queue.begin();
        m_queue.erase(m_queue.begin());
        assert(min_node->distance_from_source() < GEODESIC_INF);

        visible_nodes.clear();
        distances_between_nodes.clear();
        list_nodes_visible_from_node(min_node, 
                                     visible_nodes, 
                                     distances_between_nodes, 
                                     min_node->distance_from_source());

        for(unsigned i=0; i<visible_nodes.size(); ++i)      //update all the adgecent vertices
        {
            node_pointer next_node = visible_nodes[i];

            if(next_node->distance_from_source() > min_node->distance_from_source() +
                                                   distances_between_nodes[i])
            {
                if(next_node->distance_from_source() < GEODESIC_INF)        //remove it from the queue
                {
                    typename 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() +
                                                    distances_between_nodes[i];
                next_node->source_index() = min_node->source_index();
                next_node->previous() = min_node;
                m_queue.insert(next_node);
            }
        }
    } 

    m_propagation_distance_stopped = m_queue.empty() ? GEODESIC_INF : (*m_queue.begin())->distance_from_source();
    clock_t finish = clock();
    m_time_consumed = (static_cast<double>(finish)-static_cast<double>(start))/CLOCKS_PER_SEC;
    //std::cout << std::endl;
}

template<class Node>
inline bool GeodesicAlgorithmGraphBase<Node>::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[node_index(v)];
        if(queue_min_distance < node.distance_from_source() + m_stop_vertices[index].second)
        {
            return false;
        }
        ++index;
    }
    return true;
}

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

    double total_path_length;
    node_pointer node = best_first_node(destination, total_path_length);

    if(total_path_length>GEODESIC_INF/2.0)      //unable to find the path
    {
        return;
    }

    path.push_back(destination);

    if(node->distance(&destination) > 1e-50)
    {
        path.push_back(node->surface_point());
    }

    while(node->previous())         //follow the path
    {
        node = node->previous();
        path.push_back(node->surface_point());
    }

    SurfacePoint& source = m_sources[node->source_index()];     //add source to the path if it is not already there
    if(node->distance(&source) > 1e-50)
    {
        path.push_back(source);
    }
}


template<class Node>
inline unsigned GeodesicAlgorithmGraphBase<Node>::best_source(SurfacePoint& point,          //quickly find what source this point belongs to and what is the distance to this source
                                                                 double& best_source_distance)
{
    node_pointer node = best_first_node(point, best_source_distance);
    return node ? node->source_index() : 0;
};

}       //geodesic

#endif //GEODESIC_ALGORITHM_GRAPH_BASE_010907