Robert K / Lees_Algorithm

A library for solving mazes.

Lees_Algorithm.cpp@0:dddf6e50f1e7, 2011-06-24 (annotated)

Committer:
Pinski1
Date:
Fri Jun 24 22:37:49 2011 +0000
Revision:
0:dddf6e50f1e7
Child:
1:80b73beb7742
Initial one, dirty!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Pinski1 0:dddf6e50f1e7 1 #include "Lees_Algorithm.h"
Pinski1 0:dddf6e50f1e7 2 #include "mbed.h"
Pinski1 0:dddf6e50f1e7 3
Pinski1 0:dddf6e50f1e7 4 Lees_Algorithm::Lees_Algorithm(unsigned char size_): mazeSize(size_) {
Pinski1 0:dddf6e50f1e7 5
Pinski1 0:dddf6e50f1e7 6 map = new _cell*[mazeSize];
Pinski1 0:dddf6e50f1e7 7 for(int i = 0; i < mazeSize; i++)
Pinski1 0:dddf6e50f1e7 8 {
Pinski1 0:dddf6e50f1e7 9 map[i] = new _cell[mazeSize];
Pinski1 0:dddf6e50f1e7 10 }
Pinski1 0:dddf6e50f1e7 11
Pinski1 0:dddf6e50f1e7 12 map[0][0].weight = 0;
Pinski1 0:dddf6e50f1e7 13 map[0][0].weight -= 1;
Pinski1 0:dddf6e50f1e7 14 maxWeight = map[0][0].weight;
Pinski1 0:dddf6e50f1e7 15
Pinski1 0:dddf6e50f1e7 16 clearWeights();
Pinski1 0:dddf6e50f1e7 17
Pinski1 0:dddf6e50f1e7 18 for(int i = 0; i < mazeSize; i++)
Pinski1 0:dddf6e50f1e7 19 {
Pinski1 0:dddf6e50f1e7 20 for(int j = 0; j < mazeSize; j++)
Pinski1 0:dddf6e50f1e7 21 {
Pinski1 0:dddf6e50f1e7 22 if(i == 0) map[0][j].north = true;
Pinski1 0:dddf6e50f1e7 23 else map[i][j].north = false;
Pinski1 0:dddf6e50f1e7 24 if(j == 0) map[i][0].west = true;
Pinski1 0:dddf6e50f1e7 25 else map[i][j].west = false;
Pinski1 0:dddf6e50f1e7 26 }
Pinski1 0:dddf6e50f1e7 27 }
Pinski1 0:dddf6e50f1e7 28
Pinski1 0:dddf6e50f1e7 29 nextRead = 0;
Pinski1 0:dddf6e50f1e7 30 nextWrite = 0;
Pinski1 0:dddf6e50f1e7 31 stackSize = 0;
Pinski1 0:dddf6e50f1e7 32
Pinski1 0:dddf6e50f1e7 33 return;
Pinski1 0:dddf6e50f1e7 34 }
Pinski1 0:dddf6e50f1e7 35
Pinski1 0:dddf6e50f1e7 36 Lees_Algorithm::~Lees_Algorithm(void) {
Pinski1 0:dddf6e50f1e7 37 for(int i = 0; i < mazeSize; i++)
Pinski1 0:dddf6e50f1e7 38 {
Pinski1 0:dddf6e50f1e7 39 delete[] map[i];
Pinski1 0:dddf6e50f1e7 40 }
Pinski1 0:dddf6e50f1e7 41 delete[] map;
Pinski1 0:dddf6e50f1e7 42 }
Pinski1 0:dddf6e50f1e7 43
Pinski1 0:dddf6e50f1e7 44
Pinski1 0:dddf6e50f1e7 45 void Lees_Algorithm::updateMap(_location position, bool wNorth, bool wWest, bool wSouth, bool wEast) {
Pinski1 0:dddf6e50f1e7 46 _location buffer, newLoc;
Pinski1 0:dddf6e50f1e7 47 unsigned char minimumWeight;
Pinski1 0:dddf6e50f1e7 48
Pinski1 0:dddf6e50f1e7 49 // copy new walls to database
Pinski1 0:dddf6e50f1e7 50 if(position.x == 0) wWest = true; // force some walls to true
Pinski1 0:dddf6e50f1e7 51 if(position.y == 0) wNorth = true;
Pinski1 0:dddf6e50f1e7 52 if(position.x == (mazeSize - 1)) wEast = true;
Pinski1 0:dddf6e50f1e7 53 if(position.y == (mazeSize - 1)) wSouth = true;
Pinski1 0:dddf6e50f1e7 54
Pinski1 0:dddf6e50f1e7 55 map[position.y][position.x].north = wNorth;
Pinski1 0:dddf6e50f1e7 56 map[position.y][position.x].west = wWest;
Pinski1 0:dddf6e50f1e7 57 map[((position.y + 1) % mazeSize)][position.x].north = wSouth;
Pinski1 0:dddf6e50f1e7 58 map[position.y][((position.x + 1) % mazeSize)].west = wEast;
Pinski1 0:dddf6e50f1e7 59
Pinski1 0:dddf6e50f1e7 60 // run lees algorthim
Pinski1 0:dddf6e50f1e7 61 push(position);
Pinski1 0:dddf6e50f1e7 62
Pinski1 0:dddf6e50f1e7 63 while(stackSize > 0)
Pinski1 0:dddf6e50f1e7 64 {
Pinski1 0:dddf6e50f1e7 65 buffer = pull();
Pinski1 0:dddf6e50f1e7 66 minimumWeight = 255;
Pinski1 0:dddf6e50f1e7 67
Pinski1 0:dddf6e50f1e7 68 if(!map[buffer.y][buffer.x].north && map[(buffer.y - 1) % mazeSize][buffer.x].weight < minimumWeight) minimumWeight = map[(buffer.y - 1) % mazeSize][buffer.x].weight;
Pinski1 0:dddf6e50f1e7 69 if(!map[buffer.y][buffer.x].west && map[buffer.y][(buffer.x - 1) % mazeSize].weight < minimumWeight) minimumWeight = map[buffer.y][(buffer.x - 1) % mazeSize].weight;
Pinski1 0:dddf6e50f1e7 70 if(!map[(buffer.y + 1) % mazeSize][buffer.x].north && map[(buffer.y + 1) % mazeSize][buffer.x].weight < minimumWeight) minimumWeight = map[(buffer.y + 1) % mazeSize][buffer.x].weight;
Pinski1 0:dddf6e50f1e7 71 if(!map[buffer.y][(buffer.x + 1) % mazeSize].west && map[buffer.y][(buffer.x + 1) % mazeSize].weight < minimumWeight) minimumWeight = map[buffer.y][(buffer.x + 1) % mazeSize].weight;
Pinski1 0:dddf6e50f1e7 72
Pinski1 0:dddf6e50f1e7 73 if(map[buffer.y][buffer.x].weight != (minimumWeight + 1))
Pinski1 0:dddf6e50f1e7 74 {
Pinski1 0:dddf6e50f1e7 75 if(map[buffer.y][buffer.x].weight != 0) map[buffer.y][buffer.x].weight = minimumWeight + 1;
Pinski1 0:dddf6e50f1e7 76
Pinski1 0:dddf6e50f1e7 77 if(map[buffer.y][buffer.x].north == false)
Pinski1 0:dddf6e50f1e7 78 {
Pinski1 0:dddf6e50f1e7 79 newLoc.y = buffer.y - 1;
Pinski1 0:dddf6e50f1e7 80 newLoc.x = buffer.x;
Pinski1 0:dddf6e50f1e7 81 push(newLoc);
Pinski1 0:dddf6e50f1e7 82 }
Pinski1 0:dddf6e50f1e7 83 if(map[buffer.y][buffer.x].west == false)
Pinski1 0:dddf6e50f1e7 84 {
Pinski1 0:dddf6e50f1e7 85 newLoc.y = buffer.y;
Pinski1 0:dddf6e50f1e7 86 newLoc.x = buffer.x - 1;
Pinski1 0:dddf6e50f1e7 87 push(newLoc);
Pinski1 0:dddf6e50f1e7 88 }
Pinski1 0:dddf6e50f1e7 89 if(map[(buffer.y + 1) % mazeSize][buffer.x].north == false)
Pinski1 0:dddf6e50f1e7 90 {
Pinski1 0:dddf6e50f1e7 91 newLoc.y = (buffer.y + 1) % mazeSize;
Pinski1 0:dddf6e50f1e7 92 newLoc.x = buffer.x;
Pinski1 0:dddf6e50f1e7 93 push(newLoc);
Pinski1 0:dddf6e50f1e7 94 }
Pinski1 0:dddf6e50f1e7 95 if(map[buffer.y][(buffer.x + 1) % mazeSize].west == false)
Pinski1 0:dddf6e50f1e7 96 {
Pinski1 0:dddf6e50f1e7 97 newLoc.y = buffer.y;
Pinski1 0:dddf6e50f1e7 98 newLoc.x = (buffer.x + 1) % mazeSize;
Pinski1 0:dddf6e50f1e7 99 push(newLoc);
Pinski1 0:dddf6e50f1e7 100 }
Pinski1 0:dddf6e50f1e7 101 }
Pinski1 0:dddf6e50f1e7 102 }
Pinski1 0:dddf6e50f1e7 103
Pinski1 0:dddf6e50f1e7 104 return;
Pinski1 0:dddf6e50f1e7 105 }
Pinski1 0:dddf6e50f1e7 106
Pinski1 0:dddf6e50f1e7 107 char Lees_Algorithm::getDirection(_location position) {
Pinski1 0:dddf6e50f1e7 108 char buffer;
Pinski1 0:dddf6e50f1e7 109 return buffer;
Pinski1 0:dddf6e50f1e7 110 }
Pinski1 0:dddf6e50f1e7 111
Pinski1 0:dddf6e50f1e7 112 void Lees_Algorithm::initialFill(void) {
Pinski1 0:dddf6e50f1e7 113
Pinski1 0:dddf6e50f1e7 114 int i,j;
Pinski1 0:dddf6e50f1e7 115 bool finished = false;
Pinski1 0:dddf6e50f1e7 116
Pinski1 0:dddf6e50f1e7 117 while(finished == false)
Pinski1 0:dddf6e50f1e7 118 {
Pinski1 0:dddf6e50f1e7 119
Pinski1 0:dddf6e50f1e7 120 for(i = 0; i < mazeSize; i++)
Pinski1 0:dddf6e50f1e7 121 {
Pinski1 0:dddf6e50f1e7 122 for(j = 0; j < mazeSize; j++)
Pinski1 0:dddf6e50f1e7 123 {
Pinski1 0:dddf6e50f1e7 124 if(map[i][j].weight == 0)
Pinski1 0:dddf6e50f1e7 125 {
Pinski1 0:dddf6e50f1e7 126 for(int k = j - 1; k >= 0; k--)
Pinski1 0:dddf6e50f1e7 127 {
Pinski1 0:dddf6e50f1e7 128 if(map[i][k].weight != 0) map[i][k].weight = map[i][k+1].weight + 1;
Pinski1 0:dddf6e50f1e7 129 }
Pinski1 0:dddf6e50f1e7 130
Pinski1 0:dddf6e50f1e7 131 for(int k = j + 1; k < mazeSize; k++)
Pinski1 0:dddf6e50f1e7 132 {
Pinski1 0:dddf6e50f1e7 133 if(map[i][k].weight != 0) map[i][k].weight = map[i][k-1].weight + 1;
Pinski1 0:dddf6e50f1e7 134 }
Pinski1 0:dddf6e50f1e7 135 }
Pinski1 0:dddf6e50f1e7 136 if(i == (mazeSize - 1) && j == (mazeSize -1)) finished = true;
Pinski1 0:dddf6e50f1e7 137 }
Pinski1 0:dddf6e50f1e7 138 }
Pinski1 0:dddf6e50f1e7 139 }
Pinski1 0:dddf6e50f1e7 140
Pinski1 0:dddf6e50f1e7 141 for(int i = 0; i < mazeSize; i++)
Pinski1 0:dddf6e50f1e7 142 {
Pinski1 0:dddf6e50f1e7 143 if(map[i][0].weight < maxWeight)
Pinski1 0:dddf6e50f1e7 144 {
Pinski1 0:dddf6e50f1e7 145 for(int k = i - 1; k >= 0; k--)
Pinski1 0:dddf6e50f1e7 146 {
Pinski1 0:dddf6e50f1e7 147 if(map[k][0].weight == maxWeight)
Pinski1 0:dddf6e50f1e7 148 {
Pinski1 0:dddf6e50f1e7 149 for(int j = 0; j < mazeSize; j++)
Pinski1 0:dddf6e50f1e7 150 {
Pinski1 0:dddf6e50f1e7 151 map[k][j].weight = map[k+1][j].weight + 1;
Pinski1 0:dddf6e50f1e7 152 }
Pinski1 0:dddf6e50f1e7 153 }
Pinski1 0:dddf6e50f1e7 154 }
Pinski1 0:dddf6e50f1e7 155 for(int k = i + 1; k < mazeSize; k++)
Pinski1 0:dddf6e50f1e7 156 {
Pinski1 0:dddf6e50f1e7 157 if(map[k][0].weight == maxWeight)
Pinski1 0:dddf6e50f1e7 158 {
Pinski1 0:dddf6e50f1e7 159 for(int j = 0; j < mazeSize; j++)
Pinski1 0:dddf6e50f1e7 160 {
Pinski1 0:dddf6e50f1e7 161 map[k][j].weight = map[k-1][j].weight + 1;
Pinski1 0:dddf6e50f1e7 162 }
Pinski1 0:dddf6e50f1e7 163 }
Pinski1 0:dddf6e50f1e7 164 }
Pinski1 0:dddf6e50f1e7 165 }
Pinski1 0:dddf6e50f1e7 166 }
Pinski1 0:dddf6e50f1e7 167
Pinski1 0:dddf6e50f1e7 168
Pinski1 0:dddf6e50f1e7 169 return;
Pinski1 0:dddf6e50f1e7 170 }
Pinski1 0:dddf6e50f1e7 171
Pinski1 0:dddf6e50f1e7 172 void Lees_Algorithm::printMaze(Serial output) {
Pinski1 0:dddf6e50f1e7 173
Pinski1 0:dddf6e50f1e7 174 for(int i = 0; i < mazeSize; i ++)
Pinski1 0:dddf6e50f1e7 175 {
Pinski1 0:dddf6e50f1e7 176 for(int j = 0; j < mazeSize; j++)
Pinski1 0:dddf6e50f1e7 177 {
Pinski1 0:dddf6e50f1e7 178 if(map[i][j].north == true) output.printf("+---");
Pinski1 0:dddf6e50f1e7 179 else output.printf("+ ");
Pinski1 0:dddf6e50f1e7 180 }
Pinski1 0:dddf6e50f1e7 181 output.printf("+\r\n");
Pinski1 0:dddf6e50f1e7 182
Pinski1 0:dddf6e50f1e7 183 for(int j = 0; j < mazeSize; j++)
Pinski1 0:dddf6e50f1e7 184 {
Pinski1 0:dddf6e50f1e7 185 if(map[i][j].west == true) output.printf("|");
Pinski1 0:dddf6e50f1e7 186 else output.printf(" ");
Pinski1 0:dddf6e50f1e7 187 output.printf("%3i", map[i][j].weight);
Pinski1 0:dddf6e50f1e7 188 }
Pinski1 0:dddf6e50f1e7 189 if(map[i][0].west == true) output.printf("|\r\n");
Pinski1 0:dddf6e50f1e7 190 else output.printf(" \r\n");
Pinski1 0:dddf6e50f1e7 191 }
Pinski1 0:dddf6e50f1e7 192 for(int j = 0; j < mazeSize; j ++)
Pinski1 0:dddf6e50f1e7 193 {
Pinski1 0:dddf6e50f1e7 194 if(map[0][j].north == 1) output.printf("+---");
Pinski1 0:dddf6e50f1e7 195 else output.printf("+ ");
Pinski1 0:dddf6e50f1e7 196 }
Pinski1 0:dddf6e50f1e7 197 output.printf("+\r\n");
Pinski1 0:dddf6e50f1e7 198
Pinski1 0:dddf6e50f1e7 199 return;
Pinski1 0:dddf6e50f1e7 200 }
Pinski1 0:dddf6e50f1e7 201
Pinski1 0:dddf6e50f1e7 202 void Lees_Algorithm::setDestination(_location target) {
Pinski1 0:dddf6e50f1e7 203 map[target.y][target.x].weight = 0;
Pinski1 0:dddf6e50f1e7 204 }
Pinski1 0:dddf6e50f1e7 205
Pinski1 0:dddf6e50f1e7 206 void Lees_Algorithm::clearWeights(void) {
Pinski1 0:dddf6e50f1e7 207 for (int j = 0; j < mazeSize; j++)
Pinski1 0:dddf6e50f1e7 208 {
Pinski1 0:dddf6e50f1e7 209 for (int i = 0; i < mazeSize; i++)
Pinski1 0:dddf6e50f1e7 210 {
Pinski1 0:dddf6e50f1e7 211 //map[0][0].weight = 0;
Pinski1 0:dddf6e50f1e7 212 //map[0][0].weight -= 1; // should auto-set to 255
Pinski1 0:dddf6e50f1e7 213 map[j][i].weight = maxWeight;
Pinski1 0:dddf6e50f1e7 214 }
Pinski1 0:dddf6e50f1e7 215 }
Pinski1 0:dddf6e50f1e7 216 return;
Pinski1 0:dddf6e50f1e7 217 }