Some people may find this 3D pathfinding code useful.
It uses waypoints, which can be manually specified or calculated based on level geometry, to create paths around a 3D world for AI to follow.
How it works
Load your level, add waypoint data and then tell it to calculate. It uses some maths and temporary objects & raycasting to work out which waypoints are visible to each other. Once the precalculations are done you can call the GetRoute(startWP, endWP) function at any time in your game loop to generate a path between two way points.
Iterate backwards over the route array to get from the start to the finish. Here's an example of the code in use (assuming a big cube is placed at [0, 0, 0]:
ClearWaypoints()
// Load & position your 3D objects (level)
// Generate waypoints and pre-calculate:
AddWaypoint(0, 1, -10, 0)
AddWayPoint(0, 1, 10, 0)
AddWayPoint(10, 1, 0, 0)
CalculateWaypoints(50, 2)
// Get a route:
local route as integer[]
route = GetRoute(0, 1)
More complex levels just require more well place waypoints:
In my current project I generate levels in code using primitives, so it is really easy to simultaneously generate the required waypoints. I use the "level" property so waypoints on higher/lowe levels cannot be "seen" by waypoints on other, unaccessible, levels. Waypoints can only see other waypoints on the same level of one level above or below.
Performance is good, even with a couple of hundred waypoints. I dare say there could be further optimizations.
Pathfinding code:
dim waypoints[] as Waypoint
function AddWaypoint(x#, y#, z#, level)
local w as Waypoint
w.X = x#
w.Y = y#
w.Z = z#
w.Level = level
waypoints.Insert(w)
waypoints[waypoints.Length].ID = waypoints.Length
endfunction
function GetRoute(startWaypoint, endWaypoint)
CalculateRoutes(startWaypoint, endWaypoint)
local route as integer[]
current = endWaypoint
shortest# = 99999999
while current <> startWaypoint
nxt = current
for i = 0 to waypoints[current].Neighbours.Length
if waypoints[waypoints[current].Neighbours[i].WaypointID].Distance < shortest#
shortest# = waypoints[waypoints[current].Neighbours[i].WaypointID].Distance
nxt = waypoints[current].Neighbours[i].WaypointID
endif
next i
current = nxt
route.Insert(nxt)
endwhile
endfunction route
function ClearWaypoints()
for i = 0 to waypoints.Length
waypoints[i].Neighbours.Length = -1
next i
waypoints.Length = -1
endfunction
function CalculateWaypoints(maxWaypointDistance#, waypointDiameter#)
for i = 0 to waypoints.Length
waypoints[i].WaypointObject = CreateObjectSphere(waypointDiameter#, 4, 4)
SetObjectPosition(waypoints[i].WaypointObject, waypoints[i].X, waypoints[i].Y, waypoints[i].Z)
SetObjectColor(waypoints[i].WaypointObject, 255, 0, 0, 31)
SetObjectTransparency(waypoints[i].WaypointObject, 1)
next i
for p1 = 0 to waypoints.Length
for p2 = 0 to waypoints.Length
if p1 <> p2
if Abs(waypoints[p1].Level - waypoints[p2].Level) <= 1
dist# = WaypointDistance(waypoints[p1].X, waypoints[p1].Y, waypoints[p1].Z, waypoints[p2].X, waypoints[p2].Y, waypoints[p2].Z)
if dist# <= maxWaypointDistance#
r = ObjectRayCast(0, waypoints[p1].X, waypoints[p1].Y, waypoints[p1].Z, waypoints[p2].X, waypoints[p2].Y, waypoints[p2].Z)
if r = waypoints[p2].WaypointObject
local n as WaypointNeighbour
n.WaypointID = p2
n.Distance = dist#
waypoints[p1].Neighbours.Insert(n)
endif
endif
endif
endif
next p2
next p1
for i = 0 to waypoints.Length
DeleteObject(waypoints[i].WaypointObject)
next i
endfunction
function CalculateRoutes(startWaypoint, endWaypoint)
ResetWaypoints()
waypoints[startWaypoint].Distance = 0
candidate = startWaypoint
while candidate > -1
waypoints[candidate].Processed = 1
for i = 0 to waypoints[candidate].Neighbours.Length
di# = waypoints[candidate].Distance + waypoints[candidate].Neighbours[i].Distance
if di# < waypoints[waypoints[candidate].Neighbours[i].WaypointID].Distance
waypoints[waypoints[candidate].Neighbours[i].WaypointID].Distance = di#
endif
next i
candidate = GetShortest()
if candidate = endWaypoint
exitfunction
endif
endwhile
endfunction
function ResetWaypoints()
for i = 0 to waypoints.Length
waypoints[i].Processed = 0
waypoints[i].Distance = 99999999
next i
endfunction
function GetShortest()
short# = 99999999
candidate = -1
for i = 0 to waypoints.Length
if waypoints[i].Processed = 0 and waypoints[i].Distance < short#
short# = waypoints[i].Distance
candidate = i
endif
next i
endfunction candidate
function WaypointDistance(x1#, y1#, z1#, x2#, y2#, z2#)
xx# = x2# - x1#
yy# = y2# - y1#
zz# = z2# - z1#
r = Sqrt((xx# * xx#) + (yy# * yy#) + (zz# * zz#))
endfunction r
type Waypoint
ID as integer
WaypointObject as integer
X as float
Y as float
Z as float
Level as integer
Distance as float
Processed as integer
Neighbours as WaypointNeighbour[]
endtype
type WaypointNeighbour
WaypointID as integer
Distance as float
endtype
