Finding the cheapest path between two points using Dijkstra

Question

I am trying to use Dijkstra to find the cheapest path between two pixels in an image.

Implementation:

#include <vector>
#include <queue>
#include <map>
#include <cmath>
#include <numbers>
#include <array>
#include <algorithm>

    using vec2i_t [[gnu::vector_size(16)]] = int64_t;
    using vec2f_t [[gnu::vector_size(16)]] = double;

    template<class T, auto tag>
    struct tagged_value
    {
    public:
        tagged_value() = default;
        explicit tagged_value(T value):m_value{value}{}
        T value() const { return m_value; }

    private:
        T m_value;
    };

    template<class T>
    using from = tagged_value<T, 0>;

    template<class T>
    using to = tagged_value<T, 1>;

    template<class T, auto tag>
    inline tagged_value<T, tag> operator+(tagged_value<T, tag> a, T b)
    {
        return tagged_value<T, tag>{a.value() + b};
    }

    template< auto tag>
    inline tagged_value<int64_t, tag> operator*(int64_t b, tagged_value<vec2i_t, tag> a)
    {
        return tagged_value<int64_t, tag>{b*a.value()};
    }

    struct pending_route_node
    {
        to<vec2i_t> loc;
        double total_cost;
    };

    bool is_cheaper(pending_route_node const& a, pending_route_node const& b)
    {
        return a.total_cost < b.total_cost;
    }

    struct route_node
    {
        from<vec2i_t> loc;
        double total_cost = std::numeric_limits<double>::infinity();
    };

    constexpr auto scale_factor = 1;

    template<auto tag>
    constexpr auto scale_by_factor(tagged_value<vec2i_t, tag> val)
    {
        auto const x = val.value();
        return tagged_value<vec2f_t, tag>
            {vec2f_t{static_cast<double>(x[0]), static_cast<double>(x[1])}/ static_cast<double>(scale_factor)};
    }

    constexpr auto gen_neigbour_offset_table()
    {
        std::array<vec2i_t, 8> ret{};
        constexpr auto r = static_cast<double>(scale_factor);
        for(size_t k = 0; k != std::size(ret); ++k)
        {
            auto const theta = k*2.0*std::numbers::pi/std::size(ret);
            ret[k] = vec2i_t{static_cast<int64_t>(std::round(r*std::cos(theta))),
                static_cast<int64_t>(std::round(r*std::sin(theta)))};
        }
        return ret;
    }

    constexpr auto neigbour_offsets = gen_neigbour_offset_table();

    template<class CostFunction>
    auto search(from<vec2i_t> origin, CostFunction&& f)
    {
        auto cmp = [](pending_route_node const& a, pending_route_node const& b)
        { return is_cheaper(b, a); };

        std::priority_queue<pending_route_node, std::vector<pending_route_node>, decltype(cmp)> nodes_to_visit;
        nodes_to_visit.push(pending_route_node{to<vec2i_t>{scale_factor*origin.value()}, 0.0});

        auto loc_cmp=[](to<vec2i_t> p1, to<vec2i_t> p2) {
            auto const a = p1.value();
            auto const b = p2.value();
            return (a[0] == b[0]) ? a[1] < b[1] : a[0] < b[0];
        };

        std::map<to<vec2i_t>, std::pair<route_node, bool>, decltype(loc_cmp)> cost_table;
        while(!nodes_to_visit.empty())
        {
            auto current = nodes_to_visit.top();
            nodes_to_visit.pop();
            auto& cost_item = cost_table[current.loc];
            cost_item.second = true;

            for(auto item : neigbour_offsets)
            {
                auto const next_loc = current.loc + item;

                auto const cost_increment = f(scale_by_factor(from{current.loc.value()}), scale_by_factor(next_loc));
                static_assert(std::is_same_v<std::decay_t<decltype(cost_increment)>, double>);
                if(cost_increment == std::numeric_limits<double>::infinity())
                { break; }

                auto& new_cost_item = cost_table[next_loc];
                if(new_cost_item.second)
                { break; }

                auto const new_cost = current.total_cost + cost_increment;
                if(new_cost < new_cost_item.first.total_cost)
                {
                    new_cost_item.first.total_cost = new_cost;
                    new_cost_item.first.loc = from<vec2i_t>{current.loc.value()};
                    nodes_to_visit.push(pending_route_node{next_loc, new_cost});
                }
            }
        }

        return cost_table;
    }

Example usage:

    auto result = search(cheapest_route::from{cheapest_route::vec2i_t{0, 0}},
        [](cheapest_route::from<cheapest_route::vec2f_t> a, cheapest_route::to<cheapest_route::vec2f_t> b) {
        auto const v1 = a.value();
        auto const v2 = b.value();

        constexpr auto size = 4.0;

        if((v2[0] < -size || v2[0] > size) || (v2[1] < -size || v2[1] > size))
        { return std::numeric_limits<double>::infinity(); }

        // Use Euclidian distance as a simple test case
        auto delta = v1 - v2;
        auto d2 = delta*delta;
        return d2[0] + d2[1];
    });

    std::ranges::for_each(result, [](auto const& item) {
        auto [to, node_info] = item;
        printf("to = (%ld, %ld), from = (%ld, %ld). total_cost = %.8g\n",
               to.value()[0], to.value()[1],
               node_info.first.loc.value()[0], node_info.first.loc.value()[1],
               node_info.first.total_cost);
    });

Output:

 ... to = (-1, -1), from = (0, 0). total_cost = 2
 ... to = (-1, 0), from = (0, 0). total_cost = 1
 ... to = (-1, 1), from = (0, 0). total_cost = 2
 ... to = (-1, 2), from = (0, 1). total_cost = 3
 ... to = (-1, 3), from = (0, 2). total_cost = 4
 ... to = (-1, 4), from = (0, 3). total_cost = 5
 ... to = (0, -1), from = (0, 0). total_cost = 1
 ... to = (0, 0), from = (0, 0). total_cost = inf
 ... to = (0, 1), from = (0, 0). total_cost = 1
 ... to = (0, 2), from = (0, 1). total_cost = 2
 ... to = (0, 3), from = (0, 2). total_cost = 3
 ... to = (0, 4), from = (0, 3). total_cost = 4
 ... to = (1, -1), from = (0, 0). total_cost = 2
 ... to = (1, 0), from = (0, 0). total_cost = 1
 ... to = (1, 1), from = (0, 0). total_cost = 2
 ... to = (1, 2), from = (0, 1). total_cost = 3
 ... to = (1, 3), from = (0, 2). total_cost = 4
 ... to = (1, 4), from = (0, 3). total_cost = 5
 ... to = (2, -1), from = (1, -1). total_cost = 3
 ... to = (2, 0), from = (1, 0). total_cost = 2
 ... to = (2, 1), from = (1, 0). total_cost = 3
 ... to = (2, 2), from = (1, 1). total_cost = 4
 ... to = (2, 3), from = (1, 2). total_cost = 5
 ... to = (2, 4), from = (1, 3). total_cost = 6
 ... to = (3, -1), from = (2, -1). total_cost = 4
 ... to = (3, 0), from = (2, 0). total_cost = 3
 ... to = (3, 1), from = (2, 0). total_cost = 4
 ... to = (3, 2), from = (2, 1). total_cost = 5
 ... to = (3, 3), from = (2, 2). total_cost = 6
 ... to = (3, 4), from = (2, 3). total_cost = 7
 ... to = (4, -1), from = (3, -1). total_cost = 5
 ... to = (4, 0), from = (3, 0). total_cost = 4
 ... to = (4, 1), from = (3, 0). total_cost = 5
 ... to = (4, 2), from = (3, 1). total_cost = 6
 ... to = (4, 3), from = (3, 2). total_cost = 7
 ... to = (4, 4), from = (3, 3). total_cost = 8

I think the output is correct, but if I increase the domain size beyond anything but a very small value, the algorithm starts to consume tons of RAM, and I cannot run it to completion, because I would run out of RAM.

1. Is it expected to take that much system resources, or is there something that I have missed?
2. I do not need the cost between start point and any other point. How can I terminate early?

Answer 1

Don't use compiler-specific vector extensions unnecessarily

Using [[gnu::vector_size(16)]] makes your code unportable. I don't think there is any great benefit for your code to force coordinates to be in vector registers. Instead I would just write:

struct vec2i {
     uint64_t x;
     uint64_t y;
};

Or consider using an external library that provides you with vector types (like GLM, but there are many others), so that you don't have to worry about portability.

Where is your graph?

Normally, I would expect an implementation of Dijkstra's to take as input a pre-existing graph (usually stored as either a set of edges or as an adjacency list) and a starting vertex, and then it will iterate over the graph. However, your graph is not explicitly stored in memory. Instead, the cost function will implicitly generate a graph for you. And I think that is where your main problem is: even for a relatively small value of size, this can generate infinitely many paths that are within the square defined by size. Sure, you prune paths you know for sure will be too long, but the larger size gets, the bigger the chance is that a huge amount of paths will be visited by your code. And in turn that means nodes_to_visit and cost_table will grow larger and larger.

I suggest you first generate an explicit graph, then have your algorithm work on that.

Avoid C functions

I would avoid using C functions where possible, and use equivalent C++ functionality. Sure, shifting things into std::cout isn't as nice as printf(), but since C++17 you can use std::format, or if you cannot use that function yet, you can use the {fmt} library.

Answer 2

Overview

This code is very dense and hard to read.
It could definitely do with some additional comments to make it more readable.

Not convinced this is an accurate implementation of Dijkstra algorithm; it does not seem to have a set of vertexes that have been completed, and thus you can potentially find the same node again and again and add its children back to the frontier list (causing an explosion in the frontier). That is my initial reading, I could be mistaken because of the denseness of the code.

Don't be afraid to put your code in its own namespace.

Code Review

OK. I have never seen this.

    using vec2i_t [[gnu::vector_size(16)]] = int64_t;
    using vec2f_t [[gnu::vector_size(16)]] = double;

That's on me. I am not sure if I understand this syntax, can somebody explain or post a link so that I can learn what is happening here.

---

If the type of T is small then no issue here. But if T is large (like a vector) then you are always copying the input into this structure. Could there be situations where it could be moved.

    template<class T, auto tag>
    struct tagged_value
    {
    public:
        tagged_value() = default;
        explicit tagged_value(T value):m_value{value}{}
        T value() const { return m_value; }

    private:
        T m_value;
    };

I would change the main constructor to allow for move opportunities.

        explicit tagged_value(T value)
             :m_value{std::move(value)}
        {}

It looks like you have some non trivial type below:

   using vec2i_t [[gnu::vector_size(16)]] = int64_t;
   ....
   to<vec2i_t> loc;

So if it was movable, this would be a benefit. If it's not movable you have not lost anything and when you upgrade your code to make it movable you automatically get the enhancement.

---

OK. Not bad. But we can prevent two copies by passing the parameters by const reference.

    template<class T, auto tag>
    inline tagged_value<T, tag> operator+(tagged_value<T, tag> const& a, T const& b)
    {
        return tagged_value<T, tag>{a.value() + b};
    }

Is there an opportunity to use operator+= that would be slightly more efficient as we can remove a copy operation.

    template<class T, auto tag>
    inline tagged_value<T, tag>& operator+=(tagged_value<T, tag>& a, T const& b)
    {
        a.value() += b; // append the b into a.
        return a;       // return a reference to the input value.
    }
    // Now we can define `operator+` in terms of `+=`!
    template<class T, auto tag>
    inline tagged_value<T, tag> operator+(tagged_value<T, tag> const& a, T const& b)
    {
        // Create a copy then invoke the +=
        return tagged_value<T, tag>{a} += b;
    }

---

OK as far as it goes.

    struct pending_route_node
    {
        to<vec2i_t> loc;
        double total_cost;
    };

But in the function search() you have to jump through some extra hoops to get a std::priority_queue of pending_route_node as they don't know how to compare against themsleves.

        auto cmp = [](pending_route_node const& a, pending_route_node const& b) { return is_cheaper(b, a); };

        std::priority_queue<pending_route_node, std::vector<pending_route_node>, decltype(cmp)> nodes_to_visit;

If you define the comparison operator it makes life simpler.

    struct pending_route_node
    {
        to<vec2i_t> loc;
        double total_cost;
        bool operator<(pending_route_node const& rhs)
        {
            return is_cheaper(rhs, *this);
        };
    };
    ....
    // Notice how you don't need any other template arguments
    std::priority_queue<pending_route_node> nodes_to_visit;

---

How is this different from pending_route_node?

    struct route_node
    {
        from<vec2i_t> loc;
        double total_cost = std::numeric_limits<double>::infinity();
    };

You could have defined one template and two usings.

    template<typename Loc>
    struct route_node
    {
        Loc loc;
        double total_cost;
        bool operator<(pending_route_node const& rhs)
        {
            return is_cheaper(rhs, *this);
        };
    };
    using to_route_node   = route_node<to<vec2i_t>>;
    using from_route_node = route_node<from<vec2i_t>>;

---

Not a fan of auto here.

    constexpr auto scale_factor = 1;

This may be personal but I want to be explicit about the types of constants. If I read further on you explicitly cast this to double before use. Why not make it a double here.

---

    template<auto tag>
    constexpr auto scale_by_factor(tagged_value<vec2i_t, tag> val)
    {
        // This is a copy.
        // You really don't want this copy an alias is fine.
        // So make this a const reference.
        auto const x = val.value();
        ...

---

This looks very much like something that is already defined for std::tuple.

        auto loc_cmp=[](to<vec2i_t> p1, to<vec2i_t> p2) {
            auto const a = p1.value();
            auto const b = p2.value();
            return (a[0] == b[0]) ? a[1] < b[1] : a[0] < b[0];
        };

But this is simply to make a std::map operation work better. I think I would have done it differently. Not sure mine is better (that would need to be tested). But I think this is cleaner to read.

       template<typename T, typename tag>
       bool operator<(tagged_value<T, tag> const& lhs, tagged_value<T, tag> const& rhs)
       {
           return lhs.value() < rhs.value ();
       }

       bool operator<(vec2i_t const& lhs, vec2i_t const& rhs)
       {
           return std::tie(lhs[0], lhs[1]) < std::tie(rhs[0], rhs[1]);
       }

Then your map declaration looks like:

       // Original.
       std::map<to<vec2i_t>, std::pair<route_node, bool>, decltype(loc_cmp)> cost_table;

       // Now
       using Cost = std::pair<route_node, bool>;
       std::map<to<vec2i_t>, Cost>    cost_table;

---

OK. The soup and nuts of the code.

        while(!nodes_to_visit.empty())
        {
            auto current = nodes_to_visit.top();
            nodes_to_visit.pop();

            // At this point I would expect a check to see if we have
            // already found this node.
            //
            // Note: The reason we use a `std::priority_queue<>` here
            //       is that the queue is sorted by the shortest distance
            //       to this point. So the first time you find a location
            //       at the top of the  `nodes_to_visit` list this is the
            //       shortest route to this point and any subsequent
            //       entries in the list that arrived here are sub optimal
            //       and can thus be simply ignored.



            auto& cost_item = cost_table[current.loc];
            cost_item.second = true;



            for(auto item : neigbour_offsets)
            {
                //
                // the names in here are not quite enough for me
                // to understand what is going on. I have tried
                // to parse this a couple of times and can not quite
                // put my finger on what is happening.
                //
                // Maybe some cvomments to explain the logic here
                // would be useful.
                auto const next_loc = current.loc + item;

                auto const cost_increment = f(scale_by_factor(from{current.loc.value()}), scale_by_factor(next_loc));
                static_assert(std::is_same_v<std::decay_t<decltype(cost_increment)>, double>);
                if(cost_increment == std::numeric_limits<double>::infinity())
                { break; }

                auto& new_cost_item = cost_table[next_loc];
                if(new_cost_item.second)
                { break; }

                auto const new_cost = current.total_cost + cost_increment;
                if(new_cost < new_cost_item.first.total_cost)
                {
                    new_cost_item.first.total_cost = new_cost;
                    new_cost_item.first.loc = from<vec2i_t>{current.loc.value()};
                    nodes_to_visit.push(pending_route_node{next_loc, new_cost});
                }
            }
        }

        return cost_table;
    }