remstart
Project: freeflight
Created: 12/23/2003 6:21:08 AM
Features:
   Basic Flight simulator (no physics)
   Basic flight simulation AI (waypoint following and stalking)
   Includes arbitruary axis rotations by manipulating mesh data via
      arbAxisRotation(x,y,x,angle) command.

   Uses no outside media (but could easily do so)
Revised:
   Now includes Analog control for the AI.
Problems:
   Texturing planes objects is basic at best.
   Speed concerns of its math intensive manipulation of the meshes, basically
      taking away from the 3D acceleration done by video card.
Programming:
   Twilightzone: Everything but quaternion and matrix functions
   Various Sources(books and web sites): quaternion and matrix functions

   Currently only written for DBPro ver 5.2
remend
dim current_vec#(2)
dim current_quat#(3)
dim operator_quat#(3)
dim current_matrix#(3,3)
dim operator_matrix#(3,3)
dim plane#(50,8)
dim waypoints#(10,2)

sync on : sync rate 30 : hide mouse : autocam off
set window off
set camera range 1, 5000
randomize timer()

global plane_object = 1000
global dummy_object = 5000
global plane_mesh = 100
global building_objects = 100

drones = 5 : ` number of simultaneos planes up to 50
plane#(0,0) = 0.0 : `Roll
plane#(0,1) = 0.0 : `Pitch
plane#(0,2) = 0.0 : `Yaw
plane#(0,3) = 6 : `throttle (1-10)
plane#(0,4) = 750.0 :`x position
plane#(0,5) = 500.0 :`y position
plane#(0,6) = 50.0  :`z position
plane#(0,7) = 7 :`current waypoint

for i = 1 to drones
plane#(i,3) = rnd(5) +5
plane#(i,4) = 650.0+ rnd(100)
plane#(i,5) = 400.0+ rnd(100)
plane#(i,6) = 200.0+ rnd(100)
plane#(i,7) = rnd( 9)  :` closest waypoint along path is 7
next i

camera_target = 0 : `which plane the camera follows

generate_texture()
Build_world()
generate_waypoints()

make object sphere 10000,5:color object 10000,rgb(255,0,0):`formation marker

rem Make a dummy object to get trampeled over by the model
rem you have to make one of these for each model.
for i = 0 to drones
   make object cube i + dummy_object,1
next i

rem load or build your object here
build_plane() : rem creates the plane object as 1000

rem make a more planes to play with by copying the first
for i = 1 to drones
   clone object plane_object + i,plane_object
next i

rem you could also load you own media but it wont use the texture data eg: load object "plane0.x",1002
rem Make a mesh from the unaltered object for reference

for i = 0 to drones
   make mesh from object plane_mesh + i,plane_object + i
next i

rem Main loop

debounce = 0
do
u=upkey()
d=downkey()
l=leftkey()
r=rightkey()
key$ = inkey$()

th = 0
if key$ = "=" and debounce = 0
   th = 1
   debounce = 1
endif
if key$ = "-" and debounce = 0
   th = -1
   debounce = 1
endif

if upper$(key$) = "A" and debounce = 0
    if autopilot = 1
      autopilot = 0
      rem Remove the transitional flutter
      rem When the plane recenters values that are not
      rem precise multiples of 3 will cause it to
      rem oscillate, trying to seek 0.0.
      plane#(plane_number,0) = int(plane#(plane_number,0)/3.0) * 3
      plane#(plane_number,1) = int(plane#(plane_number,1)/3.0) * 3
    else
      autopilot = 1
    endif
    debounce = 1
endif
if upper$(key$) = "Q"
   cleanup()
   end
endif

if upper$(key$) = "C" and debounce = 0
   camera_target = camera_target + 1
   if camera_target > drones then camera_target = 0
   debounce = 1
endif

if key$ = "" then debounce = 0

rem control and AI
if autopilot = 0
   control_plane(0,u,d,l,r,th,0)
else
   waypoint_ai(0)
endif

`make all but the last drone seek waypoints
for i = 1 to drones - 1
   waypoint_ai(i)
next i
`make the last drone stalk the player
generate_formation_matrix(0,drones)

rem rotate and move all models
for i= 0 to drones
   rotate_position_object(i + dummy_object,i + plane_mesh,plane#(i,2),plane#(i,1),plane#(i,0),plane#(i,4),plane#(i,5),plane#(i,6),1)
   xt = object screen x(i + dummy_object)
   yt = object screen y(i + dummy_object)
   if xt > 10 and xt < 630 and yt>10 and yt < 470
      set cursor xt, yt
      print i
   endif
next i

set camera to follow plane#(camera_target,4),plane#(camera_target,5),plane#(camera_target,6), 0, 1,5, 5 ,0
point camera plane#(camera_target,4),plane#(camera_target,5),plane#(camera_target,6)

set cursor 0,0
print screen fps()
print "Use Arrow keys to fly."
print "Use '-' and '=' keys to adjust speed."
print "Use 'a' to engage and disengage autopilot(waypoint behavior)."
print "Use 'c' to watch another plane."
print "Use 'q' to quit."
print "Roll:";plane#(0,0)
print "Pitch:";plane#(0,1)
print "Yaw:";plane#(0,2)
print "Throttle:";plane#(0,3)
print "Current Waypoint:";plane#(0,7)

rem Update screen
sync
rem End loop
loop
end

function control_plane(plane_number,u,d,l,r,t,ad)
rem This function has 2 modes of operation Analog and Digital control
rem Analog will be used for AI and maybe a joystick
rem Digital wil be use for keyboard play

rem Analog or digital will be set by the 'ad' flag (0 for digital, 1 for analog)
rem In digital mode "u,d,l,r" are bits set to determine direction
rem In Analog mode 'u' is the desired angle of pitch and 'l' it the desired
rem angle of yaw
if ad = 0
   if d then plane#(plane_number,1) = plane#(plane_number,1) + ((plane#(plane_number,1)<45.0)*3.0)
   if u then plane#(plane_number,1) = plane#(plane_number,1) - ((plane#(plane_number,1)>-45.0)*3.0)
   if r and plane#(plane_number,0) < 45 then plane#(plane_number,0) = plane#(plane_number,0) + ((plane#(plane_number,0) < 45) * 3.0)
   if l and plane#(plane_number,0) > -45 then plane#(plane_number,0) = plane#(plane_number,0) - ((plane#(plane_number,0) > -45) * 3.0)
   rem ** Recenter plane
   if u=0 and d=0 and plane#(plane_number,1)<>0.0 then plane#(plane_number,1) = plane#(plane_number,1) - ((plane#(plane_number,1)/abs(plane#(plane_number,1)))*3.0)
   if l=0 and r=0 and plane#(plane_number,0)<>0.0 then plane#(plane_number,0) = plane#(plane_number,0) - ((plane#(plane_number,0)/abs(plane#(plane_number,0)))*3.0)
else
   if l <=180
      lt# = l
   else
      lt# = l - 360
   endif
   if u <=180
      ut# = u / 2.0 : `the 2.0 is a value that will need to be tuned for the situation
   else
      ut# = (u - 360) / 2.0
   endif
   plane#(plane_number,0)=plane#(plane_number,0) + (plane#(plane_number,0)<lt# and plane#(plane_number,0)<60.0) - (plane#(plane_number,0)>lt# and plane#(plane_number,0)>-60)
   plane#(plane_number,1)=plane#(plane_number,1) + (plane#(plane_number,1)<ut# and plane#(plane_number,1)<60.0) - (plane#(plane_number,1)>ut# and plane#(plane_number,1)>-60)
endif


if t=1 then plane#(plane_number,3) = plane#(plane_number,3) + (plane#(plane_number,3)<10)
if t=-1 then plane#(plane_number,3) = plane#(plane_number,3) - (plane#(plane_number,3)>4)
rem *** This is just a hack for Airplane physics
rem *** Adjust yaw
plane#(plane_number,2) = plane#(plane_number,2) - ((sin(plane#(plane_number,0))*2.0) + (abs(sin(plane#(plane_number,1))) * sin(plane#(plane_number,0))))
plane#(plane_number,2) = wrapvalue(plane#(plane_number,2))

x#=sin(wrapvalue(0-plane#(plane_number,2)))
z#=cos(wrapvalue(0-plane#(plane_number,2)))

rem *** adjusting for angle of pitch.
horizontal_speed# = cos(wrapvalue(abs(plane#(plane_number,1))))*plane#(plane_number,3)
x1# = horizontal_speed#*x#
y1# = sin(plane#(plane_number,1))*plane#(plane_number,3)
z1# = horizontal_speed#*z#

plane#(plane_number,4)=plane#(plane_number,4)+ x1#
plane#(plane_number,5)=plane#(plane_number,5)+ y1#
plane#(plane_number,6)=plane#(plane_number,6)+ z1#
endfunction

function rotate_position_object(dummy_object_number,original_mesh_number,yaw#,pitch#,roll#,obx#,oby#,obz#,image)
rem remove memblock if it already exists
if memblock exist(1) then delete memblock 1
rem make memblock to manipulate
make memblock from mesh 1,original_mesh_number
rem get number of vertices to manipulate
vnum = memblock dword(1,8)
rem get their offset
vpos = 12
rem size of each vector this changes due different formats
vsize = memblock dword(1,4)
rem always start out with the identity matrix ... this will set current_matrix#() to identity
identity_current_matrix()
rem insert any other matrix functions in here like x_rotate, scale ... whatever
rem rotate mesh on any axis

arbAxisRotation(0.0,1.0,0.0,yaw#) : rem yaw
arbAxisRotation(1.0,0.0,0.0,pitch#) : rem pitch
arbAxisRotation(0.0,0.0,1.0,roll#) : rem roll

rem Alter the vertices of the mesh
for v=0 to vnum - 1 step 1
   rem find positions of the vertices
   posx = vpos + (v * vsize)
   posy = posx + 4
   posz = posx + 8
   rem get values of the vertices
   xval# = memblock float(1,posx)
   yval# = memblock float(1,posy)
   zval# = memblock float(1,posz)
   rem multiply each vertex by current_matrix#() set up earlier
   matrix_vertex_multiply(xval#,yval#,zval#)
   write memblock float 1,posx,current_vec#(0)
   write memblock float 1,posy,current_vec#(1)
   write memblock float 1,posz,current_vec#(2)
next v
rem We are done manipulating the object

rem Make a new mesh from the new memblock
if mesh exist(2) then delete mesh 2
make mesh from memblock 2,1
rem Copy the mesh to the original object
change mesh dummy_object_number,0,2
rem texture your object
texture object dummy_object_number, image
rem position your object
position object dummy_object_number,obx#,oby#,obz#
endfunction

function distance3d( x1#,y1#,z1#,x2#,y2#,z2#)
   x3# = x1# - x2#
   y3# = y1# - y2#
   z3# = z1# - z2#
   d# = sqrt((x3#*x3#)+(y3#*y3#)+(z3#*z3#))
endfunction d#

function matrix_vertex_multiply(x#,y#,z#)
   x1# = (current_matrix#(0, 0) * x#) + (current_matrix#(1, 0) * y#) + (current_matrix#(2, 0) * z#) + current_matrix#(3, 0)
   y1# = (current_matrix#(0, 1) * x#) + (current_matrix#(1, 1) * y#) + (current_matrix#(2, 1) * z#) + current_matrix#(3, 1)
   z1# = (current_matrix#(0, 2) * x#) + (current_matrix#(1, 2) * y#) + (current_matrix#(2, 2) * z#) + current_matrix#(3, 2)
   w1# = (current_matrix#(0, 3) * x#) + (current_matrix#(1, 3) * y#) + (current_matrix#(2, 3) * z#) + current_matrix#(3, 3)
if w1#
   inv# = 1.0 / w1#
   current_vec#(0) = x1# * inv#
   current_vec#(1) = y1# * inv#
   current_vec#(2) = z1# * inv#
else
   current_vec#(0) = x1#
   current_vec#(1) = y1#
   current_vec#(2) = z1#
endif
endfunction

function matrix_multiply()
dim temp_matrix#(3,3)
for x = 0 to 3
   for y = 0 to 3
   temp_matrix#(x,y) = 0.0
   next y
next x
for i = 0 to 3
   for j = 0 to 3
      for k = 0 to 3
      temp_matrix#(i,j) = temp_matrix#(i,j) + (current_matrix#(k,j) * operator_matrix#(i,k))
      next k
   next j
next i
for x = 0 to 3
   for y = 0 to 3
   current_matrix#(x,y) = temp_matrix#(x,y)
   next y
next x
endfunction

function zero_operator_matrix()
   for x = 0 to 3
      for y = 0 to 3
         operator_matrix#(x,y) = 0.0
      next y
   next x
endfunction

function zero_current_matrix()
   for x = 0 to 3
      for y = 0 to 3
         current_matrix#(x,y) = 0.0
      next y
   next x
endfunction

function identity_operator_matrix()
zero_operator_matrix()
operator_matrix#(0,0) = 1.0
operator_matrix#(1,1) = 1.0
operator_matrix#(2,2) = 1.0
operator_matrix#(3,3) = 1.0
endfunction

function identity_current_matrix()
zero_current_matrix()
current_matrix#(0,0) = 1.0
current_matrix#(1,1) = 1.0
current_matrix#(2,2) = 1.0
current_matrix#(3,3) = 1.0
endfunction

function current_matrix_equal_operator_matrix()
for x = 0 to 3
   for y = 0 to 3
      current_matrix#(x,y) = operator_matrix#(x,y)
   next y
next x
endfunction

function operator_matrix_equal_current_matrix()
for x = 0 to 3
   for y = 0 to 3
       operator_matrix#(x,y) = current_matrix#(x,y)
   next y
next x
endfunction

function matrix_translate(dx#,dy#,dz#)
identity_operator_matrix()
operator_matrix#(3,0) = dx#
operator_matrix#(3,1) = dy#
operator_matrix#(3,2) = dz#
matrix_multiply()
endfunction

function matrix_rotate_x(degrees#)
identity_operator_matrix()
cosine# = cos(degrees#)
sine# = sin(degrees#)
operator_matrix#(1,1) = cosine#
operator_matrix#(2,2) = cosine#
operator_matrix#(1,2) = 0.0-sine#
operator_matrix#(2,1) = sine#
matrix_multiply()
endfunction

function matrix_rotate_y(degrees#)
identity_operator_matrix()
cosine# = cos(degrees#)
sine# = sin(degrees#)
operator_matrix#(0,0) = cosine#
operator_matrix#(2,2) = cosine#
operator_matrix#(0,2) = sine#
operator_matrix#(2,0) = 0.0-sine#
matrix_multiply()
endfunction

function matrix_rotate_z(degrees#)
identity_operator_matrix()
cosine# = cos(degrees#)
sine# = sin(degrees#)
operator_matrix#(0,0) = cosine#
operator_matrix#(1,1) = cosine#
operator_matrix#(0,1) = 0.0-sine#
operator_matrix#(1,0) = sine#
matrix_multiply()
endfunction

function matrix_scale(size#)
identity_operator_matrix()
operator_matrix#(0,0) = size#
operator_matrix#(1,1) = size#
operator_matrix#(2,2) = size#
matrix_multiply()
endfunction

rem arbAxisRotation(x#,y#,z#,d#)
rem x, y, z: Describe the axis
rem 'd' is how much to rotate on that axis
rem Basically imagine a line starting from 0,0,0
rem going to the surface of a sphere with a radius of 1.
rem this line is the axis in which the object is going
rem to be rotated.
function arbAxisRotation(x#,y#,z#,d#)
quat_rotate(x#,y#,z#,d#)
from_quat()
matrix_multiply()
endfunction

function from_quat()
    x2# = current_quat#(0) * current_quat#(0)
    y2# = current_quat#(1) * current_quat#(1)
    z2# = current_quat#(2) * current_quat#(2)

    xy# = current_quat#(0) * current_quat#(1)
    wz# = current_quat#(3) * current_quat#(2)
    xz# = current_quat#(0) * current_quat#(2)
    wy# = current_quat#(3) * current_quat#(1)
    yz# = current_quat#(1) * current_quat#(2)
    wx# = current_quat#(3) * current_quat#(0)

    operator_matrix#(0,0) = 1.0 - 2.0 * (y2# + z2#)
    operator_matrix#(0,1) = 2.0 * (xy# - wz#)
    operator_matrix#(0,2) = 2.0 * (xz# + wy#)
    operator_matrix#(1,0) = 2.0 * (xy# + wz#)
    operator_matrix#(1,1) = 1.0 - 2.0 * (z2# + x2#)
    operator_matrix#(1,2) = 2.0 * (yz# - wx#)
    operator_matrix#(2,0) = 2.0 * (xz# - wy#)
    operator_matrix#(2,1) = 2.0 * (yz# + wx#)
    operator_matrix#(2,2) = 1.0 - 2.0 * (x2# + y2#)
endfunction

function normalize_quat()
oneOverMod# = 1.0 / sqrt(current_quat#(0) * current_quat#(0) + current_quat#(1) * current_quat#(1) + current_quat#(2) * current_quat#(2) + current_quat#(3) * current_quat#(3))
current_quat#(0) = current_quat#(0) * oneOverMod#
current_quat#(1) = current_quat#(1) * oneOverMod#
current_quat#(2) = current_quat#(2) * oneOverMod#
current_quat#(3) = current_quat#(3) * oneOverMod#
endfunction

function normalize_vector()
modA2# =  current_vec#(0) * current_vec#(0) + current_vec#(1) * current_vec#(1) +current_vec#(2) * current_vec#(2)
if modA2#< 1e-10 then exitfunction
oneOverMod# = 1.0 / sqrt(modA2#)
current_vec#(0) = current_vec#(0) * oneOverMod#
current_vec#(1) = current_vec#(1) * oneOverMod#
current_vec#(2) = current_vec#(2) * oneOverMod#
endfunction

function quat_rotate(x#,y#,z#,d#)
current_vec#(0) = x#
current_vec#(1) = y#
current_vec#(2) = z#
normalize_vector()
angle2# = d# / 2.0
current_quat#(3) = cos(angle2#)
s# = sin(angle2#)
current_quat#(0) = current_vec#(0) * s#
current_quat#(1) = current_vec#(1) * s#
current_quat#(2) = current_vec#(2) * s#
endfunction

function Build_world()
`make and place buildings
o = building_objects
for i = 1 to 8
   for ii = 1 to 8
      x1 = ((i) * 250)+ 1125
      y1 = ((ii) * 250)+ 1000
      h = rnd(400)+100
      w = 75
      d = 75
      make object box o,w,h,d
      texture object o,2
      position object o,x1,h/2,y1
      o = o + 1
   next ii
next i
`build ground
make matrix 1,4000,4000,31,31
prepare matrix texture 1,10,4,4
`rem lay roads
   for i = 8 to 22 step 2
      for ii = 8 to 22 step 2
         set matrix tile 1, i,ii,2
         set matrix tile 1, i+1,ii,4
         set matrix tile 1, i+1,ii+1,3
      next ii
   next i
for i = 0 to 31
   set matrix height 1,0,i,rnd(500) + 1000
   set matrix height 1,i,0,rnd(500) + 1000
   set matrix height 1,31,i,rnd(500) + 1000
   set matrix height 1,i,31,rnd(500) + 1000
next i
position matrix 1, 0,0,0
update matrix 1
endfunction

function Generate_texture()
`Plane texture
create bitmap 1, 256,256
set current bitmap 1
for i = 0 to 255 step 32
   ink rgb(255,0,0), 0
   for ii =0 to 15
      line i+ii,0,i+ii,255
   next ii
   ink rgb(255,255,0), 0
   for ii =16 to 31
      line i+ii,0,i+ii,255
   next ii
next i
get image 1,0,0,256,256
set current bitmap 0
delete bitmap 1

`building texture
create bitmap 1, 256,256
set current bitmap 1
ink rgb(192,192,192),0
for i= 0 to 256
   line 0,i,256,i
next i

for i = 0 to 15
   for ii = 0 to 15
      ink 0, 0
      box i*16+4,ii*16+4,i*16+12,ii*16+12
   next ii
next i
blur bitmap 1,2
get image 2,0,0,256,256
set current bitmap 0
delete bitmap 1

`grass texture
create bitmap 1, 256,256
set current bitmap 1
ink rgb(0,127,0),0
for i= 0 to 256
   line 0,i,256,i
next i

for i = 1 to 2000
   ink rgb(0,127+rnd(127),0), 0
   circle rnd(256),rnd(256),rnd(5)+2
next i


blur bitmap 1,3
get image 3,0,0,256,256
set current bitmap 0
delete bitmap 1

`street texture 1 horz
create bitmap 1, 256,256
set current bitmap 1
ink rgb(127,127,127),0
for i= 0 to 256
   line 0,i,256,i
next i

for i = 64 to 192
   ink rgb(64,64,64), 0
   line 0,i,256,i
next i

for i = 126 to 130
   ink rgb(255,255,0), 0
   line 0,i,256,i
next i

blur bitmap 1,3
get image 4,0,0,256,256
set current bitmap 0
delete bitmap 1

`street texture 2 vert
create bitmap 1, 256,256
set current bitmap 1
ink rgb(127,127,127),0
for i= 0 to 256
   line 0,i,256,i
next i

for i = 64 to 192
   ink rgb(64,64,64), 0
   line i,0,i,256
next i

for i = 126 to 130
   ink rgb(255,255,0), 0
   line i,0,i,256
next i

blur bitmap 1,3
get image 5,0,0,256,256
set current bitmap 0
delete bitmap 1

`street texture 3 cross
create bitmap 1, 256,256
set current bitmap 1
ink rgb(127,127,127),0
for i= 0 to 256
   line 0,i,256,i
next i

for i = 64 to 192
   ink rgb(64,64,64), 0
   line 0,i,256,i
   line i,0,i,256
next i

for i = 126 to 130
   ink rgb(255,255,0), 0
   line 0,i,256,i
   line i,0,i,256
next i

ink rgb(64,64,64),0
for i= 64 to 192
   line 64,i,192,i
next i

for i = -1 to 1
   ink rgb(255,255,255), 0
   line 64,64+i,192,64+i
   line 64,192+i,192,192+i
   line 64+i,64,64+i,192
   line 192+i,64,192+i,192

   line 64,56+i,192,56+i
   line 64,200+i,192,200+i
   line 56+i,64,56+i,192
   line 200+i,64,200+i,192
next i

blur bitmap 1,3
get image 6,0,0,256,256
set current bitmap 0
delete bitmap 1

`make matrix image
create bitmap 1, 1023,1023
set current bitmap 1
paste image 3, 0,0
paste image 4, 256,0
paste image 5, 512,0
paste image 6, 768,0
get image 10, 0,0,1023,1023
delete image 3
delete image 4
delete image 5
delete image 6
delete bitmap 1

endfunction

function build_plane()
`main body
make object box plane_object,2,2,5
`lower wing
make object box 3000,10,.125,2
make mesh from object 500,3000
add limb plane_object,1,500
offset limb plane_object,1,0,-1,0
delete mesh 500
delete object 3000
`upper wing
make object box 3000,12,.125,2
make mesh from object 501,3000
add limb plane_object,2,501
offset limb plane_object,2,0,1.5,0.5
delete mesh 501
delete object 3000
`more body
make object box 3000,1,1,5
make mesh from object 502,3000
add limb plane_object,3,502
offset limb plane_object,3,0,0,-5
delete mesh 502
delete object 3000
`tail wing
make object box 3000,6,.125,1.5
make mesh from object 503,3000
add limb plane_object,4,503
offset limb plane_object,4,0,0,-7.5
delete mesh 503
delete object 3000
`more tail wing
make object box 3000,.125,3,1.5
make mesh from object 504,3000
add limb plane_object,5,504
offset limb plane_object,5,0,1.5,-7.5
delete mesh 504
delete object 3000

endfunction

function generate_waypoints()
rem A circle of waypoints around city
for i = 0 to 9
   waypoints#(i,0) = 2000 + 1500 * sin(i * 36)
   waypoints#(i,1) = rnd(800) + 50
   waypoints#(i,2) = 2000 + 1500 * cos(i * 36)
   make object cube 2000 + i,10
   color object 2000 + i, rgb(255,0,0)
   position object 2000 + i, waypoints#(i,0),waypoints#(i,1),waypoints#(i,2)
next i
endfunction

function seek_behavior(plane,x#,y#,z#)
rem This is just a hack for seeking AI
th = 0
xp# = plane#(plane,4)
yp# = plane#(plane,5)
zp# = plane#(plane,6)
d# = distance3d(xp#,yp#,zp#,x#,y#,z#)
angxz# = wrapvalue(atanfull((zp# - z#),(xp# - x#)))
Hdiff# = (sin(plane#(plane,2))*(xp#-x#))-(cos(plane#(plane,2))*(zp#-z#))
angxy# = wrapvalue(atanfull((yp# - y#),Hdiff#))

diffxz# = wrapvalue(plane#(plane,2) - angxz# - 90.0)
diffxy# = wrapvalue(plane#(plane,1) - angxy# - 360.0)

control_plane(plane,int(diffxy#),0,int(diffxz#),0,th,1)
endfunction

function waypoint_ai(plane)
xp# = plane#(plane,4)
yp# = plane#(plane,5)
zp# = plane#(plane,6)
wp = int(plane#(plane,7))
wpx# = waypoints#(wp,0)
wpy# = waypoints#(wp,1)
wpz# = waypoints#(wp,2)
d# = distance3d(xp#,yp#,zp#,wpx#,wpy#,wpz#)
if d# < 100.0
   if tick = 0
      rem tick is to debounce the waypoint increment
      plane#(plane,7) = plane#(plane,7) + 1
      tick = 1
      if plane#(plane,7) > 9 then plane#(plane,7) = 0
   endif
else
   tick = 0
endif
rem goto waypoint
seek_behavior(plane,wpx#,wpy#,wpz#)
endfunction

function generate_formation_matrix(plane, follower)
rem This is basically a stalking behavior or could be used for general formation,
rem realistic gun targeting and missle tracking.
rem Currently the follower plane chases a red ball in front of the players plane
identity_current_matrix()
arbAxisRotation(0.0,1.0,0.0,plane#(plane,2)) : rem yaw
arbAxisRotation(1.0,0.0,0.0,plane#(plane,1)) : rem pitch
arbAxisRotation(0.0,0.0,1.0,plane#(plane,0)) : rem roll
matrix_vertex_multiply(0,0,100):`put the ball 100 units in front of plane
rem get follower to seek the ball
seek_behavior(follower,current_vec#(0) + plane#(plane,4),current_vec#(1) + plane#(plane,5),current_vec#(2) + plane#(plane,6))
position object 10000,current_vec#(0) + plane#(plane,4),current_vec#(1) + plane#(plane,5),current_vec#(2) + plane#(plane,6)
plane#(follower,3) = 10
endfunction

function cleanup()
   for i=1 to 32000
      if object exist(i) then delete object i
      if mesh exist(i) then delete mesh i
      if image exist(i) then delete image i
      if bitmap exist(i) then delete bitmap i
   next i
   delete memblock 1
undim current_vec#(2)
undim current_quat#(3)
undim operator_quat#(3)
undim current_matrix#(3,3)
undim operator_matrix#(3,3)
undim plane#(50,8)
undim waypoints#(10,2)
endfunction