Well, here's what I've been working on.
Pathfinding demo:
`Triple-click me
`AStar Pathfinding
`by BMacZero
sync on : sync rate 60
global debug_process as boolean
global debug_pathing as boolean
debug_process=0
debug_pathing=0
type Vector2
X as float
Y as float
endtype
type LineType
V1 as integer
V2 as integer
endtype
type AStarNode
F as integer
G as integer
H as integer
Open as boolean
Closed as boolean
Parent as integer
endtype
type Connection
V1 as integer
V2 as integer
CX as integer
CY as integer
Distance as integer
endtype
type Connection2
V1 as integer
V2 as integer
Distance as integer
endtype
DIM Lines(4) as LineType
DIM Verts(4) as Vector2
Verts(1).X=0
Verts(1).Y=0
Verts(2).X=screen width()
Verts(2).Y=0
Verts(3).X=screen width()
Verts(3).Y=screen height()
Verts(4).X=0
Verts(4).Y=screen height()
Lines(1).V1=1
Lines(1).V2=2
Lines(2).V1=2
Lines(2).V2=3
Lines(3).V1=3
Lines(3).V2=4
Lines(4).V1=4
Lines(4).V2=1
`Catmull-rom vectors
#constant V2_BEND1 1
#constant V2_ANCHOR1 2
#constant V2_ANCHOR2 3
#constant V2_BEND2 4
#constant V2_RESULT 5
result = make vector2(V2_BEND1)
result = make vector2(V2_ANCHOR1)
result = make vector2(V2_ANCHOR2)
result = make vector2(V2_BEND2)
result = make vector2(V2_RESULT)
`=====================================================================================
`Allow the user to draw in some lines:
repeat
`Display the lines drawn so far
cls
Path_DrawDebug()
print "Click and drag to draw lines"
print "Press space when done"
`Some circles to help the user lock onto existing vertices
ink rgb(150,0,0),0
for c=1 to array count(Verts(0))
circle Verts(c).X,Verts(c).Y,10
next c
if mouseclick()=1
`Start drawing a line on mouseclick
if clicked=0
clicked=1
tstartvert=0
`Check if any verts are close enough to lock to
for c=1 to array count(Verts(0))
if SQRT((Verts(c).X-mousex())^2+(Verts(c).Y-mousey())^2)<=10 then tstartvert=c
next c
if tstartvert>0
startvert=tstartvert
startx=Verts(startvert).X
starty=Verts(startvert).Y
else
startvert=0
startx=mousex()
starty=mousey()
endif
else
`Keep drawing a line
`See if it can be locked to an existing vertice
tendvert=0
for c=1 to array count(Verts(0))
if SQRT((Verts(c).X-mousex())^2+(Verts(c).Y-mousey())^2)<=10 then tendvert=c
next c
if tendvert>0
endvert=tendvert
endx=Verts(endvert).X
endy=Verts(endvert).Y
else
endvert=0
endx=mousex()
endy=mousey()
endif
`Draw the line in progress
ink rgb(255,0,0),0
line startx,starty,endx,endy
endif
else
`Complete a line
if clicked=1
clicked=0
`If the line is valid..
if SQRT((startx-endx)^2+(starty-endy)^2)>10
`Make any verts needed for it:
if startvert=0
array insert at bottom Verts(0)
Verts(array count(Verts(0))).X=startx
Verts(array count(Verts(0))).Y=starty
startvert=array count(Verts(0))
endif
if endvert=0
array insert at bottom Verts(0)
Verts(array count(Verts(0))).X=endx
Verts(array count(Verts(0))).Y=endy
endvert=array count(Verts(0))
endif
`And draw a line between them:
array insert at bottom Lines(0)
Lines(array count(Lines(0))).V1=startvert
Lines(array count(Lines(0))).V2=endvert
endif
startvert=0
endvert=0
startx=0
starty=0
endx=0
endy=0
endif
endif
sync
`Done Drawing
until spacekey()=1
pressed=1
clicked=1
`=====================================================================================
start=timer()
Path_ProcessData()
time=timer()-start
UnitRadius#=10
DIM Paths(0) as Vector2
MoveSpeed#=1
`sprite
cls
ink rgb(255,255,255),0
circle UnitRadius#,UnitRadius#,UnitRadius#
get image 1,0,0,(UnitRadius#*2)+1,(UnitRadius#*2)+1,1
sprite 1,100,100,1
offset sprite 1,UnitRadius#,UnitRadius#
DO
cls
text 10,10,str$(time)+" ms"
text 10,20,"Click to move"
Path_DrawDebug()
if mouseclick()=1 and clicked=0
clicked=1
start=timer()
UNDIM Paths(0)
DIM Paths(0) as VertType
Path(sprite x(1),sprite y(1),mousex(),mousey())
Paths(0).X=sprite x(1)
Paths(0).Y=sprite y(1)
time=timer()-start
target=1
endif
if mouseclick()=0 then clicked=0
if target<=array count(Paths(0)) and target>0
`Catmull-rom
if target<=1
set vector2 V2_BEND1,Paths(0).X,Paths(0).Y
else
set vector2 V2_BEND1,Paths(target-2).X,Paths(target-2).Y
endif
set vector2 V2_ANCHOR1, Paths(target-1).X,Paths(target-1).Y
set vector2 V2_ANCHOR2, Paths(target).X, Paths(target).Y
if target<array count(Paths(0))
set vector2 V2_BEND2, Paths(target+1).X, Paths(target+1).Y
else
set vector2 V2_BEND2, Paths(target).X, Paths(target).Y
endif
temp=0
`Approximate mvmt rate along curve by linear distance between points
inc dist#,2000/SQRT((Paths(target-1).X-Paths(target).X)^2+(Paths(target-1).Y-Paths(target).Y)^2)
if dist#>=1000 then temp=1 : dist#=1000
catmullrom vector2 V2_RESULT, V2_BEND1, V2_ANCHOR1, V2_ANCHOR2, V2_BEND2, 0.001*dist#
sprite 1, x vector2(V2_RESULT), y vector2(V2_RESULT), 1
if temp=1 then inc target : dist#=0
`pointsprite(1,Paths(1).X,Paths(1).Y)
`move sprite 1,MoveSpeed#
`if near(sprite x(1),Paths(1).X) and near(sprite y(1),Paths(1).Y) then inc target : dist#=0
endif
for c=1 to array count(Paths(0))
text Paths(c).X,Paths(c).Y,"X"
next c
SYNC
LOOP
`=====================================================================================
function Path(StartX,StartY,EndX,EndY)
`If there is a direct path, don't bother pathfinding!
if Raycast(StartX,StartY,EndX,EndY)=0
array insert at bottom Paths(0)
Paths(array count(Paths(0))).X=EndX
Paths(array count(Paths(0))).Y=EndY
exitfunction 1
endif
local resetconn as integer
DIM TempNodes(0) as AStarNode
DIM TempNodes(array count(Connections(0))+2) as AStarNode
`Make start and end nodes
temp=array count(Connections(0))+1
TempNodes(temp).Open=1
TempNodes(temp).Parent=0
TempNodes(temp).G=0
TempNodes(temp).H=abs(StartX-EndX)+abs(StartY-EndY)
TempNodes(temp).F=TempNodes(temp).G+TempNodes(temp).H
temp=array count(Connections(0))+2
TempNodes(temp).Open=0
TempNodes(temp).Parent=0
`Connect them to the rest
resetconn = array count(Connections2(0))
for c=1 to array count(Connections(0))
`Start
if Raycast(Connections(c).CX,Connections(c).CY,StartX,StartY)=0
if RaycastConn(Connections(c).CX,Connections(c).CY,StartX,StartY,0,c)=0
array insert at bottom Connections2(0)
temp=array count(Connections2(0))
Connections2(temp).V1=c
Connections2(temp).V2=array count(Connections(0))+1
Connections2(temp).Distance=SQRT((Connections(c).CX-StartX)^2+(Connections(c).CY-StartY)^2)
endif
endif
`End
if Raycast(Connections(c).CX,Connections(c).CY,EndX,EndY)=0
if RaycastConn(Connections(c).CX,Connections(c).CY,EndX,EndY,0,c)=0
array insert at bottom Connections2(0)
temp=array count(Connections2(0))
Connections2(temp).V1=c
Connections2(temp).V2=array count(Connections(0))+2
Connections2(temp).Distance=SQRT((Connections(c).CX-EndX)^2+(Connections(c).CY-EndY)^2)
endif
endif
next c
array insert at bottom Connections(0)
Connections(array count(Connections(0))).CX=StartX
Connections(array count(Connections(0))).CY=StartY
array insert at bottom Connections(0)
Connections(array count(Connections(0))).CX=EndX
Connections(array count(Connections(0))).CY=EndY
`While we can't see the destination...
repeat
`Find the open node with the lowest score
score=99999
winner=0
for c=1 to array count(TempNodes(0))
if TempNodes(c).Open=1 and TempNodes(c).Closed=0
if TempNodes(c).F<score
winner=c
score=TempNodes(c).F
endif
endif
next c
`If we didn't find any open nodes, we're stuck. Quit and blacklist.
if winner=0 then exit
TempNodes(winner).Closed=1
`Open any nodes visible from the one we found above
for c=1 to array count(Connections2(0))
temp=0
if Connections2(c).V1=winner then temp=Connections2(c).V2
if Connections2(c).V2=winner then temp=Connections2(c).V1
if temp>0
if TempNodes(temp).Open=0
TempNodes(temp).Open=1
TempNodes(temp).Parent=winner
TempNodes(temp).G=TempNodes(winner).G+Connections2(c).Distance
TempNodes(temp).H=abs(EndX-Connections(temp).CX)+abs(EndY-Connections(temp).CY)
TempNodes(temp).F=TempNodes(temp).G+TempNodes(temp).H
else
`If the winner node has a more direct connection than the current parent of the target,
` winner becomes parent.
if TempNodes(temp).G>TempNodes(winner).G+Connections2(c).Distance
TempNodes(temp).Parent=winner
TempNodes(temp).G=TempNodes(winner).G+Connections2(c).Distance
TempNodes(temp).F=TempNodes(temp).G+TempNodes(temp).H
endif
endif
endif
next c
`Debug
if debug_pathing
cls
Path_DrawDebug()
for d=1 to array count(Connections(0))
if TempNodes(d).Open=1
if d=winner
text Connections(d).CX,Connections(d).CY,str$(d)+"X"
else
text Connections(d).CX,Connections(d).CY,str$(d)
endif
text Connections(d).CX,Connections(d).CY+15,str$(TempNodes(d).Parent)
endif
next d
sync
wait key
endif
until TempNodes(array count(Connections(0))).Closed = 1
winner = array count(Connections(0))
`Record path backwards
if winner>0
array insert at bottom Paths(0)
Paths(array count(Paths(0))).X=EndX
Paths(array count(Paths(0))).Y=EndY
elem=array count(Paths(0))
repeat
array insert at element Paths(0),elem
Paths(elem).X=Connections(winner).CX
Paths(elem).Y=Connections(winner).CY
winner=TempNodes(winner).Parent
until winner=array count(Connections(0))-1 `This is the start node
endif
`Clear out temporary start/end node connections
for c=array count(Connections2(0)) to resetconn+1 step -1
array delete element Connections2(0),c
next c
array delete element Connections(0),array count(Connections(0))
array delete element Connections(0),array count(Connections(0))
endfunction 1
`by DavidT
function pointsprite(s,x,y)
dx = sprite x(s) - x
dy = sprite y(s) - y
ang# = wrapvalue(atanfull(dx,dy)*-1)
rotate sprite s,ang#
endfunction
function near(a#,b#)
temp#=1
if a#+temp#>b# and a#-temp#<b# or b#+temp#>a# and b#-temp#<a# then exitfunction 1
endfunction 0
`Nav-mesh pathfinding
`by Brian MacIntosh
function Path_DrawDebug()
`Verts
remstart
ink rgb(255,255,255),0
for c=1 to array count(Verts(0))
text Verts(c).X,Verts(c).Y,str$(c)
next c
remend
`Connection
remstart
ink rgb(100,100,100),0
for c=1 to array count(Connections(0))
line Verts(Connections(c).V1).X,Verts(Connections(c).V1).Y,Verts(Connections(c).V2).X,Verts(Connections(c).V2).Y
text (Verts(Connections(c).V1).X+Verts(Connections(c).V2).X)/2,(Verts(Connections(c).V1).Y+Verts(Connections(c).V2).Y)/2,str$(Connections(c).Distance)
next c
remend
`Draw lines
`remstart
ink rgb(255,0,0),0
for c=1 to array count(Lines(0))
line Verts(Lines(c).V1).X,Verts(Lines(c).V1).Y,Verts(Lines(c).V2).X,Verts(Lines(c).V2).Y
next c
`remend
`Connections2
remstart
ink rgb(50,50,50)
for c=1 to array count(Connections2(0))
X1=Connections(Connections2(c).V1).CX
Y1=Connections(Connections2(c).V1).CY
X2=Connections(Connections2(c).V2).CX
Y2=Connections(Connections2(c).V2).CY
line X1,Y1,X2,Y2
next c
remend
ink rgb(255,255,255)
endfunction
function Path_ProcessData()
DIM Connections(0) as Connection
DIM Connections2(0) as Connection2
`Process connections
for c=1 to array count(Verts(0))
for d=c+1 to array count(Verts(0))
if Raycast(Verts(c).X,Verts(c).Y,Verts(d).X,Verts(d).Y)=0
array insert at top Connections(0)
Connections(1).V1=c
Connections(1).V2=d
Connections(1).Distance=SQRT((Verts(c).X-Verts(d).X)^2+(Verts(c).Y-Verts(d).Y)^2)
Connections(1).CX=(Verts(Connections(1).V1).X+Verts(Connections(1).V2).X)/2
Connections(1).CY=(Verts(Connections(1).V1).Y+Verts(Connections(1).V2).Y)/2
endif
next d
next c
DIM Connectiondels(array count(Connections(0))) as boolean
`Remove overlaps
for d=1 to array count(Connections(0))-1
for c=d+1 to array count(Connections(0))
if FastLineIntersection(Verts(Connections(d).V1).X,Verts(Connections(d).V1).Y,Verts(Connections(d).V2).X,Verts(Connections(d).V2).Y,Verts(Connections(c).V1).X,Verts(Connections(c).V1).Y,Verts(Connections(c).V2).X,Verts(Connections(c).V2).Y)
if Connections(d).Distance<Connections(c).Distance
Connectiondels(c)=1
else
Connectiondels(d)=1
endif
endif
next c
next d
`Actual deletion 1
for c=array count(Connections(0)) to 1 step -1
if Connectiondels(c)=1 then array delete element Connections(0),c
next c
UNDIM Connectiondels(0)
DIM Connectiondels(array count(Connections(0))) as boolean
`Connections2
for c=1 to array count(Connections(0))-1
for d=c+1 to array count(Connections(0))
if RaycastConn(Connections(c).CX,Connections(c).CY,Connections(d).CX,Connections(d).CY,c,d)=0
array insert at bottom Connections2(0)
Connections2(array count(Connections2(0))).V1=c
Connections2(array count(Connections2(0))).V2=d
Connections2(array count(Connections2(0))).Distance=SQRT((Connections(c).CX-Connections(d).CX)^2+(Connections(c).CY-Connections(d).CY)^2)
endif
next d
next c
`Divide into convexes
`If every connection and line linked the the one being checked can see every other one, delete
for c=1 to array count(Connections(0))
`Find connections in sight of this one
DIM TempConnections(0) as integer
for d=1 to array count(Connections2(0))
if Connections2(d).V1=c
array insert at top TempConnections(0)
TempConnections(1)=Connections2(d).V2
endif
if Connections2(d).V2=c
array insert at top TempConnections(0)
TempConnections(1)=Connections2(d).V1
endif
next d
bad=0
for d=1 to array count(TempConnections(0))
for e=d+1 to array count(TempConnections(0))
if Raycast(Connections(TempConnections(d)).CX,Connections(TempConnections(d)).CY,Connections(TempConnections(e)).CX,Connections(TempConnections(e)).CY)=1 then bad=1
next e
next d
if bad=0
Connectiondels(c)=1
`Reconstruct local Connections2 around the deletion
`Delete old ones
for d=array count(Connections2(0)) to 1 step -1
if Connections2(d).V1=c or Connections2(d).V2=c then array delete element Connections2(0),d
next d
`Make new ones
for d=1 to array count(TempConnections(0))
for e=d+1 to array count(TempConnections(0))
array insert at top Connections2(0)
Connections2(1).V1=TempConnections(d)
Connections2(1).V2=TempConnections(e)
Connections2(1).Distance=SQRT((Connections(TempConnections(e)).CX-Connections(TempConnections(d)).CX)^2+(Connections(TempConnections(e)).CY-Connections(TempConnections(d)).CY)^2)
next e
next d
endif
UNDIM TempConnections(0)
next c
`Delete connections on lines
for c=1 to array count(Connections(0))
for d=1 to array count(Lines(0))
if (Lines(d).V1=Connections(c).V1 and Lines(d).V2=Connections(c).V2) or (Lines(d).V2=Connections(c).V1 and Lines(d).V1=Connections(c).V2)
Connectiondels(c)=1
endif
next d
next c
`Delete Connection2s with now-invalid ends
for c=array count(Connections2(0)) to 1 step -1
for d=1 to array count(Connectiondels(0))
if Connectiondels(d)
if Connections2(c).V1=d or Connections2(c).V2=d then array delete element Connections2(0),c : exit
endif
next d
next c
`Actual deletion 2
for c=array count(Connections(0)) to 1 step -1
if Connectiondels(c)=1 then array delete element Connections(0),c
next c
`Redo Connections2 (improve this!)
`Connections2
DIM Connections2(0) as Connection2
for c=1 to array count(Connections(0))-1
for d=c+1 to array count(Connections(0))
if Raycast(Connections(c).CX,Connections(c).CY,Connections(d).CX,Connections(d).CY)=0
if RaycastConn(Connections(c).CX,Connections(c).CY,Connections(d).CX,Connections(d).CY,c,d)=0
array insert at bottom Connections2(0)
Connections2(array count(Connections2(0))).V1=c
Connections2(array count(Connections2(0))).V2=d
Connections2(array count(Connections2(0))).Distance=SQRT((Connections(c).CX-Connections(d).CX)^2+(Connections(c).CY-Connections(d).CY)^2)
endif
endif
next d
next c
UNDIM Connectiondels(0)
endfunction
`===================================================
`==== Support Functions ============================
function Raycast(X1 as integer,Y1 as integer,X2 as integer,Y2 as integer)
for c=1 to array count(Lines(0))
if FastLineIntersection(X1,Y1,X2,Y2,Verts(Lines(c).V1).X,Verts(Lines(c).V1).Y,Verts(Lines(c).V2).X,Verts(Lines(c).V2).Y) then exitfunction 1
next c
endfunction 0
function RaycastConn(X1 as integer,Y1 as integer,X2 as integer,Y2 as integer,C1,C2)
for c=1 to array count(Connections(0))
if c<>C1 and c<>C2
if FastLineIntersection(X1,Y1,X2,Y2,Verts(Connections(c).V1).X,Verts(Connections(c).V1).Y,Verts(Connections(c).V2).X,Verts(Connections(c).V2).Y) then exitfunction 1
endif
next c
endfunction 0
`by Ian Mold
function FastLineIntersection(Ax as float,Ay as float,Bx as float,By as float,Cx as float,Cy as float,Dx as float,Dy as float)
local r as float : local s as float : local d as float : local n as float
n = ((Ay - Cy) * (Dx - Cx)) - ((Ax - Cx) * (Dy - Cy))
d = ((Bx - Ax) * (Dy - Cy)) - ((By - Ay) * (Dx - Cx))
if d = 0 then exitfunction 0
r = n / d
s = ( ((Ay-Cy)*(Bx-Ax))-((Ax-Cx)*(By-Ay)) ) / d
if r <= 0 then exitfunction 0
if r >= 1 then exitfunction 0
if s <= 0 then exitfunction 0
if s >= 1 then exitfunction 0
endfunction 1
It needs a lot of cleanup and a few tweaks. The catmull-rom paths are pretty nifty, but I that's more of an RTS thing so we probably won't use them.
