ダイクストラアルゴリズムで二次元経路探索.

DijkstraAlgorithm.cpp

#include <algorithm>
#include <iostream>
#include <limits>
#include <map>
#include <vector>

class Vector2D {
  public:
    Vector2D() : x_(0), y_(0) {}
    Vector2D(int x, int y) : x_(x), y_(y) {}
    Vector2D(const Vector2D& other) : x_(other.x_), y_(other.y_) {}
    int x() const { return x_; }
    int y() const { return y_; }
    void setX(int x) { x_ = x; }
    void setY(int y) { y_ = y; }
    Vector2D& operator = (const Vector2D& other) {
        x_ = other.x_;
        y_ = other.y_;
        return *this;
    }
    Vector2D& operator += (const Vector2D& other) {
        x_ += other.x_;
        y_ += other.y_;
        return *this;
    }
    Vector2D& operator -= (const Vector2D& other) {
        x_ -= other.x_;
        y_ -= other.y_;
        return *this;
    }
    friend Vector2D operator + (Vector2D lhs, const Vector2D& rhs) {
        return lhs += rhs;
    }
    friend Vector2D operator - (Vector2D lhs, const Vector2D& rhs) {
        return lhs -= rhs;
    }
    friend std::ostream& operator << (std::ostream& os, const Vector2D& obj) {
        os << "{" << obj.x_ << ", " << obj.y_ << "}";
        return os;
    }
    friend bool operator == (const Vector2D& lhs, const Vector2D& rhs) {
        return (lhs.x_ == rhs.x_ && lhs.y_ == rhs.y_);
    }
    friend bool operator != (const Vector2D& lhs, const Vector2D& rhs) {
        return !(lhs == rhs);
    }
    // for std::map
    friend bool operator < (const Vector2D& lhs, const Vector2D& rhs) {
        if (lhs.x_ < rhs.x_) return true;
        if (lhs.x_ > rhs.x_) return false;
        if (lhs.y_ < rhs.y_) return true;
        if (lhs.y_ > rhs.y_) return false;
        return false;
    }
  private:
    int x_;
    int y_;
};

using Field = std::vector<std::vector<int> >;
using Route = std::vector<Vector2D>;

bool enableMove(const Field& field, const Vector2D& from, const Vector2D& dir) {
    Vector2D to = from + dir;
    if (std::abs(field[from.x()][from.y()] - field[to.x()][to.y()]) > 2)
        return false;
    return true;
}

const std::vector<Vector2D> directions(
    { Vector2D(1, 0), Vector2D(-1, 0), Vector2D(0, 1), Vector2D(0, -1) });

Route mainAlgorithm(const Field& field,
                    const Vector2D& start, const Vector2D& goal) {
    std::map<Vector2D, int> costs;
    std::map<Vector2D, Vector2D> prevs;
    std::map<Vector2D, bool> visited;
    costs[start] = 0;
    prevs[start] = start;
    Vector2D nextPoint = start;
    bool continueFlag = true;
    while (continueFlag) {
        visited[nextPoint] = true;
        continueFlag = false;
        Vector2D currentPoint = nextPoint;

        for (const Vector2D& dir : directions) {
            Vector2D point = currentPoint + dir;
            if (visited[point]
                || point.x() < 0 || point.y() < 0
                || point.x() >= field[0].size() || point.y() >= field.size()
                || !enableMove(field, nextPoint, dir)) continue;
            else {
                int newCost = costs[currentPoint] + 1;
                auto itr = costs.find(point);
                if (itr != costs.end() && itr->second > newCost) {
                    itr->second = newCost;
                    prevs[point] = currentPoint;
                } else {
                    costs[point] = newCost;
                    prevs[point] = currentPoint;
                }
            }
        }

        int min = std::numeric_limits<int>::max();
        for (const auto& kv : costs) {
            if (!visited[kv.first] && min > kv.second) {
                nextPoint = kv.first; min = kv.second; continueFlag = true;
            }
        }
    }
    Route result;
    nextPoint = goal;
    while (true) {
        result.push_back(nextPoint);
        if (nextPoint == start) break;
        nextPoint = prevs[nextPoint];
    }
    std::reverse(result.begin(), result.end());
    return result;
}

void printRoute(const Route& route) {
    for (int i = 0; i < route.size() - 1; i++) {
        std::cout << route[i] << "->";
    }
    std::cout << route[route.size() - 1] << std::endl;
}

int main(int argc, char **argv) {
    Vector2D start(1, 1);
    Vector2D goal(4, 4);
    Field field(
        {
            { 1, 2, 1, 1, 1 },
            { 1, 2, 2, 3, 4 },
            { 5, 5, 2, 3, 5 },
            { 1, 2, 3, 5, 5 },
            { 2, 3, 4, 5, 5 }
        });
    Route route = mainAlgorithm(field, start, goal);
    printRoute(route);
    return 0;
}