Diagonal Solver Pseudo Code

This part of the series on creating a diagonal micromouse maze solver presents the actual pseudo code that was used to implement a diagonal solver and it closely matches the C implementation. You will want to check out some of the data structures in the previous post (Diagonal Solver Data Structures).

Initialization on starting a flood:

ExpandQueue = {StartCell, CurrentDirection};
If (StartCell has no back wall)
{
  AddExpandQueue (StartCell, Opposite(CurrentDirection))
}

All elements of MoveCost Array = {None, MAXCOST}
MinCost = MAXCOST;
MinDir = CurrentDirection;

Flood:

While (ExpandQueue != EMPTY)
{
  // Get coordinate and direction of next cell to examine
  Cell = GetExpandQueue()

  // Since we just exited a turn, this is the start of a straight
  StraightLength = 0;

  If (Ortho(Cell)) {
    While (!OrthoFrontWall(Cell)) {
      If ((Cell.coordinate == DestinationCell) &&
          (MinCost > MoveCostArray[Cell.coordinate][Cell.direction].cost)) {
        MinCost = MoveCostArray[Cell.coordinate][Cell.direction].cost;
        MinDir = Cell.direction;
      }

      TrySD45Turn(Cell);
      TrySS90Turn(Cell);
      TrySD135Turn(Cell);
      TrySS180Turn(Cell);

      // Compute the cost of moving forward one cell
      StraightLength++;

      CostTemp = MoveCostArray[Cell.coordinate][Cell.direction].cost +
                 OrthoFwdDeltaCost[StraightLength];

      FwdCell = GetOrthoFwdCellCoord(Cell);

      If(MoveCostArray[FwdCell.coordinate][FwdCell.direction].cost > CostTemp) {
        MoveCostArray[FwdCell.coordinate][FwdCell.direction].cost = CostTemp;
        MoveCostArray[FwdCell.coordinate][FwdCell.direction].move = FWD;
        AddExpandQueue (FwdCell);
      }

      Cell = FwdCell;
    }
  }
  Else {
    While (!DiagFrontWall (Cell)) {
      If ((Cell.coordinate == DestinationCell) &&
      (MinCost > MoveCostArray[Cell.coordinate][Cell.direction].cost)) {
        MinCost = MoveCostArray[Cell.coordinate][Cell.direction].cost;
        MinDir = Cell.direction;
      }

      TryDiag45CellExpand(Cell);
      TryDiag90CellExpand(Cell);
      TryDiag135CellExpand(Cell);

      // Compute the cost of moving forward one cell
      StraightLength++;

      CostTemp = MoveCostArray[Cell.coordinate][Cell.direction].cost +
                 DiagFwdDeltaCost[StraightLength];

      FwdCell = GetDiagFwdCellCoord(Cell);

      If(MoveCostArray[FwdCell.coordinate][FwdCell.direction].cost > CostTemp) {
        MoveCostArray[FwdCell.coordinate][FwdCell.direction].cost = CostTemp;
        MoveCostArray[FwdCell.coordinate][FwdCell.direction].move = DIAG_FWD;
        AddExpandQueue (FwdCell);
      }

      Cell = FwdCell;
    }
  }
}

// If there is a path, then MoveCostArray[DestinationCell][MinDir] 
// has the last move of the fastest path
// between the StartCell and the DestinationCell.
// You follow the moves backwards from MoveCostArray[DestinationCell][MinDir].move 
// to the start.

Example of a Try*CellExpand(parameter) routine is shown below. The basics of all these routines is check whether expanding with the specified type of turn will result in another move or not and if it will result in another move, then add it to ExpandQueue. See “Flooding Details” for more information.

  TryRtSD45Turn(Cell)
  {
    Wall1 = DiagRt45Wall1st(Cell);
    Wall2 = DiagRt45Wall2nd(Cell);

    If(!Wall1  && !Wall2) {
      // Cost of getting to here.
      CostTemp = MoveCostArray[Cell.coordinate][Cell.direction] .cost + CostTurnSD45;

      // Cell coordinate and direction after doing at SD45 turn from Cell.
      CellTemp = DiagRt45(Cell);

      // If this path is faster than any previous path, then update the cost and move info.
      If(MoveCostArray[CellTemp.coordinate][CellTemp.direction].cost > CostTemp) {
        MoveCostArray[CellTemp.coordinate][CellTemp.direction].cost = CostTemp;
        MoveCostArray[CellTemp.coordinate][CellTemp.direction].move = SDRt45;
        AddExpandQueue (CellTemp);
      }
    }
  }

TrySD45Turn(Cell)
{
  TryRtSD45Turn(Cell);
  TryLtSD45Turn(Cell);
}

NOTE: Diagonal cell expand routines are different than the Orthogonal cell expand in that the turn direction is a function of which edge is being expanded. See Flooding for legal move sequences.

Ortho(parameter) is a routine that checks whether the parameter.coordinate is the center of a square or an edge.

OrthoFrontWall(parameter) and DiagFrontWall(parameter) are routines that check whether the parameter.coordinate has a front wall in the direction parameter.direction.

GetOrthoFwdCellCoord(parameter) and GetDiagFwdCellCoord(parameter) are routines that return the coordinates for the next square in the cell.direction.

DiagRt45(parameter) is a routine that returns the cell coordinate and direction after doing at SD45 turn from parameter.coordinate and parameter.direction.