Dijkstra/Graph.cpp at master · Hirdaya-Shrestha/Dijkstra · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
#include "Graph.hpp"

#include <algorithm>
#include <cmath>

namespace {
constexpr double kDiagonalCost = 1.41421356237;
}

Graph::Graph(const int rows, const int cols) : rows_(rows), cols_(cols) {
    nodes_.resize(rows_ * cols_);
    for (int r = 0; r < rows_; ++r)
        for (int c = 0; c < cols_; ++c) {
            const int idx = Index(r, c);
            nodes_[idx].row = r;
            nodes_[idx].col = c;
        }
}

int Graph::GetRows() const {
    return rows_;
}

int Graph::GetCols() const {
    return cols_;
}

int Graph::Index(const int row, const int col) const {
    return row * cols_ + col;
}

bool Graph::InBounds(const int row, const int col) const {
    return row >= 0 && row < rows_ && col >= 0 && col < cols_;
}

Node& Graph::GetNode(int index) {
    return nodes_[index];
}

const Node& Graph::GetNode(int index) const {
    return nodes_[index];
}

const std::vector<Node>& Graph::GetNodes() const {
    return nodes_;
}

void Graph::SetDiagonal(bool enabled) {
    diagonalEnabled_ = enabled;
}

bool Graph::IsDiagonalEnabled() const {
    return diagonalEnabled_;
}

std::vector<Neighbor> Graph::GetNeighbors(int index) const {
    const Node& node = nodes_[index];
    std::vector<Neighbor> neighbors;

    for (int i = 0; i < 4; ++i) {
        constexpr int dc[4] = {0, 0, -1, 1};
        constexpr int dr[4] = {-1, 1, 0, 0};
        const int nr = node.row + dr[i];
        const int nc = node.col + dc[i];
        if (!InBounds(nr, nc))
            continue;
        const int nidx = Index(nr, nc);
        neighbors.push_back({nidx, 1.0});
    }

    if (diagonalEnabled_)
        for (int i = 0; i < 4; ++i) {
            constexpr int ddc[4] = {-1, 1, -1, 1};
            constexpr int ddr[4] = {-1, -1, 1, 1};
            const int nr = node.row + ddr[i];
            const int nc = node.col + ddc[i];
            if (!InBounds(nr, nc))
                continue;
            const int nidx = Index(nr, nc);
            neighbors.push_back({nidx, kDiagonalCost});
        }

    return neighbors;
}

void Graph::ResetStatesKeepObstacles(int startIdx, int targetIdx) {
    for (auto& node : nodes_)
        if (node.obstacle)
            node.state = NodeState::Obstacle;
        else
            node.state = NodeState::Unvisited;
    ApplySpecialStates(startIdx, targetIdx);
}

void Graph::ResetAll(const int startIdx, const int targetIdx) {
    for (auto& node : nodes_) {
        node.obstacle = false;
        node.state = NodeState::Unvisited;
    }
    ApplySpecialStates(startIdx, targetIdx);
}

void Graph::ClearObstacles(const int startIdx, const int targetIdx) {
    for (auto& node : nodes_) {
        node.obstacle = false;
        node.state = NodeState::Unvisited;
    }
    ApplySpecialStates(startIdx, targetIdx);
}

void Graph::ToggleObstacle(const int index, const int startIdx, const int targetIdx) {
    if (index == startIdx || index == targetIdx)
        return;
    Node& node = nodes_[index];
    node.obstacle = !node.obstacle;
    node.state = node.obstacle ? NodeState::Obstacle : NodeState::Unvisited;
}

void Graph::SetStart(const int index, int& startIdx) {
    if (index == startIdx)
        return;

    nodes_[startIdx].state = nodes_[startIdx].obstacle ? NodeState::Obstacle : NodeState::Unvisited;
    nodes_[startIdx].obstacle = false;
    startIdx = index;
    nodes_[startIdx].obstacle = false;
    nodes_[startIdx].state = NodeState::Start;
}

void Graph::SetTarget(const int index, int& targetIdx) {
    if (index == targetIdx)
        return;

    nodes_[targetIdx].state = nodes_[targetIdx].obstacle ? NodeState::Obstacle : NodeState::Unvisited;
    nodes_[targetIdx].obstacle = false;
    targetIdx = index;
    nodes_[targetIdx].obstacle = false;
    nodes_[targetIdx].state = NodeState::Target;
}

void Graph::ApplySpecialStates(const int startIdx, const int targetIdx) {
    if (startIdx >= 0 && startIdx < static_cast<int>(nodes_.size())) {
        nodes_[startIdx].obstacle = false;
        nodes_[startIdx].state = NodeState::Start;
    }
    if (targetIdx >= 0 && targetIdx < static_cast<int>(nodes_.size())) {
        nodes_[targetIdx].obstacle = false;
        nodes_[targetIdx].state = NodeState::Target;
    }
}