Navigator.cpp

#include "Navigator.h"
#include "Debugger.h"
#include "StringHelper.h"
//#include "MapWindow.h"

using namespace std;

Navigator::Navigator(MapWindow * mapWindow) {
    world = mapWindow;
}

Path Navigator::findPath(Vector2 startpos, Vector2 destinationPos) {
    Debugger::print("Starting with pos:" + toString(startpos.x) + "," + toString(startpos.y) + "\n");
    //initialization
    //Define variables
    vector<Node> nodes;//all the frontier nodes with more paths to explore
    int x = startpos.x, y = startpos.y;
    //If already at goal, ezpz
    if (x == (int)(destinationPos.x) && y == (int)(destinationPos.y)) {
        nodes.push_back(Node((int)destinationPos.x, (int)destinationPos.y, 0));
        return Path(nodes);
    }
    //more variables
    vector<vector<Node>> referenceNodes; //For storing where the node was reached from
    vector<vector<bool>> temp;
    flags = temp;
    for (size_t y = 0; y < world->getMap()->tiles.size(); y++) {
        vector<bool> tempb;
        vector<Node> tempn;
        flags.push_back(tempb);
        referenceNodes.push_back(tempn);
        for (size_t x = 0; x < world->getMap()->tiles[y].size(); x++) {
            flags[y].push_back(false);
            referenceNodes[y].push_back(Node(x,y,0));
        }
    }
    //Flag start position as explored, dont add to que.
    flags[startpos.y][startpos.x] = true;

    //Debugger::print("initialized node checkpoints:" + toString(nodes[0].checkpoints.size()) + "\n");
    nodes = findUnexploredSurroundingPaths(Vector2(x,y)); //Start out with the nodes around the start position;
    for (int i = 0; i < nodes.size(); i++) {//Flag initial nodes
        Debugger::print("Flagging:" + toString(nodes[i].pos.x) + ";" + toString(nodes[i].pos.y) + "  ");
        flags[nodes[i].pos.y][nodes[i].pos.x] = true;
        if (nodes[i].pos.x == (int)destinationPos.x && nodes[i].pos.y == (int)destinationPos.y) { //If one is the destination, return as vector
            Debugger::print("\nDestination was adjacent\n");
            vector<Node> n;
            n.push_back(nodes[i]);
            return Path(n);
        }
    }
    Debugger::print("\n");

    int counter = 0;
    while (nodes.size() > 0 && counter < 10000) { //while there are still more frontiers to explore
        Node current = findShortestNode(nodes, true); //Select the shortest frontier
        Debugger::print("Current:" + toString(current.pos.x) + "," + toString(current.pos.y) + "   Goal:" + toString((int)destinationPos.x) + "," + toString((int)destinationPos.y) + "\n");
        //Debugger::print("Current node has size: " + toString(current.size) + " and position: " + toString(current.pos.x) + "," + toString(current.pos.y) + " and length " + toString(current.checkpoints.size()) + "\n");
        vector<Node> next = findUnexploredSurroundingPaths(current.pos); //Find best direction to advance
        for (int i = 0; i < next.size(); i++) {
            Debugger::print("Next" + toString(i) + ":" + toString(next[i].pos.x) + "," + toString(next[i].pos.y) + " ");
            //Manage the reference node
            Node * refCur = &referenceNodes[(size_t)current.pos.y][(size_t)current.pos.x];
            Node * refNext = &referenceNodes[(size_t)next[i].pos.y][(size_t)next[i].pos.x];
            //Set where it was explored from
            refNext->previousNode = refCur;
            //Set total size
            refNext->size = world->getMap()->getTravelWeightAt(refNext->pos.x, refNext->pos.y) + refCur->size;
            //Create node on frontier
            nodes.push_back(refNext->clone());
            //Flag the position as explored
            flags[next[i].pos.y][next[i].pos.x] = true;

            //Debugger::print("Returned node has size: " + toString(next.size) + " and position: " + toString(next.pos.x) + "," + toString(next.pos.y) + " and length " + toString(next.checkpoints.size()) + "\n");
            //Debugger::print("Checkpoints are: ");
            //for (int i = 0; i < next.checkpoints.size(); i++) {
            //  Debugger::print(toString(next.checkpoints[i].pos.x) + "," + toString(next.checkpoints[i].pos.y) + " ");
            //}
            //Debugger::print("\n");

            //If the explored node is the goal
            if (next[i].pos.x == (int)destinationPos.x && next[i].pos.y == (int)destinationPos.y) {
                //create a vector containing the explored path
                int i = 1;
                vector<Node> path;
                path.insert(path.begin(), *refNext);
                do {
                    Debugger::print("Bulding path: " + toString(i) + "\n");
                    refNext = refNext->previousNode; //If the path before that node does not have a path, stop
                    path.insert(path.begin(), *refNext); //Insert current node
                    i++;
                } while (refNext->previousNode != NULL);
                //return the path
                Debugger::print("\nFound path: " + toString(path.size()) + "\n");
                Debugger::print("\n");
                Debugger::print("\n");
                Debugger::print("\n");
                Debugger::print("\n");
                return Path(path);
            }
            //Create reference node
        }
        Debugger::print("\n");
        //Node now explored, erase
        Debugger::print("Position to be erased: " + toString(cIndex) + " contains:" + toString(nodes[cIndex].pos.x) + "," + toString(nodes[cIndex].pos.y) + "\n");
        nodes.erase(nodes.begin() + cIndex);
        //Go to new frontier

        //counter++;
        for (int i = 0; i < flags.size(); i++) {
            for (int j = 0; j < flags[i].size(); j++) {
                Debugger::print(toString(flags[i][j]));
            }
            Debugger::print("\n");
        }
        Debugger::print("\n");
    }

    //If there is no path
    Debugger::print("No path... Frontier size: "+ toString(nodes.size()) + "\n");

    Debugger::print("\n");
    Debugger::print("\n");
    Debugger::print("\n");
    Debugger::print("\n");
    vector<Node> start;
    start.push_back(Node(startpos.x, startpos.y, 0));
    return Path(start);

}

Node Navigator::findShortestNode(vector<Node> nodes, bool frontierNodes) {
    //Look through all nodes and return the smallest. If it is the frontier node list, also note cIndex, to mark it for erasion if empty frontier
    //(When searching for 'current')
    //Compare to first
    Node smallestN = nodes[0];
    if(frontierNodes) cIndex = 0;
    for (size_t i = 1; i < nodes.size(); i++) {//for each other element in frontier
        if (nodes[i].size < smallestN.size) { //If size is smaller, set as prefered
            smallestN = nodes[i];
            if (frontierNodes) cIndex = i;
        }
    }
    return smallestN;
}
vector<Node> Navigator::findUnexploredSurroundingPaths(Vector2 pos) {
    //Find the surrounding paths, that are not flagged as explored
    vector<Node> steps;
    if (pos.x - 1 >= 0 //index x cant be below zero
        && !flags[pos.y][pos.x - 1]) { //Flag must be false
        steps.push_back(Node((int)(pos.x - 1), (int)pos.y, world->getMap()->getTravelWeightAt((int)(pos.x - 1), (int)pos.y))); //Add node with position and step weight
    }
    if (pos.x + 1 < flags[pos.y].size() //Index x cant be above the size of the y array at pos
        && !flags[pos.y][pos.x + 1]) { //Flag must be false
        steps.push_back(Node((int)(pos.x + 1), (int)pos.y, world->getMap()->getTravelWeightAt((int)(pos.x + 1), (int)pos.y))); //Add node with position and step weight
    }
    if (pos.y - 1 >= 0 && pos.x < flags[pos.y - 1].size() //Index y cant be above 0, and index cant be above the size of the y array at pos
        && !flags[pos.y - 1][pos.x]) { //Flag must be false
        steps.push_back(Node((int)pos.x, (int)(pos.y - 1), world->getMap()->getTravelWeightAt((int)pos.x, (int)(pos.y - 1)))); //Add node with position and step weight
    }
    if (pos.y + 1 < flags.size() && pos.x < flags[pos.y + 1].size() //Pos y must be below the amount of y's and index x cant be above the size of the y array at pos
        && !flags[pos.y + 1][pos.x]) { //Flag must be false
        steps.push_back(Node((int)pos.x, (int)(pos.y + 1), world->getMap()->getTravelWeightAt((int)pos.x, (int)(pos.y + 1)))); //Add node with position and step weight
    }
    return steps;
}
Node Navigator::findShortestSurroundingPaths(Node n) {
    //find unexplored surrounding paths
    vector<Node> surroundingPaths = findUnexploredSurroundingPaths(n.pos);

    //Find the shortest of them
    Node shortest = findShortestNode(surroundingPaths);
    //Debugger::print("Shortest path has size: " + toString((int)(shortest.size)) + "\n");
    return shortest;
}