// 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; }