
(* Vector length *)
let vec3_length { x = x ; y = y ; z = z } =
  sqrt (x *. x +. y *. y +. z *. z)
;;


(* Produit scalaire *)
let vec3_dotprod { x = a1 ; y = a2 ; z = a3 } { x = b1 ; y = b2 ; z = b3 } =
  a1 *. b1 +. a2 *. b2 +. a3 *. b3
;;


(* Produit vectoriel *)
let vec3_crossprod { x = a1 ; y = a2 ; z = a3 } { x = b1 ; y = b2 ; z = b3 } =
  { x = a2 *. b3 -. a3 *. b2 ;
    y = a3 *. b1 -. a1 *. b3 ;
    z = a1 *. b2 -. a2 *. b1 }
;;





(* ******************* *)
(* COLLISION DETECTION *)
(* ******************* *)

let epsilon = 0.01 ;;

let internal_pos =
  { Qheader.x = 0.0; Qheader.y = 0.0 ; Qheader.z = 0.0 } ;;

(*
En fait parcours front to back. Plus exactement: find_lying_leaf
et quand on est a la feuille, on regarde si -1 ou -2
*)
let rec minside_bsp p node_id =
  if node_id >= 0 then
    begin
    let node = !Global.clipnodes.(node_id) in
    let plane = !Global.planes.(node.planenum) in
    (* Calcul de dist invers... *)
    let dist = plane.dist -.
	(plane.normal.x *. p.x +. plane.normal.y *. p.y +.
	   plane.normal.z *. p.z) in
    (* ... Donc test invers aussi *)
    if dist > 0.0
     then minside_bsp p node.cback
     else minside_bsp p node.cfront
    end
  else
    (* ... Et encore invers ici *)
    if node_id = -2 then false else true  (* En fait, -2 ou -1 seulement *)
;;


(* From bsptree FAQ
   How do you perform collision detection with a BSP Tree?
       
       Overview
       Detecting whether or not a point moving along a line intersects
       some object in space is essentially a ray tracing problem.
       Detecting whether or not two complex objects intersect is
       something of a tree merging problem.
       
       Typically, motion is computed in a series of Euler steps. This
       just means that the motion is computed at discrete time intervals
       using some description of the speed of motion. For any given point
       P moving from point A with a velocity V, it's location can be
       computed at time T as P = A + (T * V).
       
       Consider the case where T = 1, and we are computing the motion in
       one second steps. To find out if the point P has collided with any
       part of the scene, we will first compute the endpoints of the
       motion for this time step. P1 = A + V, and P2 = A + (2 * V). These
       two endpoints will be classified with respect to the BSP tree. If
       P1 is outside of all objects, and P2 is inside some object, then
       an intersection has clearly occurred. However, if P2 is also
       outside, we still have to check for a collision in between.
       
       Two approaches are possible. The first is commonly used in
       applications like games, where speed is critical, and accuracy is
       not. This approach is to recursively divide the motion segment in
       half, and check the midpoint for containment by some object.
       Typically, it is good enough to say that an intersection occurred,
       and not be very accurate about where it occurred.
       
       The second approach, which is more accurate, but also more time
       consuming, is to treat the motion segment as a ray, and intersect
       the ray with the BSP Tree. This also has the advantage that the
       motion resulting from the impact can be computed more accurately.
       --
       Last Update: 04/30/95 15:45:20
*)


(* From CLIFF engine (1997) by Nick Komodakis *)
(* I don't understand yet... *)
exception Exit_clip of bool ;;

let rec clip_trajectory p0 p1 node_id =
  let node_id = ref node_id in
  let p = { x = 0.0 ; y = 0.0 ; z = 0.0 } in
  while !node_id >= 0 do
    let node = !Global.clipnodes.(!node_id) in
    let plane = !Global.planes.(node.planenum) in
    let dist = plane.dist -.
	(plane.normal.x *. p0.x +. plane.normal.y *. p0.y +.
	   plane.normal.z *. p0.z) in
    if dist <= epsilon && dist >= (-. epsilon) then
      begin
      let dist = plane.dist -.
	  (plane.normal.x *. p1.x +. plane.normal.y *. p1.y +.
	     plane.normal.z *. p1.z) in
      if dist > epsilon then node_id := node.cback
      else if dist < (-. epsilon) then node_id := node.cfront
      else
	begin
	if not (minside_bsp p0 node.cfront) then node_id := node.cback
	else node_id := node.cfront
	end
      end
    else
      begin
      let den = plane.normal.x *. (p1.x -. p0.x) +.
          plane.normal.y *. (p1.y -. p0.y) +.
          plane.normal.z *. (p1.z -. p0.z) in
      if (dist *. den > 0.0 &&
         ((den > 0.0 && dist < den) ||
          (den < 0.0 && dist > den))) then           (* EPSILON here ? *)
	begin
        let t = dist /. den in
        p.x <- p0.x +. t *. (p1.x -. p0.x) ;
        p.y <- p0.y +. t *. (p1.y -. p0.y) ;
        p.z <- p0.z +. t *. (p1.z -. p0.z) ;

        if dist > 0.0 then
	  begin
          if not (mclip_trajectory p0 p node.cback) then
	    node_id := node.cfront
          else raise (Exit_clip true)
          end
        else
	  begin
          if not (mclip_trajectory p0 p node.cfront) then
            node_id := node.cback
          else raise (Exit_clip true)
          end ;

        p0.x <- p.x ;
        p0.y <- p.y ;
        p0.z <- p.z
	end
      else if dist > 0.0 then node_id := node.cback
      else node_id := node.cfront
      end
  done ;

  if !node_id = -2 then
    begin
    internal_pos.Qheader.x <- p0.Qheader.x ;
    internal_pos.Qheader.y <- p0.Qheader.y ;
    internal_pos.Qheader.z <- p0.Qheader.z ;
    raise (Exit_clip true)
    end
  else raise (Exit_clip false)


and mclip_trajectory p0 p1 node_id =
  (* Pour eviter la modif en place de l'argument *)
  let p0 = { Qheader.x = p0.Qheader.x ; Qheader.y = p0.Qheader.y ;
	     Qheader.z = p0.Qheader.z } in
  let p1 = { Qheader.x = p1.Qheader.x ; Qheader.y = p1.Qheader.y ;
	     Qheader.z = p1.Qheader.z } in
  try let _ = clip_trajectory p0 p1 node_id in assert false
  with Exit_clip r -> r
;;

(* ************************** *)
(* END OF COLLISION DETECTION *)
(* ************************** *)





(* ************************************ *)
(* MAIN - CONTROLER - MOVEMENT HANDLING *)
(* ************************************ *)

let handle_mouse mouse_x mouse_y =
  (* Vise horizontale *)
  let angle = -. (float mouse_x) /. 5.0 in
  Matrix.fill_rz_matrix (rad angle) tmp_matrix ;
  Matrix.mult_matrices Global.camera.rotation tmp_matrix
         Global.camera.rotation ;
  Matrix.fill_rz_matrix (rad (-. angle)) tmp_matrix ;
  Matrix.mult_matrices tmp_matrix Global.camera.irotation
         Global.camera.irotation ;
  (* Vise verticale *)
  let angle = -. (float mouse_y) /. 5.0 in
  Matrix.fill_ry_matrix (rad angle) tmp_matrix ;
  Matrix.mult_matrices Global.camera.rotation tmp_matrix
         Global.camera.rotation ;
  Matrix.fill_ry_matrix (rad (-. angle)) tmp_matrix ;
  Matrix.mult_matrices tmp_matrix Global.camera.irotation
         Global.camera.irotation
;;


let handle_key = function
  | 97 ->  (* Home : move forward *)
      let tmp = { Qheader.x = 20.0 ; Qheader.y = 0.0; Qheader.z = 0.0 } in
      let tmp' = vec3_rotate tmp Global.camera.rotation in
      Global.camera.position.Qheader.x <-
             Global.camera.position.Qheader.x +. tmp'.Qheader.x ;
      Global.camera.position.Qheader.y <-
             Global.camera.position.Qheader.y +. tmp'.Qheader.y ;
      Global.camera.position.Qheader.z <-
             Global.camera.position.Qheader.z +. tmp'.Qheader.z
  | 103 -> (* End : move backward *)
      let tmp = { Qheader.x = -.20.0 ; Qheader.y = 0.0; Qheader.z = 0.0 } in
      let tmp' = vec3_rotate tmp Global.camera.rotation in
      Global.camera.position.Qheader.x <-
      Global.camera.position.Qheader.x +. tmp'.Qheader.x ;
      Global.camera.position.Qheader.y <-
      Global.camera.position.Qheader.y +. tmp'.Qheader.y ;
      Global.camera.position.Qheader.z <-
      Global.camera.position.Qheader.z +. tmp'.Qheader.z
  | 82 -> (* Minus : go down *)
      let tmp = { Qheader.x = 0.0 ; Qheader.y = 0.0; Qheader.z = -20.0 } in
      let tmp' = vec3_rotate tmp Global.camera.rotation in
      Global.camera.position.Qheader.x <-
             Global.camera.position.Qheader.x +. tmp'.Qheader.x ;
      Global.camera.position.Qheader.y <-
             Global.camera.position.Qheader.y +. tmp'.Qheader.y ;
      Global.camera.position.Qheader.z <-
             Global.camera.position.Qheader.z +. tmp'.Qheader.z
  | 86 -> (* Minus : go up *)
      let tmp = { Qheader.x = 0.0 ; Qheader.y = 0.0; Qheader.z = 20.0 } in
      let tmp' = vec3_rotate tmp Global.camera.rotation in
      Global.camera.position.Qheader.x <-
             Global.camera.position.Qheader.x +. tmp'.Qheader.x ;
      Global.camera.position.Qheader.y <-
             Global.camera.position.Qheader.y +. tmp'.Qheader.y ;
      Global.camera.position.Qheader.z <-
             Global.camera.position.Qheader.z +. tmp'.Qheader.z
  | 107 -> (* del : strafe left *)
      let tmp = { Qheader.x = 0.0 ; Qheader.y = 20.0; Qheader.z = 0.0 } in
      let tmp' = vec3_rotate tmp Global.camera.rotation in
      Global.camera.position.Qheader.x <-
             Global.camera.position.Qheader.x +. tmp'.Qheader.x ;
      Global.camera.position.Qheader.y <-
             Global.camera.position.Qheader.y +. tmp'.Qheader.y ;
      Global.camera.position.Qheader.z <-
             Global.camera.position.Qheader.z +. tmp'.Qheader.z
  | 105 -> (* pgdn : strafe right *)
      let tmp = { Qheader.x = 0.0 ; Qheader.y = -.20.0; Qheader.z = 0.0 } in
      let tmp' = vec3_rotate tmp Global.camera.rotation in
      Global.camera.position.Qheader.x <-
             Global.camera.position.Qheader.x +. tmp'.Qheader.x ;
      Global.camera.position.Qheader.y <-
             Global.camera.position.Qheader.y +. tmp'.Qheader.y ;
      Global.camera.position.Qheader.z <-
             Global.camera.position.Qheader.z +. tmp'.Qheader.z
  | 99 -> (* pgup : turn right *)
      Matrix.fill_rz_matrix (rad (-. 10.0)) tmp_matrix ;
      Matrix.mult_matrices Global.camera.rotation tmp_matrix
             Global.camera.rotation ;
      Matrix.fill_rz_matrix (rad (10.0)) tmp_matrix ;
      Matrix.mult_matrices tmp_matrix Global.camera.irotation
             Global.camera.irotation
  | 106 -> (* insert : turn left *)
      Matrix.fill_rz_matrix (rad 10.0) tmp_matrix ;
      Matrix.mult_matrices Global.camera.rotation tmp_matrix
             Global.camera.rotation ;
      Matrix.fill_rz_matrix (rad (-. 10.0)) tmp_matrix ;
      Matrix.mult_matrices tmp_matrix Global.camera.irotation
             Global.camera.irotation
  | 54 -> (* c : clipping on / off *)
      Global.coldet_active := not !Global.coldet_active
  | 34 -> (* [ Roll left *)
      Matrix.fill_rx_matrix (rad (-. 10.0)) tmp_matrix ;
      Matrix.mult_matrices Global.camera.rotation tmp_matrix
             Global.camera.rotation ;
      Matrix.fill_rx_matrix (rad 10.0) tmp_matrix ;
      Matrix.mult_matrices tmp_matrix Global.camera.irotation
             Global.camera.irotation
  | 35 -> (* ] Roll right *)
      Matrix.fill_rx_matrix (rad 10.0) tmp_matrix ;
      Matrix.mult_matrices Global.camera.rotation tmp_matrix
             Global.camera.rotation ;
      Matrix.fill_rx_matrix (rad (-. 10.0)) tmp_matrix ;
      Matrix.mult_matrices tmp_matrix Global.camera.irotation
             Global.camera.irotation
  | 9 -> (* Escape : quit *)
      clean_exit ()
  | _ -> ()
;;


(* ******************************************* *)
(* END OF MAIN - CONTROLER - MOVEMENT HANDLING *)
(* ******************************************* *)
