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>();
}