// 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();
}
// Populates the current MazeNode with the newly discovered intersection
private void populateCurrent(int direction, MazeNode node, bool[] nextIntersection)
{
if (node.explored)
return;
if (nextIntersection[0]) // Left
{
node.neighbors[(direction + 1) % 4] = new MazeNode();
}
if (nextIntersection[1]) // Straight
{
node.neighbors[direction] = new MazeNode();
}
if (nextIntersection[2]) // Right
{
node.neighbors[(direction + 3) % 4] = new MazeNode();
}
// Link back
for (int i = 0; i < 4; i++)
{
if (node.neighbors[i] != null)
{
node.neighbors[i].neighbors[(i + 2) % 4] = node;
}
else
{
Console.WriteLine("AHHH YES :)");
}
}
}
public MazeEdge(int direction, MazeNode node)
{
this.direction = direction;
this.node = node;
}
