PathFinding.simba

{$include_once Reflection/Internal/RSCollisionMap.simba}
{$include_once Reflection/Internal/RSClient.simba}

Function R_AStar(Start: RSTile; Destination: RSTile): Array of RSTile;
type
  RSTileArray = Array of RSTile;

  RSTileExtended = record(RSTile)
    Flags: Int32;
    P: TPoint;
  end;

  RSCameFromElement = record
    Key: Int64;
    Value: ^RSTileExtended;
  end;

  RSCostSoFarElement = record
    Key: Int64;
    Value: Double;
  end;

  RSCameFromSet = Array of RSCameFromElement;
  RSCostSoFarSet = Array of RSCostSoFarElement;

  RSPriorityQueuePair = record
    Key: Double;
    Value: ^RSTileExtended;
  end;

  RSPriorityQueue = record
    Bucket: Array of RSPriorityQueuePair;
  end;

  Function RSTileExtended.Hash: Int32; constref;
  begin
    Result := 7 * (X * Y) + (P.X * P.Y);
  end;

  Procedure RSCameFromSet.Put(Key: ^RSTileExtended; Value: ^RSTileExtended);
  var
    I: Int32;
    Hash: Int32;
  begin
    Hash := Key^.Hash;
    For I := 0 To High(Self) do
    begin
      If Self[I].Key = Hash then
      begin
        Self[I].Value := Value;
        Exit;
      end;
    end;

    Self += [Hash, Value];
  end;

  Function RSCameFromSet.Get(Key: ^RSTileExtended): ^RSTileExtended; constref;
  var
    I: Int32;
    Hash: Int32;
  begin
    Hash := Key^.Hash;
    For I := 0 To High(Self) do
    begin
      If Self[I].Key = Hash then
      begin
        Exit(Self[I].Value);
      end;
    end;
  end;

  Function RSCameFromSet.Contains(Key: ^RSTileExtended): Boolean; constref;
  var
    I: Int32;
    Hash: Int32;
  begin
    Hash := Key^.Hash;
    For I := 0 To High(Self) do
    begin
      If Self[I].Key = Hash then
      begin
        Exit(True);
      end;
    end;
  end;

  Procedure RSCostSoFarSet.Put(Key: ^RSTileExtended; Value: Double);
  var
    I: Int32;
    Hash: Int32;
  begin
    Hash := Key^.Hash;
    For I := 0 To High(Self) do
    begin
      If Self[I].Key = Hash then
      begin
        Self[I].Value := Value;
        Exit;
      end;
    end;

    Self += [Hash, Value];
  end;

  Function RSCostSoFarSet.Get(Key: ^RSTileExtended): Double; constref;
  var
    I: Int32;
    Hash: Int32;
  begin
    Hash := Key^.Hash;
    For I := 0 To High(Self) do
    begin
      If Self[I].Key = Hash then
      begin
        Exit(Self[I].Value);
      end;
    end;

    Result := 0.0;
  end;

  Function RSCostSoFarSet.Contains(Key: ^RSTileExtended): Boolean; constref;
  var
    I: Int32;
    Hash: Int32;
  begin
    Hash := Key^.Hash;
    For I := 0 To High(Self) do
    begin
      If Self[I].Key = Hash then
      begin
        Exit(True);
      end;
    end;
  end;

  Function RSPriorityQueue.Compare(constref A, B: RSPriorityQueuePair): Int32; static;
  begin
    if A.Key < B.Key then
      Exit(-1);

    if A.Key = B.Key then
      Exit(0);

    Exit(1);
  end;

  Function RSPriorityQueue.Empty: Boolean; constref;
  begin
    Result := Length(Bucket) = 0;
  end;

  Procedure RSPriorityQueue.Put(Key: Double; Value: ^RSTileExtended);
  begin
    Bucket += [Key, Value];
    Sort(Bucket, @RSPriorityQueue.Compare);
  end;

  Function RSPriorityQueue.Pop: ^RSTileExtended;
  begin
    Result := Bucket[0].Value;
    Delete(Bucket, 0, 1);
  end;

  Function GetNeighbours(Node: ^RSTileExtended; var Map: Array of RSTileExtended; W, H: Int32): Array of ^RSTileExtended;
  var
    C, X, Y: Int32;
  begin
    C := 0;
    X := Node^.P.X;
    Y := Node^.P.Y;
    SetLength(Result,  8);

    // N
    if (Y > 0) then
    begin
      Result[C] := @Map[(Y - 1) * W + X];
      Inc(C);
    end;

    // E
    if (X + 1 < W) then
    begin
      Result[C] := @Map[Y * W + (X + 1)];
      Inc(C);
    end;

    // S
    if (Y + 1 < H) then
    begin
      Result[C] := @Map[(Y + 1) * W + X];
      Inc(C);
    end;

    // W
    if (X > 0) then
    begin
      Result[C] := @Map[Y * W + (X - 1)];
      Inc(C);
    end;

    // Diagonal NE
    if (X + 1 < W) and (Y > 0) then
    begin
      Result[C] := @Map[(Y - 1) * W + (X + 1)];
      Inc(C);
    end;

    // Diagonal SE
    if (X + 1 < W) and (Y + 1 < H) then
    begin
      Result[C] := @Map[(Y + 1) * W + (X + 1)];
      Inc(C);
    end;

    // Diagonal SW
    if (X > 0) and (Y + 1 < H) then
    begin
      Result[C] := @Map[(Y + 1) * W + (X - 1)];
      Inc(C);
    end;

    // Diagonal NW
    if (X > 0) and (Y > 0) then
    begin
      Result[C] := @Map[(Y - 1) * W + (X - 1)];
      Inc(C);
    end;

    SetLength(Result, C);
  end;

  Function IsValid(Current, Next: ^RSTileExtended; W, H: Int32): Boolean;
  const
    BLOCKED_NORTH = 19136800;
    BLOCKED_EAST = 19136896;
    BLOCKED_SOUTH = 19136770;
    BLOCKED_WEST = 19136776;

    BLOCKED_NORTHEAST = 19136992;
    BLOCKED_SOUTHEAST = 19136899;
    BLOCKED_SOUTHWEST = 19136782;
    BLOCKED_NORTHWEST = 19136824;
  var
    DX, DY, Flags: Int32;
  begin
    // Valid Position / Bounds Check
    Result := (Current^.P.X >= 0) and
              (Current^.P.Y >= 0) and
              (Current^.P.X < W) and
              (Current^.P.Y < H);

    // Direction Check
    if Result then
    begin
      Flags := 0;
      DX := (Next^.P.X - Current^.P.X);
      DY := (Next^.P.Y - Current^.P.Y);

      if (DX = 0) and (DY > 0) then // Moving North
      begin
        Flags := Flags OR BLOCKED_NORTH;
      end;

      if (DX > 0) and (DY = 0) then //Moving East
      begin
        Flags := BLOCKED_EAST;
      end;

      if (DX = 0) and (DY < 0) then // Moving South
      begin
        Flags := Flags OR BLOCKED_SOUTH;
      end;

      if (DX < 0) and (DY = 0) then // Moving West
      begin
        Flags := Flags OR BLOCKED_WEST;
      end;

      if (DX > 0) and (DY > 0) then //Moving North East
      begin
        Flags := Flags OR BLOCKED_EAST OR BLOCKED_NORTH OR BLOCKED_NORTHEAST;
      end;

      if (DX > 0) and (DY < 0) then //Moving South East
      begin
        Flags := Flags OR BLOCKED_EAST OR BLOCKED_SOUTH OR BLOCKED_SOUTHEAST;
      end;

      if (DX < 0) and (DY < 0) then //Moving South West
      begin
        Flags := Flags OR BLOCKED_WEST OR BLOCKED_SOUTH OR BLOCKED_SOUTHWEST;
      end;

      if (DX < 0) and (DY > 0) then //Moving North West
      begin
        Flags := Flags OR BLOCKED_WEST OR BLOCKED_NORTH OR BLOCKED_NORTHWEST;
      end;

      Result := Result and ((Next^.Flags and Flags) = 0);
    end;
  end;

  Function Cost(A, B: ^RSTileExtended): Double;
  begin
    Result := Distance(A^.X, A^.Y, B^.X, B^.Y);
  end;

  Function Heuristic(A, B: ^RSTileExtended): Double;
  begin
    Result := Abs(a^.X - b^.X) + Abs(a^.Y - b^.Y);
  end;

  Procedure Reverse(var Self: RSTileArray);
  var
    T: RSTile;
    L, H: ^RSTile;
  begin
    if (Length(Self) = 0) then
      Exit;

    L := @Self[0];
    H := @Self[High(self)];

    while (PtrUInt(L) < PtrUInt(H)) do
    begin
      T := H^;
      H^ := L^;
      L^ := T;
      Dec(H);
      Inc(L);
    end;
  end;
const
  SceneWidth = 104;
  SceneHeight = 104;
var
  Index: Int32;
  X, Y, Plane: Int32;
  TileMap: Array of RSTileExtended;
  CollisionMaps: Array of RSCollisionMap;
  Flags: Array of Array of Int32;
  Frontier: RSPriorityQueue;
  CameFrom: RSCameFromSet;
  CostSoFar: RSCostSoFarSet;
  S, D: RSTileExtended;
  Current: ^RSTileExtended;
  P: RSTile;
  Neighbours: Array of ^RSTileExtended;
  NewCost: Double;
begin
  Plane := RSClient.Plane;
  CollisionMaps := RSClient.CollisionMaps;
  if Length(CollisionMaps) = 0 then
  begin
    Exit;
  end;

  if CollisionMaps[Plane].ref = nil then
  begin
    RSTypeArray(CollisionMaps).Free;
    Exit;
  end;

  Flags := CollisionMaps[Plane].Adjacency;
  RSTypeArray(CollisionMaps).Free;

  SetLength(CollisionMaps, 0);
  SetLength(TileMap, SceneWidth * SceneHeight);

  For Y := 0 to SceneHeight - 1 do
  begin
    For X := 0 to SceneWidth - 1 do
    begin
      // Convert from Scene To Local
      Index := Y * SceneWidth + X;
      TileMap[Index].X := (X * (1 shl 7)) + (1 shl 6); //(X + BaseX).ToLocal;
      TileMap[Index].Y := (Y * (1 shl 7)) + (1 shl 6); //(Y + BaseY).ToLocal;
      TileMap[Index].Flags := Flags[X][Y]; // Store Tile Flags
      TileMap[Index].P := [X, Y];
    end;
  end;

  SetLength(Flags, 0);

  P := Start.ToLocal;
  S := [P.X, P.Y, 0, [-1, -1]];

  P := Destination.ToLocal;
  D := [P.X, P.Y, 0, [-1, -1]];

  For Index := 0 to High(TileMap) do
  begin
    If (TileMap[Index].X = S.X) and (TileMap[Index].Y = S.Y) then
    begin
      S.Flags := TileMap[Index].Flags;
      S.P := TileMap[Index].P;
    end else
      If (TileMap[Index].X = D.X) and (TileMap[Index].Y = D.Y) then
      begin
        D.Flags := TileMap[Index].Flags;
        D.P := TileMap[Index].P;
      end;
  end;

  if S.P = [-1, -1] then
    Exit;

  if D.P = [-1, -1] then
    Exit;

  Frontier.Put(0.0, @S);
  CameFrom.Put(@S, @S);
  CostSoFar.Put(@S, 0.0);

  While (Not Frontier.Empty) do
  begin
    Current := Frontier.Pop;
    if Current^.Hash = D.Hash then
      break;

    Neighbours := GetNeighbours(Current, TileMap, SceneWidth, SceneHeight);
    For Index := 0 To High(Neighbours) do
    begin
      // Path is Out of Bounds
      // Next Node (Neighbour) is a Barrier
      if (Not IsValid(Current, Neighbours[Index], SceneWidth, SceneHeight)) then
        Continue;

      NewCost := CostSoFar.Get(Current) + Cost(Current, Neighbours[Index]);
      if (Not CostSoFar.Contains(Neighbours[Index])) or (NewCost < CostSoFar.Get(Neighbours[Index])) then
      begin
        CostSoFar.Put(Neighbours[Index], NewCost);
        CameFrom.Put(Neighbours[Index], Current);
        Frontier.Put(NewCost + Heuristic(Neighbours[Index], @D), Neighbours[Index]);
      end;
    end;
  end;

  if CameFrom.Contains(@D) then
  begin
    Current := @D;
    While Current^.Hash <> S.Hash do
    begin
      P := [Current^.X, Current^.Y];
      Result += P.LocalToWorldTile;
      Current := CameFrom.Get(Current);
    end;

    P := [S.X, S.Y];
    Result += P.LocalToWorldTile;
    Reverse(Result);
  end;
end;

Function R_PathToEx(Start, Destination: RSTile; FindClosest, IncludeAllTiles: Boolean): Array of RSTile;
const
  FLAG_NORTH = 19136800;
  FLAG_EAST = 19136896;
  FLAG_SOUTH = 19136770;
  FLAG_WEST = 19136776;
  FLAG_NORTH_EAST = 19136992;
  FLAG_SOUTH_EAST = 19136899;
  FLAG_SOUTH_WEST = 19136782;
  FLAG_NORTH_WEST = 19136824;
var
  I, J, Plane: Int32;
  CollisionMaps: Array of RSCollisionMap;
  Flags: Array of Array of Int32;
  Directions: Array [0..127] of Array [0..127] of Int32;
  Distances: Array [0..127] of Array [0..127] of Int32;
  Buffer: Array [0..4095] of TPoint;
  P1, P2, Middle, Current, MapLocation, Offset: TPoint;
  Index, Index1, Index2, CurrentDistance: Int32;
  TileCount, DirectionOld, DirectionNew: Int32;
  D, DX, DY, MaxApproximateRange, MaxDistance: Int32;
  ApproximateDestination: TPoint;
  Reachable: Boolean;
begin
  Plane := RSClient.Plane;
  CollisionMaps := RSClient.CollisionMaps;
  if Length(CollisionMaps) = 0 then
  begin
    Exit;
  end;

  if CollisionMaps[Plane].ref = nil then
  begin
    RSTypeArray(CollisionMaps).Free;
    Exit;
  end;

  Flags := CollisionMaps[Plane].Adjacency;
  RSTypeArray(CollisionMaps).Free;
  SetLength(CollisionMaps, 0);

  For I := 0 to High(Directions) do
  begin
    For J := 0 to High(Directions) do
    begin
      Directions[I][J] := 0;
      Distances[I][J] := High(Int32);
    end;
  end;

  P1 := Start.GlobalToScene;
  P2 := Destination.GlobalToScene;
  Middle := P1;
  Current := Middle;
  Offset := [64, 64];

  Directions[Offset.X][Offset.Y] := 99;
  Distances[Offset.X][Offset.Y] := 0;

  Index1 := 0;
  Index2 := 1;
  Buffer[0] := Current;

  Reachable := False;
  While (Index1 <> Index2) do
  begin
    Current := Buffer[Index1];
    Index1 := (Index1 + 1) and 4095;

    MapLocation.X := Current.X - Middle.X + Offset.X;
    MapLocation.Y := Current.Y - Middle.Y + Offset.Y;

    if (Current = P2) then
    begin
      Reachable := True;
      Break;
    end;

    CurrentDistance := Distances[MapLocation.X][MapLocation.Y] + 1;

    // Can Move West
    if (MapLocation.X > 0) and
       (Directions[MapLocation.X - 1][MapLocation.Y] = 0) and
       ((Flags[Current.X - 1][Current.Y] and FLAG_WEST) = 0) then
    begin
      Buffer[Index2].X := Current.X - 1;
      Buffer[index2].Y := Current.Y;
      Index2 := (Index2 + 1) and 4095;

      Directions[MapLocation.X - 1][MapLocation.Y] := 2;
      Distances[MapLocation.X - 1][MapLocation.Y] := CurrentDistance;
    end;

    // Can Move East
    if (MapLocation.X < 127) and
       (Directions[MapLocation.X + 1][MapLocation.Y] = 0) and
       ((Flags[Current.X + 1][Current.Y] and FLAG_EAST) = 0) then
    begin
      Buffer[Index2].X := Current.X + 1;
      Buffer[Index2].Y := Current.Y;
      Index2 := (Index2 + 1) and 4095;

      Directions[MapLocation.X + 1][MapLocation.Y] := 8;
      Distances[MapLocation.X + 1][MapLocation.Y] := CurrentDistance;
    end;

    // Can Move South
    if (MapLocation.Y > 0) and
       (Directions[MapLocation.X][MapLocation.Y - 1] = 0) and
       ((Flags[Current.X][Current.Y - 1] and FLAG_SOUTH) = 0) then
    begin
      Buffer[Index2].X := Current.X;
      Buffer[Index2].Y := Current.Y - 1;
      Index2 := (Index2 + 1) and 4095;

      Directions[MapLocation.X][MapLocation.Y - 1] := 1;
      Distances[MapLocation.X][MapLocation.Y - 1] := CurrentDistance;
    end;

    // Can Move North
    if (MapLocation.Y < 127) and
       (Directions[MapLocation.X][MapLocation.Y + 1] = 0) and
       ((Flags[Current.X][Current.Y + 1] and FLAG_NORTH) = 0) then
    begin
      Buffer[Index2].X := Current.X;
      Buffer[Index2].Y := Current.Y + 1;
      Index2 := (Index2 + 1) and 4095;

      Directions[MapLocation.X][MapLocation.Y + 1] := 4;
      Distances[MapLocation.X][MapLocation.Y + 1] := CurrentDistance;
    end;

    // Can Move South-West
    if (MapLocation.X > 0) and
       (MapLocation.Y > 0) and
       (Directions[MapLocation.X - 1][MapLocation.Y - 1] = 0) and
       ((Flags[Current.X - 1][Current.Y - 1] and FLAG_SOUTH_WEST) = 0) and
       ((Flags[Current.X - 1][Current.Y] and FLAG_WEST) = 0) and
       ((Flags[Current.X][Current.Y - 1] and FLAG_SOUTH) = 0) then
    begin
      Buffer[Index2].X := Current.X - 1;
      Buffer[Index2].Y := Current.Y - 1;
      Index2 := (Index2 + 1) and 4095;

      Directions[MapLocation.X - 1][MapLocation.Y - 1] := 3;
      Distances[MapLocation.X - 1][MapLocation.Y - 1] := CurrentDistance;
    end;

    // Can Move South-East
    if (MapLocation.X < 127) and
       (MapLocation.Y > 0) and
       (Directions[MapLocation.X + 1][MapLocation.Y - 1] = 0) and
       ((Flags[Current.X + 1][Current.Y - 1] and FLAG_SOUTH_EAST) = 0) and
       ((Flags[Current.X + 1][Current.Y] and FLAG_EAST) = 0) and
       ((Flags[Current.X][Current.Y - 1] and FLAG_SOUTH) = 0) then
		begin
		  Buffer[Index2].X := Current.X + 1;
			Buffer[Index2].Y := Current.Y - 1;
			Index2 := (Index2 + 1) and 4095;

			Directions[MapLocation.X + 1][MapLocation.Y - 1] := 9;
			Distances[MapLocation.X + 1][MapLocation.Y - 1] := CurrentDistance;
		end;

    // Can Move North-West
    if (MapLocation.X > 0) and
       (MapLocation.Y < 127) and
       (Directions[MapLocation.X - 1][MapLocation.Y + 1] = 0) and
       ((Flags[Current.X - 1][Current.Y + 1] and FLAG_NORTH_WEST) = 0) and
       ((Flags[Current.X - 1][Current.Y] and FLAG_WEST) = 0) and
       ((Flags[Current.X][Current.Y + 1] and FLAG_NORTH) = 0) then
		begin
		  Buffer[Index2].X := Current.X - 1;
			Buffer[Index2].Y := Current.Y + 1;
			Index2 := (Index2 + 1) and 4095;

			Directions[MapLocation.X - 1][MapLocation.Y + 1] := 6;
			Distances[MapLocation.X - 1][MapLocation.Y + 1] := CurrentDistance;
		end;

    // Can Move North-East
    if (MapLocation.X < 127) and
       (MapLocation.Y < 127) and
       (Directions[MapLocation.X + 1][MapLocation.Y + 1] = 0) and
       ((Flags[Current.X + 1][Current.Y + 1] and FLAG_NORTH_EAST) = 0) and
       ((Flags[Current.X + 1][Current.Y] and FLAG_EAST) = 0) and
       ((Flags[Current.X][Current.Y + 1] and FLAG_NORTH) = 0) then
		begin
		  Buffer[Index2].X := Current.X + 1;
			Buffer[Index2].Y := Current.Y + 1;
			Index2 := (Index2 + 1) and 4095;

			Directions[MapLocation.X + 1][MapLocation.Y + 1] := 12;
			Distances[MapLocation.X + 1][MapLocation.Y + 1] := CurrentDistance;
		end;
  end;

  if (Not Reachable) then
  begin
    if Not FindClosest then
    begin
      Exit;
    end;

    // Find Tile Closest to the target Destination
    MaxDistance := High(Int32);
    TileCount := High(Int32);
    ApproximateDestination := P2;
    MaxApproximateRange := 10;

    I := ApproximateDestination.X - MaxApproximateRange;
    While (I <= MaxApproximateRange + ApproximateDestination.X) do
    begin
      J := ApproximateDestination.Y - MaxApproximateRange;

      While (J <= MaxApproximateRange + ApproximateDestination.Y) do
      begin
        MapLocation.X := I - Middle.X + Offset.X;
        MapLocation.Y := J - Middle.Y + Offset.Y;

        if (MapLocation.X >= 0) and
           (MapLocation.Y >= 0) and
           (MapLocation.X < 128) and
           (MapLocation.Y < 128) and
           (Distances[MapLocation.X][MapLocation.Y] < 100) then
        begin
          DX := 0;
          DY := 0;

          if (I < ApproximateDestination.X) then
          begin
            DX := ApproximateDestination.X - I;
          end else if (I > ApproximateDestination.X) then
            begin
              DX := I - ApproximateDestination.X;
            end;

          if (J < ApproximateDestination.Y) then
          begin
            DY := ApproximateDestination.Y - J;
          end else if (J > ApproximateDestination.Y) then
            begin
              DY := J - ApproximateDestination.Y;
            end;

          D := (DX * DX) + (DY * DY);
          if (D < MaxDistance) or
             ((D = MaxDistance) and
              (Distances[MapLocation.X][MapLocation.Y] < TileCount)) then
          begin
            MaxDistance := D;
            TileCount := Distances[MapLocation.X][MapLocation.Y];
            Current := [I, J];
          end;
        end;

        Inc(J);
      end;

      Inc(I);
    end;

    // No Path
    if MaxDistance = High(Int32) then
    begin
      Exit;
    end;
  end;

  // Convert Direction & Distances to Path
  Buffer[0] := Current;
  Index := 1;

  DirectionOld := Directions[Current.X - Middle.X + Offset.X][Current.Y - Middle.Y + Offset.Y];
  DirectionNew := DirectionOld;

  While(P1 <> Current) do
  begin
    if IncludeAllTiles or (DirectionOld <> DirectionNew) then
    begin
      DirectionOld := DirectionNew;
      Buffer[Index] := Current;
      Inc(Index);
    end;

    if ((DirectionNew and 2) <> 0) then
    begin
      Inc(Current.X); // East
    end else if ((DirectionNew and 8) <> 0) then
    begin
      Dec(Current.X); // West
    end;

    if ((DirectionNew and 1) <> 0) then
    begin
      Inc(Current.Y); // South
    end else if ((DirectionNew and 4) <> 0) then
    begin
      Dec(Current.Y); // North
    end;

    DirectionNew := Directions[Current.X - Middle.X + Offset.X][Current.Y - Middle.Y + Offset.Y];
  end;

  TileCount := 1;
  While (Index > 0) do
  begin
    Dec(Index);
    Result += RSTile(Buffer[Index]).SceneToGlobal;

    if TileCount = 25 then // Math path length of 25..
      break;
    Inc(TileCount);
  end;
end;

Function R_PathTo(Start, Destination: RSTile): Array of RSTile;
begin
  Result := R_PathToEx(Start, Destination, True, False);
end;

Function R_CanReach(Start, Destination: RSTile): Boolean;
const
  FLAG_NORTH = 19136800;
  FLAG_EAST = 19136896;
  FLAG_SOUTH = 19136770;
  FLAG_WEST = 19136776;
  FLAG_NORTH_EAST = 19136992;
  FLAG_SOUTH_EAST = 19136899;
  FLAG_SOUTH_WEST = 19136782;
  FLAG_NORTH_WEST = 19136824;
var
  I, J, Plane: Int32;
  CollisionMaps: Array of RSCollisionMap;
  Flags: Array of Array of Int32;
  Directions: Array [0..127] of Array [0..127] of Int32;
  Distances: Array [0..127] of Array [0..127] of Int32;
  Buffer: Array [0..4095] of TPoint;
  P1, P2, Middle, Current, MapLocation, Offset: TPoint;
  Index1, Index2, CurrentDistance: Int32;
  TileCount: Int32;
  D, DX, DY, MaxApproximateRange, MaxDistance: Int32;
  ApproximateDestination: TPoint;
  Reachable: Boolean;
begin
  Plane := RSClient.Plane;
  CollisionMaps := RSClient.CollisionMaps;
  if Length(CollisionMaps) = 0 then
  begin
    Exit;
  end;

  if CollisionMaps[Plane].ref = nil then
  begin
    RSTypeArray(CollisionMaps).Free;
    Exit;
  end;

  Flags := CollisionMaps[Plane].Adjacency;
  RSTypeArray(CollisionMaps).Free;
  SetLength(CollisionMaps, 0);

  For I := 0 to High(Directions) do
  begin
    For J := 0 to High(Directions) do
    begin
      Directions[I][J] := 0;
      Distances[I][J] := High(Int32);
    end;
  end;

  P1 := Start.GlobalToScene;
  P2 := Destination.GlobalToScene;
  Middle := P1;
  Current := Middle;
  Offset := [64, 64];

  Directions[Offset.X][Offset.Y] := 99;
  Distances[Offset.X][Offset.Y] := 0;

  Index1 := 0;
  Index2 := 1;
  Buffer[0] := Current;

  Reachable := False;
  While (Index1 <> Index2) do
  begin
    Current := Buffer[Index1];
    Index1 := (Index1 + 1) and 4095;

    MapLocation.X := Current.X - Middle.X + Offset.X;
    MapLocation.Y := Current.Y - Middle.Y + Offset.Y;

    if (Current = P2) then
    begin
      Reachable := True;
      Break;
    end;

    CurrentDistance := Distances[MapLocation.X][MapLocation.Y] + 1;

    // Can Move West
    if (MapLocation.X > 0) and
       (Directions[MapLocation.X - 1][MapLocation.Y] = 0) and
       ((Flags[Current.X - 1][Current.Y] and FLAG_WEST) = 0) then
    begin
      Buffer[Index2].X := Current.X - 1;
      Buffer[index2].Y := Current.Y;
      Index2 := (Index2 + 1) and 4095;

      Directions[MapLocation.X - 1][MapLocation.Y] := 2;
      Distances[MapLocation.X - 1][MapLocation.Y] := CurrentDistance;
    end;

    // Can Move East
    if (MapLocation.X < 127) and
       (Directions[MapLocation.X + 1][MapLocation.Y] = 0) and
       ((Flags[Current.X + 1][Current.Y] and FLAG_EAST) = 0) then
    begin
      Buffer[Index2].X := Current.X + 1;
      Buffer[Index2].Y := Current.Y;
      Index2 := (Index2 + 1) and 4095;

      Directions[MapLocation.X + 1][MapLocation.Y] := 8;
      Distances[MapLocation.X + 1][MapLocation.Y] := CurrentDistance;
    end;

    // Can Move South
    if (MapLocation.Y > 0) and
       (Directions[MapLocation.X][MapLocation.Y - 1] = 0) and
       ((Flags[Current.X][Current.Y - 1] and FLAG_SOUTH) = 0) then
    begin
      Buffer[Index2].X := Current.X;
      Buffer[Index2].Y := Current.Y - 1;
      Index2 := (Index2 + 1) and 4095;

      Directions[MapLocation.X][MapLocation.Y - 1] := 1;
      Distances[MapLocation.X][MapLocation.Y - 1] := CurrentDistance;
    end;

    // Can Move North
    if (MapLocation.Y < 127) and
       (Directions[MapLocation.X][MapLocation.Y + 1] = 0) and
       ((Flags[Current.X][Current.Y + 1] and FLAG_NORTH) = 0) then
    begin
      Buffer[Index2].X := Current.X;
      Buffer[Index2].Y := Current.Y + 1;
      Index2 := (Index2 + 1) and 4095;

      Directions[MapLocation.X][MapLocation.Y + 1] := 4;
      Distances[MapLocation.X][MapLocation.Y + 1] := CurrentDistance;
    end;

    // Can Move South-West
    if (MapLocation.X > 0) and
       (MapLocation.Y > 0) and
       (Directions[MapLocation.X - 1][MapLocation.Y - 1] = 0) and
       ((Flags[Current.X - 1][Current.Y - 1] and FLAG_SOUTH_WEST) = 0) and
       ((Flags[Current.X - 1][Current.Y] and FLAG_WEST) = 0) and
       ((Flags[Current.X][Current.Y - 1] and FLAG_SOUTH) = 0) then
    begin
      Buffer[Index2].X := Current.X - 1;
      Buffer[Index2].Y := Current.Y - 1;
      Index2 := (Index2 + 1) and 4095;

      Directions[MapLocation.X - 1][MapLocation.Y - 1] := 3;
      Distances[MapLocation.X - 1][MapLocation.Y - 1] := CurrentDistance;
    end;

    // Can Move South-East
    if (MapLocation.X < 127) and
       (MapLocation.Y > 0) and
       (Directions[MapLocation.X + 1][MapLocation.Y - 1] = 0) and
       ((Flags[Current.X + 1][Current.Y - 1] and FLAG_SOUTH_EAST) = 0) and
       ((Flags[Current.X + 1][Current.Y] and FLAG_EAST) = 0) and
       ((Flags[Current.X][Current.Y - 1] and FLAG_SOUTH) = 0) then
		begin
		  Buffer[Index2].X := Current.X + 1;
			Buffer[Index2].Y := Current.Y - 1;
			Index2 := (Index2 + 1) and 4095;

			Directions[MapLocation.X + 1][MapLocation.Y - 1] := 9;
			Distances[MapLocation.X + 1][MapLocation.Y - 1] := CurrentDistance;
		end;

    // Can Move North-West
    if (MapLocation.X > 0) and
       (MapLocation.Y < 127) and
       (Directions[MapLocation.X - 1][MapLocation.Y + 1] = 0) and
       ((Flags[Current.X - 1][Current.Y + 1] and FLAG_NORTH_WEST) = 0) and
       ((Flags[Current.X - 1][Current.Y] and FLAG_WEST) = 0) and
       ((Flags[Current.X][Current.Y + 1] and FLAG_NORTH) = 0) then
		begin
		  Buffer[Index2].X := Current.X - 1;
			Buffer[Index2].Y := Current.Y + 1;
			Index2 := (Index2 + 1) and 4095;

			Directions[MapLocation.X - 1][MapLocation.Y + 1] := 6;
			Distances[MapLocation.X - 1][MapLocation.Y + 1] := CurrentDistance;
		end;

    // Can Move North-East
    if (MapLocation.X < 127) and
       (MapLocation.Y < 127) and
       (Directions[MapLocation.X + 1][MapLocation.Y + 1] = 0) and
       ((Flags[Current.X + 1][Current.Y + 1] and FLAG_NORTH_EAST) = 0) and
       ((Flags[Current.X + 1][Current.Y] and FLAG_EAST) = 0) and
       ((Flags[Current.X][Current.Y + 1] and FLAG_NORTH) = 0) then
		begin
		  Buffer[Index2].X := Current.X + 1;
			Buffer[Index2].Y := Current.Y + 1;
			Index2 := (Index2 + 1) and 4095;

			Directions[MapLocation.X + 1][MapLocation.Y + 1] := 12;
			Distances[MapLocation.X + 1][MapLocation.Y + 1] := CurrentDistance;
		end;
  end;

  if (Not Reachable) then
  begin
    // Find Tile Closest to the target Destination
    MaxDistance := High(Int32);
    TileCount := High(Int32);
    ApproximateDestination := P2;
    MaxApproximateRange := 10;

    I := ApproximateDestination.X - MaxApproximateRange;
    While (I <= MaxApproximateRange + ApproximateDestination.X) do
    begin
      J := ApproximateDestination.Y - MaxApproximateRange;

      While (J <= MaxApproximateRange + ApproximateDestination.Y) do
      begin
        MapLocation.X := I - Middle.X + Offset.X;
        MapLocation.Y := J - Middle.Y + Offset.Y;

        if (MapLocation.X >= 0) and
           (MapLocation.Y >= 0) and
           (MapLocation.X < 128) and
           (MapLocation.Y < 128) and
           (Distances[MapLocation.X][MapLocation.Y] < 100) then
        begin
          DX := 0;
          DY := 0;

          if (I < ApproximateDestination.X) then
          begin
            DX := ApproximateDestination.X - I;
          end else if (I > ApproximateDestination.X) then
            begin
              DX := I - ApproximateDestination.X;
            end;

          if (J < ApproximateDestination.Y) then
          begin
            DY := ApproximateDestination.Y - J;
          end else if (J > ApproximateDestination.Y) then
            begin
              DY := J - ApproximateDestination.Y;
            end;

          D := (DX * DX) + (DY * DY);
          if (D < MaxDistance) or
             ((D = MaxDistance) and
              (Distances[MapLocation.X][MapLocation.Y] < TileCount)) then
          begin
            MaxDistance := D;
            TileCount := Distances[MapLocation.X][MapLocation.Y];
            Current := [I, J];
          end;
        end;

        Inc(J);
      end;

      Inc(I);
    end;

    // No Path
    if MaxDistance = High(Int32) then
    begin
      Exit;
    end;
  end;

  Result := True;
end;