geodesic_cpp_03_02_2008/geodesic_algorithm_exact.h

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

#include "geodesic_memory.h"
#include "geodesic_algorithm_base.h"
#include "geodesic_algorithm_exact_elements.h"
#include <vector>
#include <cmath>
#include <assert.h>
#include <set>

namespace geodesic{

class GeodesicAlgorithmExact : public GeodesicAlgorithmBase
{
public:
    GeodesicAlgorithmExact(geodesic::Mesh* mesh):
        GeodesicAlgorithmBase(mesh),
        m_memory_allocator(mesh->edges().size(), mesh->edges().size()),
        m_edge_interval_lists(mesh->edges().size())
    {
        m_type = EXACT;

        for(unsigned i=0; i<m_edge_interval_lists.size(); ++i)
        {
            m_edge_interval_lists[i].initialize(&mesh->edges()[i]);
        }
    };  

    ~GeodesicAlgorithmExact(){};

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

private:
    typedef std::set<interval_pointer, Interval> IntervalQueue;

    void update_list_and_queue(list_pointer list,
                               IntervalWithStop* candidates,    //up to two candidates
                               unsigned num_candidates);

    unsigned compute_propagated_parameters(double pseudo_x, 
                                            double pseudo_y, 
                                            double d,       //parameters of the interval
                                            double start, 
                                            double end,     //start/end of the interval
                                            double alpha,   //corner angle
                                            double L,       //length of the new edge
                                            bool first_interval,        //if it is the first interval on the edge
                                            bool last_interval,
                                            bool turn_left,
                                            bool turn_right,
                                            IntervalWithStop* candidates);      //if it is the last interval on the edge

    void construct_propagated_intervals(bool invert, 
                                      edge_pointer edge, 
                                      face_pointer face,        //constructs iNew from the rest of the data
                                      IntervalWithStop* candidates,
                                      unsigned& num_candidates,
                                      interval_pointer source_interval);

    double compute_positive_intersection(double start,
                                         double pseudo_x,
                                         double pseudo_y,
                                         double sin_alpha,
                                         double cos_alpha);     //used in construct_propagated_intervals

    unsigned intersect_intervals(interval_pointer zero, 
                                    IntervalWithStop* one);         //intersecting two intervals with up to three intervals in the end

    interval_pointer best_first_interval(SurfacePoint& point, 
                                        double& best_total_distance, 
                                        double& best_interval_position,
                                        unsigned& best_source_index);

    bool check_stop_conditions(unsigned& index);

    void clear()
    {
        m_memory_allocator.clear();
        m_queue.clear();
        for(unsigned i=0; i<m_edge_interval_lists.size(); ++i)
        {
            m_edge_interval_lists[i].clear();
        }
        m_propagation_distance_stopped = GEODESIC_INF;
    };

    list_pointer interval_list(edge_pointer e)
    {
        return &m_edge_interval_lists[e->id()];
    };

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

    void initialize_propagation_data();     

    void list_edges_visible_from_source(MeshElementBase* p,
                                        std::vector<edge_pointer>& storage); //used in initialization

    long visible_from_source(SurfacePoint& point);  //used in backtracing

    void best_point_on_the_edge_set(SurfacePoint& point, 
                                    std::vector<edge_pointer> const& storage,
                                    interval_pointer& best_interval,
                                    double& best_total_distance,
                                    double& best_interval_position);

    void possible_traceback_edges(SurfacePoint& point, 
                                  std::vector<edge_pointer>& storage);

    bool erase_from_queue(interval_pointer p);

    IntervalQueue m_queue;  //interval queue

    MemoryAllocator<Interval> m_memory_allocator;           //quickly allocate and deallocate intervals 
    std::vector<IntervalList> m_edge_interval_lists;        //every edge has its interval data 

    enum MapType {OLD, NEW};        //used for interval intersection
    MapType map[5];     
    double start[6];
    interval_pointer i_new[5];

    unsigned m_queue_max_size;          //used for statistics
    unsigned m_iterations;          //used for statistics

    SortedSources m_sources;
};

inline void GeodesicAlgorithmExact::best_point_on_the_edge_set(SurfacePoint& point, 
                                                               std::vector<edge_pointer> const& storage,
                                                               interval_pointer& best_interval,
                                                               double& best_total_distance,
                                                               double& best_interval_position)
{
    best_total_distance = 1e100;
    for(unsigned i=0; i<storage.size(); ++i)
    {
        edge_pointer e = storage[i];
        list_pointer list = interval_list(e);

        double offset;
        double distance;
        interval_pointer interval;

        list->find_closest_point(&point, 
                                 offset, 
                                 distance, 
                                 interval);

        if(distance < best_total_distance)
        {
            best_interval = interval;
            best_total_distance = distance;
            best_interval_position = offset;
        }
    }
}

inline void GeodesicAlgorithmExact::possible_traceback_edges(SurfacePoint& point, 
                                                             std::vector<edge_pointer>& storage)
{
    storage.clear();

    if(point.type() == VERTEX)
    {
        vertex_pointer v = static_cast<vertex_pointer>(point.base_element());
        for(unsigned i=0; i<v->adjacent_faces().size(); ++i)
        {
            face_pointer f = v->adjacent_faces()[i];
            storage.push_back(f->opposite_edge(v));
        }
    }
    else if(point.type() == EDGE)
    {
        edge_pointer e = static_cast<edge_pointer>(point.base_element());
        for(unsigned i=0; i<e->adjacent_faces().size(); ++i)
        {
            face_pointer f = e->adjacent_faces()[i];

            storage.push_back(f->next_edge(e,e->v0()));
            storage.push_back(f->next_edge(e,e->v1()));
        }
    }
    else
    {
        face_pointer f = static_cast<face_pointer>(point.base_element());
        storage.push_back(f->adjacent_edges()[0]);
        storage.push_back(f->adjacent_edges()[1]);
        storage.push_back(f->adjacent_edges()[2]);
    }
}


inline long GeodesicAlgorithmExact::visible_from_source(SurfacePoint& point)    //negative if not visible
{
    assert(point.type() != UNDEFINED_POINT);

    if(point.type() == EDGE)        
    {
        edge_pointer e = static_cast<edge_pointer>(point.base_element());
        list_pointer list = interval_list(e);
        double position = std::min(point.distance(e->v0()), e->length());
        interval_pointer interval = list->covering_interval(position);
        //assert(interval);
        if(interval && interval->visible_from_source())
        {
            return (long)interval->source_index();
        }
        else
        {
            return -1;
        }
    }
    else if(point.type() == FACE)       
    {
        return -1;
    }
    else if(point.type() == VERTEX)     
    {
        vertex_pointer v = static_cast<vertex_pointer>(point.base_element());
        for(unsigned i=0; i<v->adjacent_edges().size(); ++i)
        {
            edge_pointer e = v->adjacent_edges()[i];
            list_pointer list = interval_list(e);

            double position = e->v0()->id() == v->id() ? 0.0 : e->length();
            interval_pointer interval = list->covering_interval(position);
            if(interval && interval->visible_from_source())
            {
                return (long)interval->source_index();
            }
        }

        return -1;
    }

    assert(0);
    return 0;
}

inline double GeodesicAlgorithmExact::compute_positive_intersection(double start,
                                                                    double pseudo_x,
                                                                    double pseudo_y,
                                                                    double sin_alpha,
                                                                    double cos_alpha)
{
    assert(pseudo_y < 0);

    double denominator = sin_alpha*(pseudo_x - start) - cos_alpha*pseudo_y;
    if(denominator<0.0)
    {
        return -1.0;
    }

    double numerator = -pseudo_y*start;

    if(numerator < 1e-30)
    {
        return 0.0;
    }

    if(denominator < 1e-30)
    {
        return -1.0;
    }

    return numerator/denominator;
}

inline void GeodesicAlgorithmExact::list_edges_visible_from_source(MeshElementBase* p,
                                                                   std::vector<edge_pointer>& storage)
{
    assert(p->type() != UNDEFINED_POINT);

    if(p->type() == FACE)
    {
        face_pointer f = static_cast<face_pointer>(p);
        for(unsigned i=0; i<3; ++i)
        {
            storage.push_back(f->adjacent_edges()[i]);
        }
    }
    else if(p->type() == EDGE)
    {
        edge_pointer e = static_cast<edge_pointer>(p);
        storage.push_back(e);
    }
    else            //VERTEX
    {
        vertex_pointer v = static_cast<vertex_pointer>(p);
        for(unsigned i=0; i<v->adjacent_edges().size(); ++i)
        {
            storage.push_back(v->adjacent_edges()[i]);
        }

    }
}

inline bool GeodesicAlgorithmExact::erase_from_queue(interval_pointer p)
{
    if(p->min() < GEODESIC_INF/10.0)// && p->min >= queue->begin()->first)
    {
        assert(m_queue.count(p)<=1);            //the set is unique

        IntervalQueue::iterator it = m_queue.find(p);

        if(it != m_queue.end())
        {
            m_queue.erase(it);
            return true;
        }
    }

    return false;
}

inline unsigned GeodesicAlgorithmExact::intersect_intervals(interval_pointer zero, 
                                                               IntervalWithStop* one)           //intersecting two intervals with up to three intervals in the end
{
    assert(zero->edge()->id() == one->edge()->id());
    assert(zero->stop() > one->start() && zero->start() < one->stop());
    assert(one->min() < GEODESIC_INF/10.0);

    double const local_epsilon = SMALLEST_INTERVAL_RATIO*one->edge()->length(); 

    unsigned N=0;
    if(zero->min() > GEODESIC_INF/10.0)
    {
        start[0] = zero->start();
        if(zero->start() < one->start() - local_epsilon)
        {
            map[0] = OLD;
            start[1] = one->start();
            map[1] = NEW;
            N = 2;
        }
        else
        {
            map[0] = NEW;
            N = 1;
        }

        if(zero->stop() > one->stop() + local_epsilon)
        {
            map[N] = OLD;                           //"zero" interval
            start[N++] = one->stop();
        }

        start[N+1] = zero->stop();
        return N;
    }

    double const local_small_epsilon = 1e-8*one->edge()->length(); 

    double D = zero->d() - one->d();
    double x0 = zero->pseudo_x();
    double x1 = one->pseudo_x();
    double R0 = x0*x0 + zero->pseudo_y()*zero->pseudo_y();
    double R1 = x1*x1 + one->pseudo_y()*one->pseudo_y();

    double inter[2];                                    //points of intersection
    char Ninter=0;                                      //number of the points of the intersection

    if(std::abs(D)<local_epsilon)                   //if d1 == d0, equation is linear
    {
        double denom = x1 - x0;
        if(std::abs(denom)>local_small_epsilon)
        {
            inter[0] =  (R1 - R0)/(2.*denom);                   //one solution
            Ninter = 1;
        }
    }
    else
    {
        double D2 = D*D;
        double Q = 0.5*(R1-R0-D2);
        double X = x0 - x1;

        double A = X*X - D2;
        double B = Q*X + D2*x0;
        double C = Q*Q - D2*R0;

        if (std::abs(A)<local_small_epsilon)                            //if A == 0, linear equation
        {
            if(std::abs(B)>local_small_epsilon)
            {
                inter[0] =  -C/B;                           //one solution
                Ninter = 1;
            }
        }
        else
        {
            double det = B*B-A*C;
            if(det>local_small_epsilon*local_small_epsilon)         //two roots
            {
                det = sqrt(det);
                if(A>0.0)                               //make sure that the roots are ordered
                {
                    inter[0] = (-B - det)/A;
                    inter[1] = (-B + det)/A;
                }
                else
                {
                    inter[0] = (-B + det)/A;
                    inter[1] = (-B - det)/A;
                }
                
                if(inter[1] - inter[0] > local_small_epsilon)
                {
                    Ninter = 2;
                }
                else
                {
                    Ninter = 1;
                }
            }
            else if(det>=0.0)                   //single root
            {
                inter[0] = -B/A;
                Ninter = 1;
            }
        }
    }
    //---------------------------find possible intervals---------------------------------------
    double left = std::max(zero->start(), one->start());        //define left and right boundaries of the intersection of the intervals
    double right = std::min(zero->stop(), one->stop());

    double good_start[4];                                       //points of intersection within the (left, right) limits +"left" + "right"
    good_start[0] = left;
    char Ngood_start=1;                                     //number of the points of the intersection  

    for(char i=0; i<Ninter; ++i)                            //for all points of intersection
    {
        double x = inter[i];
        if(x > left + local_epsilon && x < right - local_epsilon)
        {
            good_start[Ngood_start++] = x;
        }
    }
    good_start[Ngood_start++] = right;

    MapType mid_map[3];
    for(char i=0; i<Ngood_start-1; ++i)
    {
        double mid = (good_start[i] + good_start[i+1])*0.5;
        mid_map[i] = zero->signal(mid) <= one->signal(mid) ? OLD : NEW;
    }

    //-----------------------------------output----------------------------------
    N = 0;
    if(zero->start() < left - local_epsilon)                        //additional "zero" interval
    {
        if(mid_map[0] == OLD)               //first interval in the map is already the old one
        {
            good_start[0] = zero->start();
        }
        else
        {
            map[N] = OLD;                   //"zero" interval
            start[N++] = zero->start();
        }
    }

    for(long i=0;i<Ngood_start-1;++i)                           //for all intervals
    {
        MapType current_map = mid_map[i];
        if(N==0 || map[N-1] != current_map)
        {
            map[N] = current_map;               
            start[N++] = good_start[i];
        }
    }

    if(zero->stop() > one->stop() + local_epsilon)
    {
        if(N==0 || map[N-1] == NEW)
        {
            map[N] = OLD;                           //"zero" interval
            start[N++] = one->stop();
        }
    }

    start[0] = zero->start();       // just to make sure that epsilons do not damage anything
    //start[N] = zero->stop();

    return N; 
}

inline void GeodesicAlgorithmExact::initialize_propagation_data()
{
    clear();

    IntervalWithStop candidate;
    std::vector<edge_pointer> edges_visible_from_source;
    for(unsigned i=0; i<m_sources.size(); ++i)      //for all edges adjacent to the starting vertex         
    {
        SurfacePoint* source = &m_sources[i];
        
        edges_visible_from_source.clear();
        list_edges_visible_from_source(source->base_element(), 
                                       edges_visible_from_source);
        
        for(unsigned j=0; j<edges_visible_from_source.size(); ++j)
        {
            edge_pointer e = edges_visible_from_source[j];
            candidate.initialize(e, source, i);
            candidate.stop() = e->length();
            candidate.compute_min_distance(candidate.stop());
            candidate.direction() = Interval::FROM_SOURCE;

            update_list_and_queue(interval_list(e), &candidate, 1);
        }
    }
}

inline void GeodesicAlgorithmExact::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)
{
    set_stop_conditions(stop_points, max_propagation_distance);
    set_sources(sources);
    initialize_propagation_data();

    clock_t start = clock();

    unsigned satisfied_index = 0;

    m_iterations = 0;       //for statistics
    m_queue_max_size = 0;

    IntervalWithStop candidates[2];

    while(!m_queue.empty())
    {
        m_queue_max_size = std::max(m_queue.size(), m_queue_max_size);

        unsigned const check_period = 10;
        if(++m_iterations % check_period == 0)      //check if we covered all required vertices
        {
            if (check_stop_conditions(satisfied_index))
            {
                break;
            }
        }

        interval_pointer min_interval = *m_queue.begin();
        m_queue.erase(m_queue.begin());
        edge_pointer edge = min_interval->edge();
        list_pointer list = interval_list(edge);

        assert(min_interval->d() < GEODESIC_INF);

        bool const first_interval = min_interval->start() == 0.0;
        //bool const last_interval = min_interval->stop() == edge->length();
        bool const last_interval = min_interval->next() == NULL;

        bool const turn_left = edge->v0()->saddle_or_boundary();
        bool const turn_right = edge->v1()->saddle_or_boundary();

        for(unsigned i=0; i<edge->adjacent_faces().size(); ++i)     //two possible faces to propagate
        {
            if(!edge->is_boundary())        //just in case, always propagate boundary edges
            {
                if((i == 0 && min_interval->direction() == Interval::FROM_FACE_0) ||
                    (i == 1 && min_interval->direction() == Interval::FROM_FACE_1))
                {
                    continue;
                }
            }

            face_pointer face = edge->adjacent_faces()[i];          //if we come from 1, go to 2
            edge_pointer next_edge = face->next_edge(edge,edge->v0());

            unsigned num_propagated = compute_propagated_parameters(min_interval->pseudo_x(), 
                                                                     min_interval->pseudo_y(), 
                                                                     min_interval->d(),     //parameters of the interval
                                                                     min_interval->start(), 
                                                                     min_interval->stop(),      //start/end of the interval
                                                                     face->vertex_angle(edge->v0()),    //corner angle
                                                                     next_edge->length(),       //length of the new edge
                                                                     first_interval,        //if it is the first interval on the edge
                                                                     last_interval,
                                                                     turn_left,
                                                                     turn_right,
                                                                     candidates);       //if it is the last interval on the edge
            bool propagate_to_right = true;

            if(num_propagated)
            {
                if(candidates[num_propagated-1].stop() != next_edge->length()) 
                {
                    propagate_to_right = false;
                }
                
                bool const invert = next_edge->v0()->id() != edge->v0()->id(); //if the origins coinside, do not invert intervals

                construct_propagated_intervals(invert,      //do not inverse 
                                             next_edge, 
                                             face,
                                             candidates,
                                             num_propagated,
                                             min_interval);
                
                update_list_and_queue(interval_list(next_edge), 
                                      candidates, 
                                      num_propagated);
            }

            if(propagate_to_right)
            {
                                    //propogation to the right edge
                double length = edge->length();
                next_edge = face->next_edge(edge,edge->v1());

                num_propagated = compute_propagated_parameters(length - min_interval->pseudo_x(), 
                                                             min_interval->pseudo_y(), 
                                                             min_interval->d(),     //parameters of the interval
                                                             length - min_interval->stop(), 
                                                             length - min_interval->start(),        //start/end of the interval
                                                             face->vertex_angle(edge->v1()),    //corner angle
                                                             next_edge->length(),       //length of the new edge
                                                             last_interval,     //if it is the first interval on the edge
                                                             first_interval,
                                                             turn_right,
                                                             turn_left,
                                                             candidates);       //if it is the last interval on the edge

                if(num_propagated)
                {
                    bool const invert = next_edge->v0()->id() != edge->v1()->id();      //if the origins coinside, do not invert intervals

                    construct_propagated_intervals(invert,      //do not inverse 
                                                 next_edge, 
                                                 face,
                                                 candidates,
                                                 num_propagated,
                                                 min_interval);

                    update_list_and_queue(interval_list(next_edge), 
                                          candidates, 
                                          num_propagated);
                }
            }
        } 
    } 

    m_propagation_distance_stopped = m_queue.empty() ? GEODESIC_INF : (*m_queue.begin())->min();
    clock_t stop = clock();
    m_time_consumed = (static_cast<double>(stop)-static_cast<double>(start))/CLOCKS_PER_SEC;

/*  for(unsigned i=0; i<m_edge_interval_lists.size(); ++i)
    {
        list_pointer list = &m_edge_interval_lists[i];
        interval_pointer p = list->first();
        assert(p->start() == 0.0);
        while(p->next())
        {
            assert(p->stop() == p->next()->start());
            assert(p->d() < GEODESIC_INF);
            p = p->next();
        }
    }*/
}


inline bool GeodesicAlgorithmExact::check_stop_conditions(unsigned& index)
{
    double queue_distance = (*m_queue.begin())->min();
    if(queue_distance < stop_distance())
    {
        return false;
    }

    while(index < m_stop_vertices.size())
    {
        vertex_pointer v = m_stop_vertices[index].first;
        edge_pointer edge = v->adjacent_edges()[0];             //take any edge

        double distance = edge->v0()->id() == v->id() ? 
                          interval_list(edge)->signal(0.0) :
                          interval_list(edge)->signal(edge->length());

        if(queue_distance < distance + m_stop_vertices[index].second)
        {
            return false;
        }

        ++index;
    }
    return true;
}


inline void GeodesicAlgorithmExact::update_list_and_queue(list_pointer list,
                                                IntervalWithStop* candidates,   //up to two candidates
                                                unsigned num_candidates)
{
    assert(num_candidates <= 2);
    //assert(list->first() != NULL);
    edge_pointer edge = list->edge();
    double const local_epsilon = SMALLEST_INTERVAL_RATIO * edge->length(); 

    if(list->first() == NULL) 
    {
        interval_pointer* p = &list->first();
        IntervalWithStop* first;
        IntervalWithStop* second; 

        if(num_candidates == 1)
        {
            first = candidates;
            second = candidates;
            first->compute_min_distance(first->stop());
        }
        else 
        {   
            if(candidates->start() <= (candidates+1)->start())
            {
                first = candidates;
                second = candidates+1;
            }
            else 
            {
                first = candidates+1;
                second = candidates;
            }
            assert(first->stop() == second->start());

            first->compute_min_distance(first->stop());
            second->compute_min_distance(second->stop());
        }

        if(first->start() > 0.0)
        {
            *p = m_memory_allocator.allocate();
            (*p)->initialize(edge);
            p = &(*p)->next();
        }

        *p = m_memory_allocator.allocate();
        memcpy(*p,first,sizeof(Interval));
        m_queue.insert(*p);

        if(num_candidates == 2)
        {
            p = &(*p)->next();
            *p = m_memory_allocator.allocate();
            memcpy(*p,second,sizeof(Interval));
            m_queue.insert(*p);
        }

        if(second->stop() < edge->length())
        {
            p = &(*p)->next();
            *p = m_memory_allocator.allocate();
            (*p)->initialize(edge);
            (*p)->start() = second->stop();
        }
        else
        {
            (*p)->next() = NULL;
        }
        return;
    }

    bool propagate_flag;

    for(unsigned i=0; i<num_candidates; ++i)                //for all new intervals
    {
        IntervalWithStop* q = &candidates[i];
    
        interval_pointer previous = NULL;

        interval_pointer p = list->first();
        assert(p->start() == 0.0);

        while(p != NULL && p->stop() - local_epsilon < q->start())
        {
            p = p->next(); 
        }

        while(p != NULL && p->start() < q->stop() - local_epsilon)          //go through all old intervals
        {
            unsigned const N = intersect_intervals(p, q);                               //interset two intervals

            if(N == 1)          
            {
                if(map[0]==OLD) //if "p" is always better, we do not need to update anything)
                {
                    if(previous)        //close previous interval and put in into the queue
                    {
                        previous->next() = p;
                        previous->compute_min_distance(p->start());
                        m_queue.insert(previous);
                        previous = NULL;
                    }

                    p = p->next(); 
                    
                }
                else if(previous)   //extend previous interval to cover everything; remove p
                {
                    previous->next() = p->next(); 
                    erase_from_queue(p);
                    m_memory_allocator.deallocate(p);

                    p = previous->next();
                }
                else                //p becomes "previous"
                {
                    previous = p;
                    interval_pointer next = p->next();
                    erase_from_queue(p);

                    memcpy(previous,q,sizeof(Interval));

                    previous->start() = start[0];
                    previous->next() = next;

                    p = next; 
                }
                continue;
            }

            //update_flag = true;

            Interval swap(*p);                          //used for swapping information
            propagate_flag = erase_from_queue(p);

            for(unsigned j=1; j<N; ++j)             //no memory is needed for the first one
            {
                i_new[j] = m_memory_allocator.allocate();   //create new intervals
            }

            if(map[0]==OLD) //finish previous, if any
            {
                if(previous)
                {
                    previous->next() = p;
                    previous->compute_min_distance(previous->stop());
                    m_queue.insert(previous);
                    previous = NULL;
                }
                i_new[0] = p;
                p->next() = i_new[1];
                p->start() = start[0];
            }
            else if(previous)   //extend previous interval to cover everything; remove p
            {
                i_new[0] = previous;
                previous->next() = i_new[1]; 
                m_memory_allocator.deallocate(p);
                previous = NULL;
            }
            else                //p becomes "previous"
            {
                i_new[0] = p;
                memcpy(p,q,sizeof(Interval));

                p->next() = i_new[1];
                p->start() = start[0];
            }

            assert(!previous);

            for(unsigned j=1; j<N; ++j)                 
            {
                interval_pointer current_interval = i_new[j];

                if(map[j] == OLD)   
                {
                    memcpy(current_interval,&swap,sizeof(Interval));
                }
                else
                {
                    memcpy(current_interval,q,sizeof(Interval));
                }
                
                if(j == N-1)    
                {
                    current_interval->next() = swap.next();
                }
                else            
                {
                    current_interval->next() = i_new[j+1];
                }

                current_interval->start() = start[j];
            }

            for(unsigned j=0; j<N; ++j)                             //find "min" and add the intervals to the queue
            {
                if(j==N-1 && map[j]==NEW)
                {
                    previous = i_new[j];
                }
                else
                {
                    interval_pointer current_interval = i_new[j];

                    current_interval->compute_min_distance(current_interval->stop());                   //compute minimal distance

                    if(map[j]==NEW || (map[j]==OLD && propagate_flag))
                    {
                        m_queue.insert(current_interval);
                    }
                }
            }

            p = swap.next();
        }

        if(previous)        //close previous interval and put in into the queue
        {
            previous->compute_min_distance(previous->stop());
            m_queue.insert(previous);
            previous = NULL;
        }
    }
}

inline unsigned GeodesicAlgorithmExact::compute_propagated_parameters(double pseudo_x, 
                                                                        double pseudo_y, 
                                                                        double d,       //parameters of the interval
                                                                        double begin, 
                                                                        double end,     //start/end of the interval
                                                                        double alpha,   //corner angle
                                                                        double L,       //length of the new edge
                                                                        bool first_interval,        //if it is the first interval on the edge
                                                                        bool last_interval,
                                                                        bool turn_left,
                                                                        bool turn_right,
                                                                        IntervalWithStop* candidates)       //if it is the last interval on the edge
{               
    assert(pseudo_y<=0.0);
    assert(d<GEODESIC_INF/10.0);
    assert(begin<=end);
    assert(first_interval ? (begin == 0.0) : true);

    IntervalWithStop* p = candidates;

    if(std::abs(pseudo_y) <= 1e-30)             //pseudo-source is on the edge
    {
        if(first_interval && pseudo_x <= 0.0)
        {
            p->start() = 0.0;
            p->stop() = L;
            p->d() = d - pseudo_x;
            p->pseudo_x() = 0.0;
            p->pseudo_y() = 0.0;
            return 1;
        }
        else if(last_interval && pseudo_x >= end)
        {
            p->start() = 0.0;
            p->stop() = L;
            p->d() = d + pseudo_x-end;
            p->pseudo_x() = end*cos(alpha);
            p->pseudo_y() = -end*sin(alpha);
            return 1;
        }
        else if(pseudo_x >= begin && pseudo_x <= end)
        {
            p->start() = 0.0;
            p->stop() = L;
            p->d() = d;
            p->pseudo_x() = pseudo_x*cos(alpha);
            p->pseudo_y() = -pseudo_x*sin(alpha);
            return 1;
        }
        else
        {
            return 0;
        }
    }

    double sin_alpha = sin(alpha);
    double cos_alpha = cos(alpha);

    //important: for the first_interval, this function returns zero only if the new edge is "visible" from the source
    //if the new edge can be covered only after turn_over, the value is negative (-1.0)
    double L1 = compute_positive_intersection(begin, 
                                              pseudo_x, 
                                              pseudo_y, 
                                              sin_alpha, 
                                              cos_alpha);

    if(L1 < 0 || L1 >= L)
    {
        if(first_interval && turn_left)
        {
            p->start() = 0.0;
            p->stop() = L;
            p->d() = d + sqrt(pseudo_x*pseudo_x + pseudo_y*pseudo_y);
            p->pseudo_y() = 0.0;
            p->pseudo_x() = 0.0;
            return 1;
        }
        else
        {
            return 0;
        }
    }

    double L2 = compute_positive_intersection(end, 
                                              pseudo_x, 
                                              pseudo_y, 
                                              sin_alpha, 
                                              cos_alpha);

    if(L2 < 0 || L2 >= L)
    {
        p->start() = L1;
        p->stop() = L;
        p->d() = d;
        p->pseudo_x() = cos_alpha*pseudo_x + sin_alpha*pseudo_y;
        p->pseudo_y() = -sin_alpha*pseudo_x + cos_alpha*pseudo_y;

        return 1;
    }

    p->start() = L1;
    p->stop() = L2;
    p->d() = d;
    p->pseudo_x() = cos_alpha*pseudo_x + sin_alpha*pseudo_y;
    p->pseudo_y() = -sin_alpha*pseudo_x + cos_alpha*pseudo_y;
    assert(p->pseudo_y() <= 0.0);

    if(!(last_interval && turn_right))
    {
        return 1;
    }
    else
    {
        p = candidates + 1;

        p->start() = L2;
        p->stop() = L;
        double dx = pseudo_x - end;
        p->d() = d + sqrt(dx*dx + pseudo_y*pseudo_y);
        p->pseudo_x() = end*cos_alpha;
        p->pseudo_y() = -end*sin_alpha;

        return 2;
    }
}

inline void GeodesicAlgorithmExact::construct_propagated_intervals(bool invert, 
                                                                    edge_pointer edge, 
                                                                    face_pointer face,      //constructs iNew from the rest of the data
                                                                    IntervalWithStop* candidates,
                                                                    unsigned& num_candidates,
                                                                    interval_pointer source_interval)   //up to two candidates
{
    double edge_length = edge->length();
    double local_epsilon = SMALLEST_INTERVAL_RATIO * edge_length;
        
        //kill very small intervals in order to avoid precision problems
    if(num_candidates == 2)     
    {
        double start = std::min(candidates->start(), (candidates+1)->start());
        double stop = std::max(candidates->stop(), (candidates+1)->stop());
        if(candidates->stop()-candidates->start() < local_epsilon) // kill interval 0
        {
            *candidates = *(candidates+1);
            num_candidates = 1;
            candidates->start() = start;
            candidates->stop() = stop;
        }
        else if ((candidates+1)->stop() - (candidates+1)->start() < local_epsilon)
        {
            num_candidates = 1;
            candidates->start() = start;
            candidates->stop() = stop;
        }
    }

    IntervalWithStop* first; 
    IntervalWithStop* second; 
    if(num_candidates == 1)
    {
        first = candidates;
        second = candidates;
    }
    else
    {
        if(candidates->start() <= (candidates+1)->start())
        {
            first = candidates;
            second = candidates+1;
        }
        else 
        {
            first = candidates+1;
            second = candidates;
        }
        assert(first->stop() == second->start());
    }

    if(first->start() < local_epsilon)
    {
        first->start() = 0.0;
    }
    if(edge_length - second->stop() < local_epsilon)
    {
        second->stop() = edge_length;
    }

        //invert intervals if necessary; fill missing data and set pointers correctly
    Interval::DirectionType direction = edge->adjacent_faces()[0]->id() == face->id() ? 
                                        Interval::FROM_FACE_0 : 
                                        Interval::FROM_FACE_1;

    if(!invert)                 //in this case everything is straighforward, we do not have to invert the intervals
    {
        for(unsigned i=0; i<num_candidates; ++i)
        {
            IntervalWithStop* p = candidates + i;

            p->next() = (i == num_candidates - 1) ? NULL : candidates + i + 1;
            p->edge() = edge;
            p->direction() = direction;
            p->source_index() = source_interval->source_index();

            p->min() = 0.0;                 //it will be changed later on

            assert(p->start() < p->stop());
        }
    }
    else                //now we have to invert the intervals 
    {
        for(unsigned i=0; i<num_candidates; ++i)
        {
            IntervalWithStop* p = candidates + i;

            p->next() = (i == 0) ? NULL : candidates + i - 1;
            p->edge() = edge;
            p->direction() = direction;
            p->source_index() = source_interval->source_index();

            double length = edge_length;
            p->pseudo_x() = length - p->pseudo_x();

            double start = length - p->stop();
            p->stop() = length - p->start();
            p->start() = start;

            p->min() = 0;

            assert(p->start() < p->stop());
            assert(p->start() >= 0.0);
            assert(p->stop() <= edge->length());
        }
    }
}


inline unsigned GeodesicAlgorithmExact::best_source(SurfacePoint& point,            //quickly find what source this point belongs to and what is the distance to this source
                                                       double& best_source_distance)
{
    double best_interval_position; 
    unsigned best_source_index;

    best_first_interval(point, 
                        best_source_distance, 
                        best_interval_position, 
                        best_source_index);
    
    return best_source_index;
} 

inline interval_pointer GeodesicAlgorithmExact::best_first_interval(SurfacePoint& point, 
                                                             double& best_total_distance, 
                                                             double& best_interval_position, 
                                                             unsigned& best_source_index)
{
    assert(point.type() != UNDEFINED_POINT);

    interval_pointer best_interval = NULL;  
    best_total_distance = GEODESIC_INF;

    if(point.type() == EDGE)        
    {
        edge_pointer e = static_cast<edge_pointer>(point.base_element());
        list_pointer list = interval_list(e);

        best_interval_position = point.distance(e->v0());
        best_interval = list->covering_interval(best_interval_position);
        if(best_interval)
        {
            //assert(best_interval && best_interval->d() < GEODESIC_INF);
            best_total_distance = best_interval->signal(best_interval_position);
            best_source_index = best_interval->source_index();
        }
    }
    else if(point.type() == FACE)       
    {
        face_pointer f = static_cast<face_pointer>(point.base_element());
        for(unsigned i=0; i<3; ++i)
        {
            edge_pointer e = f->adjacent_edges()[i];
            list_pointer list = interval_list(e);

            double offset;
            double distance;
            interval_pointer interval;

            list->find_closest_point(&point, 
                                     offset, 
                                     distance, 
                                     interval);

            if(interval && distance < best_total_distance)
            {
                best_interval = interval;
                best_total_distance = distance;
                best_interval_position = offset;
                best_source_index = interval->source_index();
            }
        }

            //check for all sources that might be located inside this face
        SortedSources::sorted_iterator_pair local_sources = m_sources.sources(f);
        for(SortedSources::sorted_iterator it=local_sources.first; it != local_sources.second; ++it)
        {
            SurfacePointWithIndex* source = *it;
            double distance = point.distance(source);
            if(distance < best_total_distance)
            {
                best_interval = NULL;
                best_total_distance = distance;
                best_interval_position = 0.0;
                best_source_index = source->index();
            }
        }
    }
    else if(point.type() == VERTEX)     
    {
        vertex_pointer v = static_cast<vertex_pointer>(point.base_element());
        for(unsigned i=0; i<v->adjacent_edges().size(); ++i)
        {
            edge_pointer e = v->adjacent_edges()[i];
            list_pointer list = interval_list(e);
            
            double position = e->v0()->id() == v->id() ? 0.0 : e->length();
            interval_pointer interval = list->covering_interval(position);
            if(interval)
            {
                double distance = interval->signal(position);

                if(distance < best_total_distance)
                {
                    best_interval = interval;
                    best_total_distance = distance;
                    best_interval_position = position;
                    best_source_index = interval->source_index();
                }
            }
        }
    }

    if(best_total_distance > m_propagation_distance_stopped)        //result is unreliable
    {
        best_total_distance = GEODESIC_INF;
        return NULL;
    }
    else
    {
        return best_interval;
    }
}

inline void GeodesicAlgorithmExact::trace_back(SurfacePoint& destination,       //trace back piecewise-linear path
                                        std::vector<SurfacePoint>& path)
{                   
    path.clear();
    double best_total_distance; 
    double best_interval_position;
    unsigned source_index = std::numeric_limits<unsigned>::max();
    interval_pointer best_interval = best_first_interval(destination, 
                                                         best_total_distance, 
                                                         best_interval_position, 
                                                         source_index);

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

    path.push_back(destination);

    if(best_interval)   //if we did not hit the face source immediately  
    {
        std::vector<edge_pointer> possible_edges;
        possible_edges.reserve(10);
    
        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)
        {
            SurfacePoint& q = path.back();

            possible_traceback_edges(q, possible_edges);

            interval_pointer interval;
            double total_distance;
            double position;

            best_point_on_the_edge_set(q, 
                                       possible_edges,
                                       interval,
                                       total_distance,
                                       position);

            //std::cout << total_distance + length(path) << std::endl;
            assert(total_distance<GEODESIC_INF);
            source_index = interval->source_index();

            edge_pointer e = interval->edge();
            double local_epsilon = SMALLEST_INTERVAL_RATIO*e->length();
            if(position < local_epsilon)
            {
                path.push_back(SurfacePoint(e->v0()));
            }
            else if(position > e->length()-local_epsilon)
            {
                path.push_back(SurfacePoint(e->v1()));
            }
            else
            {
                double normalized_position = position/e->length();
                path.push_back(SurfacePoint(e, normalized_position));
            }
        }
    }

    SurfacePoint& source = static_cast<SurfacePoint&>(m_sources[source_index]);
    if(path.back().distance(&source) > 0)
    {
        path.push_back(source);
    }
}

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

    unsigned interval_counter = 0;
    for(unsigned i=0; i<m_edge_interval_lists.size(); ++i)
    {
        interval_counter += m_edge_interval_lists[i].number_of_intervals();
    }
    double intervals_per_edge = (double)interval_counter/(double)m_edge_interval_lists.size();

    double memory = m_edge_interval_lists.size()*sizeof(IntervalList) + 
                    interval_counter*sizeof(Interval);

    std::cout << "uses about " << memory/1e6 << "Mb of memory" <<std::endl;
    std::cout << interval_counter << " total intervals, or " 
              << intervals_per_edge << " intervals per edge"
              << std::endl;
    std::cout << "maximum interval queue size is " << m_queue_max_size << std::endl;
    std::cout << "number of interval propagations is " << m_iterations << std::endl;
}

}       //geodesic

#endif //GEODESIC_ALGORITHM_EXACT_20071231