Question

We have some set of points (each point has its X and Y) and a multy map of roots [point, point]. We can move via roots from any point to any in any possible direction. We are given some path of 2d points we want to follow as close to as possible:

how to calculate a path like this:

that would look as alike as possible to given path? What are usefull algorithms that can do such thing (and are thay implemented in Boost Geometry or Graph or any other common opensource C++ library)?

Answer 1

This is a really cute little problem. If your graph is well connected a greedy approach might work quite well. As in: (1) set current position to be the node closest to the start of the path, (2) move to the adjacent node which is closest to the next point in the path until there is no closer point, (3) select next point in path and goto (2) if not finished.

#include <assert.h>
#include <stddef.h>
#include <iostream>
#include <iterator>
#include <vector>
#include <limits>

double sq(double const d)
{
    return d * d;
}

size_t min_dist_point(
    std::vector<double> const& x,
    std::vector<double> const& y,
    std::vector<bool> const& adjacent,
    double const fx,
    double const fy
)
{
    size_t const points = x.size();

    double min_dist_sq = std::numeric_limits<double>::max();
    size_t min_point;

    for (size_t j = 0; j < points; ++j) {
        if (!adjacent[j]) { continue; }
        double const dist_sq = sq(x[j] - fx) + sq(y[j] - fy);

        if (dist_sq < min_dist_sq) {
            min_point = j;
            min_dist_sq = dist_sq;
        }
    }
    assert(min_dist_sq != std::numeric_limits<double>::max());

    return min_point;
}

template <class out_t>
out_t f(
    std::vector<double> const& x,
    std::vector<double> const& y,
    std::vector<std::vector<bool> > adjacent,
    std::vector<double> const& follow_x,
    std::vector<double> const& follow_y,
    out_t out
)
{
    size_t const points = x.size();
    size_t const follow_len = follow_x.size();
    for (size_t i = 0; i < points; ++i) {
        adjacent[i][i] = 1;
    }

    std::vector<bool> const all (points, true);
    size_t pos = min_dist_point(x, y, all, follow_x[0], follow_y[0]);
    *out++ = pos;

    for (size_t i = 1; i < follow_len; ++i) {
        for (;;) {
            size_t const next = min_dist_point(x, y, adjacent[pos], follow_x[i], follow_y[i]);
            if (next == pos) { break; }
            *out++ = (pos = next);
        }
    }

    return out;
}

If this algorithm gets stuck in cycles you will need A* search.

http://www.boost.org/doc/libs/1_47_0/libs/graph/doc/astar_search.html