Shortest path (GBFS/A*)

program MazeSolver;
{.$I SRL/OSR.simba}
{$R-}


type
  EDistanceHeuistics = (dhEuclidean, dhSqaredEuclidean, dhManhattan, dhChebyshev);

  TPathFinder = record
    Image: TImage;
    Color: TColor;
    Eightway: Boolean;

    Heuristics: EDistanceHeuistics;
    HeuristicsMultiplier: Single;

    BFSMeasureBacktrace: Boolean;
  end;

  // home brewed search structure for the heuristic BFS lookup
  // maintains order. High cost of insertion O(n). But O(1) pop.
  // Used as an alternative to Min heap for this exact purpose.
  TPathStruct = record
    Node: TPoint;
    Hdist: single;
    Rdist: single;
  end;

  TSearchArray = record
    lo, hi: Int32;
    data: array of TPathStruct;
  end;


procedure TSearchArray.Init(sz: Int32);
begin
  SetLength(Self.data, sz);

  Self.hi := -1;
  Self.lo := 0;
end;

// find the current value or the closest value in the array.
// Time complexity: O(log n)
function TSearchArray.SearchClosest(value: single): Int32;
var
  l, h: Int32;
begin
  l := Self.Lo;
  h := Self.Hi;

  while l <= h do
  begin
    Result := (l + h) div 2;
    if Self.Data[Result].hdist < value then
      h := Result - 1
    else if Self.Data[Result].hdist > value then
      l := Result + 1
    else
      Exit(Result);
  end;
end;

// Insert the a value into it's correct position by searching.
// Time complexity: O(n)
// For this usage we are looking at time complexity in range of
// average O(log n) from the binary search, few moves in practice
function TSearchArray.Insert(_node:TPoint; _hdist, _rdist: single): Int32;
var
  idx,i: Int32;
begin
  if (Self.hi = -1) or (Self.Data[Self.hi].hdist >= _hdist) then
    idx := self.hi + 1
  else if (Self.hi >= 5) and (_hdist <= Self.Data[Self.hi-5].hdist) then
  begin
    for idx := self.hi+1 downto Self.hi-5 do
    begin
      Self.data[idx] := Self.data[idx-1];
      if Self.data[idx].hdist >= _hdist then break;
    end;
  end else begin
    idx := Self.SearchClosest(_hdist);
    if Self.Data[idx].hdist > _hdist then Inc(idx);

    if (self.hi+1-idx) > 0 then
      Move(Self.data[idx], Self.data[idx+1], (self.hi+1-idx)*(SizeOf(TPathStruct)));
  end;

  with Self.data[idx] do
  begin
    hdist := _hdist;
    rdist := _rdist;
    node  := _node;
  end;

  Inc(Self.hi);
end;

// Pop the largest or smallest value depending on insertion order
// Time complexity: O(1)
procedure TSearchArray.Pop(out _node:TPoint; out _hdist, _rdist: single);
begin
  with self.data[self.hi] do
  begin
    _node  := node;
    _hdist := hdist;
    _rdist := rdist;
  end;

  Dec(Self.Hi);
end;

function TSearchArray.AsString(): String;
var
  tmp: array of TPathStruct;
begin
  tmp := self.data;
  SetLength(tmp, self.hi+1);
  Result := ToString(tmp);
end;


function Euclidean(p,q: TPoint): Single;   begin Result := Sqrt(Sqr(p.x-q.x) + Sqr(p.y-q.y)); end;
function SqEuclidean(p,q: TPoint): Single; begin Result := Sqr(p.x-q.x) + Sqr(p.y-q.y); end;
function Manhattan(p,q: TPoint): Single;   begin Result := Abs(p.x-q.x) + Abs(p.y-q.y); end;
function Chebyshev(p,q: TPoint): Single;   begin Result := Max(Abs(p.x-q.x), Abs(p.y-q.y)); end;



(*
  Returns the (approximate) shortest path from start to stop, appximcation depends on the paramters.

  * Start: Has to be a valid pixel that is Self.Color;
  * Stop:  Has to be a valid pixel that is Self.Color;

  Use TPathFinder.Init to set these parameters for the search:

  * Eightway: Floodfill in eight of four directions.
  * Heuristics:
      0: Euclidian distance
      1: Squared euclidian distance
      2: Manhattan distance
      3: Chebyshev distance
  * HeuristicsScale: A mutiplier for the distance heuristics allowing for more appxiate paths (faster!)
  * BFSMeasureBacktrace: Adds a distance value to the map for (often) more accurate backtrace, specially useful
    when you are using sqrared euclidian distance or in the case of HeuristicsScale greater than 1.


  Note: Default parameters will result in shortest possible path, but is much slower than using any form
        of heuristics. Adding some scaling can result in runtime being up to 10 times faster.

  Algorithm is built around the principles of greedy BFS, but extended to match that of an
  A* algorithm under default parameters which ensures actual shortest path. And further if multiplier is set to `0`
  it will act as dijkstras.


  ```pascal
  procedure TPathfinder.Init(AImage: TImage; AColor: TColor; AEightway:Boolean; AHeuristics:EDistanceHeuistics; AHeuristicsMultiplier:Single=1; AMeasureBacktrace:Boolean=False);
  procedure TPathfinder.InitAStar(AImage: TImage; AColor: TColor; AEightway: Boolean);
  procedure TPathfinder.InitDijkstras(AImage: TImage; AColor: TColor; AEightway: Boolean);

  function TPathfinder.FloodFillMatrix(Start, Stop: TPoint): TSingleMatrix;
  function TPathfinder.Backtrace(Matrix: TSingleMatrix; Start, Stop: TPoint): TPointArray;
  function TPathfinder.Solve(Start, Stop: TPoint): TPointArray;
  ```
*)
procedure TPathfinder.Init(AImage: TImage; AColor: TColor; AEightway:Boolean; AHeuristics:EDistanceHeuistics; AHeuristicsMultiplier:Single=1; AMeasureBacktrace:Boolean=False);
begin
  Self.Image                := AImage;
  Self.Color                := AColor;
  Self.BFSMeasureBacktrace  := AMeasureBacktrace;
  Self.Eightway             := AEightway;
  Self.HeuristicsMultiplier := AHeuristicsMultiplier;
  Self.Heuristics           := AHeuristics;
end;

procedure TPathfinder.InitAStar(AImage: TImage; AColor: TColor; AEightway: Boolean);
begin
  Self.Image                := AImage;
  Self.Color                := AColor;
  Self.BFSMeasureBacktrace  := False;
  Self.Eightway             := AEightway;
  Self.HeuristicsMultiplier := 1;
  Self.Heuristics           := EDistanceHeuistics.dhEuclidean;
end;

procedure TPathfinder.InitDijkstras(AImage: TImage; AColor: TColor; AEightway: Boolean);
begin
  Self.Image                := AImage;
  Self.Color                := AColor;
  Self.BFSMeasureBacktrace  := False;
  Self.Eightway             := AEightway;
  Self.HeuristicsMultiplier := 0;
  Self.Heuristics           := EDistanceHeuistics.dhManhattan;
end;


(*
  Floodfills a matrix from `start` until it hits `stop`, with incrementing values appeoximating the
  distance from start.
  Can be used for backtracing and visual purposes to show a distance matrix from start to stop.
*)
function TPathfinder.FloodFillMatrix(Start, Stop: TPoint): TSingleMatrix;
var
  score,hdist,a: single;
  arr: TSearchArray;
  p,q: TPoint;
  adj: TPointArray = [[-1,0],[1,0],[0,-1],[0,1],[1,-1],[1,1],[-1,-1],[-1,1]];
  h:function(p,q: TPoint): Single;
begin
  case Self.Heuristics of
    dhEuclidean:       h := @Euclidean;
    dhSqaredEuclidean: h := @SqEuclidean;
    dhManhattan:       h := @Manhattan;
    dhChebyshev:       h := @Chebyshev;
    else               h := @Euclidean;
  end;

  arr.Init(Self.Image.Width * Self.Image.Height);
  arr.Insert(start, h(start,stop)*Self.HeuristicsMultiplier, 0);

  Result.SetSize(Self.Image.Width, Self.Image.Height);

  if not Self.Eightway then
    SetLength(adj, 4);

  // heuristical forward search for goal
  while (arr.hi >= 0) do
  begin
    arr.Pop(p, hdist, score); //inline for higher performance
    if (p = stop) then
      Exit;

    for q in adj do
      with q do
      begin
        x += p.x;
        y += p.y;
        if Self.Image.InImage(x,y) and (Self.Image.Pixel[x,y] = Self.Color) and (Result[y,x] = 0) then
        begin
          arr.Insert(q, score + h(q,stop) * Self.HeuristicsMultiplier, score+1);

          if Self.BFSMeasureBacktrace then
            Result[y,x] := p.DistanceTo(start) + Sqr(score)+1
          else
            Result[y,x] := score+1;
        end;
      end;
  end;
end;


(*
  Backtraces a distance matrix from stop to start, returning the final path.

  For description see overload method.
*)
function TPathfinder.Backtrace(Matrix: TSingleMatrix; Start, Stop: TPoint): TPointArray;
var
  score: single;
  p,q,t: TPoint;
  adj: TPointArray = [[-1,0],[1,0],[0,-1],[0,1],[1,-1],[1,1],[-1,-1],[-1,1]];
begin
  if not Self.Eightway then SetLength(adj, 4);

  // backtrace towards 0 distance from stop using the distance matrix
  p := stop;
  score := Matrix[p.y,p.x];
  Result += p; //pre-allocate result size for another boost SetLen(res, Ceil(score))
  repeat
    for q in adj do
    begin
      q.x += p.x;
      q.y += p.y;
      if Self.Image.InImage(q.x,q.y) and
         (Matrix[q.y, q.x] < score) and (Matrix[q.y, q.x] > 0) then
      begin
        t := q;
        score := Matrix[t.y, t.x];
      end;
    end;
    p := t;
    score := Matrix[p.y, p.x];
    Matrix[p.y, p.x] := 0;
    Result += p;
  until score = 0;
  Result += start;
end;

function TPathfinder.Solve(Start, Stop: TPoint): TPointArray;
var
  matrix: TSingleMatrix;
begin
  matrix := Self.FloodFillMatrix(Start, Stop);
  Result := Self.Backtrace(matrix, Start, Stop);
end;


var
  //start := Point(1,18); stop  := Point(564,547);
  stop := Point(637,478); start := Point(4,1);

  img: TImage;
  path1: TPointArray;
  t:Double;
  pf: TPathFinder;
begin
  img := new TImage('images\maze.png');
  Swap(start,stop);

  pf.Init(img, $FFFFFF, True, EDistanceHeuistics.dhEuclidean, 1);

  for 0 to 5 do
  begin
    t := Time();
    path1 := pf.Solve(start, stop);
    WriteLn(Time() - t, 'ms');
  end;
  WriteLn('Length: ', Length(path1));

  img.DrawColor := $FF00FF;
  img.DrawTPA(path1);

  img.Show();
end.