Source file transform.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
120
121
122
123
124
125
126
127
128
129
130
131
open Util

type t = {
  e00 : float;
  e10 : float;
  e20 : float;
  (* col 0 *)
  e01 : float;
  e11 : float;
  e21 : float;
  (* col 1 *)
  e02 : float;
  e12 : float;
  e22 : float; (* col 2 *)
}

let v e00 e01 e02 e10 e11 e12 e20 e21 e22 =
  { e00; e10; e20; e01; e11; e21; e02; e12; e22 }

let id = v 1. 0. 0. 0. 1. 0. 0. 0. 1.

(* Functions *)

let neg a =
  v (-.a.e00) (-.a.e01) (-.a.e02) (-.a.e10) (-.a.e11) (-.a.e12) (-.a.e20)
    (-.a.e21) (-.a.e22)

let add a b =
  v (a.e00 +. b.e00) (a.e01 +. b.e01) (a.e02 +. b.e02) (a.e10 +. b.e10)
    (a.e11 +. b.e11) (a.e12 +. b.e12) (a.e20 +. b.e20) (a.e21 +. b.e21)
    (a.e22 +. b.e22)

let sub a b =
  v (a.e00 -. b.e00) (a.e01 -. b.e01) (a.e02 -. b.e02) (a.e10 -. b.e10)
    (a.e11 -. b.e11) (a.e12 -. b.e12) (a.e20 -. b.e20) (a.e21 -. b.e21)
    (a.e22 -. b.e22)

let mmul a b =
  if a == id then b
  else if b == id then a
  else
    v
      ((a.e00 *. b.e00) +. (a.e01 *. b.e10) +. (a.e02 *. b.e20))
      ((a.e00 *. b.e01) +. (a.e01 *. b.e11) +. (a.e02 *. b.e21))
      ((a.e00 *. b.e02) +. (a.e01 *. b.e12) +. (a.e02 *. b.e22))
      ((a.e10 *. b.e00) +. (a.e11 *. b.e10) +. (a.e12 *. b.e20))
      ((a.e10 *. b.e01) +. (a.e11 *. b.e11) +. (a.e12 *. b.e21))
      ((a.e10 *. b.e02) +. (a.e11 *. b.e12) +. (a.e12 *. b.e22))
      ((a.e20 *. b.e00) +. (a.e21 *. b.e10) +. (a.e22 *. b.e20))
      ((a.e20 *. b.e01) +. (a.e21 *. b.e11) +. (a.e22 *. b.e21))
      ((a.e20 *. b.e02) +. (a.e21 *. b.e12) +. (a.e22 *. b.e22))

let mul a b =
  v (a.e00 *. b.e00) (a.e01 *. b.e01) (a.e02 *. b.e02) (a.e10 *. b.e10)
    (a.e11 *. b.e11) (a.e12 *. b.e12) (a.e20 *. b.e20) (a.e21 *. b.e21)
    (a.e22 *. b.e22)

let div a b =
  v (a.e00 /. b.e00) (a.e01 /. b.e01) (a.e02 /. b.e02) (a.e10 /. b.e10)
    (a.e11 /. b.e11) (a.e12 /. b.e12) (a.e20 /. b.e20) (a.e21 /. b.e21)
    (a.e22 /. b.e22)

let smul a s =
  v (s *. a.e00) (s *. a.e01) (s *. a.e02) (s *. a.e10) (s *. a.e11)
    (s *. a.e12) (s *. a.e20) (s *. a.e21) (s *. a.e22)

let transpose a = v a.e00 a.e10 a.e20 a.e01 a.e11 a.e21 a.e02 a.e12 a.e22

let det a =
  let m00 = (a.e11 *. a.e22) -. (a.e21 *. a.e12) in
  (* minor. *)
  let m10 = (a.e01 *. a.e22) -. (a.e21 *. a.e02) in
  let m20 = (a.e01 *. a.e12) -. (a.e11 *. a.e02) in
  (a.e00 *. m00) -. (a.e10 *. m10) +. (a.e20 *. m20)

let inv a =
  let m00 = (a.e11 *. a.e22) -. (a.e21 *. a.e12) in
  (* minor. *)
  let m10 = (a.e01 *. a.e22) -. (a.e21 *. a.e02) in
  let m20 = (a.e01 *. a.e12) -. (a.e11 *. a.e02) in
  let m01 = (a.e10 *. a.e22) -. (a.e20 *. a.e12) in
  let m11 = (a.e00 *. a.e22) -. (a.e20 *. a.e02) in
  let m21 = (a.e00 *. a.e12) -. (a.e10 *. a.e02) in
  let m02 = (a.e10 *. a.e21) -. (a.e20 *. a.e11) in
  let m12 = (a.e00 *. a.e21) -. (a.e20 *. a.e01) in
  let m22 = (a.e00 *. a.e11) -. (a.e10 *. a.e01) in
  let det = (a.e00 *. m00) -. (a.e10 *. m10) +. (a.e20 *. m20) in
  v (m00 /. det) (-.m10 /. det) (m20 /. det) (-.m01 /. det) (m11 /. det)
    (-.m21 /. det) (m02 /. det) (-.m12 /. det) (m22 /. det)

(* 2D space transforms *)

let move2 d = v 1. 0. (fst d) 0. 1. (snd d) 0. 0. 1.

let rot2 ?pt theta =
  let c = cos theta in
  let s = sin theta in
  match pt with
  | None -> v c (-.s) 0. s c 0. 0. 0. 1.
  | Some pt ->
      let px = fst pt in
      let py = snd pt in
      v c (-.s)
        ((-.c *. px) +. (s *. py) +. px)
        s c
        ((-.s *. px) -. (c *. py) +. py)
        0. 0. 1.

let scale2 s = v (fst s) 0. 0. 0. (snd s) 0. 0. 0. 1.

let transform m p =
  ( (m.e00 *. fst p) +. (m.e01 *. snd p) +. m.e02,
    (m.e10 *. fst p) +. (m.e11 *. snd p) +. m.e12 )

let translate x y = move2 (x, y)

let rotate ?center angle =
  let angle = -.Angle.to_radians angle in
  rot2 ?pt:center angle

let scale x y = scale2 (1.0 /. x, 1.0 /. y)

module Infix = struct
  let ( + ) = add

  let ( - ) = sub

  let ( * ) = mul

  let ( / ) = div
end