Sorry your browser is not supported!

// Project: 3D path Finding Way Points 
// Created: 2023-04-10
// code by JohnStabler : https://forum.thegamecreators.com/thread/217692
// exemple by blendman

// show all errors
SetErrorMode(2)

// set window properties
SetWindowTitle( "3D path Finding Way Points" )
SetWindowSize( 1024, 768, 0 )
SetWindowAllowResize( 1 ) // allow the user to resize the window

// set display properties
SetVirtualResolution( 1024, 768 ) // doesn't have to match the window
SetOrientationAllowed( 1, 1, 1, 1 ) // allow both portrait and landscape on mobile devices
SetSyncRate( 30, 0 ) // 30fps instead of 60 to save battery
SetScissor( 0,0,0,0 ) // use the maximum available screen space, no black borders
UseNewDefaultFonts( 1 ) // since version 2.0.22 we can use nicer default fonts



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

global dim waypoints[] as Waypoint
 
 
ClearWaypoints()
 
 
 
// Load & position your 3D objects (level)
s=15
CreateObjectBox(1,s,5,s)
SetObjectColor(1, 255, 250, 100, 255)

// Generate waypoints and pre-calculate:
AddWaypoint(0, 1, -20, 0)
AddWaypoint(0, 1, -10, 0)
AddWayPoint(10, 1, -10, 0)
AddWayPoint(10, 1, 10, 0) 
AddWayPoint(0, 1, 10, 0) 
AddWayPoint(0, 1, 40, 0) 
CalculateWaypoints(50, 2)


// create a ground, a player
CreateObjectPlane(2,1000,1000)
SetObjectColor(2, 50,150, 50, 255)
SetObjectRotation(2, 90, 0, 0)


SetCameraposition(1,0,50,-30)
SetCameraLookAt(1,0,0,0,0)

player = CreateObjectBox(2,2,2)
SetObjectColor(player, 100, 100, 255, 255)
playerx = 0
playerz = 20
SetObjectPosition(player, playerx, 0, playerz)

// Get a route:
global route as integer[-1]
GetRoute(0, 3)


// get the first waypoint
CurrentId = 0
px = waypoints[route[CurrentId]].X
pz = waypoints[route[CurrentId]].z
SetObjectLookAt(player, px, 0, pz,0)
a as float = 1
move=1

do
	
	print(str(playerx)+"/"+str(px)+" : "+str(playerz)+"/"+str(pz))
	print("Current routeId : "+str(CurrentId)+"/ route length : "+str(route.length)+"/ move : "+str(move))
	print("Target start "+str(cx)+"/ end : "+str(cy))
	
	if Move =1
		MoveObjectLocalZ(player, 0.5)
		playerx = getobjectx(player)
		playerz = getobjectz(player)
		
		if abs(playerx-px)>a or abs(playerz-pz)>a
		else
			print("ok move")
			if CurrentId< route.length
				inc CurrentId
				px = waypoints[route[CurrentId]].X
				pz = waypoints[route[CurrentId]].z
				SetObjectLookAt(player, px, 0, pz,0)
				move=1
			else
				move =0
				route.length = -1
				
				repeat 
					cx = random(0,waypoints.Length)
					cy = random(0,waypoints.Length)
					GetRoute(cx, cy)
					if route.length > -1
						CurrentId = 0
						px = waypoints[route[CurrentId]].X
						pz = waypoints[route[CurrentId]].z
						SetObjectLookAt(player, px, 0, pz,0)
						move=1
					endif
				until route.length > -1
			endif
		endif
	endif
	
	
    Print( ScreenFPS() )
    
    Sync()
loop



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)
	
	if startWaypoint <> endWaypoint
		
		CalculateRoutes(startWaypoint, endWaypoint)

		//~ local route as integer[]
		current = endWaypoint
		shortest# = 999999999
		
		
		while current <> startWaypoint

			nxt = current
			for i = 0 to waypoints[current].Neighbours.Length
				//~ message(str(nxt)+"/"+str(i))
				if waypoints[waypoints[current].Neighbours[i].WaypointID].Distance <= shortest#
					shortest# = waypoints[waypoints[current].Neighbours[i].WaypointID].Distance
					nxt = waypoints[current].Neighbours[i].WaypointID
					//~ message("ok "+str(nxt)+"/"+str(i)+str(shortest#))
				endif
			next i

			current = nxt
			route.Insert(nxt)
			
		endwhile
	
	endif
	
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, 100)
		//~ 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)
						
						//~ message( str(r))
						
						
						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

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
	
	for i = 0 to waypoints.Length
		if waypoints[i].Processed = 0 and waypoints[i].Distance < short#
			For j=0 to waypoints[i].Neighbours.length
				short# = waypoints[i].Distance + waypoints[i].Neighbours[j].Distance 
				candidate = i
			next
		endif
	next i
	
endfunction candidate