Robert K / Lees_Algorithm

A library for solving mazes.

Lees_Algorithm.cpp@2:5343f19381c8, 2011-06-25 (annotated)

Committer:
Pinski1
Date:
Sat Jun 25 07:55:05 2011 +0000
Revision:
2:5343f19381c8
Parent:
1:80b73beb7742 
Minor change to getDirection

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 1:80b73beb7742 107 char Lees_Algorithm::getDirection(_location position, char facing) {
Pinski1 0:dddf6e50f1e7 108 char buffer;
Pinski1 1:80b73beb7742 109 unsigned char minimumWeight = 255;
Pinski1 1:80b73beb7742 110 bool direction[4] = {false, false, false, false};
Pinski1 1:80b73beb7742 111
Pinski1 1:80b73beb7742 112 if(!map[position.y][position.x].north && map[(position.y - 1) % mazeSize][position.x].weight < minimumWeight) minimumWeight = map[(position.y - 1) % mazeSize][position.x].weight;
Pinski1 1:80b73beb7742 113 if(!map[position.y][position.x].west && map[position.y][(position.x - 1) % mazeSize].weight < minimumWeight) minimumWeight = map[position.y][(position.x - 1) % mazeSize].weight;
Pinski1 1:80b73beb7742 114 if(!map[(position.y + 1) % mazeSize][position.x].north && map[(position.y + 1) % mazeSize][position.x].weight < minimumWeight) minimumWeight = map[(position.y + 1) % mazeSize][position.x].weight;
Pinski1 1:80b73beb7742 115 if(!map[position.y][(position.x + 1) % mazeSize].west && map[position.y][(position.x + 1) % mazeSize].weight < minimumWeight) minimumWeight = map[position.y][(position.x + 1) % mazeSize].weight;
Pinski1 1:80b73beb7742 116
Pinski1 1:80b73beb7742 117 if(!map[position.y][position.x].north && map[(position.y - 1) % mazeSize][position.x].weight == minimumWeight) direction[0] = true;
Pinski1 1:80b73beb7742 118 if(!map[position.y][position.x].west && map[position.y][(position.x - 1) % mazeSize].weight == minimumWeight) direction[1] = true;
Pinski1 1:80b73beb7742 119 if(!map[(position.y + 1) % mazeSize][position.x].north && map[(position.y + 1) % mazeSize][position.x].weight == minimumWeight) direction[2] = true;
Pinski1 1:80b73beb7742 120 if(!map[position.y][(position.x + 1) % mazeSize].west && map[position.y][(position.x + 1) % mazeSize].weight == minimumWeight) direction[3] = true;
Pinski1 1:80b73beb7742 121
Pinski1 1:80b73beb7742 122 switch(facing) {
Pinski1 1:80b73beb7742 123 case 'n':
Pinski1 1:80b73beb7742 124 if(direction[3] == true) buffer = 's';
Pinski1 1:80b73beb7742 125 if(direction[2] == true) buffer = 'e';
Pinski1 1:80b73beb7742 126 if(direction[1] == true) buffer = 'w';
Pinski1 1:80b73beb7742 127 if(direction[0] == true) buffer = 'n';
Pinski1 1:80b73beb7742 128 break;
Pinski1 1:80b73beb7742 129 case 'w':
Pinski1 1:80b73beb7742 130 if(direction[2] == true) buffer = 'e';
Pinski1 1:80b73beb7742 131 if(direction[3] == true) buffer = 's';
Pinski1 1:80b73beb7742 132 if(direction[0] == true) buffer = 'n';
Pinski1 1:80b73beb7742 133 if(direction[1] == true) buffer = 'w';
Pinski1 1:80b73beb7742 134 break;
Pinski1 1:80b73beb7742 135 case 's':
Pinski1 1:80b73beb7742 136 if(direction[0] == true) buffer = 'n';
Pinski1 1:80b73beb7742 137 if(direction[2] == true) buffer = 'e';
Pinski1 1:80b73beb7742 138 if(direction[1] == true) buffer = 'w';
Pinski1 1:80b73beb7742 139 if(direction[3] == true) buffer = 's';
Pinski1 1:80b73beb7742 140 break;
Pinski1 1:80b73beb7742 141 case 'e':
Pinski1 1:80b73beb7742 142 if(direction[1] == true) buffer = 'w';
Pinski1 1:80b73beb7742 143 if(direction[0] == true) buffer = 'n';
Pinski1 1:80b73beb7742 144 if(direction[3] == true) buffer = 's';
Pinski1 1:80b73beb7742 145 if(direction[2] == true) buffer = 'e';
Pinski1 1:80b73beb7742 146 break;
Pinski1 1:80b73beb7742 147 default:
Pinski1 1:80b73beb7742 148 break;
Pinski1 1:80b73beb7742 149 }
Pinski1 2:5343f19381c8 150 if(map[position.y][position.x].weight == 0) buffer = 'x';
Pinski1 0:dddf6e50f1e7 151 return buffer;
Pinski1 0:dddf6e50f1e7 152 }
Pinski1 0:dddf6e50f1e7 153
Pinski1 0:dddf6e50f1e7 154 void Lees_Algorithm::initialFill(void) {
Pinski1 0:dddf6e50f1e7 155
Pinski1 0:dddf6e50f1e7 156 int i,j;
Pinski1 0:dddf6e50f1e7 157 bool finished = false;
Pinski1 0:dddf6e50f1e7 158
Pinski1 0:dddf6e50f1e7 159 while(finished == false)
Pinski1 0:dddf6e50f1e7 160 {
Pinski1 0:dddf6e50f1e7 161
Pinski1 0:dddf6e50f1e7 162 for(i = 0; i < mazeSize; i++)
Pinski1 0:dddf6e50f1e7 163 {
Pinski1 0:dddf6e50f1e7 164 for(j = 0; j < mazeSize; j++)
Pinski1 0:dddf6e50f1e7 165 {
Pinski1 0:dddf6e50f1e7 166 if(map[i][j].weight == 0)
Pinski1 0:dddf6e50f1e7 167 {
Pinski1 0:dddf6e50f1e7 168 for(int k = j - 1; k >= 0; k--)
Pinski1 0:dddf6e50f1e7 169 {
Pinski1 0:dddf6e50f1e7 170 if(map[i][k].weight != 0) map[i][k].weight = map[i][k+1].weight + 1;
Pinski1 0:dddf6e50f1e7 171 }
Pinski1 0:dddf6e50f1e7 172
Pinski1 0:dddf6e50f1e7 173 for(int k = j + 1; k < mazeSize; k++)
Pinski1 0:dddf6e50f1e7 174 {
Pinski1 0:dddf6e50f1e7 175 if(map[i][k].weight != 0) map[i][k].weight = map[i][k-1].weight + 1;
Pinski1 0:dddf6e50f1e7 176 }
Pinski1 0:dddf6e50f1e7 177 }
Pinski1 0:dddf6e50f1e7 178 if(i == (mazeSize - 1) && j == (mazeSize -1)) finished = true;
Pinski1 0:dddf6e50f1e7 179 }
Pinski1 0:dddf6e50f1e7 180 }
Pinski1 0:dddf6e50f1e7 181 }
Pinski1 0:dddf6e50f1e7 182
Pinski1 0:dddf6e50f1e7 183 for(int i = 0; i < mazeSize; i++)
Pinski1 0:dddf6e50f1e7 184 {
Pinski1 0:dddf6e50f1e7 185 if(map[i][0].weight < maxWeight)
Pinski1 0:dddf6e50f1e7 186 {
Pinski1 0:dddf6e50f1e7 187 for(int k = i - 1; k >= 0; k--)
Pinski1 0:dddf6e50f1e7 188 {
Pinski1 0:dddf6e50f1e7 189 if(map[k][0].weight == maxWeight)
Pinski1 0:dddf6e50f1e7 190 {
Pinski1 0:dddf6e50f1e7 191 for(int j = 0; j < mazeSize; j++)
Pinski1 0:dddf6e50f1e7 192 {
Pinski1 0:dddf6e50f1e7 193 map[k][j].weight = map[k+1][j].weight + 1;
Pinski1 0:dddf6e50f1e7 194 }
Pinski1 0:dddf6e50f1e7 195 }
Pinski1 0:dddf6e50f1e7 196 }
Pinski1 0:dddf6e50f1e7 197 for(int k = i + 1; k < mazeSize; k++)
Pinski1 0:dddf6e50f1e7 198 {
Pinski1 0:dddf6e50f1e7 199 if(map[k][0].weight == maxWeight)
Pinski1 0:dddf6e50f1e7 200 {
Pinski1 0:dddf6e50f1e7 201 for(int j = 0; j < mazeSize; j++)
Pinski1 0:dddf6e50f1e7 202 {
Pinski1 0:dddf6e50f1e7 203 map[k][j].weight = map[k-1][j].weight + 1;
Pinski1 0:dddf6e50f1e7 204 }
Pinski1 0:dddf6e50f1e7 205 }
Pinski1 0:dddf6e50f1e7 206 }
Pinski1 0:dddf6e50f1e7 207 }
Pinski1 0:dddf6e50f1e7 208 }
Pinski1 0:dddf6e50f1e7 209
Pinski1 0:dddf6e50f1e7 210
Pinski1 0:dddf6e50f1e7 211 return;
Pinski1 0:dddf6e50f1e7 212 }
Pinski1 0:dddf6e50f1e7 213
Pinski1 0:dddf6e50f1e7 214 void Lees_Algorithm::printMaze(Serial output) {
Pinski1 0:dddf6e50f1e7 215
Pinski1 0:dddf6e50f1e7 216 for(int i = 0; i < mazeSize; i ++)
Pinski1 0:dddf6e50f1e7 217 {
Pinski1 0:dddf6e50f1e7 218 for(int j = 0; j < mazeSize; j++)
Pinski1 0:dddf6e50f1e7 219 {
Pinski1 0:dddf6e50f1e7 220 if(map[i][j].north == true) output.printf("+---");
Pinski1 0:dddf6e50f1e7 221 else output.printf("+ ");
Pinski1 0:dddf6e50f1e7 222 }
Pinski1 0:dddf6e50f1e7 223 output.printf("+\r\n");
Pinski1 0:dddf6e50f1e7 224
Pinski1 0:dddf6e50f1e7 225 for(int j = 0; j < mazeSize; j++)
Pinski1 0:dddf6e50f1e7 226 {
Pinski1 0:dddf6e50f1e7 227 if(map[i][j].west == true) output.printf("|");
Pinski1 0:dddf6e50f1e7 228 else output.printf(" ");
Pinski1 0:dddf6e50f1e7 229 output.printf("%3i", map[i][j].weight);
Pinski1 0:dddf6e50f1e7 230 }
Pinski1 0:dddf6e50f1e7 231 if(map[i][0].west == true) output.printf("|\r\n");
Pinski1 0:dddf6e50f1e7 232 else output.printf(" \r\n");
Pinski1 0:dddf6e50f1e7 233 }
Pinski1 0:dddf6e50f1e7 234 for(int j = 0; j < mazeSize; j ++)
Pinski1 0:dddf6e50f1e7 235 {
Pinski1 0:dddf6e50f1e7 236 if(map[0][j].north == 1) output.printf("+---");
Pinski1 0:dddf6e50f1e7 237 else output.printf("+ ");
Pinski1 0:dddf6e50f1e7 238 }
Pinski1 0:dddf6e50f1e7 239 output.printf("+\r\n");
Pinski1 0:dddf6e50f1e7 240
Pinski1 0:dddf6e50f1e7 241 return;
Pinski1 0:dddf6e50f1e7 242 }
Pinski1 0:dddf6e50f1e7 243
Pinski1 0:dddf6e50f1e7 244 void Lees_Algorithm::setDestination(_location target) {
Pinski1 0:dddf6e50f1e7 245 map[target.y][target.x].weight = 0;
Pinski1 0:dddf6e50f1e7 246 }
Pinski1 0:dddf6e50f1e7 247
Pinski1 0:dddf6e50f1e7 248 void Lees_Algorithm::clearWeights(void) {
Pinski1 0:dddf6e50f1e7 249 for (int j = 0; j < mazeSize; j++)
Pinski1 0:dddf6e50f1e7 250 {
Pinski1 0:dddf6e50f1e7 251 for (int i = 0; i < mazeSize; i++)
Pinski1 0:dddf6e50f1e7 252 {
Pinski1 0:dddf6e50f1e7 253 //map[0][0].weight = 0;
Pinski1 0:dddf6e50f1e7 254 //map[0][0].weight -= 1; // should auto-set to 255
Pinski1 0:dddf6e50f1e7 255 map[j][i].weight = maxWeight;
Pinski1 0:dddf6e50f1e7 256 }
Pinski1 0:dddf6e50f1e7 257 }
Pinski1 0:dddf6e50f1e7 258 return;
Pinski1 0:dddf6e50f1e7 259 }