(* 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