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.