This is a project that I've worked on and off for a few months. It is waypoint utilities that allow you to place waypoints' connect them and use them. It's great for pathfinding for enemies throughout enclosed areas with lots of turns and bends. It uses the A* algorithm to successfuly find the shortest possible route to a destination. Although it still needs some improvement, I think it's good enough to post about.
All you need is to call 4 functions to setup and use the waypoints.
WP_Setup(amount) 'Call this first before anything, the amount is the maximum waypoints you have.
WP_MakeWaypoint(Waypoint Num,X,Y,Z) `This makes the waypoint and places it. Make sure when creating waypoints to not skip numbers.
WP_ConnectWaypoints(WP1,WP2) `This connects waypoints together
WP_MoveObjToPos(obj,x,y,z) `This gets called up in the main loop. It moves the object along the waypoints to get to the x,y,z position.
WP_DisplayWaypointNumber() This displays the waypoint number on the screen where it is located.
WP_DrawConnections() This draws the waypoint connections visibly.
UPDATES:
7/10/10: Fixed bug where object would follow a path into a dead-end
Heres the utility
Rem ***** Included Source File *****
type Waypoint
Global WpNum as Dword
Global X as float
Global Y as float
Global Z as float
endtype
Function WP_Setup(Amount)
Global TotalWP as integer = Amount
Global NextWP as integer = 1
Global Dim Waypoint(TotalWP) as Waypoint
Global Dim WPCon(TotalWP,4)
Global Dim WPPath(100)
Global WPPriority as integer
Global WPAtPosition as integer
Endfunction
Function WP_MakeWaypoint(Waypoint,Xpos#,Ypos#,Zpos#)
Waypoint(Waypoint).WPNum=Waypoint
Waypoint(Waypoint).X=Xpos#
Waypoint(Waypoint).Y=Ypos#
Waypoint(Waypoint).Z=Zpos#
ENDFUNCTION
Function WP_ConnectWaypoints(WP1,WP2)
Node=1
While WPCon(WP1,Node)>0
inc Node
ENDWHILE
WPCon(WP1,Node)=WP2
Node=1
While WPCon(WP2,Node)>0
inc Node
ENDWHILE
WPCon(WP2,Node)=WP1
ENDFUNCTION
Function WP_MoveObjToPos(obj,Xpos#,Ypos#,Zpos#,Speed#)
WPAtPosition=0
ObjX#=Object Position X(obj)
ObjY#=Object Position Y(obj)
ObjZ#=Object Position Z(obj)
ObjToDestDist#=WP_ObjectPosDist(obj,Xpos#,Ypos#,Zpos#)
ClosestWP=Get_Nearest_Waypoint(Xpos#,Ypos#,Zpos#)
ObjToWPDist#=WP_ObjectPosDist(obj,Waypoint(ClosestWP).X,Waypoint(ClosestWP).Y,Waypoint(ClosestWP).Z)
if ObjToDestDist#>0.01 then AtDest=0 else Atdest=1
If AtDest=0
For WP = 1 to TotalWP
if Waypoint(WP).WPnum>0
If WP_WaypointDistPos(Waypoint(WP).WPnum,ObjX#,ObjY#,ObjZ#)<0.01 then WPAtPosition=Waypoint(WP).WPnum
endif
Next WP
if WPPath(NextWP)=Get_Nearest_Waypoint(Xpos#,Ypos#,Zpos#) then PathFound=1
if PathFound=0
if NextWP=1
if WPAtPosition<>0 then Find_Path(obj,Xpos#,Ypos#,Zpos#)
if WPPath(NextWP)=WPAtPosition then inc NextWP
else
if WPPath(NextWP)=WPAtPosition
Find_Path(obj,Xpos#,Ypos#,Zpos#)
inc NextWP
endif
endif
endif
WPPriority=WPPath(NextWP)
Point object obj,Waypoint(WPPriority).X,Waypoint(WPPriority).Y,Waypoint(WPPriority).Z
if WPAtPosition<>Get_Nearest_Waypoint(Xpos#,Ypos#,Zpos#)
move object obj,speed#
endif
endif
ENDFUNCTION
Function Find_Path(obj,Xpos#,Ypos#,Zpos#)
CurrentWP=Get_Nearest_Waypoint(Object position X(obj),Object position Y(obj),Object position Z(obj))
DestWP=Get_Nearest_Waypoint(Xpos#,Ypos#,Zpos#)
dim Avlbl(100)
dim Clsd(100)
PathFound=0
PathPoint=1
if CurrentWP=DestWP then PathFound=1
While PathFound=0 and Pathpoint<100
order=1
For Node=1 to 4
If WPCon(CurrentWP,Node)>0
possible=1
if NodeCount(WPCon(CurrentWP,Node))=1 then possible=0
if NodeCount(WPCon(CurrentWP,Node))=1 and WPCon(CurrentWP,Node)=DestWP then possible=1
If possible=1
Avlbl(order)=WPCon(CurrentWP,Node)
inc order
endif
endif
next Node
dec order
PathDest=1
Repeats=1
While Repeats=1
if Avlbl(PathDest)>0
d1#=WP_WaypointDist(Avlbl(PathDest),CurrentWP)
d2#=WP_WaypointDistPos(Avlbl(PathDest),Xpos#,Ypos#,Zpos#)
Ldist#=d1#+d2#
Repeats=0
Clsd(PathPoint)=Avlbl(PathDest)
endif
if Repeats=1 then inc PathDest
endwhile
For check=1 to order
if Avlbl(check)>0
d1#=WP_WaypointDist(Avlbl(check),CurrentWP)
d2#=WP_WaypointDistPos(Avlbl(check),Xpos#,Ypos#,Zpos#)
dist#=d1#+d2#
if dist#<=Ldist#
Clsd(PathPoint)=Avlbl(check)
Avlbl(check)=-1
Ldist#=dist#
endif
endif
next check
CurrentWP=Clsd(PathPoint)
if CurrentWP=DestWP then PathFound=1
inc PathPoint
endwhile
For x=1 to Pathpoint-1
WPPath(x)=Clsd(x)
next x
endfunction
Function NodeCount(WP)
Nodes=0
for Node=1 to 4
if WPCon(WP,Node)>0 then inc Nodes
next Node
Endfunction Nodes
Function Get_Nearest_Waypoint(Xpos#,Ypos#,Zpos#)
WP=1
FoundWP=0
while FoundWP=0
if Waypoint(WP).WPnum>0
Dist2#=WP_WaypointDistPos(Waypoint(WP).WPnum,Xpos#,Ypos#,Zpos#)
Shortest=Waypoint(WP).WPnum
FoundWP=1
endif
endwhile
for WP=1 to TotalWP
if Waypoint(WP).WPnum>0
Dist1#=WP_WaypointDistPos(Waypoint(WP).WPnum,Xpos#,Ypos#,Zpos#)
if Dist1#<=Dist2#
Shortest=Waypoint(WP).WPnum
Dist2#=Dist1#
endif
endif
next Wp
Endfunction Shortest
Function WP_WaypointDist(WP1,WP2)
Xd#=abs(Waypoint(WP1).X-Waypoint(WP2).X)
Yd#=abs(Waypoint(WP1).Y-Waypoint(WP2).Y)
Zd#=abs(Waypoint(WP1).Z-Waypoint(WP2).Z)
d#=sqrt((Xd#^2)+(Yd#^2)+(Zd#^2))
endfunction d#
Function WP_WaypointDistPos(WP,Xpos#,Ypos#,Zpos#)
Xd#=abs(Waypoint(WP).X-Xpos#)
Yd#=abs(Waypoint(WP).Y-Ypos#)
Zd#=abs(Waypoint(WP).Z-Zpos#)
d#=sqrt((Xd#^2)+(Yd#^2)+(Zd#^2))
endfunction d#
Function WP_ObjectPosDist(obj,Xpos#,Ypos#,Zpos#)
Xd#=abs(object position X(obj)-Xpos#)
Yd#=abs(object position Y(obj)-Ypos#)
Zd#=abs(object position Z(obj)-Zpos#)
d#=sqrt((Xd#^2)+(Yd#^2)+(Zd#^2))
Endfunction d#
Function WP_DisplayWaypointNumber()
For WP = 1 to totalWP
If Waypoint(WP).WPnum>0
X=Waypoint(WP).X
Y=Waypoint(WP).Y
Z=Waypoint(WP).Z
Obj=WP_GetObjNum()
make object plane Obj,0.1,0.1
position object Obj,X,Y,Z
If object in screen(Obj)
TextX=Object Screen X(Obj)
TextY=Object Screen Y(Obj)
Text TextX,TextY, str$(WP)
endif
delete object obj
endif
NEXT WP
ENDFUNCTION
Function WP_DrawConnections()
for Wp=1 to TotalWP
If Waypoint(WP).WPnum>0
OldX=Waypoint(WP).X
OldY=Waypoint(WP).Y
OldZ=Waypoint(WP).Z
for Node=1 to 4
if WPCon(WP,Node)>0
NewX=Waypoint(WPCon(WP,Node)).X
NewY=Waypoint(WPCon(WP,Node)).Y
NewZ=Waypoint(WPCon(WP,Node)).Z
OldObj=WP_GetObjNum()
Make object plane OldObj,0.1,0.1
Position object OldObj,OldX,OldY,OldZ
NewObj=WP_GetObjNum()
Make object plane NewObj,0.1,0.1
Position object NewObj,NewX,NewY,NewZ
line Object Screen X(OldObj),Object Screen Y(OldObj),Object Screen X(NewObj),Object Screen Y(NewObj)
delete object OldObj
delete object NewObj
endif
next Node
endif
Next WP
Endfunction
Function WP_GetObjNum()
obj=1
while object exist(obj)
inc obj
endwhile
Endfunction obj
READ:
-So far you can't have more than 100 waypoints at one time yet.
-You can't attach a waypoint to more that 4 other waypoints.
-When you attach a waypoint, they are attached both ways so if attaching waypoint A to B you only need to call the connect function
once. No need for calling it up to attach B to A and A to B.
Here's a demo.
Rem Project: Waypoint Test
Rem Created: Wednesday, March 10, 2010
Rem ***** Main Source File *****
set display mode 1024,768,32
sync on: sync rate 60
hide mouse
WP_Setup(12)
Make object cube 1,0.2
color object 1,rgb(255,0,0)
turn object right 1,90
Make object cube 3,0.2
color object 3,rgb(255,0,255)
position object 3,0,0,5.2
make object plane 2,50,50
position object 2,0,-2,0
pitch object down 2,90
position camera 0,5,0,0
Make camera 1
position camera 1,0,10,0
pitch camera down 1,90
point camera 1,object position X(1),object position Y(1),object position Z(1)
remstart
WP_MakeWaypoint(1,0,0,0)
WP_MakeWaypoint(2,0,0,1)
WP_MakeWaypoint(3,-1,0,2)
WP_MakeWaypoint(4,-2,0,3)
WP_MakeWaypoint(5,0,0,4)
WP_MakeWaypoint(6,0,0,5)
WP_MakeWaypoint(7,1,0,2)
WP_MakeWaypoint(8,1,0,3)
WP_ConnectWaypoints(1,2)
WP_ConnectWaypoints(2,3)
WP_ConnectWaypoints(2,7)
WP_ConnectWaypoints(3,4)
WP_ConnectWaypoints(4,5)
WP_ConnectWaypoints(5,6)
WP_ConnectWaypoints(7,8)
WP_ConnectWaypoints(8,5)
remend
WP_MakeWaypoint(1,-1,0,3)
WP_MakeWaypoint(2,0,0,3)
WP_MakeWaypoint(3,1,0,3)
WP_MakeWaypoint(4,-1,0,2)
WP_MakeWaypoint(5,0,0,2)
WP_MakeWaypoint(6,1,0,2)
WP_MakeWaypoint(7,-1,0,1)
WP_MakeWaypoint(8,0,0,1)
WP_MakeWaypoint(9,1,0,1)
WP_MakeWaypoint(10,-1,0,0)
WP_MakeWaypoint(11,0,0,0)
WP_MakeWaypoint(12,1,0,0)
WP_ConnectWaypoints(1,2)
WP_ConnectWaypoints(1,4)
WP_ConnectWaypoints(2,3)
WP_ConnectWaypoints(2,5)
WP_ConnectWaypoints(3,6)
WP_ConnectWaypoints(4,7)
WP_ConnectWaypoints(4,5)
WP_ConnectWaypoints(5,8)
WP_ConnectWaypoints(5,6)
WP_ConnectWaypoints(6,9)
WP_ConnectWaypoints(7,10)
WP_ConnectWaypoints(7,8)
WP_ConnectWaypoints(8,11)
WP_ConnectWaypoints(8,9)
WP_ConnectWaypoints(9,12)
WP_ConnectWaypoints(10,11)
WP_ConnectWaypoints(11,12)
for O =1 to totalWP
make object sphere O+100,0.1
color object O+100,rgb(0,255,0)
position object O+100,Waypoint(O).X,Waypoint(O).Y,Waypoint(O).Z
next O
do
X#=Object Position X(3)
Y#=Object Position Y(3)
Z#=Object Position Z(3)
WP_MoveObjToPos(1,X#,Y#,Z#,0.02)
WP_DisplayWaypointNumber()
WP_DrawConnections()
move object 3,(keystate(200)-keystate(208))/50.0
turn object right 3,90
move object 3,(keystate(205)-keystate(203))/50.0
turn object right 3,-90
t=0
text 0,t,"WP Priority: "+str$(WPPriority)
inc t,15
Text 0,T,"NextWP: "+str$(NextWP)
inc t,15
Text 0,T,"AtPosition: "+str$(WPAtPosition)
inc t,15
Text 0,T,"WPPath(NextWP): "+str$(WPPath(NextWP))
sync
LOOP
Screenshots:
If it hasn't exploded yet, I haven't touched it.
