Robot Navigation(Artificial Intelligence – Breadth-First Search Algorithmns C++

The output is not showing full navigation direction/path using the below code:

Output

Please help me to fix the code and show me where it is wrong at.

vector<Cardinal> BFS(Agent& aAgent)
{
    vector<CoordinatePath> lDetermined; // Set of determined nodes, contains path to given node from Agent starting point.
    queue<Coordinate> lNodeQueue; // Queue of nodes left to assess.

    lDetermined.push_back(CoordinatePath(aAgent.getCoordinate(), vector<Cardinal>()));

    if (aAgent.validMovement(UP))
    {
        vector<Cardinal> lPathUp;
        lPathUp.push_back(UP);
        Coordinate lCoordinateUp = aAgent.getCoordinate().getUp();

        // Check if determined node has been visited
        bool lVisited = false;
        for (int i = 0; i < lDetermined.size(); i++)
        {
            if (lCoordinateUp == lDetermined[i].getCoordinate())
            {
                lVisited = true;
                break;
            }
        }

        if (!lVisited)
        {
            // if not visited add node to queue and determined list.
            lNodeQueue.push(lCoordinateUp);
            lDetermined.push_back(CoordinatePath(lCoordinateUp, lPathUp));
        }
    }

    if (aAgent.validMovement(LEFT))
    {
        vector<Cardinal> lPathTemp;
        lPathTemp.push_back(LEFT);
        Coordinate lCoordinate = aAgent.getCoordinate().getLeft();

        // Check if determined node has been visited
        bool lVisited = false;
        for (int i = 0; i < lDetermined.size(); i++)
        {
            if (lCoordinate == lDetermined[i].getCoordinate())
            {
                lVisited = true;
                break;
            }
        }

        if (!lVisited)
        {
            // if not visited add node to queue and determined list.
            lNodeQueue.push(lCoordinate);
            lDetermined.push_back(CoordinatePath(lCoordinate, lPathTemp));
        }
    }

    if (aAgent.validMovement(DOWN))
    {
        vector<Cardinal> lPathTemp;
        lPathTemp.push_back(DOWN);
        Coordinate lCoordinate = aAgent.getCoordinate().getDown();

        // Check if determined node has been visited
        bool lVisited = false;
        for (int i = 0; i < lDetermined.size(); i++)
        {
            if (lCoordinate == lDetermined[i].getCoordinate())
            {
                lVisited = true;
                break;
            }
        }

        if (!lVisited)
        {
            // if not visited add node to queue and determined list.
            lNodeQueue.push(lCoordinate);
            lDetermined.push_back(CoordinatePath(lCoordinate, lPathTemp));
        }
    }

    if (aAgent.validMovement(RIGHT))
    {
        vector<Cardinal> lPathTemp;
        lPathTemp.push_back(RIGHT);
        Coordinate lCoordinate = aAgent.getCoordinate().getRight();

        // Check if determined node has been visited
        bool lVisited = false;
        for (int i = 0; i < lDetermined.size(); i++)
        {
            if (lCoordinate == lDetermined[i].getCoordinate())
            {
                lVisited = true;
                break;
            }
        }

        if (!lVisited)
        {
            // if not visited add node to queue and determined list.
            lNodeQueue.push(lCoordinate);
            lDetermined.push_back(CoordinatePath(lCoordinate, lPathTemp));
        }
    }

    while (!lNodeQueue.empty())
    {
        vector<Cardinal> lPath; // Path, used for backtracking.
        // Get path to next node.
        Coordinate lNext = lNodeQueue.front();
        lNodeQueue.pop();

        for (int i = 0; i < lDetermined.size(); i++)
            if (lDetermined[i].getCoordinate() == lNext)
                lPath = lDetermined[i].getPath();

        // Move to next node.
        for (int i = 0; i < lPath.size(); i++)
            aAgent.move(lPath[i]);

        // Check if at goal.
        if (aAgent.atGoal())
            return lPath;

        // Load connected nodes into queue.
        if (aAgent.validMovement(UP))
        {
            vector<Cardinal> lPathUp = lPath;
            lPathUp.push_back(UP);
            Coordinate lCoordinateUp = aAgent.getCoordinate().getUp();

            // Check if determined node has been visited
            bool lVisited = false;
            for (int i = 0; i < lDetermined.size(); i++)
            {
                if (lCoordinateUp == lDetermined[i].getCoordinate())
                {
                    lVisited = true;
                    break;
                }
            }

            if (!lVisited)
            {
                // if not visited add node to queue and determined list.
                lNodeQueue.push(lCoordinateUp);
                lDetermined.push_back(CoordinatePath(lCoordinateUp, lPathUp));
            }
        }

        if (aAgent.validMovement(LEFT))
        {
            vector<Cardinal> lPathTemp = lPath;
            lPathTemp.push_back(LEFT);
            Coordinate lCoordinate = aAgent.getCoordinate().getLeft();

            // Check if determined node has been visited
            bool lVisited = false;
            for (int i = 0; i < lDetermined.size(); i++)
            {
                if (lCoordinate == lDetermined[i].getCoordinate())
                {
                    lVisited = true;
                    break;
                }
            }

            if (!lVisited)
            {
                // if not visited add node to queue and determined list.
                lNodeQueue.push(lCoordinate);
                lDetermined.push_back(CoordinatePath(lCoordinate, lPathTemp));
            }
        }

        if (aAgent.validMovement(DOWN))
        {
            vector<Cardinal> lPathTemp = lPath;
            lPathTemp.push_back(DOWN);
            Coordinate lCoordinate = aAgent.getCoordinate().getDown();

            // Check if determined node has been visited
            bool lVisited = false;
            for (int i = 0; i < lDetermined.size(); i++)
            {
                if (lCoordinate == lDetermined[i].getCoordinate())
                {
                    lVisited = true;
                    break;
                }
            }
            if (!lVisited)
            {
                // if not visited add node to queue and determined list.
                lNodeQueue.push(lCoordinate);
                lDetermined.push_back(CoordinatePath(lCoordinate, lPathTemp));
            }
        }

        if (aAgent.validMovement(RIGHT))
        {
            vector<Cardinal> lPathTemp = lPath;
            lPathTemp.push_back(RIGHT);
            Coordinate lCoordinate = aAgent.getCoordinate().getRight();

            // Check if determined node has been visited
            bool lVisited = false;
            for (int i = 0; i < lDetermined.size(); i++)
            {
                if (lCoordinate == lDetermined[i].getCoordinate())
                {
                    lVisited = true;
                    break;
                }
            }
            if (!lVisited)
            {
                // if not visited add node to queue and determined list.
                lNodeQueue.push(lCoordinate);
                lDetermined.push_back(CoordinatePath(lCoordinate, lPathTemp));
            }
        }

        // Return to starting point.
        for (int i = lPath.size() - 1; i >= 0; i--)
        {
            switch (lPath[i])
            {
            case UP:
                aAgent.move(DOWN);
                break;
            case LEFT:
                aAgent.move(RIGHT);
                break;
            case DOWN:
                aAgent.move(UP);
                break;
            case RIGHT:
                aAgent.move(LEFT);
                break;
            }
        }
    }

    return vector<Cardinal>();
}

Leave a Comment