(* Approximation of zero *)                
let leps l = (abs_float l) < epsilon_float

type point = float array

type vector = float array

(* vector addition *)
let ( |+| ) a b =
        [| a.(0) +. b.(0); a.(1) +. b.(1); a.(2) +. b.(2) |] 

(* vector subtraction *)
let (|-|) a b =
        [| a.(0) -. b.(0); a.(1) -. b.(1); a.(2) -. b.(2) |] 

(* scalar-vector multiplication*)
let (|*|) k a =
        [| k *. a.(0); k *. a.(1); k *. a.(2) |] 

(* dot product *)
let (|**|) a b =
        a.(0) *. b.(0) +. a.(1) *. b.(1) +. a.(2) *. b.(2)
        
(* cross product *)        
let (|^|) v1 v2 = 
        [| v1.(1)*.v2.(2) -. v2.(1)*.v1.(2);  v2.(0)*.v1.(2) -. v1.(0)*.v2.(2);  v1.(0)*.v2.(1) -. v2.(0)*.v1.(1) |]

(* length of a vector *)
let vlength a =
        sqrt (a |**| a)

(* normalize vector *)
let vnorm a = 
        (1.0 /. (vlength a)) |*| a                

(* Calculate the normal of 3 non-collinear points *)
let vnormal p1 p2 p3 =
        vnorm ((p2 |-| p1) |^| (p3 |-| p1))

type intersection = Intersection of point * point * point * point | NoIntersection 
 
(*
 * Axis-aligned bounding box intersection test
 * An AABB is represented by two vectors: the first represents the minimum
 * bounding point (min x, min y, min z), the second the maximal bounding point
 * (max x, max y, max z)
 *)
type aabb = 
{
        bmin: float array;
        bmax: float array; 
}
let aabb_aabb_intersect b1 b2 =
        b1.bmin.(0) <= b2.bmax.(0) && b1.bmax.(0) >= b2.bmin.(0) &&
        b1.bmin.(1) <= b2.bmax.(1) && b1.bmax.(1) >= b2.bmin.(1) &&
        b1.bmin.(2) <= b2.bmax.(2) && b1.bmax.(2) >= b2.bmin.(2) 

(*
 *        Triangle - triangle intersection test.
 *        (1) Construct plane equation coefficients for both triangles.
 *        (2) Check to see if either or both triangles have all three points on the same side of the other triangle's plane.
 *            This can be done by comparing the sign of the distance of the point from the plane.
 *            If so, there is no intersection.
 *        (3) If not, calculate the two intersection points of each triangle with the plane of the other
 *        (4) The segment formed by the two intersection points of each triangle forms a degenerate axis-aligned bounding box
 *            Check if the aabb's of both triangles overlap.
 *            If not, there is no intersection.
 *            If so, return all 4 points.  
 *            This does not handle coplanar triangles well.
 *) 

let tri_tri_intersect a b =
        let na = vnormal a.(0) a.(1) a.(2)
        and nb = vnormal b.(0) b.(1) b.(2) in
        let da =  -.(a.(0) |**| na)
        and db =  -.(b.(0) |**| nb) in
        let distance_to_plane pt n d =
                (d +. (n |**| pt)) 
        and sign k = 
                if (k >= 0.0) then true else false 
        in
        let da1 = distance_to_plane a.(0) nb db 
        and da2 = distance_to_plane a.(1) nb db 
        and da3 = distance_to_plane a.(2) nb db in
        if ((sign da1) = (sign da2) && (sign da2) = (sign da3)) then
                NoIntersection 
        else
        let db1 = distance_to_plane b.(0) na da 
        and db2 = distance_to_plane b.(1) na da 
        and db3 = distance_to_plane b.(2) na da in
        if ((sign db1) = (sign db2) && (sign db2) = (sign db3)) then
                NoIntersection 
        else        
        let get_point p1 p2 d1 d2 =
                let k = (abs_float d1) /. ((abs_float d1) +. (abs_float d2)) in
                p1 |+| ((k) |*| (p2 |-| p1)) 
        in
        let intersect tri d0 d1 d2 = 
                if ((sign d0) = (sign d1)) then
                        ((get_point tri.(1) tri.(2) d1 d2), (get_point tri.(2) tri.(0) d2 d0))
                else
                        if ((sign d1) = (sign d2)) then
                                ((get_point tri.(0) tri.(1) d0 d1), (get_point tri.(2) tri.(0) d2 d0))
                        else        
                                ((get_point tri.(0) tri.(1) d0 d1), (get_point tri.(1) tri.(2) d1 d2))
        in
        let (a,b) = intersect a da1 da2 da3
        and (c,d) = intersect b db1 db2 db3 in
        let bba = {
                        bmax = [|if (a.(0) > b.(0)) then a.(0) else b.(0); 
                                        if (a.(1) > b.(1)) then a.(1) else b.(1); 
                                        if (a.(2) > b.(2)) then a.(2) else b.(2);|];
                        bmin = [|if (a.(0) < b.(0)) then a.(0) else b.(0); 
                                        if (a.(1) < b.(1)) then a.(1) else b.(1); 
                                        if (a.(2) < b.(2)) then a.(2) else b.(2);|];
                }
        and bbb = {
                        bmax = [|if (c.(0) > d.(0)) then c.(0) else d.(0); 
                                        if (c.(1) > d.(1)) then c.(1) else d.(1); 
                                        if (c.(2) > d.(2)) then c.(2) else d.(2);|];
                        bmin = [|if (c.(0) < d.(0)) then c.(0) else d.(0); 
                                        if (c.(1) < d.(1)) then c.(1) else d.(1); 
                                        if (c.(2) < d.(2)) then c.(2) else d.(2);|];
                }
        in 
        if (not (aabb_aabb_intersect bba bbb)) then
                NoIntersection
        else        
                Intersection (a,b,c,d) 
        
        
(**************************************************************************)

let project_point p =
        if (leps p.(2)) then (p.(0), p.(1)) else (p.(0) /. p.(2), p.(1) /. p.(2))
        
let screen_coord (x, y) =
        let w = Graphics.size_x () / 2 and h = Graphics.size_y () / 2 in
        ((truncate x) + w, (truncate y) + h)        

let draw_triangle t =
        if (t.(0).(2) >= 0. && t.(1).(2) >= 0. && t.(2).(2) >= 0.) then
                Graphics.draw_poly (Array.map (fun c -> screen_coord (project_point c)) t)
        else
        ()

let draw_point p =
        let x,y = (screen_coord (project_point p)) in Graphics.draw_circle x y 2
        
let print_intersection p =
        match p with
        | NoIntersection  -> Printf.sprintf "No intersection"
        | Intersection (a, b, c, d) -> 
                Printf.sprintf "(%.2f %.2f %.2f) (%.2f %.2f %.2f) (%.2f %.2f %.2f) (%.2f %.2f %.2f)"  
                                a.(0) a.(1) a.(2)  b.(0) b.(1) b.(2) c.(0) c.(1) c.(2)  d.(0) d.(1) d.(2)
        
let draw_intersection p =
        match p with
        | NoIntersection  -> ()
        | Intersection (a, b,c,d) -> draw_point a; draw_point b; draw_point c; draw_point d
        
        
let random_triangle w h z =
        [|         [| (Random.float w) -. (w/.2.0); (Random.float h) -. (h/.2.0); (Random.float z) +. 1.0|];
                [| (Random.float w) -. (w/.2.0); (Random.float h) -. (h/.2.0); (Random.float z) +. 1.0|];
                [| (Random.float w) -. (w/.2.0); (Random.float h) -. (h/.2.0); (Random.float z) +. 1.0|];        |]
                

(***********************************************************************)

let width = 600
let height = 600

let init_graphics () =
        let init_string = (Printf.sprintf " %dx%d" width height) in
        Graphics.open_graph init_string;
        Graphics.set_window_title "Demo";
        Random.self_init ()
        

let rec main t0 st0 =
        let _ = Graphics.auto_synchronize false in
        let _ = Graphics.clear_graph () in 
        let tri1 = random_triangle (float width) (float height) 1.0  
        and tri2 = random_triangle (float width) (float height) 1.0 in
        let _ = draw_triangle tri1 
        and _ = draw_triangle tri2 in 
        
        let start = Unix.gettimeofday () in
        let intersects = tri_tri_intersect tri1 tri2 in
        let stop = Unix.gettimeofday () in
        
        
        let _ = draw_intersection intersects in
        let _ = Graphics.moveto 10 25 in
        let _ = Graphics.draw_string (Printf.sprintf "intersects: %s" (print_intersection intersects)) in
        let _ = Graphics.moveto 10 10 in
        let _ = Graphics.draw_string (Printf.sprintf "time required: %e sec" (stop -. start)) in
        
        
        let t1 = Unix.gettimeofday () in 
        let _ = Graphics.auto_synchronize true in
        let status = Graphics.wait_next_event [Graphics.Key_pressed] in
        if (status.Graphics.keypressed = true) then main t1 st0 

let ()  = init_graphics (); main (Unix.gettimeofday ()) 1