Tobias Berger / Mbed 2 deprecated Robocode

Tobis Programm forked to not destroy your golden files

Dependencies:   mbed

Fork of Robocode by PES 2 - Gruppe 1

source/Pathfinding.cpp@132:8ae08f41bb43, 2017-05-15 (annotated)

Committer:
cittecla
Date:
Mon May 15 14:58:27 2017 +0000
Revision:
132:8ae08f41bb43
Parent:
130:670a954495bf 
bugfixes

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cittecla 14:9e2ce5880fb0 1 /**
cittecla 14:9e2ce5880fb0 2 * Pathfinding function library
cittecla 14:9e2ce5880fb0 3 * Handels Pathfinding inside the Arena
cittecla 14:9e2ce5880fb0 4 * Version 3.0.0
cittecla 14:9e2ce5880fb0 5 **/
cittecla 14:9e2ce5880fb0 6
cittecla 14:9e2ce5880fb0 7 /**
cittecla 14:9e2ce5880fb0 8 * Release notes:
cittecla 14:9e2ce5880fb0 9 * Storage and run time optimized
cittecla 14:9e2ce5880fb0 10 * Starts from target to reduce code and run time
cittecla 14:9e2ce5880fb0 11 **/
cittecla 14:9e2ce5880fb0 12
cittecla 14:9e2ce5880fb0 13 /**
cittecla 14:9e2ce5880fb0 14 * Storage table:
cittecla 14:9e2ce5880fb0 15 * 80kB uint16_t [40000] open_list (contains open f values as well as closed list value = max of 16 bit = 65535)
cittecla 14:9e2ce5880fb0 16 * 80kB uint16_t [40000] g_list (contains g values from A to current incl. target, needed for pathtracing)
cittecla 14:9e2ce5880fb0 17 * 40kB uint8_t [40000] obstacle_list (contains hard surface as well as red and green LEGO stones)
cittecla 14:9e2ce5880fb0 18 * 2kB uint8_t [2000] walkpath (contains the path coordinates in a list, [0] = start, [n] = target)
cittecla 14:9e2ce5880fb0 19 **/
cittecla 14:9e2ce5880fb0 20
cittecla 14:9e2ce5880fb0 21 //#include "mbed.h"
cittecla 34:40d8d29b44b8 22 #include "Pathfinding.h"
cittecla 14:9e2ce5880fb0 23 #include <stdio.h>
cittecla 14:9e2ce5880fb0 24 #include <iostream>
cittecla 14:9e2ce5880fb0 25
cittecla 32:777976c4d733 26
cittecla 14:9e2ce5880fb0 27
cittecla 14:9e2ce5880fb0 28
cittecla 14:9e2ce5880fb0 29 using namespace std;
cittecla 15:32b5d97a4281 30
cittecla 41:462d379e85c4 31 //global
cittecla 113:c7afe49752b9 32 position walkpath[3 * row] = { 0 };
cittecla 41:462d379e85c4 33 position target = {0};
cittecla 128:6bde4483ce7b 34 position start = {0};
cittecla 14:9e2ce5880fb0 35
cittecla 14:9e2ce5880fb0 36 //should be local
cittecla 15:32b5d97a4281 37 static uint16_t open_list[row][col] = { 0 }; // contains open slots marked with their F value (G + H)
cittecla 15:32b5d97a4281 38 static uint16_t g_list[row][col] = { 0 }; // G = A to square (C) in absolut number
cittecla 14:9e2ce5880fb0 39
cittecla 15:32b5d97a4281 40 static uint16_t open_list_count = 0;
cittecla 15:32b5d97a4281 41 static uint16_t counter;
cittecla 14:9e2ce5880fb0 42
cittecla 128:6bde4483ce7b 43 static int path_found = 45;
cittecla 128:6bde4483ce7b 44 static int no_path_possible = 48;
cittecla 32:777976c4d733 45
cittecla 14:9e2ce5880fb0 46
cittecla 14:9e2ce5880fb0 47 /************************************************************************************************************
cittecla 14:9e2ce5880fb0 48 Main a_star handling
cittecla 14:9e2ce5880fb0 49 ************************************************************************************************************/
cittecla 32:777976c4d733 50 int pathfinding()
cittecla 32:777976c4d733 51 {
cittecla 16:4a20536c9bb8 52 define_obst();
cittecla 128:6bde4483ce7b 53 //start = get_current_pos();
cittecla 128:6bde4483ce7b 54 //target.x = 198; target.y = 198;
cittecla 128:6bde4483ce7b 55 position current = { 0 }; //current pos of the pathfinder
cittecla 14:9e2ce5880fb0 56
cittecla 16:4a20536c9bb8 57 memset(open_list, 0, sizeof(open_list));
cittecla 16:4a20536c9bb8 58 memset(g_list, 0, sizeof(g_list));
cittecla 16:4a20536c9bb8 59 memset(walkpath, 0, sizeof(walkpath));
cittecla 16:4a20536c9bb8 60 open_list_count = 0;
cittecla 16:4a20536c9bb8 61 counter = 0;
cittecla 32:777976c4d733 62
cittecla 129:0f60bf9640bb 63 // check if position isn't reachable
cittecla 14:9e2ce5880fb0 64 if (obstacle_list[start.x][start.y] == 1 || start.x > row || start.y > col ||
cittecla 32:777976c4d733 65 obstacle_list[target.x][target.y] == 1 || target.x > row || target.y > col) {
cittecla 129:0f60bf9640bb 66 printf("Fatal Error, no path calculation possible, line 66\r\n");
cittecla 32:777976c4d733 67 return no_path_possible;
cittecla 32:777976c4d733 68 } else {
cittecla 14:9e2ce5880fb0 69 position diff;
cittecla 14:9e2ce5880fb0 70 diff.x = abs(target.x - start.x);
cittecla 14:9e2ce5880fb0 71 diff.y = abs(target.y - start.y);
cittecla 14:9e2ce5880fb0 72 uint8_t diagonal;
cittecla 14:9e2ce5880fb0 73 if (diff.x > diff.y) {
cittecla 14:9e2ce5880fb0 74 diagonal = diff.x - (diff.x - diff.y);
cittecla 32:777976c4d733 75 } else {
cittecla 14:9e2ce5880fb0 76 diagonal = diff.y - (diff.y - diff.x);
cittecla 14:9e2ce5880fb0 77 }
cittecla 14:9e2ce5880fb0 78 open_list[target.x][target.y] = diff.x * 2 + diff.y * 2 - diagonal;
cittecla 14:9e2ce5880fb0 79 open_list_count += 1;
cittecla 14:9e2ce5880fb0 80 g_list[target.x][target.y] = 1;
cittecla 14:9e2ce5880fb0 81
cittecla 14:9e2ce5880fb0 82 do {
cittecla 14:9e2ce5880fb0 83 current = openList_lowest_F(); // Get the square with the lowest F score
cittecla 14:9e2ce5880fb0 84 open_list[current.x][current.y] = 65535; // add position to the closed list; open_list 1111 1111 1111 1111 ^= closed_list = 1
cittecla 14:9e2ce5880fb0 85 open_list_count -= 1;
cittecla 14:9e2ce5880fb0 86
cittecla 14:9e2ce5880fb0 87 if (open_list[start.x][start.y] == 65535) { // if we added the destination to the closed list, we've found a path
cittecla 129:0f60bf9640bb 88 printf("Path found, line 89\r\n"); // PATH FOUND
cittecla 32:777976c4d733 89 break; // break the loop
cittecla 14:9e2ce5880fb0 90 }
cittecla 14:9e2ce5880fb0 91 calc_F(target, start, current); // Calculates F for each neighbour of current lowest F
cittecla 14:9e2ce5880fb0 92 } while (!(open_list_count == 0)); // Continue until there is no more available square in the open list (which means there is no path)
cittecla 132:8ae08f41bb43 93
cittecla 14:9e2ce5880fb0 94 if (open_list[start.x][start.y] != 65535) { // if we added the destination to the closed list, we've found a path
cittecla 129:0f60bf9640bb 95 // no path found
cittecla 129:0f60bf9640bb 96 printf("No Path possible, line 97\r\n");
cittecla 130:670a954495bf 97 print_map(0);
cittecla 130:670a954495bf 98 print_map(2);
cittecla 132:8ae08f41bb43 99 print_map(3);
cittecla 32:777976c4d733 100 return no_path_possible;
cittecla 32:777976c4d733 101 } else {
cittecla 129:0f60bf9640bb 102 // path found
cittecla 14:9e2ce5880fb0 103 mapp_path(start); //mapps the path onto the open_list, be replaced by position vector array;
cittecla 129:0f60bf9640bb 104 printf("\nPath from (%d:%d) to (%d:%d) has a total of %d steps\r\n", start.x, start.y, target.x, target.y, counter);
cittecla 32:777976c4d733 105 return path_found;
cittecla 14:9e2ce5880fb0 106 }
cittecla 14:9e2ce5880fb0 107 }
cittecla 14:9e2ce5880fb0 108 }
cittecla 14:9e2ce5880fb0 109
cittecla 14:9e2ce5880fb0 110 /************************************************************************************************************
cittecla 14:9e2ce5880fb0 111 only needed for test, will be done by mapping function, delete for use on robot
cittecla 14:9e2ce5880fb0 112 ************************************************************************************************************/
cittecla 32:777976c4d733 113 static void define_obst()
cittecla 32:777976c4d733 114 {
cittecla 14:9e2ce5880fb0 115
cittecla 14:9e2ce5880fb0 116 for (int i = 0; i < row; i++) {
cittecla 14:9e2ce5880fb0 117 obstacle_list[i][0] = 1;
cittecla 14:9e2ce5880fb0 118 obstacle_list[i][col - 1] = 1;
cittecla 14:9e2ce5880fb0 119 }
cittecla 14:9e2ce5880fb0 120
cittecla 14:9e2ce5880fb0 121 for (int i = 0; i < row; i++) {
cittecla 14:9e2ce5880fb0 122 obstacle_list[0][i] = 1;
cittecla 14:9e2ce5880fb0 123 obstacle_list[row - 1][i] = 1;
cittecla 14:9e2ce5880fb0 124 }
cittecla 14:9e2ce5880fb0 125
cittecla 14:9e2ce5880fb0 126 }
cittecla 14:9e2ce5880fb0 127
cittecla 14:9e2ce5880fb0 128
cittecla 14:9e2ce5880fb0 129 /************************************************************************************************************
cittecla 14:9e2ce5880fb0 130 Essential function for calculating the distance
cittecla 14:9e2ce5880fb0 131 ************************************************************************************************************/
cittecla 32:777976c4d733 132 static void calc_F(position a, position b, position current)
cittecla 32:777976c4d733 133 {
cittecla 14:9e2ce5880fb0 134
cittecla 14:9e2ce5880fb0 135 position adjacent = current;
cittecla 14:9e2ce5880fb0 136 position corner1 = { 0 }, corner2 = { 0 };
cittecla 14:9e2ce5880fb0 137 uint16_t numb = 2;
cittecla 14:9e2ce5880fb0 138 for (int i = 0; i < 8; i++) {
cittecla 32:777976c4d733 139 switch (i) {
cittecla 32:777976c4d733 140 case 0:
cittecla 32:777976c4d733 141 adjacent.x = current.x - 1;
cittecla 32:777976c4d733 142 adjacent.y = current.y; // top
cittecla 32:777976c4d733 143 break;
cittecla 32:777976c4d733 144 case 1:
cittecla 32:777976c4d733 145 adjacent.x = current.x + 1;
cittecla 32:777976c4d733 146 adjacent.y = current.y; // bottom
cittecla 32:777976c4d733 147 break;
cittecla 32:777976c4d733 148 case 2:
cittecla 32:777976c4d733 149 adjacent.x = current.x;
cittecla 32:777976c4d733 150 adjacent.y = current.y - 1; // left
cittecla 32:777976c4d733 151 break;
cittecla 32:777976c4d733 152 case 3:
cittecla 32:777976c4d733 153 adjacent.x = current.x;
cittecla 32:777976c4d733 154 adjacent.y = current.y + 1; // right
cittecla 32:777976c4d733 155 break;
cittecla 32:777976c4d733 156 case 4:
cittecla 32:777976c4d733 157 adjacent.x = current.x - 1;
cittecla 32:777976c4d733 158 adjacent.y = current.y - 1;
cittecla 129:0f60bf9640bb 159 numb = 3; // top left
cittecla 32:777976c4d733 160 corner1.x = current.x - 1;
cittecla 32:777976c4d733 161 corner1.y = current.y;
cittecla 32:777976c4d733 162 corner2.x = current.x;
cittecla 32:777976c4d733 163 corner2.y = current.y - 1;
cittecla 32:777976c4d733 164 break;
cittecla 32:777976c4d733 165 case 5:
cittecla 32:777976c4d733 166 adjacent.x = current.x - 1;
cittecla 32:777976c4d733 167 adjacent.y = current.y + 1; // top right
cittecla 32:777976c4d733 168 corner2.y = current.y + 1;
cittecla 32:777976c4d733 169 break;
cittecla 32:777976c4d733 170 case 6:
cittecla 32:777976c4d733 171 adjacent.x = current.x + 1;
cittecla 32:777976c4d733 172 adjacent.y = current.y + 1; // bottom right
cittecla 32:777976c4d733 173 corner1.x = current.x + 1;
cittecla 32:777976c4d733 174 break;
cittecla 32:777976c4d733 175 case 7:
cittecla 32:777976c4d733 176 adjacent.x = current.x + 1;
cittecla 32:777976c4d733 177 adjacent.y = current.y - 1; // bottom left
cittecla 32:777976c4d733 178 corner2.y = current.y - 1;
cittecla 32:777976c4d733 179 break;
cittecla 32:777976c4d733 180 default:
cittecla 128:6bde4483ce7b 181 printf("Fatal Error, unknown position\r\n");
cittecla 32:777976c4d733 182 break;
cittecla 14:9e2ce5880fb0 183 }
cittecla 14:9e2ce5880fb0 184 if (obstacle_list[adjacent.x][adjacent.y] == 0 && ((obstacle_list[corner1.x][corner1.y] == 0 && obstacle_list[corner2.x][corner2.y] == 0) ||
cittecla 32:777976c4d733 185 (corner1.x == 0 && corner1.y == 0 && corner2.x == 0 && corner2.y == 0))) {
cittecla 14:9e2ce5880fb0 186 if (open_list[adjacent.x][adjacent.y] != 65535) { // if this adjacent square is already in the closed list ignore it
cittecla 14:9e2ce5880fb0 187 uint16_t g_value = g_list[current.x][current.y] + numb;
cittecla 14:9e2ce5880fb0 188 position diff;
cittecla 14:9e2ce5880fb0 189 diff.x = abs(adjacent.x - b.x);
cittecla 14:9e2ce5880fb0 190 diff.y = abs(adjacent.y - b.y);
cittecla 14:9e2ce5880fb0 191 uint8_t diagonal;
cittecla 14:9e2ce5880fb0 192 if (diff.x > diff.y) {
cittecla 14:9e2ce5880fb0 193 diagonal = diff.x - (diff.x - diff.y);
cittecla 32:777976c4d733 194 } else {
cittecla 14:9e2ce5880fb0 195 diagonal = diff.y - (diff.y - diff.x);
cittecla 14:9e2ce5880fb0 196 }
cittecla 14:9e2ce5880fb0 197 if (open_list[adjacent.x][adjacent.y] == 0) { // if its not in the open list
cittecla 14:9e2ce5880fb0 198 open_list_count += 1;
cittecla 14:9e2ce5880fb0 199 g_list[adjacent.x][adjacent.y] = g_value; // compute its score, set the parent
cittecla 14:9e2ce5880fb0 200 open_list[adjacent.x][adjacent.y] = g_value + (diff.x * 2 + diff.y * 2 - diagonal);
cittecla 32:777976c4d733 201 } else { // if its already in the open list
cittecla 14:9e2ce5880fb0 202 if (g_value < g_list[adjacent.x][adjacent.y]) { // test if new path is better
cittecla 14:9e2ce5880fb0 203 g_list[adjacent.x][adjacent.y] = g_value;
cittecla 14:9e2ce5880fb0 204 open_list[adjacent.x][adjacent.y] = g_list[adjacent.x][adjacent.y] + (diff.x * 2 + diff.y * 2 - diagonal);
cittecla 14:9e2ce5880fb0 205 }
cittecla 14:9e2ce5880fb0 206 }
cittecla 14:9e2ce5880fb0 207 }
cittecla 14:9e2ce5880fb0 208 }
cittecla 14:9e2ce5880fb0 209 }
cittecla 14:9e2ce5880fb0 210 }
cittecla 14:9e2ce5880fb0 211
cittecla 14:9e2ce5880fb0 212 /************************************************************************************************************
cittecla 14:9e2ce5880fb0 213 Essential function for getting the next position
cittecla 14:9e2ce5880fb0 214 ************************************************************************************************************/
cittecla 32:777976c4d733 215 static position openList_lowest_F()
cittecla 32:777976c4d733 216 {
cittecla 14:9e2ce5880fb0 217 uint16_t lowest = row * col;
cittecla 14:9e2ce5880fb0 218 position lowest_pos;
cittecla 14:9e2ce5880fb0 219 for (uint8_t i = 0; i < row; i++) {
cittecla 14:9e2ce5880fb0 220 for (uint8_t j = 0; j < col; j++) {
cittecla 14:9e2ce5880fb0 221 if (open_list[i][j] > 1 && open_list[i][j] < lowest && open_list[i][j] != 65535) {
cittecla 14:9e2ce5880fb0 222 lowest_pos.x = i;
cittecla 14:9e2ce5880fb0 223 lowest_pos.y = j;
cittecla 14:9e2ce5880fb0 224 lowest = open_list[i][j];
cittecla 14:9e2ce5880fb0 225 }
cittecla 14:9e2ce5880fb0 226 }
cittecla 14:9e2ce5880fb0 227 }
cittecla 14:9e2ce5880fb0 228 return lowest_pos;
cittecla 14:9e2ce5880fb0 229 }
cittecla 14:9e2ce5880fb0 230
cittecla 14:9e2ce5880fb0 231 /************************************************************************************************************
cittecla 14:9e2ce5880fb0 232 Essential function for getting the next position
cittecla 14:9e2ce5880fb0 233 ************************************************************************************************************/
cittecla 32:777976c4d733 234 static void mapp_path(position b)
cittecla 32:777976c4d733 235 {
cittecla 14:9e2ce5880fb0 236 // write path to walkpath array
cittecla 14:9e2ce5880fb0 237
cittecla 14:9e2ce5880fb0 238 position x = b; // lower value
cittecla 14:9e2ce5880fb0 239 position y = b; // higher value
cittecla 14:9e2ce5880fb0 240 counter = 0;
cittecla 14:9e2ce5880fb0 241 walkpath[0] = y;
cittecla 132:8ae08f41bb43 242 printf("(%d | %d) \r\n", walkpath[0].x, walkpath[0].y);
cittecla 14:9e2ce5880fb0 243 while (g_list[y.x][y.y] > 1) {
cittecla 14:9e2ce5880fb0 244 counter += 1;
cittecla 14:9e2ce5880fb0 245 position s = b;
cittecla 14:9e2ce5880fb0 246 for (int i = 0; i < 8; i++) {
cittecla 32:777976c4d733 247 switch (i) {
cittecla 32:777976c4d733 248 case 0:
cittecla 32:777976c4d733 249 x.x = y.x - 1;
cittecla 32:777976c4d733 250 x.y = y.y;
cittecla 32:777976c4d733 251 break;
cittecla 32:777976c4d733 252 case 1:
cittecla 32:777976c4d733 253 x.x = y.x + 1;
cittecla 32:777976c4d733 254 x.y = y.y;
cittecla 32:777976c4d733 255 break;
cittecla 32:777976c4d733 256 case 2:
cittecla 32:777976c4d733 257 x.x = y.x;
cittecla 32:777976c4d733 258 x.y = y.y - 1;
cittecla 32:777976c4d733 259 break;
cittecla 32:777976c4d733 260 case 3:
cittecla 32:777976c4d733 261 x.x = y.x;
cittecla 32:777976c4d733 262 x.y = y.y + 1;
cittecla 32:777976c4d733 263 break;
cittecla 32:777976c4d733 264 case 4:
cittecla 32:777976c4d733 265 x.x = y.x - 1;
cittecla 32:777976c4d733 266 x.y = y.y - 1;
cittecla 32:777976c4d733 267 break;
cittecla 32:777976c4d733 268 case 5:
cittecla 32:777976c4d733 269 x.x = y.x - 1;
cittecla 32:777976c4d733 270 x.y = y.y + 1;
cittecla 32:777976c4d733 271 break;
cittecla 32:777976c4d733 272 case 6:
cittecla 32:777976c4d733 273 x.x = y.x + 1;
cittecla 32:777976c4d733 274 x.y = y.y - 1;
cittecla 32:777976c4d733 275 break;
cittecla 32:777976c4d733 276 case 7:
cittecla 32:777976c4d733 277 x.x = y.x + 1;
cittecla 32:777976c4d733 278 x.y = y.y + 1;
cittecla 32:777976c4d733 279 break;
cittecla 32:777976c4d733 280 default:
cittecla 128:6bde4483ce7b 281 printf("Fatal Error, unknown position\r\n");
cittecla 32:777976c4d733 282 break;
cittecla 14:9e2ce5880fb0 283 }
cittecla 14:9e2ce5880fb0 284 if (g_list[x.x][x.y] < g_list[y.x][y.y] && g_list[x.x][x.y] != 0 && g_list[s.x][s.y] > g_list[x.x][x.y]) {
cittecla 14:9e2ce5880fb0 285 s = x;
cittecla 14:9e2ce5880fb0 286 }
cittecla 14:9e2ce5880fb0 287 }
cittecla 14:9e2ce5880fb0 288 y = s;
cittecla 14:9e2ce5880fb0 289 walkpath[counter].x = y.x;
cittecla 14:9e2ce5880fb0 290 walkpath[counter].y = y.y;
cittecla 132:8ae08f41bb43 291 printf("(%d | %d) \r\n", walkpath[counter].x, walkpath[counter].y);
cittecla 132:8ae08f41bb43 292
cittecla 14:9e2ce5880fb0 293 }
cittecla 14:9e2ce5880fb0 294 }
cittecla 14:9e2ce5880fb0 295
cittecla 130:670a954495bf 296 //******************************************************************************
cittecla 130:670a954495bf 297 /**
cittecla 130:670a954495bf 298 * prints map defined by list
cittecla 130:670a954495bf 299 * 0 = obstacle_list
cittecla 130:670a954495bf 300 * 1 = open_list
cittecla 130:670a954495bf 301 * 2 = target_list
cittecla 130:670a954495bf 302 * very slow, shouldn't be called in final code
cittecla 130:670a954495bf 303 * by Claudio Citterio
cittecla 130:670a954495bf 304 **/
cittecla 130:670a954495bf 305 void print_map(int list)
cittecla 130:670a954495bf 306 {
cittecla 130:670a954495bf 307 // Debug function for printing the obstacle matrix on putty.
cittecla 130:670a954495bf 308 for (int y = 0; y < col; y++) {
cittecla 130:670a954495bf 309 for (int x = 0; x < row; x++) {
cittecla 130:670a954495bf 310 if(list == 0)printf("%d ", obstacle_list[y][x]);
cittecla 130:670a954495bf 311 if(list == 1)printf("%d ", open_list[y][x]);
cittecla 130:670a954495bf 312 if(list == 2)printf("%d ", target_list[y][x]);
cittecla 132:8ae08f41bb43 313 if(list == 3)printf("%d ", g_list[y][x]);
cittecla 130:670a954495bf 314 }
cittecla 130:670a954495bf 315 printf("\r\n");
cittecla 130:670a954495bf 316 }
cittecla 130:670a954495bf 317 printf("\r\n");
cittecla 130:670a954495bf 318 }