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
type t = float array array
let of_row_list_exn l =
if List.length l = 3
then (
let mat = Array.make_matrix 3 3 0. in
let set_row i (c0, c1, c2) =
mat.(i).(0) <- c0;
mat.(i).(1) <- c1;
mat.(i).(2) <- c2
in
List.iteri set_row l;
mat )
else failwith "Rotation matrix must have 3 rows."
let of_row_list l =
try Ok (of_row_list_exn l) with
| Failure e -> Error e
let of_col_list_exn l =
if List.length l = 3
then (
let mat = Array.make_matrix 3 3 0. in
let set_col i (r0, r1, r2) =
mat.(0).(i) <- r0;
mat.(1).(i) <- r1;
mat.(2).(i) <- r2
in
List.iteri set_col l;
mat )
else failwith "Rotation matrix must have 3 columns."
let of_col_list l =
try Ok (of_col_list_exn l) with
| Failure e -> Error e
let align_exn a b =
if Vec3.(
equal a b || equal a (negate b) || equal a (0., 0., 0.) || equal b (0., 0., 0.))
then failwith "Vectors must not be equal or zero."
else (
let x = Vec3.normalize a
and z = Vec3.(normalize (cross a b)) in
let y = Vec3.(normalize (cross z x)) in
of_col_list_exn [ x; y; z ] )
let align a b =
try Ok (align_exn a b) with
| Failure e -> Error e
let to_euler t =
let x = Float.atan2 t.(2).(1) t.(2).(2) in
let y =
Float.atan2
(-1. *. t.(2).(0))
(Float.sqrt ((t.(2).(1) *. t.(2).(1)) +. (t.(2).(2) *. t.(2).(2))))
in
let z = Float.atan2 t.(1).(0) t.(0).(0) in
x, y, z
let trace t = t.(0).(0) +. t.(1).(1) +. t.(2).(2)
let get t r c = t.(r).(c)
include (
SquareMatrix.Make (struct
let size = 3
end) :
SquareMatrix.Ops with type t := t )