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

pathfinding.cpp@16:4a20536c9bb8, 2017-03-01 (annotated)

Committer:
cittecla
Date:
Wed Mar 01 19:49:49 2017 +0000
Revision:
16:4a20536c9bb8
Parent:
15:32b5d97a4281 
Pathfinding test

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 14:9e2ce5880fb0 22 #include "pathfinding.h"
cittecla 14:9e2ce5880fb0 23 #include <stdio.h>
cittecla 14:9e2ce5880fb0 24 #include <iostream>
cittecla 14:9e2ce5880fb0 25
cittecla 16:4a20536c9bb8 26 #define row 100
cittecla 16:4a20536c9bb8 27 #define col 100
cittecla 14:9e2ce5880fb0 28
cittecla 14:9e2ce5880fb0 29
cittecla 14:9e2ce5880fb0 30 using namespace std;
cittecla 15:32b5d97a4281 31
cittecla 15:32b5d97a4281 32 //will be replaced with
cittecla 14:9e2ce5880fb0 33 uint8_t obstacle_list[row][col] = { 0 };
cittecla 14:9e2ce5880fb0 34
cittecla 15:32b5d97a4281 35 //global
cittecla 14:9e2ce5880fb0 36 position walkpath[5 * row] = { 0 };
cittecla 14:9e2ce5880fb0 37
cittecla 14:9e2ce5880fb0 38 //should be local
cittecla 15:32b5d97a4281 39 static uint16_t open_list[row][col] = { 0 }; // contains open slots marked with their F value (G + H)
cittecla 15:32b5d97a4281 40 static uint16_t g_list[row][col] = { 0 }; // G = A to square (C) in absolut number
cittecla 14:9e2ce5880fb0 41
cittecla 15:32b5d97a4281 42 static uint16_t open_list_count = 0;
cittecla 15:32b5d97a4281 43 static uint16_t counter;
cittecla 14:9e2ce5880fb0 44
cittecla 14:9e2ce5880fb0 45
cittecla 14:9e2ce5880fb0 46 /************************************************************************************************************
cittecla 14:9e2ce5880fb0 47 Main a_star handling
cittecla 14:9e2ce5880fb0 48 ************************************************************************************************************/
cittecla 14:9e2ce5880fb0 49 void pathfinding() {
cittecla 16:4a20536c9bb8 50 define_obst();
cittecla 14:9e2ce5880fb0 51 position start = { 1,1 }; //start
cittecla 16:4a20536c9bb8 52 position target = { 198,198 }; //target
cittecla 14:9e2ce5880fb0 53 position current = { 0 }; //current pos
cittecla 14:9e2ce5880fb0 54
cittecla 16:4a20536c9bb8 55 memset(open_list, 0, sizeof(open_list));
cittecla 16:4a20536c9bb8 56 memset(g_list, 0, sizeof(g_list));
cittecla 16:4a20536c9bb8 57 memset(walkpath, 0, sizeof(walkpath));
cittecla 16:4a20536c9bb8 58 open_list_count = 0;
cittecla 16:4a20536c9bb8 59 counter = 0;
cittecla 16:4a20536c9bb8 60
cittecla 14:9e2ce5880fb0 61 // check if position is reachable
cittecla 14:9e2ce5880fb0 62 if (obstacle_list[start.x][start.y] == 1 || start.x > row || start.y > col ||
cittecla 14:9e2ce5880fb0 63 obstacle_list[target.x][target.y] == 1 || target.x > row || target.y > col) {
cittecla 14:9e2ce5880fb0 64 printf("Fatal Error, no path calculation possible\n");
cittecla 14:9e2ce5880fb0 65 }
cittecla 14:9e2ce5880fb0 66 else {
cittecla 14:9e2ce5880fb0 67 position diff;
cittecla 14:9e2ce5880fb0 68 diff.x = abs(target.x - start.x);
cittecla 14:9e2ce5880fb0 69 diff.y = abs(target.y - start.y);
cittecla 14:9e2ce5880fb0 70 uint8_t diagonal;
cittecla 14:9e2ce5880fb0 71 if (diff.x > diff.y) {
cittecla 14:9e2ce5880fb0 72 diagonal = diff.x - (diff.x - diff.y);
cittecla 14:9e2ce5880fb0 73 }
cittecla 14:9e2ce5880fb0 74 else {
cittecla 14:9e2ce5880fb0 75 diagonal = diff.y - (diff.y - diff.x);
cittecla 14:9e2ce5880fb0 76 }
cittecla 14:9e2ce5880fb0 77 open_list[target.x][target.y] = diff.x * 2 + diff.y * 2 - diagonal;
cittecla 14:9e2ce5880fb0 78 open_list_count += 1;
cittecla 14:9e2ce5880fb0 79 g_list[target.x][target.y] = 1;
cittecla 14:9e2ce5880fb0 80
cittecla 14:9e2ce5880fb0 81 do {
cittecla 14:9e2ce5880fb0 82
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 14:9e2ce5880fb0 88 printf("Path found\n\n"); // PATH FOUND
cittecla 14:9e2ce5880fb0 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
cittecla 14:9e2ce5880fb0 93 } while (!(open_list_count == 0)); // Continue until there is no more available square in the open list (which means there is no path)
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 14:9e2ce5880fb0 95 printf("No Path possible\n");
cittecla 14:9e2ce5880fb0 96 }
cittecla 14:9e2ce5880fb0 97 else {
cittecla 14:9e2ce5880fb0 98 mapp_path(start); //mapps the path onto the open_list, be replaced by position vector array;
cittecla 14:9e2ce5880fb0 99 printf("\nPath from (%d:%d) to (%d:%d) has a total of %d steps\n\n", start.x, start.y, target.x, target.y, counter);
cittecla 14:9e2ce5880fb0 100 }
cittecla 14:9e2ce5880fb0 101 }
cittecla 14:9e2ce5880fb0 102 }
cittecla 14:9e2ce5880fb0 103
cittecla 14:9e2ce5880fb0 104 /************************************************************************************************************
cittecla 14:9e2ce5880fb0 105 only needed for test, will be done by mapping function, delete for use on robot
cittecla 14:9e2ce5880fb0 106 ************************************************************************************************************/
cittecla 16:4a20536c9bb8 107 static void define_obst() {
cittecla 14:9e2ce5880fb0 108
cittecla 14:9e2ce5880fb0 109 for (int i = 0; i < row; i++) {
cittecla 14:9e2ce5880fb0 110 obstacle_list[i][0] = 1;
cittecla 14:9e2ce5880fb0 111 obstacle_list[i][col - 1] = 1;
cittecla 14:9e2ce5880fb0 112 }
cittecla 14:9e2ce5880fb0 113
cittecla 14:9e2ce5880fb0 114 for (int i = 0; i < row; i++) {
cittecla 14:9e2ce5880fb0 115 obstacle_list[0][i] = 1;
cittecla 14:9e2ce5880fb0 116 obstacle_list[row - 1][i] = 1;
cittecla 14:9e2ce5880fb0 117 }
cittecla 14:9e2ce5880fb0 118
cittecla 14:9e2ce5880fb0 119 obstacle_list[1][5] = 1;
cittecla 14:9e2ce5880fb0 120 obstacle_list[2][5] = 1;
cittecla 14:9e2ce5880fb0 121 //obstacle_list[3][5] = 1;
cittecla 14:9e2ce5880fb0 122 obstacle_list[4][5] = 1;
cittecla 14:9e2ce5880fb0 123 //obstacle_list[5][5] = 1;
cittecla 14:9e2ce5880fb0 124
cittecla 14:9e2ce5880fb0 125 obstacle_list[5][0] = 1;
cittecla 14:9e2ce5880fb0 126 obstacle_list[5][1] = 1;
cittecla 14:9e2ce5880fb0 127 obstacle_list[5][2] = 1;
cittecla 14:9e2ce5880fb0 128 obstacle_list[5][3] = 1;
cittecla 14:9e2ce5880fb0 129 obstacle_list[5][4] = 1;
cittecla 14:9e2ce5880fb0 130
cittecla 14:9e2ce5880fb0 131 obstacle_list[7][6] = 1;
cittecla 14:9e2ce5880fb0 132 obstacle_list[7][4] = 1;
cittecla 14:9e2ce5880fb0 133 obstacle_list[7][5] = 1;
cittecla 14:9e2ce5880fb0 134 obstacle_list[7][6] = 1;
cittecla 14:9e2ce5880fb0 135 obstacle_list[7][7] = 1;
cittecla 14:9e2ce5880fb0 136 obstacle_list[7][8] = 1;
cittecla 14:9e2ce5880fb0 137 }
cittecla 14:9e2ce5880fb0 138
cittecla 14:9e2ce5880fb0 139
cittecla 14:9e2ce5880fb0 140 /************************************************************************************************************
cittecla 14:9e2ce5880fb0 141 Essential function for calculating the distance
cittecla 14:9e2ce5880fb0 142 ************************************************************************************************************/
cittecla 16:4a20536c9bb8 143 static void calc_F(position a, position b, position current) {
cittecla 14:9e2ce5880fb0 144
cittecla 14:9e2ce5880fb0 145 position adjacent = current;
cittecla 14:9e2ce5880fb0 146 position corner1 = { 0 }, corner2 = { 0 };
cittecla 14:9e2ce5880fb0 147 uint16_t numb = 2;
cittecla 14:9e2ce5880fb0 148 for (int i = 0; i < 8; i++) {
cittecla 14:9e2ce5880fb0 149 switch (i)
cittecla 14:9e2ce5880fb0 150 {
cittecla 14:9e2ce5880fb0 151 case 0: adjacent.x = current.x - 1; adjacent.y = current.y; // top
cittecla 14:9e2ce5880fb0 152 break;
cittecla 14:9e2ce5880fb0 153 case 1: adjacent.x = current.x + 1; adjacent.y = current.y; // bottom
cittecla 14:9e2ce5880fb0 154 break;
cittecla 14:9e2ce5880fb0 155 case 2: adjacent.x = current.x; adjacent.y = current.y - 1; // left
cittecla 14:9e2ce5880fb0 156 break;
cittecla 14:9e2ce5880fb0 157 case 3: adjacent.x = current.x; adjacent.y = current.y + 1; // right
cittecla 14:9e2ce5880fb0 158 break;
cittecla 14:9e2ce5880fb0 159 case 4: adjacent.x = current.x - 1; adjacent.y = current.y - 1; numb = 3; // top left
cittecla 14:9e2ce5880fb0 160 corner1.x = current.x - 1; corner1.y = current.y;
cittecla 14:9e2ce5880fb0 161 corner2.x = current.x; corner2.y = current.y - 1;
cittecla 14:9e2ce5880fb0 162 break;
cittecla 14:9e2ce5880fb0 163 case 5: adjacent.x = current.x - 1; adjacent.y = current.y + 1; // top right
cittecla 14:9e2ce5880fb0 164 corner2.y = current.y + 1;
cittecla 14:9e2ce5880fb0 165 break;
cittecla 14:9e2ce5880fb0 166 case 6: adjacent.x = current.x + 1; adjacent.y = current.y + 1; // bottom right
cittecla 14:9e2ce5880fb0 167 corner1.x = current.x + 1;
cittecla 14:9e2ce5880fb0 168 break;
cittecla 14:9e2ce5880fb0 169 case 7: adjacent.x = current.x + 1; adjacent.y = current.y - 1; // bottom left
cittecla 14:9e2ce5880fb0 170 corner2.y = current.y - 1;
cittecla 14:9e2ce5880fb0 171 break;
cittecla 14:9e2ce5880fb0 172 default:
cittecla 14:9e2ce5880fb0 173 printf("Fatal Error, unknown position");
cittecla 14:9e2ce5880fb0 174 break;
cittecla 14:9e2ce5880fb0 175 }
cittecla 14:9e2ce5880fb0 176 if (obstacle_list[adjacent.x][adjacent.y] == 0 && ((obstacle_list[corner1.x][corner1.y] == 0 && obstacle_list[corner2.x][corner2.y] == 0) ||
cittecla 14:9e2ce5880fb0 177 (corner1.x == 0 && corner1.y == 0 && corner2.x == 0 && corner2.y == 0))) {
cittecla 14:9e2ce5880fb0 178 if (open_list[adjacent.x][adjacent.y] != 65535) { // if this adjacent square is already in the closed list ignore it
cittecla 14:9e2ce5880fb0 179 uint16_t g_value = g_list[current.x][current.y] + numb;
cittecla 14:9e2ce5880fb0 180 position diff;
cittecla 14:9e2ce5880fb0 181 diff.x = abs(adjacent.x - b.x);
cittecla 14:9e2ce5880fb0 182 diff.y = abs(adjacent.y - b.y);
cittecla 14:9e2ce5880fb0 183 uint8_t diagonal;
cittecla 14:9e2ce5880fb0 184 if (diff.x > diff.y) {
cittecla 14:9e2ce5880fb0 185 diagonal = diff.x - (diff.x - diff.y);
cittecla 14:9e2ce5880fb0 186 }
cittecla 14:9e2ce5880fb0 187 else {
cittecla 14:9e2ce5880fb0 188 diagonal = diff.y - (diff.y - diff.x);
cittecla 14:9e2ce5880fb0 189 }
cittecla 14:9e2ce5880fb0 190 if (open_list[adjacent.x][adjacent.y] == 0) { // if its not in the open list
cittecla 14:9e2ce5880fb0 191 open_list_count += 1;
cittecla 14:9e2ce5880fb0 192 g_list[adjacent.x][adjacent.y] = g_value; // compute its score, set the parent
cittecla 14:9e2ce5880fb0 193 open_list[adjacent.x][adjacent.y] = g_value + (diff.x * 2 + diff.y * 2 - diagonal);
cittecla 14:9e2ce5880fb0 194 }
cittecla 14:9e2ce5880fb0 195 else { // if its already in the open list
cittecla 14:9e2ce5880fb0 196 if (g_value < g_list[adjacent.x][adjacent.y]) { // test if new path is better
cittecla 14:9e2ce5880fb0 197 g_list[adjacent.x][adjacent.y] = g_value;
cittecla 14:9e2ce5880fb0 198 open_list[adjacent.x][adjacent.y] = g_list[adjacent.x][adjacent.y] + (diff.x * 2 + diff.y * 2 - diagonal);
cittecla 14:9e2ce5880fb0 199 }
cittecla 14:9e2ce5880fb0 200 }
cittecla 14:9e2ce5880fb0 201 }
cittecla 14:9e2ce5880fb0 202 }
cittecla 14:9e2ce5880fb0 203 }
cittecla 14:9e2ce5880fb0 204 }
cittecla 14:9e2ce5880fb0 205
cittecla 14:9e2ce5880fb0 206 /************************************************************************************************************
cittecla 14:9e2ce5880fb0 207 Essential function for getting the next position
cittecla 14:9e2ce5880fb0 208 ************************************************************************************************************/
cittecla 16:4a20536c9bb8 209 static position openList_lowest_F() {
cittecla 14:9e2ce5880fb0 210 uint16_t lowest = row * col;
cittecla 14:9e2ce5880fb0 211 position lowest_pos;
cittecla 14:9e2ce5880fb0 212 for (uint8_t i = 0; i < row; i++) {
cittecla 14:9e2ce5880fb0 213 for (uint8_t j = 0; j < col; j++) {
cittecla 14:9e2ce5880fb0 214 if (open_list[i][j] > 1 && open_list[i][j] < lowest && open_list[i][j] != 65535) {
cittecla 14:9e2ce5880fb0 215 lowest_pos.x = i;
cittecla 14:9e2ce5880fb0 216 lowest_pos.y = j;
cittecla 14:9e2ce5880fb0 217 lowest = open_list[i][j];
cittecla 14:9e2ce5880fb0 218 }
cittecla 14:9e2ce5880fb0 219 }
cittecla 14:9e2ce5880fb0 220 }
cittecla 14:9e2ce5880fb0 221 return lowest_pos;
cittecla 14:9e2ce5880fb0 222 }
cittecla 14:9e2ce5880fb0 223
cittecla 14:9e2ce5880fb0 224 /************************************************************************************************************
cittecla 14:9e2ce5880fb0 225 Essential function for getting the next position
cittecla 14:9e2ce5880fb0 226 ************************************************************************************************************/
cittecla 16:4a20536c9bb8 227 static void mapp_path(position b) {
cittecla 14:9e2ce5880fb0 228 // write path to walkpath array
cittecla 14:9e2ce5880fb0 229
cittecla 14:9e2ce5880fb0 230 position x = b; // lower value
cittecla 14:9e2ce5880fb0 231 position y = b; // higher value
cittecla 14:9e2ce5880fb0 232 counter = 0;
cittecla 14:9e2ce5880fb0 233 walkpath[0] = y;
cittecla 14:9e2ce5880fb0 234 printf("(%d | %d) \n", walkpath[0].x, walkpath[0].y);
cittecla 14:9e2ce5880fb0 235 while (g_list[y.x][y.y] > 1) {
cittecla 14:9e2ce5880fb0 236 counter += 1;
cittecla 14:9e2ce5880fb0 237 position s = b;
cittecla 14:9e2ce5880fb0 238 for (int i = 0; i < 8; i++) {
cittecla 14:9e2ce5880fb0 239 switch (i)
cittecla 14:9e2ce5880fb0 240 {
cittecla 14:9e2ce5880fb0 241 case 0: x.x = y.x - 1; x.y = y.y;
cittecla 14:9e2ce5880fb0 242 break;
cittecla 14:9e2ce5880fb0 243 case 1: x.x = y.x + 1; x.y = y.y;
cittecla 14:9e2ce5880fb0 244 break;
cittecla 14:9e2ce5880fb0 245 case 2: x.x = y.x; x.y = y.y - 1;
cittecla 14:9e2ce5880fb0 246 break;
cittecla 14:9e2ce5880fb0 247 case 3: x.x = y.x; x.y = y.y + 1;
cittecla 14:9e2ce5880fb0 248 break;
cittecla 14:9e2ce5880fb0 249 case 4: x.x = y.x - 1; x.y = y.y - 1;
cittecla 14:9e2ce5880fb0 250 break;
cittecla 14:9e2ce5880fb0 251 case 5: x.x = y.x - 1; x.y = y.y + 1;
cittecla 14:9e2ce5880fb0 252 break;
cittecla 14:9e2ce5880fb0 253 case 6: x.x = y.x + 1; x.y = y.y - 1;
cittecla 14:9e2ce5880fb0 254 break;
cittecla 14:9e2ce5880fb0 255 case 7: x.x = y.x + 1; x.y = y.y + 1;
cittecla 14:9e2ce5880fb0 256 break;
cittecla 14:9e2ce5880fb0 257 default:
cittecla 14:9e2ce5880fb0 258 printf("Fatal Error, unknown position");
cittecla 14:9e2ce5880fb0 259 break;
cittecla 14:9e2ce5880fb0 260 }
cittecla 14:9e2ce5880fb0 261 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 262 s = x;
cittecla 14:9e2ce5880fb0 263 }
cittecla 14:9e2ce5880fb0 264 }
cittecla 14:9e2ce5880fb0 265 y = s;
cittecla 14:9e2ce5880fb0 266 walkpath[counter].x = y.x;
cittecla 14:9e2ce5880fb0 267 walkpath[counter].y = y.y;
cittecla 14:9e2ce5880fb0 268 printf("(%d | %d) \n", walkpath[counter].x, walkpath[counter].y);
cittecla 14:9e2ce5880fb0 269
cittecla 14:9e2ce5880fb0 270 }
cittecla 14:9e2ce5880fb0 271 }
cittecla 14:9e2ce5880fb0 272
cittecla 14:9e2ce5880fb0 273