als ich auf der Suche nach einem Wegfindungs-Algorithmus für 3D-Projekte war und nur Map-basierte gefunden habe (also mit 'nem x*y-Raster) habe ich mir eins auf Wegpunkt-Basis gebastelt das mit dem Dijkstra-Algorithmus arbeitet. Vielleicht kann das außer mir noch einer brauchen. Hier der Code/Test-Programm. Ich hoffe ich habe genügend kommentiert (kommentiere für gewöhnlich ja nix
):
Code: Alles auswählen
InitEngine3D()
InitSprite()
InitKeyboard()
Structure Str_PathConnection
*Waypoint
Weight.d
EndStructure
Structure Str_PathWaypoint
Entity.l
EndStructure
Structure Str_Path
*Waypoint
*Predecessor
Distance.d
Visited.c
List Connection.Str_PathConnection()
Nr.l
EndStructure
Global NewList Waypoint.Str_PathWaypoint()
Global NewList Path.Str_Path()
#WallPick = 1 << 1
#WaypointPick = 1 << 2
OpenWindow(0,100,100,800,600,"Test")
OpenWindowedScreen(WindowID(0),0,0,800,600)
; Camera
CreateCamera(1,0,0,100,100)
MoveCamera(1,0,100,100,#PB_Relative)
RotateCamera(1,-50,0,0)
; Textures
CreateTexture(0,256,256)
StartDrawing(TextureOutput(0))
Box(0,0,255,255,RGB(200,0,0))
StopDrawing()
CreateTexture(1,256,256)
StartDrawing(TextureOutput(1))
Box(0,0,255,255,RGB(0,0,200))
StopDrawing()
CreateTexture(2,256,256)
StartDrawing(TextureOutput(2))
Box(0,0,255,255,RGB(0,200,200))
StopDrawing()
CreateTexture(3,256,256)
StartDrawing(TextureOutput(3))
Box(0,0,255,255,RGB(0,200,0))
StopDrawing()
; Materials
CreateMaterial(0,TextureID(0))
CreateMaterial(1,TextureID(1))
CreateMaterial(2,TextureID(2))
CreateMaterial(3,TextureID(3))
; Floor
CreatePlane(2,100,100,10,10,1,1)
CreateEntity(3,MeshID(2),MaterialID(0))
; Walls
CreateCube(4,10)
CreateEntity(5,MeshID(4),MaterialID(1),0,0,0,#WallPick)
ScaleEntity(5,5,2,0.3)
CreateEntity(8,MeshID(4),MaterialID(1),-10,0,0,#WallPick)
ScaleEntity(8,0.3,2,8)
; Light
CreateLight(6,RGB(255,255,255),0,100,0)
AmbientColor(RGB(25, 25, 25))
; Waypoint, start and aim mesh
Global WaypointMesh=CreateCylinder(#PB_Any,0.2,15)
Hero=CreateEntity(#PB_Any,MeshID(WaypointMesh),MaterialID(2),10,0,10,#WaypointPick)
ScaleEntity(Hero,3,1,3)
Aim=CreateEntity(#PB_Any,MeshID(WaypointMesh),MaterialID(3),0,0,-20,#WaypointPick)
ScaleEntity(Aim,3,1,3)
; This procedure creates a waypoint graph (Path()) with all predifined waypoints in it and the start and aim mesh
; and inits the dijkstra algorithm
; StartMesh = The mesh to walk the path
; AimMesh = A mesh that represents the aim position
; YOffset = An optional offset on the y-axis (use this to make sure the line of sight does not interfere with the ground)
Procedure Pathfinding_Init(StartMesh,AimMesh,YOffset.d=0)
Protected *TempWaypoint.Str_Path
Protected LineCount=0
ClearList(Path())
; Destroy all former 3d lines
For t=100 To 150
If IsMesh(t)
FreeMesh(t)
EndIf
Next
; Add Start and Aim to the waypoint graph
AddElement(Path())
Path()\Waypoint=StartMesh
Path()\Predecessor=-1
Path()\Distance=0
Path()\Nr=0
AddElement(Path())
Path()\Waypoint=AimMesh
Path()\Predecessor=-1
Path()\Distance=Infinity()
Path()\Nr=1
; Add all predifined waypoints to the graph
Nr=2
ForEach Waypoint()
AddElement(Path())
Path()\Waypoint=Waypoint()\Entity
Path()\Predecessor=-1
Path()\Distance=Infinity()
Path()\Nr=Nr
Nr+1
Next
; Render the world to have an up-to-date world state for using RayCast()
RenderWorld()
; Check all connections between waypoints
ForEach Path()
; ClearList(Path()\Connection())
*TempWaypoint=@Path()
PushListPosition(Path())
ForEach Path()
If @Path()<>*TempWaypoint
; Check line of sight with a ray cast
Res=RayCast(EntityX(*TempWaypoint\Waypoint),EntityY(*TempWaypoint\Waypoint)+YOffset,EntityZ(*TempWaypoint\Waypoint),EntityX(Path()\Waypoint)-EntityX(*TempWaypoint\Waypoint),0,EntityZ(Path()\Waypoint)-EntityZ(*TempWaypoint\Waypoint),#WallPick|#WaypointPick)
; Claculate the distance between the waypoint to have a weighted connection
DistWaypoint.d=Sqr(Pow(EntityX(*TempWaypoint\Waypoint)-EntityX(Path()\Waypoint),2.0) + Pow(EntityY(*TempWaypoint\Waypoint)-EntityY(Path()\Waypoint),2.0) + Pow(EntityZ(*TempWaypoint\Waypoint)-EntityZ(Path()\Waypoint),2.0))
; Debug "Cast ("+StrD(EntityX(*TempWaypoint\Waypoint))+", "+StrD(EntityY(*TempWaypoint\Waypoint))+", "+StrD(EntityZ(*TempWaypoint\Waypoint))+") - ("+StrD(EntityX(Path()\Waypoint))+", "+StrD(EntityY(Path()\Waypoint))+", "+StrD(EntityZ(Path()\Waypoint))+")"
If Res=Path()\Waypoint
; Add the found connection to the connection list of the currently tested waypoint
AddElement(*TempWaypoint\Connection())
*TempWaypoint\Connection()\Waypoint=@Path()\Waypoint
*TempWaypoint\Connection()\Weight=DistWaypoint
; Also add the connection to the list of connections of the waypoint that was found if it is not already in the list
Found=0
ForEach Path()\Connection()
If Path()\Connection()\Waypoint=@*TempWaypoint\Waypoint
Found=1
Break
EndIf
Next
If Found=0
AddElement(Path()\Connection())
Path()\Connection()\Waypoint=@*TempWaypoint\Waypoint
Path()\Connection()\Weight=DistWaypoint
EndIf
; Show the connection as a 3d line
CreateLine3D(100+LineCount,EntityX(*TempWaypoint\Waypoint),EntityY(*TempWaypoint\Waypoint)+YOffset,EntityZ(*TempWaypoint\Waypoint),RGB(255,255,255),EntityX(Path()\Waypoint),EntityY(Path()\Waypoint)+YOffset,EntityZ(Path()\Waypoint),RGB(255,255,255))
LineCount+1
EndIf
EndIf
Next
PopListPosition(Path())
Next
EndProcedure
; This is the dijkstra algorithm
; The shortest path can be found after using this procedure by starting with the aim mesh in the Path() list and check all predecessors until the start mesh is found
; StartMesh = The mesh to walk the path
; AimMesh = A mesh that represents the aim position
Procedure Pathfinding_FindPath(StartMesh,AimMesh)
; Repeat until all graph entries are visited
Repeat
Searching=0
Dist.d=Infinity()
; Find the waypoint with the lowest distance
ForEach Path()
If Path()\Visited=0
If Path()\Distance<Dist
Searching=1
*CurrentWaypoint.Str_Path=@Path()
Dist=Path()\Distance
EndIf
EndIf
Next
If Searching=1
*CurrentWaypoint\Visited=1
ForEach *CurrentWaypoint\Connection()
*NextWaypoint.Str_Path=*CurrentWaypoint\Connection()\Waypoint
If *NextWaypoint\Visited=0
If *CurrentWaypoint\Distance+*CurrentWaypoint\Connection()\Weight<*NextWaypoint\Distance
*NextWaypoint\Distance=*CurrentWaypoint\Distance+*CurrentWaypoint\Connection()\Weight
*NextWaypoint\Predecessor=*CurrentWaypoint\Waypoint
EndIf
EndIf
Next
EndIf
Until Searching=0
EndProcedure
; Parses the Path() list from end to start and prints the steps in the debug window
Procedure Pathfinding_ShowPath(StartMesh,AimMesh)
ForEach Path()
If Path()\Waypoint=AimMesh
*Temp.Str_Path=@Path()
; Check if path is found or not
If Path()\Predecessor=-1
Debug "There is no path!"
Else
; Show path from aim to start (found by the predecessors)
Repeat
Debug "No: "+Str(*temp\Nr)+" Pos: "+StrD(EntityX(*Temp\Waypoint))+", "+StrD(EntityY(*Temp\Waypoint))+", "+StrD(EntityZ(*Temp\Waypoint))+" - Predecessor: "+Str(*Temp\Predecessor)
If *Temp\Predecessor=-1
; The start is reached -> break loop
Break
EndIf
ForEach Path()
If Path()\Waypoint=*Temp\Predecessor
*Temp=@Path()
Break
EndIf
Next
ForEver
Break
EndIf
EndIf
Next
Debug "-----------------------------------"
EndProcedure
; Add a waypoint
Procedure Pathfinding_AddWaypoint(x.d,y.d,z.d)
AddElement(Waypoint())
Waypoint()\Entity=CreateEntity(#PB_Any,MeshID(WaypointMesh),MaterialID(1),x,y,z,#WaypointPick)
ScaleEntity(Waypoint()\Entity,3,1,3)
EndProcedure
; Add some waypoints
Pathfinding_AddWaypoint(30,0,0)
Pathfinding_AddWaypoint(-40,0,5)
Pathfinding_AddWaypoint(-30,0,-30)
; Render world has to be called once, otherwise RayCast does not work
RenderWorld()
; Make a first pathfinding
Pathfinding_Init(Hero,Aim,4)
Pathfinding_FindPath(Hero,Aim)
; Create overlay sprite for text output
Overlay=CreateSprite(#PB_Any,800,600)
StartDrawing(SpriteOutput(Overlay))
DrawingMode(#PB_2DDrawing_Transparent)
DrawText(0,0,"Use cursor keys to move the walls.")
DrawText(0,15,"Use space key to calculate shortest path.")
DrawText(0,30,"Use D key to show distance table.")
Txt.s="Light blue is the start mesh (No. 0)"
DrawText(ScreenWidth()-TextWidth(Txt),0,Txt)
Txt.s="Green is the aim mesh (No. 1)"
DrawText(ScreenWidth()-TextWidth(Txt),15,Txt)
ForEach Path()
DrawText(CameraProjectionX(1,EntityX(Path()\Waypoint),EntityY(Path()\Waypoint),EntityZ(Path()\Waypoint)),CameraProjectionY(1,EntityX(Path()\Waypoint),EntityY(Path()\Waypoint),EntityZ(Path()\Waypoint)),"("+StrD(EntityX(Path()\Waypoint))+", "+StrD(EntityY(Path()\Waypoint))+", "+StrD(EntityZ(Path()\Waypoint))+") - No: "+Str(Path()\Nr))
Next
StopDrawing()
; Main loop
Repeat
ExamineKeyboard()
If KeyboardPushed(#PB_Key_Left)
MoveEntity(8,-1,0,0,#PB_Relative)
Pathfinding_Init(Hero,Aim,4)
Pathfinding_FindPath(Hero,Aim)
EndIf
If KeyboardPushed(#PB_Key_Right)
MoveEntity(8,1,0,0,#PB_Relative)
Pathfinding_Init(Hero,Aim,4)
Pathfinding_FindPath(Hero,Aim)
EndIf
If KeyboardPushed(#PB_Key_Down)
MoveEntity(5,0,0,1,#PB_Relative)
Pathfinding_Init(Hero,Aim,4)
Pathfinding_FindPath(Hero,Aim)
EndIf
If KeyboardPushed(#PB_Key_Up)
MoveEntity(5,0,0,-1,#PB_Relative)
Pathfinding_Init(Hero,Aim,4)
Pathfinding_FindPath(Hero,Aim)
EndIf
If KeyboardReleased(#PB_Key_Space)
Pathfinding_ShowPath(Hero,Aim)
EndIf
If KeyboardReleased(#PB_Key_D)
Pathfinding_Init(Hero,Aim,4)
Pathfinding_FindPath(Hero,Aim)
ForEach Path()
Debug "No: "+Str(Path()\Nr)+" - Distance to start mesh: "+StrD(Path()\Distance)
Next
Debug "-----------------------------------"
EndIf
RenderWorld()
DisplayTransparentSprite(Overlay,0,0)
FlipBuffers()
Until WindowEvent()=#PB_Event_CloseWindow
Mac