Wegfindungsroutine

Previous topic - Next topic

kanonet

Ich habe eine kleine Wegfindungsroutine geschrieben, sie basiert auf dem A*-Algorithmus und ermöglicht die Bewegung in acht Richtungen, also auch diagonal. Wegkosten können beachtet werden.
Aus Geschwindigkeitsgründen erfolgt die Wegsuche gleichzeitig vom Start- und dem Zielpunkt aus, was bei großen Karten zu kleineren Abweichungen in der Mitte führen kann.

Zur Benutzung:
Init_FindPath() einmal ausführen, also vor der Hauptschleife.
Advanced_FindPath() kann dann beliebig oft aufgerufen werden.
Parameter sind ähnlich Gernots FINDPATH(). Zu beachten ist noch, dass beide Routinen im Debugmodus signifikant verlangsamt werden, außerdem dauert die Berechnung um so länger, je mehr begehbare Felder es gibt.

Ich habe mich bemüht eine möglichst hohe Geschwindigkeit zu ermöglichen. Ich würde mich freuen, wenn es dem einen oder anderen nützen würde, und/oder ich weitere Verbesserungsvorschläge bekomme, vielleicht findet sich ja jemand in meine Kommentierung hinein. :D

Viele Grüße.

Hier die Routine in einem kleinen Beispiel, wollt ihr sie selbst nutzen, dann einfach die beiden Funktionen kopieren:

EDIT: ein kleiner aber gravierender Fehler wurde korrigiert. Schaut euch aber auch diesen Algorithmus an und entscheidet dann, was eure Bedürfnisse besser erfüllt.
Code (glbasic) Select
// --------------------------------- //
// Project: Wegfindung
// Autor: Kanonet
// Start: Tuesday, October 19, 2010
// Update: July 23, 2011
// IDE Version: 8.125

LOCAL map[] // dies ist die Karte mit der die Wegfindungsrotiene arbeitet
GLOBAL AFP_mapmax_x%, AFP_mapmax_y% // Größe der Karte; wir von Init_FindPath gesetzt

LOCAL x%, y%, zx%, zy%, lger[], lkan[], mx, my, mr, ml, timeger, timekan, mdown

Init_FindPath(map[], 80,60) // Initialisierung der Wegfindung

SETSCREEN AFP_mapmax_x*10, AFP_mapmax_y*10, 0
SYSTEMPOINTER TRUE

x=0
y=2
zx=AFP_mapmax_x-1
zy=AFP_mapmax_y-1


REPEAT

MOUSESTATE mx, my, mr, ml
IF mr // rechte Maustaste -> Startpunkt setzen
x=mx/10
y=my/10
ELSEIF ml // linke Maustaste -> Zielpunkt setzen
zx=mx/10
zy=my/10
ELSEIF MOUSEAXIS(5) OR KEY(56) // mittlere Maustaste -> passierbarkeit dieses Runkts der Karte ändern
IF mdown=0
mdown=-map[mx/10][my/10]
ENDIF
IF KEY(29) OR KEY (157) THEN mdown=5
map[mx/10][my/10]=mdown
ELSE
mdown=0
ENDIF

IF KEY(57) // Wegsuchen starten und benötigte Zeit messen
timeger=GETTIMERALL()
FINDPATH(map[], lger[], 0.5, x, y, zx, zy)
timeger=GETTIMERALL()-timeger
timekan=GETTIMERALL()
Advanced_FindPath(map[], lkan[], 1, x, y, zx, zy)
timekan=GETTIMERALL()-timekan
ENDIF

FOR i=0 TO AFP_mapmax_x-1 // Karte zeichnen
FOR j=0 TO AFP_mapmax_y-1
IF (map[i][j])<=0 THEN DRAWRECT 10*i, 10*j, 10, 10, RGB(255,255,255)
ALPHAMODE 0.5
IF map[i][j]>1 THEN DRAWRECT 10*i, 10*j, 10, 10, RGB(128,128,128+20*map[i][j])
ALPHAMODE 0
NEXT
NEXT

FOR i=1 TO LEN(lger)-1 // Gernots Weg einzeichnen
DRAWLINE 10*lger[i-1][0]+4,  10*lger[i-1][1]+4,  10*lger[i][0]+4,  10*lger[i][1]+4, RGB(255,0,0)
DRAWRECT 10*lger[i][0]+3,  10*lger[i][1]+3, 3, 3, RGB(255,0,0)
NEXT

FOR i=1 TO LEN(lkan)-1 // Kanonets Weg einzeichnen
DRAWLINE 10*lkan[i-1][0]+3,  10*lkan[i-1][1]+3,  10*lkan[i][0]+3,  10*lkan[i][1]+3, RGB(0,255,0)
DRAWRECT 10*lkan[i][0]+2,  10*lkan[i][1]+2, 3, 3, RGB(0,255,0)
NEXT

PRINT "A", 10*x, 10*y // Startpunkt markieren
PRINT "Z", 10*zx, 10*zy // Zielpunkt markieren

PRINT "Gernot: "+timeger+"ms "+LEN(lger)+" Schritte", 0, 0
PRINT "Kanonet: "+timekan+"ms "+LEN(lkan)+" Schritte", 0, 10

PRINT "[SPACE] - Weg suchen         [Maustasten] - Start/Ziel/Karte verändern", 135, 590

SHOWSCREEN

WAIT(20) // Timer

UNTIL KEY(1)
END



//! Timerfunction - NOT NEEDED FOR PATHFINDING!
//! Its just to slow down the program.
//? Timerfunktion - WIRD NICHT FÜR DIE WEGFINDUNG BENÖTIGT!
//? Dient ledglich dazu, dass Programm zu verlangsamen.
// \param ms - Mainloop will last ms milliseconds. | Hauptschleife wird ms Millisekunden dauern.
// \return Nothing. | Nichts.
FUNCTION WAIT: ms
STATIC WAIT_time
WAIT_time = ms - GETTIMERALL() + WAIT_time
IF WAIT_time<0 THEN WAIT_time=0
SLEEP WAIT_time
WAIT_time=GETTIMERALL()
ENDFUNCTION

//! Initialise the pathfinding. Use it once, before your mainloop.
//? Initialisiert die Wegfindung. Muss einmal ausgeführt werden, am besten vor der Hauptschleife.
// \param AFP_map[] - Map that that will be used for pathfinding. | Karte die durch die Weg genutz werden soll.
// \param x,y  - Size of the Map, you want to create. | Größe der Karte, die erzeugt werden soll.
// \return Nothing, but some global will be set. | Nichts, jedoch werden einige Globals erzeugt.
FUNCTION Init_FindPath: AFP_map[], x, y
GLOBAL AFP_mapmax_x=x
GLOBAL AFP_mapmax_y=y
LOCAL i%
DIM AFP_map[AFP_mapmax_x][AFP_mapmax_y] // Karte dimensionieren
// alle Felder der Karte auf 1 setzen, sie als passierbar machen
SETCURRENTDIR("..")
SETCURRENTDIR("..")
SETCURRENTDIR("Bismarck")
SETCURRENTDIR("Karte")
OPENFILE(1, "karte2.txt", 1)
READLINE 1, AFP_mapmax_y
READLINE 1, AFP_mapmax_y
FOR x=0 TO AFP_mapmax_x-1
FOR y=0 TO AFP_mapmax_y-1
READBYTE 1, i%
AFP_map[x][y]=-2*(i-0.5)
NEXT
NEXT
CLOSEFILE 1

// "Bewegungsrichtung" zur Erstellung weiterer Knoten
GLOBAL AFP_dirx%[], AFP_diry%[], AFP_dirz%[]
DIMDATA AFP_dirx[], 0, 1, 0, -1, 1, 1, -1, -1
DIMDATA AFP_diry[], -1, 0, 1, 0, -1, 1, 1, -1
DIM AFP_dirz[8]
FOR i=0 TO 7
AFP_dirz[i] = (AFP_dirx[i] AND AFP_diry[i])+1
NEXT

// map dient der Ermittlung der Entfernung des jeweils aktuellen Knotens zu Ziel, vorausberechnung erfolgt um Geschwindigkeit gut zu machen
GLOBAL AFP_heuristic%[]
DIM AFP_heuristic[AFP_mapmax_x][AFP_mapmax_y]
FOR y=0 TO AFP_mapmax_y-1
FOR x=0 TO AFP_mapmax_x-1
AFP_heuristic[x][y]=SQR(x*x+y*y)
NEXT
NEXT
ENDFUNCTION

//! This function search a path.
//? Diese Funktion sucht einen Weg.
// \param Look FINDPATH() for details about the parameters! | Details der Parameter können unter FINDPATH() nachgesehen werden.
// \param AFP_map[] - Map, where you search a path. | Karte, durch die ein Weg gefunden werden soll.
// \param heuristik - 0=sortest 1=cheapest Way. | 0=kürzester 1=günstigster Weg.
// \param startx,starty - Startpoint. | Startpunkt.
// \param endx,endy - Destination. | Zielpunkt.
// \return Path length, or 0 if no path was found. | Weglänge, oder 0 falls kein Weg gefunden wurde.
FUNCTION Advanced_FindPath: AFP_map[], loesung[], heuristik, startx%, starty%, endx%, endy%

REDIM loesung[0]

// Validität der Parameter prüfen
IF startx=endx AND starty=endy THEN RETURN 0
IF endx<0 OR endy<0 OR endx>AFP_mapmax_x-1 OR endy>AFP_mapmax_y-1 THEN RETURN 0
IF startx<0 OR starty<0 OR startx>AFP_mapmax_x-1 OR starty>AFP_mapmax_y-1 THEN RETURN 0
IF AFP_map[endx][endy]<=0 THEN RETURN 0
IF AFP_map[startx][starty]<=0 THEN RETURN 0
IF heuristik<0 OR heuristik>1 THEN heuristik=1

STATIC nodemap[] // dient primär der Kennzeichnung bereits besuchter Felder
REDIM nodemap[0]
REDIM nodemap[AFP_mapmax_x][AFP_mapmax_y][2]

STATIC path[] AS TNode // Zwischenspeicher des gefundenen Wegs
REDIM path[0]

// Variablen mit der Endung -s gehören zur Suche von Start, Variablen mit der Endung -g gehören zur Suche vom Ziel
STATIC x%, y%, finish%, cost%, delta%, nodezahls%, nodezahlg%
finish=0; nodezahls%=0; nodezahlg%=0


STATIC opens[] AS TNode // Openlists für noch zu untersuchende Knoten
REDIM opens[1]
STATIC openg[] AS TNode
REDIM openg[1]
STATIC topen AS TNode

STATIC nodes[] AS TNode // Closedlists für abschließend untersuchte Knoten
REDIM nodes[1]
STATIC nodeg[] AS TNode
REDIM nodeg[1]
STATIC tnode AS TNode

// Startpunktdaten einlesen
opens[0].x = startx
opens[0].y = starty
opens[0].cost = 0
opens[0].parent = -1
nodemap[startx][starty][0] = 1

// Zielpunktdaten einlesen
openg[0].x = endx
openg[0].y = endy
openg[0].cost = 0
openg[0].parent = -1
nodemap[endx][endy][0] = 2

REPEAT // Wegsuche beginnen

// Wegsuche ausgehend vom Startpunkt
x = 0; cost = 2147483647
FOREACH topen IN opens[] // Ermittlet günstigsten Open-Knoten
delta = AFP_heuristic[ABS(topen.x-endx)][ABS(topen.y-endy)]
IF topen.cost+delta<cost
cost = topen.cost+delta
y = x
ENDIF
INC x
NEXT
tnode = opens[y]
DIMPUSH nodes[], tnode // übernimmt den Knoten in die Closedlist und löscht ihn aus der Openlist
DIMDEL opens[], y
INC nodezahls

FOR i=0 TO 7 // Untersuchung der 8 benachbarten Knoten
x = tnode.x + AFP_dirx[i]
y = tnode.y + AFP_diry[i]
IF x>-1 AND y>-1 AND x<AFP_mapmax_x AND y<AFP_mapmax_y // Knoten liegt auf der Karte
    IF AFP_map[x][y]>0 AND AFP_map[x][tnode.y]>0 AND AFP_map[tnode.x][y]>0 // Knoten ist begehbar
IF nodemap[x][y][0]=0 // Knoten wurde noch nicht besucht
topen.x = x
topen.y = y
topen.parent = nodezahls
topen.cost = tnode.cost + heuristik * (AFP_map[x][y]-1)  + AFP_dirz[i]
DIMPUSH opens[], topen // erzeuge neuen Openknoten
nodemap[x][y][0]=1
nodemap[x][y][1]=nodezahls
ELSEIF nodemap[x][y][0]=2 // Knoten wurde bereits durch die andere Suchroutine erreicht -> Suche erfolgreich
topen.x = x
topen.y = y
topen.parent = nodezahls
topen.cost = tnode.cost + heuristik * (AFP_map[x][y]-1)  + AFP_dirz[i] + nodeg[nodemap[x][y][1]].cost
DIMPUSH path[], topen // erzeuge neuen Openknoten
finish = 1
ENDIF
ENDIF
ENDIF
NEXT

IF NOT LEN(opens[]) THEN finish=3 // Wenn kein Openknoten mehr existiert -> keine Verbindung zwischen Start und Ziel möglich -> Suche abbrechen

IF finish THEN BREAK

// Wegsuche ausgehend vom Zielpunkt
x = 0; cost = 2147483647
FOREACH topen IN openg[] // Ermittlet günstigsten Open-Knoten
delta = AFP_heuristic[ABS(topen.x-startx)][ABS(topen.y-starty)]
IF topen.cost+delta<cost
cost = topen.cost+delta
y = x
ENDIF
INC x
NEXT
tnode = openg[y]
DIMPUSH nodeg[], tnode // übernimmt den Knoten in die Closedlist und löscht ihn aus der Openlist
DIMDEL openg[], y
INC nodezahlg

FOR i=0 TO 7 // Untersuchung der 8 benachbarten Knoten
x = tnode.x + AFP_dirx[i]
y = tnode.y + AFP_diry[i]
IF x>-1 AND y>-1 AND x<AFP_mapmax_x AND y<AFP_mapmax_y // Knoten liegt auf der Karte
IF AFP_map[x][y]>0 AND AFP_map[x][tnode.y]>0 AND AFP_map[tnode.x][y]>0 // Knoten ist begehbar
IF nodemap[x][y][0]=0 // Knoten wurde noch nicht besucht
topen.x = x
topen.y = y
topen.parent = nodezahlg
topen.cost = tnode.cost + heuristik * (AFP_map[x][y]-1)  + AFP_dirz[i]
DIMPUSH openg[], topen // erzeuge neuen Openknoten
nodemap[x][y][0]=2
nodemap[x][y][1]=nodezahlg
ELSEIF nodemap[x][y][0]=1 // Knoten wurde bereits durch die andere Suchroutine erreicht -> Suche erfolgreich
topen.x = x
topen.y = y
topen.parent = nodemap[x][y][1]
topen.cost = tnode.cost + heuristik * (AFP_map[x][y]-1)  + AFP_dirz[i] + nodes[topen.parent].cost
DIMPUSH path[], topen // erzeuge neuen Openknoten
nodemap[x][y][0]=2
nodemap[x][y][1]=nodezahlg
finish = 1
ENDIF
ENDIF
ENDIF
NEXT

IF NOT LEN(openg[]) THEN finish=3 // Wenn kein Openknoten mehr existiert -> keine Verbindung zwischen Start und Ziel möglich -> Suche abbrechen

UNTIL finish // Wegsuche beendet

IF finish=3 THEN RETURN 0 // es wurde kein Weg gefunden

    x = 0; y=0; cost = 2147483647
FOREACH topen IN path[] // Ermittlet günstigsten Open-Knoten
IF topen.cost<cost
cost = topen.cost
y = x
ENDIF
INC x
NEXT
topen = path[y]
cost=topen.parent
delta=nodemap[topen.x][topen.y][1]
REDIM path[0]
DIMPUSH path[], topen

tnode = nodes[cost] // gefundenen Pfad vom Startpunkt aus auswerten
WHILE tnode.parent <> -1
DIMPUSH path[], tnode
tnode = nodes[tnode.parent]
WEND
DIMPUSH path[], tnode
REDIM nodes[0]

cost = LEN(path[])
REDIM loesung[cost][2]
FOR i=cost-1 TO 0 STEP -1
loesung[i][0] = path[cost-i-1].x
loesung[i][1] = path[cost-i-1].y
NEXT
REDIM path[0]

IF nodezahlg // gefundenen Pfad vom Zielpunkt aus auswerten
tnode = nodeg[delta]
WHILE tnode.parent <> -1
delta=tnode.parent
DIMPUSH path[], tnode
tnode = nodeg[tnode.parent]
WEND
DIMPUSH path[], tnode
REDIM nodeg[0]

delta = cost+LEN(path[])
REDIM loesung[delta][2]
FOR i=delta-1 TO cost STEP -1
loesung[i][0] = path[i-cost].x
loesung[i][1] = path[i-cost].y
NEXT
REDIM path[0]
ENDIF

RETURN delta

ENDFUNCTION

// Knoten, ein (möglicher) Schritt zum Ziel
TYPE TNode
cost% // bisherige Wegkosten
parent% // ID des Vorgängerknotens
x%
y%
ENDTYPE
Lenovo Thinkpad T430u: Intel i5-3317U, 8GB DDR3, NVidia GeForce 620M, Micron RealSSD C400 @Win7 x64

kanonet

FINDPATH() hat mit meiner Funktion erstmal nichts zu tun.

Jedoch habe ich sie mir als Vorbild genommen und die Parameter und den Rückgabewert fast identisch gestaltet, wodurch mir eine Erklärung der derselben hoffentlich abgenommen wurde und der Nutzer sich schnell hinein findet.

Wenn du eine Karte hast, die von FINDPATH() gelesen werden kann, dann kann sie auch von meiner Funktion gelesen werden. Welche der beiden Funktionen du dann verwendest entscheidet sich an Hand deiner Anforderungen:

Suchst du eine Funktion, die:

  • in 4 Richtungen sucht -> verwende Gernots FINDPATH()
  • in 8 Richtungen sucht -> verwende Init_AdvancedFindpath() und Advanced_FindPath()
  • von dir selbst an weitere Parameter oder zusätzliche Karten angepasst werden kann -> verwende Init_AdvancedFindpath() und Advanced_FindPath()


Im ersten Post befinden sich eben nicht nur meine beiden Funktionen, sondern diese sind in einem Beispiel-Projekt eingearbeitet, welches den gleichen Weg mit beiden Wegfindungsroutinen sucht und dem Nutzer somit einen Vergleich beider Ergebnisse und ihrer Geschwindigkeiten ermöglicht. In einem Spiel würde natürlich (vermutlich) nur eine der beiden verwendet werden. Das obige Beispiel soll die Vor- und Nachteile demonstrieren und dem Programmierer somit die Entscheidung für eine der beiden vereinfachen. Ich empfehle unbedingt die Verwendung der Mausmitteltaste im Beispiel. ;)

Ich hoffe ich konnte deine Frage beantworten? Weitere Fragen und Verbesserungsvorschläge sind weiterhin erwünscht. ;)

Viele Grüße,
Kanonet.
Lenovo Thinkpad T430u: Intel i5-3317U, 8GB DDR3, NVidia GeForce 620M, Micron RealSSD C400 @Win7 x64

Kitty Hello

FINDPATH geht auch 8 Richtungen. Deine Funktion kann aber viel besser angepasst werden. Ist gut!

kanonet

Quote from: Kitty Hello on 2010-Dec-21
FINDPATH geht auch 8 Richtungen. Deine Funktion kann aber viel besser angepasst werden. Ist gut!
Könntest du das bitte näher erläutern/mit Beispiel belegen? In der Hilfe befindet sich nur ein Beispiel für die 4-Wege-Suche. Aber du hast natürlich Recht, größter Vorteil der Routine ist es, dass man sie anpassen kann, wie man es braucht (ist auch nicht schwer sich auf 4 Richtungen zu beschränken ;)), ich dachte da Beispielsweise an die Verwendung mehrerer Karten (eine für die geografischen Beschaffenheiten, eine für den Territorialbesitz, eine für besonders gefährliche Gegenden,...) in einer Suche, wäre einfach und schnell.

Quote from: Ocean on 2010-Dec-21Danke Kanonet für diese Zusatzinfos!
Gern geschehen. Feedback und Fragen sind weiterhin erwünscht! 8)


-----------------------------------------------------------------------

Ich habe den Code im Startpost aktualisiert, es gab ein kleines Problem im Zusammenspiel der beiden Suchroutinen, welches behoben wurde, außerdem wurde dem Demonstrationsprogramm eine einfache Waitfunktion hinzugefügt um den Prozessor überflüssige Last abzunehmen (hat nichts mit der Wegfindungsroutine zu tun).
Lenovo Thinkpad T430u: Intel i5-3317U, 8GB DDR3, NVidia GeForce 620M, Micron RealSSD C400 @Win7 x64

BumbleBee

Ist deine Wegfindungsroutine eventuell auch für Navigationssysteme gedacht? Denn die finden nicht immer den richtigen Weg.  :D

Cheers
The day will come...

CPU Intel(R) Core(TM) i5-3570k, 3.4GHz, AMD Radeon 7800 , 8 GB RAM, Windows 10 Home 64Bit

kanonet

Ich bin mir nicht ganz sicher, ob ich die Intention deiner Frage richtig verstanden habe.

Möchtest du darauf hinweisen, dass sie falsche Wege gefunden hat? Wenn ja, dann bitte ich darum, dass du mir einen Screenshot oder ähnliches zukommen lässt, damit ich die Situation rekonstruieren und den Fehler suchen und beheben kann.

Ansonsten, falls es wirklich nur eine Frage ist:
Kurze Antwort: ja könnte wohl funktionieren,  jedoch wurde sie nicht dafür geschaffen/optimiert.
Lange Antwort: theoretisch könnte sie dies vermutlich leisten, jedoch würde ich sie dafür umschreiben.  Die hier gepostete Form ist auf Geschwindigkeit optimiert, sie soll nach Möglichkeit zur Laufzeit eingesetzt werden können; soll das errechnen eines (oder zur Not auch mehrerer) Wege ermöglichen, ohne das Programm/Spiel ins stocken zu bringen. Für ein Navi würde ich auf einen langsameren, aber noch exakteren Algorithmus zurückgreifen (mit nur einem Suchlauf aus nur einer Richtung), schließlich wäre es da nicht schlimm, wenn die Berechnung einige Sekunden dauern würde, diese ist da ja das einzige, was man berechnet... Für ein Navi würde man aber vermutlich keine Karte (Raster) verwende wie ich es hier tat (für die meisten Spiele sollte das in Ordnung sein, ist auch einfach zu handhaben), sondern man würde wohl eher auf eine Form neuronales Netz zurückgreifen (die wenigsten Straßennetze sind ausschließlich rechtwinklig ;)), wobei die Routine natürlich auch daran (ohne zu großem Aufwand) angepasst werden könnte. Letztendlich entscheidet ohnehin das Kartenmaterial alles...
Lenovo Thinkpad T430u: Intel i5-3317U, 8GB DDR3, NVidia GeForce 620M, Micron RealSSD C400 @Win7 x64

backslider

Ich glaube eher, dass BumbleBee meint, dass Navis den Weg nicht immer richtig finden.
Somit wäre deine Routine nicht brauchbar, weil sie immer funktioniert. :D

Falls ich mich irre, wird BumbleBee mich schon korrigieren. :P

BumbleBee

Hehehe,  richtig backslider. :) Das war auch eigentlich nur so als Vergleich und Spass gemeint. Denn deine Routine funktioniert wenigstens.  Daher würde man mit deinem Programm nämlich eher an ein Ziel kommen als manchmal mit so Navis. Naja, vielleicht könntest du denen ja trotzdem mal zeigen wo es lang geht, im wahrsten Sinne des Wortes.   =D

Cheers
The day will come...

CPU Intel(R) Core(TM) i5-3570k, 3.4GHz, AMD Radeon 7800 , 8 GB RAM, Windows 10 Home 64Bit

kanonet

Hehe, na dann danke ich für die Blumen. :enc:
Lenovo Thinkpad T430u: Intel i5-3317U, 8GB DDR3, NVidia GeForce 620M, Micron RealSSD C400 @Win7 x64