1:
2:
3:
4:
5:
6:
7:
8:
9:
10:
11:
12:
13:
14:
15:
16:
17:
18:
19:
20:

function PFFindOpenNode(const ID:Integer;const aF:Single):Integer;
var i,l:Integer;
begin
  //find most left node with equal F value
  l:=OpenBuffer.first;
  Result:=OpenBuffer.tail-1;
  //check first element
  if(OpenBuffer.Entrys[l].F^>=aF) then Result:=l
  else begin
    Repeat
      i:=(l+Result) div 2;
      if(OpenBuffer.Entrys[i].F^<aF) then l:=i
      else Result:=i;
      //l is always below
      //r(esult) is >=
    Until(l>=Result-1);
  end;
  for i:=Result to OpenBuffer.tail-1 do
    if(OpenBuffer.Entrys[i].ID=ID) then exit(i);
end;					

1:
2:
3:

  i:=PFFindOpenPos(aF);
  if(OpenBuffer.tail>i) then
    Move(OpenBuffer.Entrys[i],OpenBuffer.Entrys[i+1],(OpenBuffer.tail-i)*SizeOf(TBuffEntry));					

1:
2:
3:
4:
5:
6:
7:
8:
9:
10:
11:
12:
13:
14:
15:
16:
17:
18:
19:
20:
21:
22:
23:
24:
25:
26:
27:
28:
29:
30:
31:
32:
33:
34:
35:
36:
37:
38:
39:
40:
41:
42:
43:
44:
45:
46:
47:
48:
49:
50:
51:
52:
53:
54:
55:
56:
57:
58:
59:
60:
61:
62:
63:
64:
65:
66:
67:
68:
69:
70:
71:
72:
73:
74:
75:
76:
77:
78:
79:
80:
81:
82:
83:
84:
85:
86:
87:
88:
89:
90:
91:
92:
93:
94:
95:
96:
97:
98:
99:
100:
101:
102:
103:
104:
105:
106:
107:
108:
109:
110:
111:
112:
113:
114:
115:
116:
117:
118:
119:
120:
121:
122:
123:
124:
125:
126:
127:
128:
129:
130:
131:
132:
133:
134:
135:
136:
137:
138:
139:
140:
141:
142:
143:
144:
145:
146:
147:
148:
149:
150:
151:
152:
153:
154:
155:
156:
157:
158:
159:
160:
161:
162:
163:
164:
165:
166:
167:
168:
169:

type
  PCoords = ^TCoords;
  TCoords = packed record
    case LongInt of
     0: (Y,X  :Word );
     1: (XY   :DWord);
  end;

  PPMapItem = ^PMapItem;
  PMapItem = ^TMapItem;
  TMapItem = packed record
    Parent: PMapItem;     // Verknüpfung des Weges als einfach verkettete Liste
    Coord: TCoords;       // Position in der Karte (2D-Array)
    GCost: LongWord;      // Fixe Einflugkosten für dieses Feld
    GWCost: LongWord;     // Flugkosten zu diesem Punkt anhand des derzeitigen Weges
    HCost: LongWord;      // angenommene Flugkosten bis Ziel
    Region: TMapRegion;   // Benutzerabhängiger Zutritt zum Kartenfeld
    BinHeapPos: LongWord; // Position im BinHeap
    IsClosed: Boolean;    // wurde als Wegpunkt übernommen
  end;

  TMapItemBinHeap = class
  private
    FStruct: array of PMapItem;
    FCount: LongInt;
    procedure Swap(PItem1, PItem2: PPMapItem);
  protected
    function Compare(Item1, Item2: PMapItem):LongInt;
    procedure DeleteFirst;
  public
    destructor Destroy; override;
    procedure Add(Data: PMapItem);
    procedure Clear;
    function Drop: PMapItem;
    function Empty: Boolean;
    procedure ReSort(Index: LongInt);
  end;

implementation

{ TMapItemBinHeap }

procedure TMapItemBinHeap.Add(Data: PMapItem);
var
  Pos: LongInt;
  pp: PPMapItem;
  ppParent: PPMapItem;
begin
  Inc(FCount);
  if FCount >= Length(FStruct) then
  begin
    SetLength(FStruct, FCount + 1024);
  end;
  FStruct[FCount] := Data;
  Pos := FCount;
  Data^.BinHeapPos := Pos;
  while Pos <> 1 do
  begin
    pp := @FStruct[Pos];
    ppParent := @FStruct[Pos div 2];
    if Compare(pp^, ppParent^) < 1 then
    begin
      Swap(pp, ppParent);
      Pos := Pos div 2;
    end
    else
      Break;
  end;
end;

procedure TMapItemBinHeap.Clear;
begin
  SetLength(FStruct,0);
  FCount := 0;
end;

function TMapItemBinHeap.Compare(Item1, Item2: PMapItem): LongInt;
begin
  Result := (Item1^.GWCost + Item1^.HCost) - (Item2^.GWCost + Item2^.HCost);
end;

procedure TMapItemBinHeap.DeleteFirst;
var
  cIndex1: LongInt;
  cIndex2: LongInt;
  cLeftIndex: LongInt;
  cRightIndex: LongInt;
begin
  FStruct[1] := FStruct[FCount];
  FStruct[1]^.BinHeapPos := 1;
  Dec(FCount);
  cIndex2 := 1;
  repeat
    cIndex1 := cIndex2;
    cLeftIndex := 2*cIndex1;
    cRightIndex := cLeftIndex + 1;
    if cRightIndex <= FCount then
    begin
      if Compare(FStruct[cIndex1], FStruct[cLeftIndex]) > -1 then
        cIndex2 := cLeftIndex;
      if Compare(FStruct[cIndex2], FStruct[cRightIndex]) > -1 then
        cIndex2 := cRightIndex;
    end
    else
    begin
      if (cLeftIndex <= FCount) and
        (Compare(FStruct[cIndex1], FStruct[cLeftIndex]) > -1) then
      begin
        cIndex2 := cLeftIndex;
      end;
    end;
    if cIndex1 <> cIndex2 then
      Swap(@FStruct[cIndex1], @FStruct[cIndex2])
    else
      Break;
  until False;
end;

destructor TMapItemBinHeap.Destroy;
begin
  Clear;
  inherited Destroy;
end;

function TMapItemBinHeap.Drop: PMapItem;
begin
  Result := FStruct[1];
  DeleteFirst;
  Result^.BinHeapPos := 0;
end;

function TMapItemBinHeap.Empty: Boolean;
begin
  Result := FCount = 0;
end;

procedure TMapItemBinHeap.ReSort(Index: LongInt);
var
  pp: PPMapItem;
  ppParent: PPMapItem;
  Pos: LongInt;
begin
  Pos := Index;
  while Pos > 1 do
  begin
    pp := @FStruct[Pos];
    ppParent := @FStruct[Pos div 2];
    if Compare(pp^, ppParent^) < 1 then
    begin
      Swap(pp, ppParent);
      Pos := Pos div 2;
    end
    else
      Break;
  end;
end;

procedure TMapItemBinHeap.Swap(PItem1, PItem2: PPMapItem);
var
  temp: PMapItem;
  tempPos: LongWord;
begin
  temp := PItem1^;
  PItem1^ := PItem2^;
  PItem2^ := temp;
  tempPos := PItem1^^.BinHeapPos;
  PItem1^^.BinHeapPos := PItem2^^.BinHeapPos;
  PItem2^^.BinHeapPos := tempPos;
end;					

1:
2:
3:

    GCost: LongWord;      // Fixe Einflugkosten für dieses Feld
    GWCost: LongWord;     // Flugkosten zu diesem Punkt anhand des derzeitigen Weges
    HCost: LongWord;      // angenommene Flugkosten bis Ziel					

1:
2:
3:
4:
5:
6:
7:
8:
9:
10:
11:
12:
13:
14:
15:
16:
17:
18:
19:
20:
21:
22:
23:
24:
25:
26:
27:
28:
29:
30:
31:
32:
33:
34:
35:
36:
37:
38:
39:
40:
41:
42:
43:
44:
45:
46:
47:
48:
49:
50:
51:
52:
53:
54:
55:
56:
57:
58:
59:
60:
61:
62:
63:
64:
65:
66:
67:
68:
69:
70:
71:
72:
73:
74:
75:
76:
77:
78:
79:
80:
81:
82:
83:
84:
85:
86:
87:
88:
89:
90:
91:
92:
93:
94:
95:
96:
97:
98:
99:
100:
101:
102:
103:
104:

  PSimpleStackItem = ^TSimpleStackItem;
  TSimpleStackItem = record
    Next :PSimpleStackItem;
    Data :Pointer;
  end;

  TSimpleStack = class
  private
    FFirst :PSimpleStackItem;
    FCount :LongInt;
  public
    constructor Create;
    destructor Destroy; override;
    procedure Push(P: Pointer); virtual;
    function Pop: Pointer; virtual;
    function Empty: Boolean;
    property Count: LongInt read FCount;
    procedure Clear; virtual;
  end;

  TNextNodeList = class(TSimpleStack)
  public
    procedure AddIfExists(P: PMapItem);
    function Drop: PMapItem;
  end;

{ TSimpleStack }

procedure TSimpleStack.Clear;
var
  p, d: PSimpleStackItem;
begin
  if FFirst <> nil then
  begin
    while FFirst^.Next <> nil do
    begin
      p := FFirst^.Next;
      d := FFirst;
      Dispose(d);
      FFirst := p;
    end;
    p := FFirst;
    FFirst := nil;
    Dispose(p);
    FCount := 0;
  end;
end;

constructor TSimpleStack.Create;
begin
  inherited;
  FCount := 0;
end;

destructor TSimpleStack.Destroy;
begin
  Clear;
  inherited Destroy;
end;

function TSimpleStack.Empty: Boolean;
begin
  Result := FCount = 0;
end;

function TSimpleStack.Pop: Pointer;
var
  p :PSimpleStackItem;
begin
  if FCount > 0 then
  begin
    Result := FFirst^.Data;
    p := FFirst;
    FFirst := FFirst^.Next;
    Dispose(p);
    Dec(FCount);
  end
  else
    Result := nil;
end;

procedure TSimpleStack.Push(P: Pointer);
var
  PNew: PSimpleStackItem;
begin
  New(PNew);
  PNew^.Next := FFirst;
  FFirst := PNew;
  PNew^.Data := P;
  Inc(FCount);
end;

{ TNextNodeList }

procedure TNextNodeList.AddIfExists(P: PMapItem);
begin
  if P <> nil then
    Push(P);
end;

function TNextNodeList.Drop: PMapItem;
begin
  Result := PMapItem(Pop);
end;					

1:
2:
3:
4:
5:
6:
7:
8:
9:
10:
11:
12:
13:
14:
15:
16:
17:
18:
19:
20:
21:
22:
23:
24:
25:
26:
27:
28:
29:
30:
31:
32:
33:
34:
35:
36:
37:
38:
39:
40:
41:
42:

  if Assigned(FMap) then
  begin
    try
      FActive := True;
      FMap.InUse := True;

      if not FMap.Clean then //"Reinigung der Kartendaten für neue Wegsuche"
        FMap.Reset;

      if FDest.XY = FStart.XY then
        Exit;
      if not(Allowed(Map[FStart]) and Allowed(Map[FDest])) then
        Exit;

      FDestNode := nil;

      pNode := Map[FStart];
      pNode^.HCost := CalcHCost(pNode);
      pNode^.GWCost := 0;
      pNode^.Parent := nil;
      FOpenList.Add(pNode); //Ersten Wegpunkt eintragen
      FMap.Clean := False;

      while not FOpenList.Empty do
      begin
        pNode := FOpenList.Drop;
        pNode^.IsClosed := True; //Aus Liste entnommen
        AddNextNodes(pNode); //Umliegende Felder einfügen
        if (pNode^.Coord.XY = FDest.XY) then
        begin
          FDestNode := pNode;
          Result := True;
          Break;
        end;
      end;

    finally
      // Hier kann man sich jetzt von FDestNode 
      // durch die Parents hangeln und hat somit den Weg
      WayToString;
      FOpenList.Clear;
...					

1:
2:
3:
4:
5:
6:
7:
8:
9:
10:
11:
12:
13:
14:
15:
16:
17:
18:
19:
20:
21:
22:
23:
24:
25:
26:
27:
28:
29:
30:
31:
32:
33:
34:
35:
36:
37:
38:
39:
40:
41:
42:
43:
44:

procedure TPathFinder.AddNextNodes(pNode: PMapItem);
var
  nNodes: TNextNodeList;
  p: PMapItem;
  iJump: LongInt;
  i: LongInt;
begin
  nNodes := TNextNodeList.Create; //einfacher Stack für MapItems (Push/Pop)
  try
    GetNextNodes(pNode, nNodes); //trägt die umliegenden Felder in nNodes ein
    for i := Pred(nNodes.Count) downto 0 do
    begin
      p := nNodes.Drop; // ein umliegendes Feld
      if Allowed(p) then
      begin
        if not p^.IsClosed then
        begin
          if (p^.BinHeapPos > 0) then //wenn schon in Openlist dann Kosten neu berechnen.
          begin
            if (p^.GCost + pNode^.GWCost <= p^.GWCost) then
            begin
              p^.Parent := pNode; 
              p^.GWCost := p^.GCost + pNode^.GWCost;
              if Distance(p^.Coord, pNode^.Coord) > 1 then
              begin
                p^.GWCost := p^.GWCost + FMap.JumpCost;
              end;
              FOpenList.ReSort(p^.BinHeapPos); //hier wird bei veränderten Kosten neu sortiert
            end;
          end
          else
          begin
            p^.Parent := pNode;
            p^.GWCost := p^.GCost + pNode^.GWCost;
            p^.HCost := CalcHCost(p);
            FOpenList.Add(p);  // als potentiellen Wegpunkt hinzufügen
          end;
        end;
      end;
    end;
  finally
    nNodes.Free;
  end;
end;