File content as of revision 130:670a954495bf:

/**
* Pathfinding function library
* Handels Pathfinding inside the Arena
* Version 3.0.0
**/

/**
* Release notes:
* Storage and run time optimized
* Starts from target to reduce code and run time
**/

/**
* Storage table:
* 80kB  uint16_t    [40000] open_list       (contains open f values as well as closed list value = max of 16 bit = 65535)
* 80kB  uint16_t    [40000] g_list          (contains g values from A to current incl. target, needed for pathtracing)
* 40kB  uint8_t     [40000] obstacle_list   (contains hard surface as well as red and green LEGO stones)
* 2kB   uint8_t     [2000]  walkpath        (contains the path coordinates in a list, [0] = start, [n] = target)
**/

//#include "mbed.h"
#include "Pathfinding.h"
#include <stdio.h>
#include <iostream>




using namespace std;

//global
position walkpath[3 * row] = { 0 };
position target = {0};
position start = {0};

//should be local
static uint16_t open_list[row][col] = { 0 };           // contains open slots marked with their F value (G + H)
static uint16_t g_list[row][col] = { 0 };          // G = A to square (C) in absolut number

static uint16_t open_list_count = 0;
static uint16_t counter;

static int path_found = 45;
static int no_path_possible = 48;


/************************************************************************************************************
Main a_star handling
************************************************************************************************************/
int pathfinding()
{
    define_obst();
    //start = get_current_pos();
    //target.x = 198;     target.y = 198;
    position current = { 0 };   //current pos of the pathfinder

    memset(open_list, 0, sizeof(open_list));
    memset(g_list, 0, sizeof(g_list));
    memset(walkpath, 0, sizeof(walkpath));
    open_list_count = 0;
    counter = 0;

    // check if position isn't reachable
    if (obstacle_list[start.x][start.y] == 1 || start.x > row || start.y > col ||
            obstacle_list[target.x][target.y] == 1 || target.x > row || target.y > col) {
        printf("Fatal Error, no path calculation possible, line 66\r\n");
        return no_path_possible;
    } else {
        position diff;
        diff.x = abs(target.x - start.x);
        diff.y = abs(target.y - start.y);
        uint8_t diagonal;
        if (diff.x > diff.y) {
            diagonal = diff.x - (diff.x - diff.y);
        } else {
            diagonal = diff.y - (diff.y - diff.x);
        }
        open_list[target.x][target.y] = diff.x * 2 + diff.y * 2 - diagonal;
        open_list_count += 1;
        g_list[target.x][target.y] = 1;

        do {

            current = openList_lowest_F();                                  // Get the square with the lowest F score
            open_list[current.x][current.y] = 65535;                        // add position to the closed list; open_list 1111 1111 1111 1111 ^= closed_list = 1
            open_list_count -= 1;

            if (open_list[start.x][start.y] == 65535) {                 // if we added the destination to the closed list, we've found a path
                printf("Path found, line 89\r\n");                     // PATH FOUND
                break;                                                 // break the loop
            }
            calc_F(target, start, current);                             // Calculates F for each neighbour of current lowest F

        } while (!(open_list_count == 0));                              // Continue until there is no more available square in the open list (which means there is no path)
        if (open_list[start.x][start.y] != 65535) {                     // if we added the destination to the closed list, we've found a path
            // no path found
            printf("No Path possible, line 97\r\n");
            print_map(0);
            print_map(1);
            print_map(2);
            return no_path_possible;
        } else {
            // path found
            mapp_path(start);                                           //mapps the path onto the open_list, be replaced by position vector array;
            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);
            return path_found;
        }
    }
}

/************************************************************************************************************
only needed for test, will be done by mapping function, delete for use on robot
************************************************************************************************************/
static void define_obst()
{

    for (int i = 0; i < row; i++) {
        obstacle_list[i][0] = 1;
        obstacle_list[i][col - 1] = 1;
    }

    for (int i = 0; i < row; i++) {
        obstacle_list[0][i] = 1;
        obstacle_list[row - 1][i] = 1;
    }

}


/************************************************************************************************************
Essential function for calculating the distance
************************************************************************************************************/
static void calc_F(position a, position b, position current)
{

    position adjacent = current;
    position corner1 = { 0 }, corner2 = { 0 };
    uint16_t numb = 2;
    for (int i = 0; i < 8; i++) {
        switch (i) {
            case 0:
                adjacent.x = current.x - 1;
                adjacent.y = current.y; // top
                break;
            case 1:
                adjacent.x = current.x + 1;
                adjacent.y = current.y; // bottom
                break;
            case 2:
                adjacent.x = current.x;
                adjacent.y = current.y - 1; // left
                break;
            case 3:
                adjacent.x = current.x;
                adjacent.y = current.y + 1; // right
                break;
            case 4:
                adjacent.x = current.x - 1;
                adjacent.y = current.y - 1;
                numb = 3;                               // top left
                corner1.x = current.x - 1;
                corner1.y = current.y;
                corner2.x = current.x;
                corner2.y = current.y - 1;
                break;
            case 5:
                adjacent.x = current.x - 1;
                adjacent.y = current.y + 1;             // top right
                corner2.y = current.y + 1;
                break;
            case 6:
                adjacent.x = current.x + 1;
                adjacent.y = current.y + 1;             // bottom right
                corner1.x = current.x + 1;
                break;
            case 7:
                adjacent.x = current.x + 1;
                adjacent.y = current.y - 1;             // bottom left
                corner2.y = current.y - 1;
                break;
            default:
                printf("Fatal Error, unknown position\r\n");
                break;
        }
        if (obstacle_list[adjacent.x][adjacent.y] == 0 && ((obstacle_list[corner1.x][corner1.y] == 0 && obstacle_list[corner2.x][corner2.y] == 0) ||
        (corner1.x == 0 && corner1.y == 0 && corner2.x == 0 && corner2.y == 0))) {
            if (open_list[adjacent.x][adjacent.y] != 65535) {                       // if this adjacent square is already in the closed list ignore it
                uint16_t g_value = g_list[current.x][current.y] + numb;
                position diff;
                diff.x = abs(adjacent.x - b.x);
                diff.y = abs(adjacent.y - b.y);
                uint8_t diagonal;
                if (diff.x > diff.y) {
                    diagonal = diff.x - (diff.x - diff.y);
                } else {
                    diagonal = diff.y - (diff.y - diff.x);
                }
                if (open_list[adjacent.x][adjacent.y] == 0) {                       // if its not in the open list
                    open_list_count += 1;
                    g_list[adjacent.x][adjacent.y] = g_value;                       // compute its score, set the parent
                    open_list[adjacent.x][adjacent.y] = g_value + (diff.x * 2 + diff.y * 2 - diagonal);
                } else {                                            // if its already in the open list
                    if (g_value < g_list[adjacent.x][adjacent.y]) {             // test if new path is better
                        g_list[adjacent.x][adjacent.y] = g_value;
                        open_list[adjacent.x][adjacent.y] = g_list[adjacent.x][adjacent.y] + (diff.x * 2 + diff.y * 2 - diagonal);
                    }
                }
            }
        }
    }
}

/************************************************************************************************************
Essential function for getting the next position
************************************************************************************************************/
static position openList_lowest_F()
{
    uint16_t lowest = row * col;
    position lowest_pos;
    for (uint8_t i = 0; i < row; i++) {
        for (uint8_t j = 0; j < col; j++) {
            if (open_list[i][j] > 1 && open_list[i][j] < lowest && open_list[i][j] != 65535) {
                lowest_pos.x = i;
                lowest_pos.y = j;
                lowest = open_list[i][j];
            }
        }
    }
    return lowest_pos;
}

/************************************************************************************************************
Essential function for getting the next position
************************************************************************************************************/
static void mapp_path(position b)
{
    // write path to walkpath array

    position x = b; // lower value
    position y = b; // higher value
    counter = 0;
    walkpath[0] = y;
    printf("(%d | %d) \n", walkpath[0].x, walkpath[0].y);
    while (g_list[y.x][y.y] > 1) {
        counter += 1;
        position s = b;
        for (int i = 0; i < 8; i++) {
            switch (i) {
                case 0:
                    x.x = y.x - 1;
                    x.y = y.y;
                    break;
                case 1:
                    x.x = y.x + 1;
                    x.y = y.y;
                    break;
                case 2:
                    x.x = y.x;
                    x.y = y.y - 1;
                    break;
                case 3:
                    x.x = y.x;
                    x.y = y.y + 1;
                    break;
                case 4:
                    x.x = y.x - 1;
                    x.y = y.y - 1;
                    break;
                case 5:
                    x.x = y.x - 1;
                    x.y = y.y + 1;
                    break;
                case 6:
                    x.x = y.x + 1;
                    x.y = y.y - 1;
                    break;
                case 7:
                    x.x = y.x + 1;
                    x.y = y.y + 1;
                    break;
                default:
                    printf("Fatal Error, unknown position\r\n");
                    break;
            }
            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]) {
                s = x;
            }
        }
        y = s;
        walkpath[counter].x = y.x;
        walkpath[counter].y = y.y;
        printf("(%d | %d) \n", walkpath[counter].x, walkpath[counter].y);

    }
}

//******************************************************************************
/**
 * prints map defined by list
 * 0 = obstacle_list
 * 1 = open_list
 * 2 = target_list
 * very slow, shouldn't be called in final code
 * by Claudio Citterio
**/
void print_map(int list)
{
    // Debug function for printing the obstacle matrix on putty. 
    for (int y = 0; y < col; y++) {
        for (int x = 0; x < row; x++) {
            if(list == 0)printf("%d ", obstacle_list[y][x]);
            if(list == 1)printf("%d ", open_list[y][x]);
            if(list == 2)printf("%d ", target_list[y][x]);
        }
        printf("\r\n");
    }
    printf("\r\n");
}