:-object wasp7v : [bcilib,vectorlib].

var url = './street/street5.wrl'.

var speed = 0.5.
var sleeptime = 100.

main :-
	text_area(Browser),
	set_output(Browser),
	format('Loading street1 from ~w~n', [url]),
	loadURL(url),
 	delay(3000),
	set_initial_point(bus1,1),
 	drive_along_route(bus1,1,7).

drive_along_route(_,_,0):-!.	

drive_along_route(Object,Route,N) :- N>0, N1 is N-1,
	getPosition(Object,X,Y,Z),
	I is (8-N),
      route_point(Route,I,X1,Z1),
	direction_vector(position(X,0,Z),position(X1,0,Z1), vector(X2,Y2,Z2)),
	look_in_direction(Object,vector(1,0,0),vector(X2,Y2,Z2)),
	distance2D(X,Z,X1,Z1,Dist),
	Interpolation is Dist//speed,
	move_to_position(Object,position(X,Y,Z),position(X1,Y,Z1),Interpolation),
	drive_along_route(Object,Route,N1).


set_initial_point(O,Route):-
	getSFVec3f(O,translation,_X,Y,_Z),
	route_point(Route,0,X,Z),
	setSFVec3f(O,translation,X,Y,Z).


	

%route-point(route-id, index, X,Z)

route_point(1,0,0,1.0):-!.
route_point(1,1,96,1.0):-!.
route_point(1,2,96,45.5):-!.
route_point(1,3,-96,45.5):-!.
route_point(1,4,-96,-45.5):-!.
route_point(1,5,96,-45.5):-!.
route_point(1,6,96,-1.0):-!.
route_point(1,7,-20,-1.0):-!.


move_to_position(O,_,position(X,Y,Z),0):-
            setPosition(O,X,Y,Z).

move_to_position(O, position(X1,Y1,Z1),position(X2,Y2,Z2),C):- C > 0,
		C1 is C-1,
		Xdif is X2 -X1,
		Zdif is Z2 -Z1,
		X is X1 + Xdif/C,
		Z is Z1 + Zdif/C,
		setPosition(O,X,Y1,Z),
            sleep(sleeptime),
		move_to_position(O,position(X,Y1,Z),position(X2,Y2,Z2),C1).
	

look_in_direction(Object, InitVector,DesVector):-
	vector_cross_product(InitVector,DesVector,vector(X,Y,Z),R),
	format('X:~w,Y:~w,Z:~w,R:~w~n',[X,Y,Z,R]),
	nonNilVector(vector(X,Y,Z),vector(X1,Y1,Z1)),
	setRotation(Object,X1,Y1,Z1,R).

nonNilVector(V,V1):-
		V = vector(X,Y,Z),
		X = 0.0,
		Y = 0.0,
		Z = 0.0,
		!,
		V1 = vector(0,-1,0).

nonNilVector(V,V):-!.


:-end_object wasp7v.