Code archives/3D Graphics - Misc/3D Pathfinding Lib

;==========================
;Collision Avodance Demo v2
;By Andrew Pye AKA(3D)
;Date: 25/06/2004
;==========================
;=======================================================================
;Well at last I have come up with something that is
;simple and works well.
;The system works by using line pick with a large radius.
;If line pick picks up an entity then based on is pos it make a 
;decision  which way to turn and for how long.
;If there is no entity then head towards the target.
;But you will have to take a look through the code for all the details.
;
;Feel free to use the code in your own projects 
;But if you do just give us a little mention.
;=======================================================================


;=================
;set up enviroment
;=================
Graphics3D 640, 480,16,0
;Graphics3D 800, 600,32,0
light = CreateLight()


;=======
;globals
;=======
;======================
;Play with these values 
;======================
Global BotCount=30;number of bots
Global ObjticlesCount=50;number of obsticles
Global MAxSpeed#=0.5;speed of bots
Global TurnSpeedObsticles#=4;how fast to turn towards obsticles
Global TurnSpeedTarget#=2;how fast to turn towards target
Global SensterWidth#=1.5;how wide to check for collisions

;=====
;Types
;=====
Type Bot
	Field MainPiv,CActionCancelCount,CAction,model,CAnimationTime#
	Field SenPiv,cspeed#,CActionCount#
End Type

;======================
;create our target node
;======================
Global Target=CreateSphere()
EntityColor Target,255,0,0

;===============
;Create the bots
;===============
For i=0 To BotCount
	bots.bot=CreateBot()
Next

;store current view
Global CView

;============================================
;Create camera and attach to last created bot
;============================================
Global camera= CreateCamera(bots\MainPiv)
PositionEntity camera,0,3.6,0;raise a little

;================
;create obsticals
;================
For i=0 To ObjticlesCount
	CreateObsticles()
Next

;==========
;set up fps
;==========
Const FPS=30
period=1000/FPS
time=MilliSecs()-period

;=========
;main loop
;=========
Repeat
			
	;=======================
	;time the frames per sec
	;=======================
	Repeat
		elapsed=MilliSecs()-time
	Until elapsed
	;how many 'frames' have elapsed	
	ticks=elapsed/period
	;fractional remainder
	tween#=Float(elapsed Mod period)/Float(period)
	If KeyHit(16) Then PositionEntity Target,Rnd(-150,150),0,Rnd(-150,150)
	If KeyHit(17) Then 
		If CView=0 Then
			PositionEntity camera,0,30,0
			RotateEntity camera,90,0,0
			CView=1
		ElseIf CView=1 Then
			PositionEntity camera,0,60,0
			RotateEntity camera,90,0,0
			CView=2
		ElseIf CView=2 Then
			PositionEntity camera,0,3.6,0
			RotateEntity camera,0,0,0
			CView=0
		End If
	End If

	
	For k=1 To ticks
		time=time+period
		If k=ticks Then CaptureWorld
		;===============
		;Ai for each bot
		;===============
		For b.bot = Each bot :BotAi b :Next
	Next
	
	UpdateWorld
	RenderWorld tween
	
	Text 1,1,"Collision Avoidance Demo V2"
	Text 1,15,"BY Andrew Pye"
	Text 1,30,"25/06/2004"
	Text 1,45,"Press (Q) to chage the Target"
	Text 1,60,"Press (W) to chage the View"
	Flip
	
Until KeyHit(1)
End

;======
;bot ai
;======
Function BotAi(b.bot)
	
	;================================
	;test to see if anything in front
	;================================
	result#=TestSensor(b);test sensor
	
	;=====================================
	;Check if to reset the action count
	;=====================================
	If result#=0 And b\CActionCount#>0 Then 
		b\CActionCancelCount#=b\CActionCancelCount#-1;reduce the action count
		If b\CActionCancelCount#<-8 Then
			b\CActionCount#=0
		End If
	Else
		b\CActionCancelCount#=0
	End If
	
	If b\CActionCount#>0 Then;see if we are performing an action
		If b\CAction=0 Then;turn left
			TurnEntity b\MainPiv,0,-TurnSpeedObsticles#,0
		ElseIf b\CAction=1 Then;turn rigth
			TurnEntity b\MainPiv,0,TurnSpeedObsticles#,0
		End If
		
		;=========================
		;decrease the action count
		;=========================
		b\CActionCount#=b\CActionCount#-1
		If b\CActionCount#<0 Then 
			b\CActionCount#=0
		End If
	Else
		;======================
		;see if we hit anything
		;======================
		If result#<>0 Then
			;=========================
			;find out what way to turn
			;=========================
			If DeltaYaw#(b\MainPiv,result#)>0 Then 
				b\cspeed#=0;stop
				b\CAction=0;turn left
				b\CActionCount#=Abs(Int(DeltaYaw#(b\MainPiv,result#)))*1.5;how long to turn
				If b\CActionCount#=<0.1 Then b\CActionCount#=45	;make sure it does turn	
			ElseIf DeltaYaw#(b\MainPiv,result#)<0 Then
				b\cspeed#=0;stop
				b\CAction=1;turn rigth
				b\CActionCount#=Abs(Int(DeltaYaw#(b\MainPiv,result#)))*1.5;how long to turn
				If b\CActionCount#=<0.1 Then b\CActionCount#=45		;make sure it does turn	
			EndIf
			
			b\CActionCancelCount#=0;reset the action cancel
;			AnimateBot b;animate the bot
		Else
			;==============================
			;no collision so move to target
			;==============================
			If DeltaYaw#(b\MainPiv,target)>4 Then;0 Then
				TurnEntity b\MainPiv,0,TurnSpeedTarget#,0
			ElseIf DeltaYaw#(b\MainPiv,target)<-4 Then;0 Then
				TurnEntity b\MainPiv,0,-TurnSpeedTarget#,0
			EndIf
			b\CAction=3;action is goto the target
;			AnimateBot b		;animate bot
			b\cspeed#=b\cspeed#+0.1 : If b\cspeed#>MAxSpeed# Then b\cspeed#=MAxSpeed#;increase speed
		End If
	End If

	MoveEntity b\MainPiv,0,0,b\cspeed#

End Function

;===============
;animate the bot
;===============
Function AnimateBot(b.bot)
	b\CAnimationTime#=b\CAnimationTime#+MAxSpeed#*2.6
	If b\CAnimationTime#>30 Then b\CAnimationTime#=0
	SetAnimTime b\model,b\CAnimationTime#
End Function

;=========================
;function Create Obsticals
;=========================
Function CreateObsticles()
	barrel=CreateCylinder()
	EntityColor barrel,100,100,100 ; line added
;	ScaleEntity barrel,0.1,0.1,0.1
	EntityPickMode barrel,1;set the entity pick
	PositionEntity barrel,Rnd(-100,100),0,Rnd(-100,100);random pos
	Return barrel
End Function

;===================
;function create bot
;===================
Function CreateBot.bot()
	b.bot=New bot
	
	b\MainPiv=CreatePivot();create the bot model
	;load the model and setup
	b\model=CreateCube(b\MainPiv)
	EntityColor b\model,0,0,255 ; line added
	RotateEntity b\model,0,90,0
	PositionEntity b\model,0,0,0; he had the height set to -1
	ScaleEntity b\model,1,2,1
	;set up animation
;	ExtractAnimSeq b\model,0,30
;	ExtractAnimSeq b\model,31,100
;	ExtractAnimSeq b\model,107,107
;	Animate b\model,1,0.5,1,10
	;set up sensor pivot
	b\SenPiv=CreatePivot(b\MainPiv);add the sensor pivot
	PositionEntity b\SenPiv,0,0,2
	PositionEntity b\MainPiv,Rnd(-30,30),0,Rnd(-50,50);random pos
	EntityPickMode b\MainPiv,1;set the entity pick
	Return b
End Function


;==================
;test for collision
;==================
Function TestSensor(b.bot)
	;see if touching anything
	SenObjId=LinePick( EntityX(b\MainPiv), EntityY(b\MainPiv),EntityZ(b\MainPiv),EntityX(b\SenPiv,True)-EntityX(b\MainPiv), EntityY(b\SenPiv,True)-EntityY(b\MainPiv),EntityZ(b\SenPiv,True)-EntityZ(b\MainPiv), SensterWidth#)  
	Return SenObjId
End Function