Dijkstra path finding in C# is 15x slower than C++ version

Question

I'm implementing Dijkstra's algorithm with a priority queue for a game I'm developing in Unity with C#. I was a bit disappointed with the performance, so I decided to port the code to C++ and see if the performance was related to the language or the algorithm itself. The path finding searches through a 3D grid and selects certain edges/neighbours based on some extra criteria (cell filter).

The problem is that this grid contains only 3000 cells, and the C# algorithm takes 38 ms to find a path. The C++ version takes just 2 ms to do the exact same thing!

The two source files of the algorithm are below and I'm wondering if someone experienced with C# can point out if I've done anything horribly inefficient or if C# is just slower here. The C# version stores the grid as a multidimensional array and the C++ version simulates it with an extra get_index function that computes an index into a vector using the x, y and z coordinates. I simulate a priority queue in C# by using a SortedSet with a special queue node containing both the value and the priority value (dist). Both algorithms simulate updating the priority queue by just appending a new value that invalidates the old one. This is done by also storing the priorities in the dist hash table.

C#:

using System;
using System.Collections.Generic;
using System.IO;

namespace PathFinding.NET {
    struct Vec3 {
        public int x, y, z;

        public Vec3(int x, int y, int z) {
            this.x = x;
            this.y = y;
            this.z = z;
        }

        public static Vec3 operator +(Vec3 a, Vec3 b) {
            return new Vec3(a.x + b.x, a.y + b.y, a.z + b.z);
        }

        public static bool operator ==(Vec3 a, Vec3 b) {
            return a.x == b.x && a.y == b.y && a.z == b.z;
        }

        public static bool operator !=(Vec3 a, Vec3 b) {
            return !(a == b);
        }

        public static float Dist(Vec3 a, Vec3 b) {
            int dx = a.x - b.x;
            int dy = a.y - b.y;
            int dz = a.z - b.z;

            return (float)Math.Sqrt(dx * dx + dy * dy + dz * dz);
        }

        public static Vec3 Min(Vec3 a, Vec3 b) {
            return new Vec3(
                Math.Min(a.x, b.x),
                Math.Min(a.y, b.y),
                Math.Min(a.z, b.z)
            );
        }

        public static Vec3 Max(Vec3 a, Vec3 b) {
            return new Vec3(
                Math.Max(a.x, b.x),
                Math.Max(a.y, b.y),
                Math.Max(a.z, b.z)
            );
        }

        public override string ToString() {
            return "(" + x + ", " + y + ", " + z + ")";
        }

        public int CompareTo(object obj) {
            var other = (Vec3)obj;

            if (x == other.x) {
                if (y == other.y) {
                    return z.CompareTo(other.z);
                } else {
                    return y.CompareTo(other.y);
                }
            } else {
                return x.CompareTo(other.x);
            }
        }
    }

    struct Cell {
        public bool Occupied;
        public bool WalkableSurface;
    }

    struct QueueNode : IComparable {
        public Vec3 Value;
        public float Dist;

        public QueueNode(Vec3 value, float dist) {
            Value = value;
            Dist = dist;
        }

        public int CompareTo(object obj) {
            var other = (QueueNode)obj;

            if (Dist != other.Dist) {
                return Dist.CompareTo(other.Dist);
            } else {
                return Value.CompareTo(other.Value);
            }
        }
    }

    class Program {
        private static Cell[,,] Grid = null;
        private static int sx, sy, sz;

        private static List<Vec3> GetNeighbours(Vec3 cell) {
            var neighbours = new List<Vec3>();

            for (int dx = -1; dx <= 1; dx++) {
                for (int dy = -1; dy <= 1; dy++) {
                    for (int dz = -1; dz <= 1; dz++) {
                        var coord = cell + new Vec3(dx, dy, dz);

                        bool notSelf = !(dx == 0 && dy == 0 && dz == 0);
                        bool connectivity = Math.Abs(dx) + Math.Abs(dy) + Math.Abs(dz) <= 2;
                        bool withinGrid = coord.x >= 0 && coord.y >= 0 && coord.z >= 0 && coord.x < sx && coord.y < sy && coord.z < sz;

                        if (notSelf && connectivity && withinGrid) {
                            neighbours.Add(coord);
                        }
                    }
                }
            }

            return neighbours;
        }

        private static List<Vec3> FindPath(Vec3 start, Vec3 end, Func<Vec3, Vec3, bool> cellFilter) {
            if (!cellFilter(start, start) || !cellFilter(end, end)) {
                throw new ArgumentException("Start and/or end fail cell filter!");
            }

            // Initialize data structures
            var dist = new Dictionary<Vec3, float>();
            var prev = new Dictionary<Vec3, Vec3?>();

            // We're intentionally not using the update priority function to mimic the C++ algorithm
            var Q = new SortedSet<QueueNode>();

            for (int x = 0; x < sx; x++) {
                for (int y = 0; y < sy; y++) {
                    for (int z = 0; z < sz; z++) {
                        var coord = new Vec3(x, y, z);

                        if (cellFilter(coord, coord)) {
                            dist[coord] = float.MaxValue;
                            Q.Add(new QueueNode(coord, float.MaxValue));

                            prev[coord] = null;
                        }
                    }
                }
            }

            dist[start] = 0;
            Q.Add(new QueueNode(start, 0));

            // Search loop
            while (Q.Count > 0) {
                var u = Q.Min;
                Q.Remove(Q.Min);

                // Old priority queue value
                if (u.Dist != dist[u.Value]) {
                    continue;
                }

                if (u.Value == end) {
                    break;
                }

                foreach (var v in GetNeighbours(u.Value)) {
                    if (cellFilter(u.Value, v)) {
                        float alt = dist[u.Value] + Vec3.Dist(u.Value, v);
                        if (alt < dist[v]) {
                            dist[v] = alt;
                            Q.Add(new QueueNode(v, alt));

                            prev[v] = u.Value;
                        }
                    }
                }
            }

            // Trace path - if there is one
            var path = new List<Vec3>();

            if (prev[end] != null) {
                Vec3? current = end;

                while (current != null) {
                    path.Add(current.Value);
                    current = prev[current.Value];
                }

                path.Reverse();
            }

            return path;
        }

        private static bool IsFloor(Vec3 pos) {
            if (pos.y > 0) {
                var posBelow = pos + new Vec3(0, -1, 0);
                return !Grid[pos.x, pos.y, pos.z].Occupied && Grid[posBelow.x, posBelow.y, posBelow.z].WalkableSurface;
            } else {
                return false;
            }
        }

        private static bool CellFilter(Vec3 from, Vec3 to) {
            if (from.y == to.y) {
                // Check if all cells we're moving through are floors (important when moving diagonally)
                var min = Vec3.Min(from, to);
                var max = Vec3.Max(from, to);

                for (int x = min.x; x <= max.x; x++) {
                    for (int z = min.z; z <= max.z; z++) {
                        if (!IsFloor(new Vec3(x, min.y, z))) {
                            return false;
                        }
                    }
                }

                return true;
            } else {
                // If the movement is vertical, then perform no diagonal check
                return IsFloor(to);
            }
        }

        public static void Main(string[] args) {
            // Read grid
            string[] gridLines = File.ReadAllLines("grid.txt");

            sx = int.Parse(gridLines[0].Split(' ')[0]);
            sy = int.Parse(gridLines[0].Split(' ')[1]);
            sz = int.Parse(gridLines[0].Split(' ')[2]);

            Grid = new Cell[sx, sy, sz];

            int i = 1;
            for (int x = 0; x < sx; x++) {
                for (int y = 0; y < sy; y++) {
                    for (int z = 0; z < sz; z++) {
                        Cell cell = new Cell();
                        cell.Occupied = bool.Parse(gridLines[i].Split(' ')[0]);
                        cell.WalkableSurface = bool.Parse(gridLines[i].Split(' ')[0]);
                        Grid[x, y, z] = cell;

                        i++;
                    }
                }
            }

            // Do pathfinding
            Vec3 start = new Vec3(9, 2, 6);
            Vec3 end = new Vec3(45, 2, 0);

            var t1 = DateTime.Now;
            var path = FindPath(start, end, CellFilter);
            var t2 = DateTime.Now;

            Console.WriteLine("best path is " + path.Count + " cells long");
            Console.WriteLine("path finding took " + (t2 - t1).TotalMilliseconds + " ms");
        }
    }
}

C++

#include <iostream>
#include <fstream>
#include <algorithm>
#include <vector>
#include <functional>
#include <stdexcept>
#include <queue>
#include <unordered_map>
#include <chrono>

struct vec3 {
    int x, y, z;

    int get_index(int sx, int sy, int sz) const {
        return x * sy * sz + y * sz + z;
    }

    bool operator==(const vec3& other) const {
        return x == other.x && y == other.y && z == other.z;
    }

    vec3 operator+(const vec3& other) const {
        return{x + other.x, y + other.y, z + other.z};
    }

    static vec3 min(const vec3& a, const vec3& b) {
        return{std::min(a.x, b.x), std::min(a.y, b.y), std::min(a.z, b.z)};
    }

    static vec3 max(const vec3& a, const vec3& b) {
        return{std::max(a.x, b.x), std::max(a.y, b.y), std::max(a.z, b.z)};
    }

    static float dist(const vec3& a, const vec3& b) {
        auto dx = static_cast<float>(a.x - b.x);
        auto dy = static_cast<float>(a.y - b.y);
        auto dz = static_cast<float>(a.z - b.z);

        return sqrtf(dx*dx + dy*dy + dz*dz);
    }
};

namespace std {
    template<>
    struct hash<vec3> {
        size_t operator()(const vec3& k) const {
            return ((hash<int>()(k.x)
                ^ (hash<int>()(k.y) << 1)) >> 1)
                ^ (hash<int>()(k.z) << 1);
        }
    };
}

struct cell {
    bool occupied;
    bool walkableSurface;
};

int sx, sy, sz;
std::vector<cell> grid;

std::vector<vec3> get_neighbours(const vec3& cell) {
    std::vector<vec3> neighbours;

    for (int dx = -1; dx <= 1; dx++) {
        for (int dy = -1; dy <= 1; dy++) {
            for (int dz = -1; dz <= 1; dz++) {
                auto coord = cell + vec3{dx, dy, dz};

                bool notSelf = !(dx == 0 && dy == 0 && dz == 0);
                bool connectivity = abs(dx) + abs(dy) + abs(dz) <= 2;
                bool withinGrid = coord.x >= 0 && coord.y >= 0 && coord.z >= 0 && coord.x < sx && coord.y < sy && coord.z < sz;

                if (notSelf && connectivity && withinGrid) {
                    neighbours.push_back(coord);
                }
            }
        }
    }

    return neighbours;
}

std::vector<vec3> find_path(const vec3& start, const vec3& end, bool(*cellFilter)(const vec3&, const vec3&)) {
    if (!cellFilter(start, start) || !cellFilter(end, end)) {
        throw std::invalid_argument("start and/or end fail cell filter!");
    }

    // Initialize data structures
    std::unordered_map<vec3, float> dist;
    std::unordered_map<vec3, vec3> prev;

    struct queue_node {
        vec3 value;
        float dist;
    };

    auto cmp = [&](const queue_node& a, const queue_node& b) {
        return a.dist > b.dist;
    };

    std::priority_queue<queue_node, std::vector<queue_node>, decltype(cmp)> Q(cmp);

    for (int x = 0; x < sx; x++) {
        for (int y = 0; y < sy; y++) {
            for (int z = 0; z < sz; z++) {
                vec3 coord = {x, y, z};

                if (cellFilter(coord, coord)) {
                    dist[coord] = std::numeric_limits<float>::max();
                    Q.push({coord, std::numeric_limits<float>::max()});

                    prev[coord] = vec3{-1, -1, -1};
                }
            }
        }
    }

    dist[start] = 0;
    Q.push({start, 0});

    // Search loop
    while (!Q.empty()) {
        auto u = Q.top();
        Q.pop();

        // Old priority queue value
        if (u.dist != dist[u.value]) {
            continue;
        }

        if (u.value == end) {
            break;
        }

        for (const vec3& v : get_neighbours(u.value)) {
            if (cellFilter(u.value, v)) {
                float alt = dist[u.value] + vec3::dist(u.value, v);
                if (alt < dist[v]) {
                    dist[v] = alt;
                    Q.push({v, alt});

                    prev[v] = u.value;
                }
            }
        }
    }

    // Trace path - if there is one
    std::vector<vec3> path;

    if (prev[end].x != -1) {
        vec3 current = end;

        while (current.x != -1) {
            path.push_back(current);
            current = prev[current];
        }

        std::reverse(path.begin(), path.end());
    }

    return path;
}

bool isFloor(const vec3& pos) {
    if (pos.y > 0) {
        return !grid[pos.get_index(sx, sy, sz)].occupied && grid[(pos + vec3{0, -1, 0}).get_index(sx, sy, sz)].walkableSurface;
    } else {
        return false;
    }
}

bool cellFilter(const vec3& from, const vec3& to) {
    if (from.y == to.y) {
        // Check if all cells we're moving through are floors (important when moving diagonally)
        auto min = vec3::min(from, to);
        auto max = vec3::max(from, to);

        for (int x = min.x; x <= max.x; x++) {
            for (int z = min.z; z <= max.z; z++) {
                if (!isFloor({x, min.y, z})) {
                    return false;
                }
            }
        }

        return true;
    } else {
        // If the movement is vertical, then perform no diagonal check
        return isFloor(to);
    }
}

int main() {
    // Read grid
    std::ifstream gridFile("grid.txt");

    gridFile >> sx >> sy >> sz;

    int i = 0;
    grid.resize(sx * sy * sz);

    for (int x = 0; x < sx; x++) {
        for (int y = 0; y < sy; y++) {
            for (int z = 0; z < sz; z++) {
                bool occupied, walkableSurface;
                gridFile >> occupied >> walkableSurface;
                grid[i++] = {occupied, walkableSurface};
            }
        }
    }

    // Do pathfinding
    vec3 start = {9, 2, 6};
    vec3 end = {45, 2, 0};

    try {
        auto t1 = std::chrono::high_resolution_clock::now();
        auto path = find_path(start, end, cellFilter);
        auto t2 = std::chrono::high_resolution_clock::now();

        float ms = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1).count() / 1000.0f;

        std::cout << "best path is " << path.size() << " cells long" << std::endl;
        std::cout << "path finding took " << ms << " ms" << std::endl;
    } catch (std::exception& e) {
        std::cout << "exception: " << e.what() << std::endl;
    }

    return 0;
}

If you want to run the algorithm yourself, then you need this grid.txt file.

Answer 1

First of all, you should run the FindPath method a couple of times before measuring, to give the C# runtime a chance to optimize the code.

// Warmup iterations for profiling
for (int j = 0; j < 10; j++) {
    FindPath(start, end, CellFilter);
}

Doing this gets the time down to about 17 ms on my machine (from 38 ms initially).

Running the code in a profiler shows that over 70% of the time is spent in Dictionary and SortedSet methods. For the JIT to optimize those you have to provide it with the necessary information for its Key types, otherwise it will fall back to runtime reflection and virtual method calls.

Any struct that is used as a Key in a Dictionary should implement the IEquatable<T> interface. Also GetHashCode and Equals should be overridden (the compiler even warns about it).

struct Vec3 : IComparable<Vec3>, IEquatable<Vec3> {
    [...]
    public bool Equals(Vec3 other) {
        return other == this;
    }

    public override int GetHashCode() {
        return ((x.GetHashCode()
            ^ (y.GetHashCode() << 1)) >> 1)
            ^ (z.GetHashCode() << 1);
    }

    public override bool Equals(object obj) {
        if (obj is Vec3) {
            return (Vec3)obj == this;
        }

        return false;
    }
}

SortedSet mostlikely needs the IComparable<T> interface which QueueNode already had, but it should be changed to the generic one.

struct QueueNode : IComparable<QueueNode> {
    [...]
    public int CompareTo(QueueNode other) {
        if (Dist != other.Dist) {
            return Dist.CompareTo(other.Dist);
        } else {
            return Value.CompareTo(other.Value);
        }
    }
}

After these changes FindPath only takes 4 ms.

We can further optimize the Dictionaries by passing in a custom IEqualityComparerand eliminating the int.GetHashCode() calls.

class Vec3Comparer : IEqualityComparer<Vec3>
{
    public bool Equals(Vec3 a, Vec3 b) {
        return a == b;
    }

    public int GetHashCode(Vec3 obj) {
        return ((IntegerHash(obj.x)
                ^ (IntegerHash(obj.y) << 1)) >> 1)
                ^ (IntegerHash(obj.z) << 1);
    }

    static int IntegerHash(int a) {
        // fmix32 from murmurhash
        uint h = (uint)a;
        h ^= h >> 16;
        h *= 0x85ebca6bU;
        h ^= h >> 13;
        h *= 0xc2b2ae35U;
        h ^= h >> 16;
        return (int)h;
    }
}

void FindPath(...) {
    [...]

    // Initialize data structures
    Vec3Comparer comparer = new Vec3Comparer();
    var dist = new Dictionary<Vec3, float>(comparer);
    var prev = new Dictionary<Vec3, Vec3?>(comparer);

    [...]
}

The final code takes about 2.8 ms for FindPath.

In conclusion, always implement the correct generic interfaces on structures that are used in collections. It allows the JIT to actually optimize the code.

Useful links

• Dictionary(Of TKey, TValue) Class. See the Remarks section, thanks to @t3chb0t.

• C# performance tips for Unity, part 2: structs and enums. It talks specifically about the Unity implementation.

Final Code

using System;
using System.Collections.Generic;
using System.IO;

namespace PathFinding.NET {
    struct Vec3 : IComparable<Vec3>, IEquatable<Vec3> {
        public int x, y, z;

        public Vec3(int x, int y, int z) {
            this.x = x;
            this.y = y;
            this.z = z;
        }

        public static Vec3 operator +(Vec3 a, Vec3 b) {
            return new Vec3(a.x + b.x, a.y + b.y, a.z + b.z);
        }

        public static bool operator ==(Vec3 a, Vec3 b) {
            return a.x == b.x && a.y == b.y && a.z == b.z;
        }

        public static bool operator !=(Vec3 a, Vec3 b) {
            return !(a == b);
        }

        public static float Dist(Vec3 a, Vec3 b) {
            int dx = a.x - b.x;
            int dy = a.y - b.y;
            int dz = a.z - b.z;

            return (float)Math.Sqrt(dx * dx + dy * dy + dz * dz);
        }

        public static Vec3 Min(Vec3 a, Vec3 b) {
            return new Vec3(
                Math.Min(a.x, b.x),
                Math.Min(a.y, b.y),
                Math.Min(a.z, b.z)
            );
        }

        public static Vec3 Max(Vec3 a, Vec3 b) {
            return new Vec3(
                Math.Max(a.x, b.x),
                Math.Max(a.y, b.y),
                Math.Max(a.z, b.z)
            );
        }

        public override string ToString() {
            return "(" + x + ", " + y + ", " + z + ")";
        }

        public int CompareTo(Vec3 other) {
            if (x == other.x) {
                if (y == other.y) {
                    return z.CompareTo(other.z);
                } else {
                    return y.CompareTo(other.y);
                }
            } else {
                return x.CompareTo(other.x);
            }
        }

        public bool Equals(Vec3 other) {
            return other == this;
        }

        public override int GetHashCode() {
            return ((x.GetHashCode()
                ^ (y.GetHashCode() << 1)) >> 1)
                ^ (z.GetHashCode() << 1);
        }

        public override bool Equals(object obj) {
            if (obj is Vec3) {
                return (Vec3)obj == this;
            }

            return false;
        }
    }

    struct Cell {
        public bool Occupied;
        public bool WalkableSurface;
    }

    struct QueueNode : IComparable<QueueNode> {
        public Vec3 Value;
        public float Dist;

        public QueueNode(Vec3 value, float dist) {
            Value = value;
            Dist = dist;
        }

        public int CompareTo(QueueNode other) {
            if (Dist != other.Dist) {
                return Dist.CompareTo(other.Dist);
            } else {
                return Value.CompareTo(other.Value);
            }
        }
    }

    class Vec3Comparer : IEqualityComparer<Vec3>
    {
        public bool Equals(Vec3 a, Vec3 b) {
            return a == b;
        }

        public int GetHashCode(Vec3 obj) {
            return ((IntegerHash(obj.x)
                    ^ (IntegerHash(obj.y) << 1)) >> 1)
                    ^ (IntegerHash(obj.z) << 1);
        }

        static int IntegerHash(int a) {
            // fmix32 from murmurhash
            uint h = (uint)a;
            h ^= h >> 16;
            h *= 0x85ebca6bU;
            h ^= h >> 13;
            h *= 0xc2b2ae35U;
            h ^= h >> 16;
            return (int)h;
        }
    }

    class Program {
        private static Cell[,,] Grid = null;
        private static int sx, sy, sz;

        private static List<Vec3> GetNeighbours(Vec3 cell, List<Vec3> neighbours) {
            neighbours.Clear();

            for (int dx = -1; dx <= 1; dx++) {
                for (int dy = -1; dy <= 1; dy++) {
                    for (int dz = -1; dz <= 1; dz++) {
                        var coord = cell + new Vec3(dx, dy, dz);

                        bool notSelf = !(dx == 0 && dy == 0 && dz == 0);
                        bool connectivity = Math.Abs(dx) + Math.Abs(dy) + Math.Abs(dz) <= 2;
                        bool withinGrid = coord.x >= 0 && coord.y >= 0 && coord.z >= 0 && coord.x < sx && coord.y < sy && coord.z < sz;

                        if (notSelf && connectivity && withinGrid) {
                            neighbours.Add(coord);
                        }
                    }
                }
            }

            return neighbours;
        }

        private static List<Vec3> FindPath(Vec3 start, Vec3 end, Func<Vec3, Vec3, bool> cellFilter) {
            if (!cellFilter(start, start) || !cellFilter(end, end)) {
                throw new ArgumentException("Start and/or end fail cell filter!");
            }

            // Initialize data structures
            Vec3Comparer comparer = new Vec3Comparer();
            var dist = new Dictionary<Vec3, float>(comparer);
            var prev = new Dictionary<Vec3, Vec3?>(comparer);

            // We're intentionally not using the update priority function to mimic the C++ algorithm
            var Q = new SortedSet<QueueNode>();

            for (int x = 0; x < sx; x++) {
                for (int y = 0; y < sy; y++) {
                    for (int z = 0; z < sz; z++) {
                        var coord = new Vec3(x, y, z);

                        if (cellFilter(coord, coord)) {
                            dist[coord] = float.MaxValue;
                            Q.Add(new QueueNode(coord, float.MaxValue));

                            prev[coord] = null;
                        }
                    }
                }
            }

            dist[start] = 0;
            Q.Add(new QueueNode(start, 0));

            List<Vec3> neighbours = new List<Vec3>();

            // Search loop
            while (Q.Count > 0) {
                var u = Q.Min;
                Q.Remove(Q.Min);

                // Old priority queue value
                if (u.Dist != dist[u.Value]) {
                    continue;
                }

                if (u.Value == end) {
                    break;
                }

                foreach (var v in GetNeighbours(u.Value, neighbours)) {
                    if (cellFilter(u.Value, v)) {
                        float alt = dist[u.Value] + Vec3.Dist(u.Value, v);
                        if (alt < dist[v]) {
                            dist[v] = alt;
                            Q.Add(new QueueNode(v, alt));

                            prev[v] = u.Value;
                        }
                    }
                }
            }

            // Trace path - if there is one
            var path = new List<Vec3>();

            if (prev[end] != null) {
                Vec3? current = end;

                while (current != null) {
                    path.Add(current.Value);
                    current = prev[current.Value];
                }

                path.Reverse();
            }

            return path;
        }

        private static bool IsFloor(Vec3 pos) {
            if (pos.y > 0) {
                var posBelow = pos + new Vec3(0, -1, 0);
                return !Grid[pos.x, pos.y, pos.z].Occupied && Grid[posBelow.x, posBelow.y, posBelow.z].WalkableSurface;
            } else {
                return false;
            }
        }

        private static bool CellFilter(Vec3 from, Vec3 to) {
            if (from.y == to.y) {
                // Check if all cells we're moving through are floors (important when moving diagonally)
                var min = Vec3.Min(from, to);
                var max = Vec3.Max(from, to);

                for (int x = min.x; x <= max.x; x++) {
                    for (int z = min.z; z <= max.z; z++) {
                        if (!IsFloor(new Vec3(x, min.y, z))) {
                            return false;
                        }
                    }
                }

                return true;
            } else {
                // If the movement is vertical, then perform no diagonal check
                return IsFloor(to);
            }
        }

        public static void Main(string[] args) {
            // Read grid
            string[] gridLines = File.ReadAllLines("grid.txt");

            sx = int.Parse(gridLines[0].Split(' ')[0]);
            sy = int.Parse(gridLines[0].Split(' ')[1]);
            sz = int.Parse(gridLines[0].Split(' ')[2]);

            Grid = new Cell[sx, sy, sz];

            int i = 1;
            for (int x = 0; x < sx; x++) {
                for (int y = 0; y < sy; y++) {
                    for (int z = 0; z < sz; z++) {
                        Cell cell = new Cell();
                        cell.Occupied = bool.Parse(gridLines[i].Split(' ')[0]);
                        cell.WalkableSurface = bool.Parse(gridLines[i].Split(' ')[0]);
                        Grid[x, y, z] = cell;

                        i++;
                    }
                }
            }

            // Do pathfinding
            Vec3 start = new Vec3(9, 2, 6);
            Vec3 end = new Vec3(45, 2, 0);

            // Warmup iterations for profiling
            for (int j = 0; j < 10; j++) {
                FindPath(start, end, CellFilter);
            }

            var timer = new System.Diagnostics.Stopwatch();

            timer.Start();
            var path = FindPath(start, end, CellFilter);
            timer.Stop();

            Console.WriteLine("best path is " + path.Count + " cells long");
            Console.WriteLine("path finding took " + timer.Elapsed.TotalMilliseconds + " ms");
        }
    }
}

Answer 2

For the moment, I'm ignoring the C# code (and its speed), and reviewing the C++ code for ways it might be open to improvement in readability (but with a decent compiler, what I'm suggesting shouldn't affect its speed).

Cell

Rather than having code in main that reads in components, then composes them into a cell, I'd rather the cell knew how to read itself in from a stream:

struct cell {
    bool occupied;
    bool walkableSurface;

    friend std::istream &operator>>(std::istream &is, cell &c) {
        return is >> c.occupied >> c.walkableSurface;
    }
};

Grid

Likewise, it seems to me that right now, you have knowledge of the structure of your 3D grid distributed throughout a lot of the code. main reads data into the grid, vec3::get_index converts from a 3D vector to a grid index, and so on.

I'd rather centralize that into one class that provides a more convenient interface, something on this order:

class Grid {
    std::vector<cell> data;
public:
    int sx, sy, sz;

    cell &operator[](vec3 const &index) {
        return data[index.x * sy * sz + index.y * sz + index.z];
    }

    friend std::istream &operator>>(std::istream &is, Grid &g) {
        is >> g.sx >> g.sy >> g.sz;

        int i = 0;
        g.data.resize(g.sx * g.sy * g.sz);

        is >> std::boolalpha;

        for (int x = 0; x < g.sx; x++) {
            for (int y = 0; y < g.sy; y++) {
                for (int z = 0; z < g.sz; z++) {
                    is >> g.data[i++];
                }
            }
        }
        return is;
    }

    bool contains(vec3 const &coord) {
        return coord.x >= 0 && coord.x < sx && coord.y >= 0 && coord.y < sy && coord.z >= 0 && coord.x < sz;
    }
} grid;

With these in place, main reads in the grid something like this:

std::ifstream gridFile("grid.txt");

gridFile >> grid;

...and isFloor turns into something like this:

return pos.y > 0 && !grid[pos].occupied && grid[(pos + vec3{ 0, -1, 0 })].walkableSurface;

...and the computation of withinGrid in get_neighbors simplifies to:

bool withinGrid = grid.contains(coord);

queue_node

Looking at queue_node, I think I'd try to encapsulate its comparison criteria with a fairly minor rewrite:

struct queue_node {
    vec3 value;
    float dist;

    bool operator<(queue_node const &other) const {
        return other.dist < dist;
    }
};

With this, we can simplify the priority_queue a bit, to become:

std::priority_queue<queue_node> Q;

Naming

I think some of the names could be improved. The most obvious would be cellFilter--it tends to indicate that we're interested in whether a cell meets some set of criteria, but doesn't tell us anything about the criteria we want it to meet.

Timing

Maybe it's because I've wasted spent far too much of my time answering questions both here and on Stack Overflow, but I find it convenient to have a timing function that lets me time a function without re-writing the timing code every time. I use this:

template <typename F, typename ...Args>
auto timer(F f, std::string const &label, Args && ...args) {
    using namespace std::chrono;

    auto start = high_resolution_clock::now();
    auto holder = f(std::forward<Args>(args)...);
    auto stop = high_resolution_clock::now();
    std::cout << label << " time: " << duration_cast<microseconds>(stop - start).count() << "\n";

    return holder;
}

With this, timing your code becomes something like this:

#include "timer"

// ...

auto path = timer(find_path, "Find path", start, end, cellFilter);
std::cout << "best path is " << path.size() << " cells long\n";

Using endl

I'd recommend against (ever) using std::endl. Along with inserting a new-line character, it flushes the stream. This is rarely desired. In the rare circumstance that it really is desired, I think it's better to make that explicit, with code like:

std::cout << '\n' << std::flush;

In this particular case, it won't make a significant difference, but it's still a bad habit that can slow code by a factor of 10 or so for little real gain.

Final code

(For simplicity, I've included the timing code inline instead of using a separate header as I normally would.)

#include <iostream>
#include <fstream>
#include <algorithm>
#include <vector>
#include <functional>
#include <stdexcept>
#include <queue>
#include <unordered_map>
#include <chrono>
#include <string>

struct vec3 {
    int x, y, z;

    bool operator==(const vec3& other) const {
        return x == other.x && y == other.y && z == other.z;
    }

    vec3 operator+(const vec3& other) const {
        return{x + other.x, y + other.y, z + other.z};
    }

    static vec3 min(const vec3& a, const vec3& b) {
        return{std::min(a.x, b.x), std::min(a.y, b.y), std::min(a.z, b.z)};
    }

    static vec3 max(const vec3& a, const vec3& b) {
        return{std::max(a.x, b.x), std::max(a.y, b.y), std::max(a.z, b.z)};
    }

    static float dist(const vec3& a, const vec3& b) {
        auto dx = static_cast<float>(a.x - b.x);
        auto dy = static_cast<float>(a.y - b.y);
        auto dz = static_cast<float>(a.z - b.z);

        return sqrtf(dx*dx + dy*dy + dz*dz);
    }
};

namespace std {
    template<>
    struct hash<vec3> {
        size_t operator()(const vec3& k) const {
            return ((hash<int>()(k.x)
                ^ (hash<int>()(k.y) << 1)) >> 1)
                ^ (hash<int>()(k.z) << 1);
        }
    };
}

struct cell {
    bool occupied;
    bool walkableSurface;

    friend std::istream &operator>>(std::istream &is, cell &c) {
        return is >> c.occupied >> c.walkableSurface;
    }
};

class Grid {
    std::vector<cell> data;
public:
    int sx, sy, sz;

    cell &operator[](vec3 const &index) {
        return data[index.x * sy * sz + index.y * sz + index.z];
    }

    friend std::istream &operator>>(std::istream &is, Grid &g) {
        is >> g.sx >> g.sy >> g.sz;

        int i = 0;
        g.data.resize(g.sx * g.sy * g.sz);

        is >> std::boolalpha;

        for (int x = 0; x < g.sx; x++) {
            for (int y = 0; y < g.sy; y++) {
                for (int z = 0; z < g.sz; z++) {
                    is >> g.data[i++];
                }
            }
        }
        return is;
    }

    bool contains(vec3 const &coord) {
        return coord.x >= 0 && coord.x < sx && coord.y >= 0 && coord.y < sy && coord.z >= 0 && coord.z < sz;
    }
} grid;

std::vector<vec3> get_neighbours(const vec3& cell) {
    std::vector<vec3> neighbours;

    for (int dx = -1; dx <= 1; dx++) {
        for (int dy = -1; dy <= 1; dy++) {
            for (int dz = -1; dz <= 1; dz++) {
                auto coord = cell + vec3{dx, dy, dz};

                bool notSelf = !(dx == 0 && dy == 0 && dz == 0);
                bool connectivity = abs(dx) + abs(dy) + abs(dz) <= 2;
                bool withinGrid = grid.contains(coord); 

                if (notSelf && connectivity && withinGrid) {
                    neighbours.push_back(coord);
                }
            }
        }
    }

    return neighbours;
}

std::vector<vec3> find_path(const vec3& start, const vec3& end, bool(*cellFilter)(const vec3&, const vec3&)) {
    if (!cellFilter(start, start) || !cellFilter(end, end)) {
        throw std::invalid_argument("start and/or end fail cell filter!");
    }

    // Initialize data structures
    std::unordered_map<vec3, float> dist;
    std::unordered_map<vec3, vec3> prev;

    struct queue_node {
        vec3 value;
        float dist;

        bool operator<(queue_node const &other) const {
            return other.dist < dist;
        }
    };

    std::priority_queue<queue_node> Q;

    for (int x = 0; x < grid.sx; x++) {
        for (int y = 0; y < grid.sy; y++) {
            for (int z = 0; z < grid.sz; z++) {
                vec3 coord = {x, y, z};

                if (cellFilter(coord, coord)) {
                    dist[coord] = std::numeric_limits<float>::max();
                    Q.push({coord, std::numeric_limits<float>::max()});

                    prev[coord] = vec3{-1, -1, -1};
                }
            }
        }
    }

    dist[start] = 0;
    Q.push({start, 0});

    // Search loop
    while (!Q.empty()) {
        auto u = Q.top();
        Q.pop();

        // Old priority queue value
        if (u.dist != dist[u.value]) {
            continue;
        }

        if (u.value == end) {
            break;
        }

        for (const vec3& v : get_neighbours(u.value)) {
            if (cellFilter(u.value, v)) {
                float alt = dist[u.value] + vec3::dist(u.value, v);
                if (alt < dist[v]) {
                    dist[v] = alt;
                    Q.push({v, alt});

                    prev[v] = u.value;
                }
            }
        }
    }

    // Trace path - if there is one
    std::vector<vec3> path;

    if (prev[end].x != -1) {
        vec3 current = end;

        while (current.x != -1) {
            path.push_back(current);
            current = prev[current];
        }
        std::reverse(path.begin(), path.end());
    }
    return path;
}

bool isFloor(const vec3& pos) {
    return pos.y > 0 && !grid[pos].occupied && grid[(pos + vec3{ 0, -1, 0 })].walkableSurface;
}

bool cellFilter(const vec3& from, const vec3& to) {
    if (from.y == to.y) {
        // Check if all cells we're moving through are floors (important when moving diagonally)
        auto min = vec3::min(from, to);
        auto max = vec3::max(from, to);

        for (int x = min.x; x <= max.x; x++) {
            for (int z = min.z; z <= max.z; z++) {
                if (!isFloor({x, min.y, z})) {
                    return false;
                }
            }
        }

        return true;
    } else {
        // If the movement is vertical, then perform no diagonal check
        return isFloor(to);
    }
}

template <typename F, typename ...Args>
auto timer(F f, std::string const &label, Args && ...args) {
    using namespace std::chrono;

    auto start = high_resolution_clock::now();
    auto holder = f(std::forward<Args>(args)...);
    auto stop = high_resolution_clock::now();
    std::cout << label << " time: " << duration_cast<microseconds>(stop - start).count() << "\n";

    return holder;
}

int main() {
    // Read grid
    std::ifstream gridFile("grid.txt");

    gridFile >> grid;

    // Do pathfinding
    vec3 start = {9, 2, 6};
    vec3 end = {45, 2, 0};

    try {
        auto path = timer(find_path, "Find Path", start, end, cellFilter);
        std::cout << "best path is " << path.size() << " cells long\n";
    } catch (std::exception& e) {
        std::cout << "exception: " << e.what() << '\n';
    }

    return 0;
}

Answer 3

The multidimensional array might be the weakness of the C# implementation. Try using jagged arrays that are faster though not so easy to use.

You can read more about it in What are the differences between a multidimensional array and an array of arrays in C#? on SO. This answer compares both array systems.

EDIT: I've tested it myself and the difference is barely measureable. It looks like they have fixed it already.

---

var t1 = DateTime.Now;
var path = FindPath(start, end, CellFilter);
var t2 = DateTime.Now;

You shouldn't measure the time with DateTime. Use the Stopwatch

var sw = Stopwatch.StartNew();
var path = FindPath(start, end, CellFilter);
sw.Stop();

Console.WriteLine($"path finding took {sw.Elapsed}");

Also make sure you run the test in realease mode and outside of Visual Studio if you want to achieve maximum performance.

---

To find less obvious bottlenecks in the C# version you should use the profiler.

Answer 4

More speed for both C++ and C#

Revisiting an older question. While reading this question and its already very good answers the following occurred to me:

Hash tables are slow

Both the hashing and the subsequent hopping around RAM are not that fast. C++ unordered_map is additionally hampered because it is trying to maintain backward ABI compatibility. The inner loop of the above algorithm is all about hashtable lookups. The keys are an xor-mash of the 3 ints in vec3.

First, we can try to make the hashtable faster. One way to do this, is often a better hash function. The one used above is a bit "generic". We can use our domain knowledge to improve it. We pretty much know that x,y,z in vec3 will not exceed 1 million. So if we just mask out all but the lowest 20bits of each integer and then shift and | combine them into the 64bit output of the hash function, we would get "the perfect hash function": Fast to compute and a unique hash value for every input with zero collisions:

namespace std {
template <>
struct hash<vec3> {
  std::size_t operator()(const vec3& v) const {
    return ((static_cast<std::size_t>(v.x) & 0xfffffUL) << 40U) |
           ((static_cast<std::size_t>(v.y) & 0xfffffUL) << 20U) |
           ((static_cast<std::size_t>(v.z) & 0xfffffUL));
  }
};
} // namespace std

This is actually a huge gain already. Something like 2x faster. If we use a better hashmap implementation we can even make that 2.5x faster.

But we can do even better if we understand that the fundamental insight is:

There is usually a time / space tradeoff between a hashtable and a sparse array.

Those ints have limited range (size of grid), we could just use them as indices into an array instead of the hashing.

In C++ I could easily do this for dist and prev with the following template:

  template <typename T>
  struct grid_vector {
    explicit grid_vector(vec3 dims, const T& defv = T())
        : sx_(dims.x), sy_(dims.y), sz_(dims.z), data_(sx_ * sy_ * sz_, defv) {}

    T& operator[](const vec3& index) {
      return data_[index.x * sy_ * sz_ + index.y * sz_ + index.z];
    }

  private:
    int            sx_, sy_, sz_;
    std::vector<T> data_;
  };

Is it worth while?

Well firstly there is a space cost. There are 6000 "cells" and only ~800 of them pass the cellFilter. So by using x,y,z as the index for dist / prev we are wasting 5200 entries in each vector. It's "not a lot of memory" but your mileage may vary.

If you are worried about the memory usage, or the grid dimensions are much bigger with few valid cells (more wasted space), then use the better hash function above with the better hashmap implementation. The gains below still apply.

If you're happy to burn a bit of memory, the gains are significant. It turns out that removing the hashtable addresses the main bottleneck and the find_path section of the algorithm drops from 2ms to 0.4ms, ie a 5x gain. That's not bad. It depends on the application whether this is worthwhile, but the gains are there if they are wanted. I don't have figures for C#, but I suspect that significant gains (at the cost of space) are available there too.

There is a lot of noise in the performance numbers now as we are in sub ms territory and things like malloc become significant.

What else can we speed up?

Need to be careful not to fall into premature optimisation here. Given the OP was keen for find_path() to run as quickly as possible, there are a couple more easy gains we can make:

• get_neighbours() is called in the inner loop and materializes a std::vector only to then loop over that vector and discard it. Basic lesson: Don't materialize heap based data structures in the inner loops of the hot path, if you don't need to. So we can change that method to foreach_neighbours() which takes a Callback. This change gains us ~25% from 0.4ms to 0.3ms. This is not less readable, and a good pattern to try.
• We changed the vec3::dist calculation to usestd::hypot which is "more robust" against under/overflow, but not faster. In fact this function is only ever called for the 26 immediate 3D neighbours such that dx,dy,dz are all in range [-1, 1]. So we can statically cache these calculations in a 3x3x3 array and just lookup in that from the inner loop: A further 16% gain to ~0.24ms.

So overall that gives us another 1.7x gain, bringing our overall gain to > 8x.

But, with the "hypot cache" we are getting into the territory of micro-optimisations: Time to stop? Almost...

Cascade improvements

Often when working on performance, we find that optimisations we rejected earlier, because they were not significant, become significant as we remove other bottlenecks. Also, some changes we make, might seem fine at the time, but, as we get faster, they end up being a bottleneck.

This process was no exception. Some examples:

• The original code has dist and prev as unordered_maps. We changed that to grid_vector<vec3> and grid_vector<float> (see above). It turns out that, partially because they are indexed and accessed the same, we can combine them into a grid_vector<vertex> which holds vec3 and float. It also turns out that this is the same structure as the old grid_node (although the semantics are different), so we can eliminate a struct. Small amount of space gained, due to in struct packing, but no speed gain ...yet.
• We changed the filter passing from a function pointer to std::invoke as we made the filter a member function. std::invoke is very convenient, but it does have overhead. A Lambda is almost as convenient and has no overhead (the compiler can se straight through it and inline if appropriate). Speedup 0.24ms down to 0.20ms => 16%.
• Profiling the app with perf we find that most of the effort is now in the pop push of the priority queue. This is not a surprise as Dijkstra's tends to be very much bound by data structure efficiency. The priority queue with the "keep pushing and lazy remove" approach is a very good way, but: We don't need to fill the queue with all vertices to begin with. This means ~800 infinite distance nodes are sitting in the priority queue and slowing down the pop push which are O(log n). If we just push the neighbours as we need them, we get the same result (tested for all start/end combinations) and we have a tiny queue. Speedup: 0.20ms => 0.17ms , or 16%
• Now that we are not filling the queue during init we only have to fill the grid_vector<vertex> and that can be done with a bulk constructor and not 3 nested loops. All of init effectively disappears. Speedup: 0.17ms 0.16ms or 6%

So that makes our overall speedup from the OPs code 2ms => 0.16ms or 12.5x. Most of these changes will apply to the C# code as well.

Optimisations tried and rejected

• Pre-allocating the std::vector underpinning std::priority_queue and std::moveing it in (using the version (4) of the queue constructor ) after emplace_backing the elements during initialisation. It made no measurable difference so I took it out again. This later became irrelevant as we decided not to bulk fill the queue.
• Precomputing an adjacency matrix with all neighbours and their distances. This takes ~1ms so it's slower than running Dijkstra's. Could be worthwhile if we call Dykstra many many times and the grid (ie occupied and walkableSurface) doesn't change often. But not worth it for the example given. Another example of: Don't materialize data structures if they are fast to compute.

Other refactors and final code

While refactoring, I ended up making a bunch of other "stylistic" changes, building on the very good answer by Jerry Coffin. Including:

• Grid is now a bigger class which encapsulates its internals
• Don't use globals, particularly grid
• Changed the way cellFilter is being passed it, making use of std::invoke -- later changed to a lambda for performance.
• use "early return" to simply a bunch of the conditionals
• split the rather long main function. All functions < 15 lines now.
• const correctness: Apply const to everything that can be. This triggers a bunch of clang-tidy suggested additions of [[nodiscard]], so do those as well.
• I am using my own little Timer class, link in code, although I used Google Benchmark when I needed more robust, reliable numbers.

Final code:

// https://raw.githubusercontent.com/oschonrock/toolbelt/master/os/bch.hpp
#include "os/bch.hpp"
#include <benchmark/benchmark.h>
#include <chrono>
#include <cmath>
#include <fstream>
#include <functional>
#include <iomanip>
#include <iostream>
#include <queue>
#include <stdexcept>
#include <string>
#include <vector>

struct vec3 {
  int x, y, z;

  bool operator==(const vec3& o) const noexcept {
    return x == o.x && y == o.y && z == o.z;
  }
  vec3 operator+(const vec3& o) const noexcept {
    return {x + o.x, y + o.y, z + o.z};
  }

  static vec3 min(const vec3& a, const vec3& b) noexcept {
    return {std::min(a.x, b.x), std::min(a.y, b.y), std::min(a.z, b.z)};
  }

  static vec3 max(const vec3& a, const vec3& b) noexcept {
    return {std::max(a.x, b.x), std::max(a.y, b.y), std::max(a.z, b.z)};
  }

  static float dist(const vec3& a, const vec3& b) noexcept {
    return std::hypot(float(a.x - b.x), float(a.y - b.y), float(a.z - b.z));
  }

  // limited to immediate neighbours: (int)dx,dy,dz: -1 => 1. uses lookup table
  static float fast_dist(const vec3& a, const vec3& b) noexcept {
    return hypot[a.x - b.x + 1][a.y - b.y + 1][a.z - b.z + 1];
  }

  inline static float hypot[3][3][3]{}; // NOLINT

  static bool hypot_init() noexcept {
    for (int dx = -1; dx <= 1; dx++)
      for (int dy = -1; dy <= 1; dy++)
        for (int dz = -1; dz <= 1; dz++) {
          hypot[dx + 1][dy + 1][dz + 1] =
              std::hypot(float(dx), float(dy), float(dz));
        }
    return true;
  }

  inline static bool hypot_initialized = hypot_init();

  friend std::ostream& operator<<(std::ostream& os, const vec3& v3) {
    return os << "[" << v3.x << ", " << v3.y << ", " << v3.z << "]";
  }
};

class grid {
  struct cell; // fwd declaration

public:
  cell& operator[](vec3 const& index) noexcept {
    return cells[index.x * sy_ * sz_ + index.y * sz_ + index.z];
  }

  const cell& operator[](vec3 const& index) const noexcept {
    return cells[index.x * sy_ * sz_ + index.y * sz_ + index.z];
  }

  friend std::istream& operator>>(std::istream& istream, grid& g) {
    // os::bch::Timer t{"load"};
    istream >> g.sx_ >> g.sy_ >> g.sz_;
    g.cells.resize(g.sx_ * g.sy_ * g.sz_);
    istream >> std::boolalpha;
    int i = 0;
    for (int x = 0; x < g.sx_; ++x)
      for (int y = 0; y < g.sy_; ++y)
        for (int z = 0; z < g.sz_; ++z) istream >> g.cells[i++];
    return istream;
  }

  [[nodiscard]] vec3 dims() const { return {sx_, sy_, sz_}; }

  template <typename Filter>
  [[nodiscard]] std::vector<vec3> find_path(const vec3& start, const vec3& end,
                                            const Filter& filter) const {

    if (!filter(start, start) || !filter(end, end))
      throw std::invalid_argument("start and/or end fail cell filter!");

    // previuous coord / finalised dist to start
    // could be added to grid.cells but that would make multi-threaded access
    // very hard
    grid_vector<vertex> vertices(
        dims(), {{-1, -1, -1}, std::numeric_limits<float>::max()});

    find_path_search(start, end, vertices, filter);
    return find_path_extract(end, vertices);
  }

  [[nodiscard]] bool cellFilter(const vec3& from, const vec3& to) const {
    if (from.y != to.y)
      // If the movement is vertical, then perform no diagonal check
      return isFreeFloor(to);

    // Check if all cells we're moving through are floors
    // important when moving diagonally
    auto min = vec3::min(from, to);
    auto max = vec3::max(from, to);

    for (int x = min.x; x <= max.x; ++x)
      for (int z = min.z; z <= max.z; ++z)
        if (!isFreeFloor({x, min.y, z})) return false;
    return true;
  }

private:
  int sx_{}, sy_{}, sz_{};

  struct cell {
    bool occupied;
    bool walkableSurface;

    friend std::istream& operator>>(std::istream& is, cell& c) {
      return is >> c.occupied >> c.walkableSurface;
    }
  };

  std::vector<cell> cells;

  struct vertex {
    vec3  coord;
    float dist;

    bool operator<(vertex const& o) const { return dist > o.dist; } // min-heap!

    friend std::ostream& operator<<(std::ostream& os, const vertex& v) {
      return os << v.coord << ": " << v.dist;
    }
  };

  template <typename T>
  struct grid_vector {
    explicit grid_vector(vec3 dims, const T& defv = T())
        : sx_(dims.x), sy_(dims.y), sz_(dims.z), data_(sx_ * sy_ * sz_, defv) {}

    T& operator[](const vec3& index) {
      return data_[index.x * sy_ * sz_ + index.y * sz_ + index.z];
    }

  private:
    int            sx_, sy_, sz_;
    std::vector<T> data_;
  };

  template <typename Filter>
  void find_path_search(const vec3& start, const vec3& end,
                        grid_vector<vertex>& vertices,
                        const Filter&        filter) const {
    // os::bch::Timer t{"find_path_search"};

    // search queue: not prefilled, because not required and that slows it down
    // current coord / estimated dist to start
    std::priority_queue<vertex> queue;

    vertices[start].dist = 0;
    queue.push({start, 0});

    while (!queue.empty()) {
      auto u = queue.top();
      queue.pop();
      if (u.dist != vertices[u.coord].dist)
        continue;                // lazy remove/skip of old queue value
      if (u.coord == end) break; // we arrived. stop.

      foreach_neighbours(u.coord, [&](const vec3& v) {
        if (filter(u.coord, v)) {
          float new_dist = vertices[u.coord].dist + vec3::fast_dist(u.coord, v);
          if (new_dist < vertices[v].dist) {
            // update min distance to "start", record path "back"
            vertices[v] = {u.coord, new_dist};
            queue.push({v, new_dist}); // leave old one in, to be lazily removed
          }
        }
      });
    }
  }

  static std::vector<vec3> find_path_extract(const vec3&          end,
                                             grid_vector<vertex>& vertices) {
    // os::bch::Timer    t{"find_path_extract"};
    std::vector<vec3> path;
    if (vertices[end].coord.x != -1) {
      vec3 current = end;
      while (current.x != -1) {
        path.push_back(current);
        current = vertices[current].coord;
      }
      std::reverse(path.begin(), path.end());
    }
    return path;
  }

  [[nodiscard]] bool isFreeFloor(const vec3& pos) const {
    return pos.y > 0 && !(*this)[pos].occupied &&
           (*this)[pos + vec3{0, -1, 0}].walkableSurface;
  }

  [[nodiscard]] bool contains(vec3 const& coord) const {
    // clang-format off
    return coord.x >= 0 && coord.x < sx_ &&
           coord.y >= 0 && coord.y < sy_ &&
           coord.z >= 0 && coord.z < sz_;
    // clang-format on
  }

  // faster to loop with callback than to materialise
  // a vector on heap and RVO return it
  template <typename Callback>
  void foreach_neighbours(const vec3& coord, const Callback& callback) const {
    for (int dx = -1; dx <= 1; dx++)
      for (int dy = -1; dy <= 1; dy++)
        for (int dz = -1; dz <= 1; dz++) {
          if (dx == 0 && dy == 0 && dz == 0) continue; // ignore self
          auto new_coord  = coord + vec3{dx, dy, dz};
          bool connected  = abs(dx) + abs(dy) + abs(dz) <= 2;
          bool withinGrid = contains(new_coord);
          if (connected && withinGrid) callback(new_coord);
        }
  }
}; // Grid

int main() {
  std::ifstream gridFile("grid.txt");
  grid          g;
  gridFile >> g;

  vec3 start = {9, 2, 6};
  vec3 end   = {45, 2, 0};

  auto filter = [&g](const vec3& from, const vec3& to) {
    return g.cellFilter(from, to);
  };
  try {
    auto path = g.find_path(start, end, filter);
    std::cout << "best path is " << path.size() << " cells long\n";
    for (auto& e: path) std::cout << e << "\n";
  } catch (std::exception& e) {
    std::cout << "exception: " << e.what() << '\n';
  }
}

Answer 5

The worst problem is probably the use of the SortedSet collection. In my experience, the Sorted* collections are rarely what you really want and the performance is questionable.

The performance can be greatly improved if all the cell's neighbors have a unit distance to cell (or the a small integer distance). Then you have a queue of frontiers that can be processed/updated fast and straightforward. There is a sample implementation called DelayQueue here: Ark.Collections.DelayQueue.