MiniMicroMouse/floodfill_sketch.cpp at master · nirbheeksetia/MiniMicroMouse · 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
#include <iostream>
#include <vector>
#include <queue>
#include <stack>
#include <tuple>
#include <unordered_map>
#include <algorithm>

// this file is irrelevant for the mms but the logic might be correct

const int ROWS = 16;
const int COLS = 16;

int maze[ROWS][COLS] = {
    {1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
    {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1},
    {1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1},
    {1, 1, 0, 0, 0, 0, 0, 1, 1, 0, 1, 1, 1, 1, 1, 1},
    {1, 1, 0, 1, 1, 1, 0, 1, 1, 0, 1, 1, 1, 1, 1, 1},
    {1, 1, 0, 1, 0, 0, 0, 1, 1, 0, 1, 1, 1, 1, 1, 1},
    {1, 1, 0, 1, 0, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1},
    {1, 1, 0, 1, 0, 0, 0, 0, 1, 0, 1, 1, 1, 1, 1, 1},
    {1, 1, 0, 1, 1, 1, 1, 0, 1, 0, 1, 1, 1, 1, 1, 1},
    {1, 1, 0, 0, 0, 0, 1, 0, 1, 0, 1, 1, 1, 1, 1, 1},
    {1, 1, 1, 1, 1, 0, 1, 0, 1, 0, 1, 1, 1, 1, 1, 1},
    {1, 1, 1, 1, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1},
    {1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 1},
    {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 1},
    {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1}
};

// down, up, right, left
const std::vector<std::pair<int, int>> moves = {{1, 0}, {-1, 0}, {0, 1}, {0, -1}};

namespace std {
    template <>
    struct hash<std::pair<int, int>> {
        size_t operator()(const std::pair<int, int>& p) const {
            return std::hash<int>{}(p.first) ^ std::hash<int>{}(p.second);
        }
    };
}

// backtrack (returns the path)
std::vector<std::pair<int, int>> backtrack(std::unordered_map<std::pair<int, int>, std::pair<int, int>>& cameFrom, std::pair<int, int> start, std::pair<int, int> end) {
    std::vector<std::pair<int, int>> path;
    for (std::pair<int, int> at = end; at != start; at = cameFrom[at]) {
        path.push_back(at);
    }
    path.push_back(start);
    std::reverse(path.begin(), path.end());
    return path;
}

// flood fill usinf BFS
std::vector<std::pair<int, int>> floodFill(int maze[ROWS][COLS], std::pair<int, int> start, std::pair<int, int> end) {
    std::queue<std::pair<int, int>> q;
    std::unordered_map<std::pair<int, int>, std::pair<int, int>> cameFrom;
    q.push(start);
    cameFrom[start] = start;

    while (!q.empty()) {

        std::pair<int, int> cell = q.front();
        int row = cell.first;
        int col = cell.second;

        q.pop();

        // found
        if (std::make_pair(row, col) == end) {
            return backtrack(cameFrom, start, end);
        }

        // visited
        maze[row][col] = 2;

        // check all possible directions
        for (const auto& move : moves) {
            int newRow = row + move.first;
            int newCol = col + move.second;

            // check: not out of bounds, no wall, not visited
            if (newRow >= 0 && newRow < ROWS && newCol >= 0 && newCol < COLS && maze[newRow][newCol] == 0) {
                std::pair<int, int> neighbor = {newRow, newCol};
                if (cameFrom.find(neighbor) == cameFrom.end()) {
                    q.push(neighbor);
                    cameFrom[neighbor] = {row, col};
                }
            }
        }
    }

    return {}; // not found
}

int main() {
    std::pair<int, int> start = {0, 1};
    std::pair<int, int> end = {15, 14};

    std::vector<std::pair<int, int>> path = floodFill(maze, start, end);

    if (!path.empty()) {
        std::cout << "Path found:\n";
        for (const auto& [row, col] : path) {
            std::cout << "(" << row << ", " << col << ")\n";
        }
    } else {
        std::cout << "No path found\n";
    }

    for (int i = 0; i < ROWS; ++i) {
        for (int j = 0; j < COLS; ++j) {
            std::cout << maze[i][j] << " ";
        }
        std::cout << std::endl;
    }

    return 0;
}