private int move(MazeEdge current, MazeEdge next)
{
// Still going forward, could add flag for robot stopped?
if (current.direction == next.direction && !robotStopped())
return current.direction;
int intersectionGoal = 1;
MazeNode nextNode = next.node;
for (int i = 0; nextNode != null; i++)
{
if (!nextNode.explored)
{
intersectionGoal = 0; // Unlimited, til dead end
}
nextNode = nextNode.neighbors[next.direction];
}
goRotate((next.direction - current.direction + 4) % 4);
goStraightAsync(intersectionGoal);
/*
dest-origin+4 % 4 == 0: no turn
dest-origin+4 % 4 == 1: left turn -90
dest-origin+4 % 4 == 2: turn around -180
dest-origin+4 % 4 == 3: right turn 90
*/
return next.direction;
}
// Finds the next edge to explore based on the current edge the robot is moving on.
private void getNextEdge(MazeEdge current)
{
if (current.nextEdge != null) return;
// Search N S E W
int i = current.direction;
do
{
// If we have an unexplored edge then we have a winner.
if (current.node.neighbors[i] != null && !current.node.neighbors[i].explored)
{
MazeEdge next = new MazeEdge(i, current.node.neighbors[i]);
next.prevEdge = current;//move
current.nextEdge = next;//move
return;
}
i = (i + 1) % 4;
}
while (i != current.direction);
}
// Main maze search method. Performs a DFS search looking for the goal node.
private void mazeSearch()
{
// Init start node with an unknown node to the north.
MazeNode start = new MazeNode();
start.explored = true;
start.neighbors[MazeNode.NORTH] = new MazeNode();
start.neighbors[MazeNode.NORTH].neighbors[MazeNode.SOUTH] = start;
MazeEdge pathHead = new MazeEdge(MazeNode.NORTH, start);
MazeEdge current = pathHead;
// Move toward the unknown node
goStraightAsync(0);
while (running)
{
getNextEdge(current);
if (current.nextEdge != null)
{
worker.ReportProgress(3,"Current node(moving away from): id=" + current.node.thisId);
// Move to next physically
move(current, current.nextEdge); // Will do nothing if the two directions are the same. Will halt
current.direction = current.nextEdge.direction; // Cause its moving toward the next
current = current.nextEdge;
// Waits for an intersection
bool[] isect = getNextIntersection(lastPacketFilter);
if (isect == null)
{
break;
}
// Add newly discovered edges
populateCurrent(current.direction, current.node, isect);
current.node.explored = true;
}
else // need to backtrack
{
current.prevEdge.direction = (current.prevEdge.direction + 2) % 4; // Flip previous direction
//move(current.direction, current.prevEdge.direction); // Should block until robot hits dead end
current.nextEdge = current.prevEdge; // Backtrack one edge by setting next to the previous edge
current.prevEdge.nextEdge = null; // Start fresh
// So now current is sand, facing south. getNextEdge will now return red instead of null like it did previously
// current = sand; south
// next = red; north (with no next)
//
}
}
Console.WriteLine("MAZE SEARCH ENDED!!!");
stop();
}
