Source file Broadphase3d.ml
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
type box = Vec3.t * Vec3.t
type method_ = All_pairs | Grid | Sweep_and_prune
let methods = [ All_pairs; Grid; Sweep_and_prune ]
let name = function All_pairs -> "all pairs" | Grid -> "grid" | Sweep_and_prune -> "sweep and prune"
type result = { pairs : (int * int) list; tests : int }
let ordered i j = if i < j then (i, j) else (j, i)
let all_pairs (boxes : box array) : result =
let n = Array.length boxes in
let pairs = ref [] in
for i = 0 to n - 1 do
for j = i + 1 to n - 1 do
if Collide3d.bounds_overlap boxes.(i) boxes.(j) then pairs := (i, j) :: !pairs
done
done;
{ pairs = List.rev !pairs; tests = n * (n - 1) / 2 }
let cell_size (boxes : box array) : float =
Array.fold_left
(fun m ((x0, y0, z0), (x1, y1, z1)) -> Float.max m (Float.max (x1 -. x0) (Float.max (y1 -. y0) (z1 -. z0))))
0. boxes
let grid ?cell (boxes : box array) : result =
let cell = match cell with Some c -> c | None -> cell_size boxes in
let cell = if cell > 0. then cell else 1. in
let index v = int_of_float (Float.floor (v /. cell)) in
let cells : (int * int * int, int list) Hashtbl.t = Hashtbl.create 64 in
boxes
|> Array.iteri (fun i ((x0, y0, z0), (x1, y1, z1)) ->
for cx = index x0 to index x1 do
for cy = index y0 to index y1 do
for cz = index z0 to index z1 do
Hashtbl.replace cells (cx, cy, cz) (i :: Option.value ~default:[] (Hashtbl.find_opt cells (cx, cy, cz)))
done
done
done);
let tested = Hashtbl.create 64 and pairs = ref [] in
cells
|> Hashtbl.iter (fun _ inside ->
List.iter
(fun i ->
List.iter
(fun j ->
if i < j && not (Hashtbl.mem tested (i, j)) then (
Hashtbl.add tested (i, j) ();
if Collide3d.bounds_overlap boxes.(i) boxes.(j) then pairs := (i, j) :: !pairs))
inside)
inside);
{ pairs = List.sort compare !pairs; tests = Hashtbl.length tested }
let spread (boxes : box array) : float * float * float =
let n = Array.length boxes in
if n = 0 then (0., 0., 0.)
else
let sum = ref (0., 0., 0.) and sum2 = ref (0., 0., 0.) in
Array.iter
(fun ((x0, y0, z0), (x1, y1, z1)) ->
let c = ((x0 +. x1) /. 2., (y0 +. y1) /. 2., (z0 +. z1) /. 2.) in
sum := Vec3.add !sum c;
let cx, cy, cz = c in
sum2 := Vec3.add !sum2 (cx *. cx, cy *. cy, cz *. cz))
boxes;
let f = float_of_int n in
let mx, my, mz = Vec3.scale (1. /. f) !sum and sx, sy, sz = Vec3.scale (1. /. f) !sum2 in
(sx -. (mx *. mx), sy -. (my *. my), sz -. (mz *. mz))
let widest_axis (boxes : box array) : int =
let vx, vy, vz = spread boxes in
if vx >= vy && vx >= vz then 0 else if vy >= vz then 1 else 2
let along (axis : int) ((lo, hi) : box) : float * float =
let x0, y0, z0 = lo and x1, y1, z1 = hi in
match axis with 0 -> (x0, x1) | 1 -> (y0, y1) | _ -> (z0, z1)
let sweep_and_prune ?axis (boxes : box array) : result =
let axis = match axis with Some a -> a | None -> widest_axis boxes in
let low i = fst (along axis boxes.(i)) and high i = snd (along axis boxes.(i)) in
let order = List.sort (fun i j -> compare (low i) (low j)) (List.init (Array.length boxes) Fun.id) in
let tests = ref 0 and pairs = ref [] in
let (_ : int list) =
List.fold_left
(fun active i ->
let active = List.filter (fun a -> high a >= low i) active in
List.iter
(fun a ->
incr tests;
if Collide3d.bounds_overlap boxes.(a) boxes.(i) then pairs := ordered a i :: !pairs)
active;
i :: active)
[] order
in
{ pairs = List.sort compare !pairs; tests = !tests }
let pairs (m : method_) (boxes : box array) : result =
match m with All_pairs -> all_pairs boxes | Grid -> grid boxes | Sweep_and_prune -> sweep_and_prune boxes