183
\$\begingroup\$

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.

\$\endgroup\$
  • 4
    \$\begingroup\$ C++'s priority_queue is backed by a contiguous array, while SortedSet is backed by a balanced binary search tree. The latter requires much more dynamic allocation and node adjustments than the former. Try using std::set in C++ and compare the performance again. \$\endgroup\$ – Siyuan Ren Jan 16 '17 at 11:19
  • 38
    \$\begingroup\$ Answer performance questions with science; use a profiler to discover what is actually slow. A significant fraction of educated guesses about what is actually slow in a program are wrong. \$\endgroup\$ – Eric Lippert Jan 16 '17 at 15:10
  • 12
    \$\begingroup\$ The only reason for public static float Dist(Vec3 a, Vec3 b) is to compare distances. So you don't actually need the distance just there relative values. You should implement public static float DistSquared(Vec3 a, Vec3 b) function that does not perform the expensive square root operation and use that to compare relative distances. \$\endgroup\$ – Martin York Jan 16 '17 at 16:38
  • 6
    \$\begingroup\$ @SiyuanRen The idea is to make the C# faster not the C++ slower. Sure any code can be made to look fast by making the translations badly written. Would be better to suggest what backing store the C# code could use to make it better for this situation. \$\endgroup\$ – Martin York Jan 16 '17 at 16:51
  • 9
    \$\begingroup\$ @LokiAstari: My idea is to first compare the speed with std::set. If it makes a big difference, then you know that the bottleneck lies with the difference with binary search tree and heap, and then you can implement a heap in C# to speed things up. You can directly write a heap in C# and compare the speed instead, but that will be a waste of time if it turns out that the difference is negligible. \$\endgroup\$ – Siyuan Ren Jan 17 '17 at 6:59
285
\$\begingroup\$

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

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");
        }
    }
}
\$\endgroup\$
  • 47
    \$\begingroup\$ I've always been wondering whether I should listen to those warnings. \$\endgroup\$ – t3chb0t Jan 16 '17 at 12:02
  • 1
    \$\begingroup\$ Why do you need the IntegerHash? Can't you simply use the int.GetHashCode method? You write that the GetHashCode should be eliminated but why? \$\endgroup\$ – t3chb0t Jan 16 '17 at 12:08
  • 5
    \$\begingroup\$ @t3chb0t Performance mostly, it saves about 1m from 4ms. So about a 25% increase in performance. I don't know what the implementation for int.GetHashCode is, but GetHashCode is definetely a virtual call and the JIT might not inline that. The greatest advantage that the C++ compiler has, is that it can inline almost everything in this example relatively easily. \$\endgroup\$ – mordecai154 Jan 16 '17 at 12:38
  • 2
    \$\begingroup\$ @mordecai154 if I run the example in BenchmarkingDotNET I get 2.5131ms±0.0081 with your hash, 2.3125ms±0.0064 with a factored sum and int.GetHashCode, and 2.3052ms±0.0040 with factored sum and just the value of the integer. \$\endgroup\$ – Johnbot Jan 16 '17 at 15:29
  • 2
    \$\begingroup\$ @t3chb0t Never ignore warnings. And certainly don't commit code that produces them. \$\endgroup\$ – jpmc26 Jan 18 '17 at 12:41
46
\$\begingroup\$

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.x < 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;
}
\$\endgroup\$
  • 45
    \$\begingroup\$ @Christoph all code should be readable, no matter whether it's a throw-away or production code, it's a habit and you should always be able to write readable code especially if you're going to show it to others like posting it on CR. This answer doesn't contain any invalid information so downvoting it just for commenting on readability is unfair. A review can include all topics and not necessarily those explicitly requested by OP. \$\endgroup\$ – t3chb0t Jan 16 '17 at 11:12
  • 14
    \$\begingroup\$ @Christoph: Partly out of habit. mordecai154's code improved The C# code's speed a lot--but at least for me, the C++ code is still close to twice as fast. With similar effort in optimizing the C++, its speed can undoubtedly be improved as well, so remaining 5-8x faster than C# looks entirely feasible. In short, he might want to consider throwing away the C# and maintaining the C++. \$\endgroup\$ – Jerry Coffin Jan 16 '17 at 15:00
  • 11
    \$\begingroup\$ @Christoph: That doesn't seem to me to fall within the realm of reviewing code. I reviewed code. It's up to him to draw conclusions. \$\endgroup\$ – Jerry Coffin Jan 16 '17 at 15:56
  • \$\begingroup\$ Thanks for your code review, I appreciate your suggestions because some of them also apply to the structure of the C# code. I don't think I can fully switch to C++, because Unity requires me to write at least some C# code to interface with it. Do you have a naming suggestion for cellFilter? The idea is that different objects have different requirements for path finding (e.g. available space, can they fly or walk?). \$\endgroup\$ – Overv Jan 16 '17 at 17:15
  • \$\begingroup\$ @Overv: The obvious possibility would be that a cellFilter should do more to reflect the filtering it does, not just the fact that it's doing filtering. Using your example, a SpaceAvailableFilter (or maybe just "EmptyCell", if that's at least reasonably accurate). \$\endgroup\$ – Jerry Coffin Jan 16 '17 at 22:46
11
\$\begingroup\$

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.

\$\endgroup\$
  • \$\begingroup\$ I would assume on the contrary that the contiguous memory provided by a true array increases cache hits and thus leads to better performance than a jagged array which is scattered all over the memory; provided that the elements are structs. \$\endgroup\$ – Peter A. Schneider Jan 16 '17 at 10:59
  • 1
    \$\begingroup\$ After googling a bit, I saw in a stackoverflow discussion that the C' compiler/CLR used to create faster code for jagged arrays (Mono behaved better). It sounds as if that issue has been resolved on the Microsoft side by now. \$\endgroup\$ – Peter A. Schneider Jan 16 '17 at 11:19
  • \$\begingroup\$ @PeterA.Schneider the link I've posted is one of the answers from the question you've posted a link to. I tried to google for something newer about this issue but didn't find anything. Jagged arrays still seem to be faster on windows. \$\endgroup\$ – t3chb0t Jan 16 '17 at 11:23
  • \$\begingroup\$ Same post: Oh :-). Amro's comment on that answer (third from below) as well as Eglin's answer, both from 2013, seem to indicate that the issue was resolved or at least improved by then. \$\endgroup\$ – Peter A. Schneider Jan 16 '17 at 11:31
  • 1
    \$\begingroup\$ @mathreadler The only way to avoid all bounds checking is to use unsafe code. C# can do that, but it basically means you're coding C at that point. Not too bad if you have very thorough assertions and testing, but rarely worth the danger. For 3000 cells, it's obvious that the issue is algorithmic, and not "bounds-checking-bound" - the first thing that jumped out on me was "you're using hashsets without providing good hashes!" And note that it's getting cheaper with each new Intel CPU generation; and there's no branch misprediction, since correct code always accesses inside the bounds anyway. \$\endgroup\$ – Luaan Jan 19 '17 at 14:56
6
\$\begingroup\$

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.

\$\endgroup\$

protected by Jamal Jan 20 '17 at 23:07

Thank you for your interest in this question. Because it has attracted low-quality or spam answers that had to be removed, posting an answer now requires 10 reputation on this site (the association bonus does not count).

Would you like to answer one of these unanswered questions instead?

Not the answer you're looking for? Browse other questions tagged or ask your own question.