summaryrefslogtreecommitdiff
path: root/shapes/bspline.ml
blob: bb60227f5ff0fb6a26225331e32bc1fffd4292c9 (plain)
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
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
open StdLabels

type err = [`InvalidPath ]

module M = Matrix.MakeMatrix (struct

    type t = Float.t

    let compare a b =

      let v = Float.compare a b in
      if v = 0 then Matrix.Order.Equal
      else if v > 0 then Matrix.Order.Greater
      else Matrix.Order.Less

    let zero = Float.zero
    let one = Float.one

    let divide = (/.)
    let multiply = ( *. )
    let add = (+.)
    let subtract = (-.)
    exception NonElt


  end)

type t = Gg.v2 list

let from_points
  : Gg.v2 array -> (Gg.v2 array, [> `InvalidPath]) Result.t
  = fun points ->

    let n = (Array.length points - 2) in

    if n <= 1 then
      Result.error `InvalidPath
    else

      (* Create the initial matrix.

         The matrix is augmented with two additionals columns, which will be
         populated with the points from the path.
      *)
      let arr = Array.init n ~f:(fun line ->
          Array.init (n +2) ~f:(fun row ->
              match row - line with
              | (-1) -> 1.
              | 0 -> 4.
              | 1 -> 1.
              | _ -> 0.
            )
        ) in
      let matrix = M.from_array arr in

      (* Add the points from the augmented matrix *)
      let points_array = points in
      for line = 0 to (n -1) do

        let point =
          if line = 0 then
            let p0 = points_array.(0)
            and p1 = points_array.(1) in
            Gg.V2.(6. * p1 - p0)
          else if (line + 1) = n  then
            let p_n_2 = points_array.(n)
            and p_n_1 = points_array.(n + 1) in
            Gg.V2.(6. * p_n_2 - p_n_1)
          else
            let n' = line + 1 in
            Gg.V2.(6. * points_array.(n'))
        in
        let x = (Gg.V2.x point)
        and y = (Gg.V2.y point) in


        M.set_elt matrix (line + 1, n + 1) x;
        M.set_elt matrix (line + 1, n + 2) y;
      done;

      (* Resolve the matrix *)
      let res' = M.row_reduce matrix in

      (* Extract the result as points *)
      let _, col_x = M.get_column res' (n + 1)
      and _, col_y = M.get_column res' (n + 2) in

      (* Build the result *)
      let res = Array.make (n + 2) (Array.get points_array (n + 1) ) in
      for i = 1 to n do
        let point = Gg.V2.v col_x.(i - 1) col_y.(i - 1) in
        Array.set res i point;
      done;
      Array.set res 0 (Array.get points_array 0);
      Result.ok res

let (let*) = Result.bind

(** Build a continue curve from path

    see https://www.math.ucla.edu/~baker/149.1.02w/handouts/dd_splines.pdf
*)
let to_bezier
  : ?connexion0:Gg.v2 -> ?connexion1:Gg.v2 -> t -> (Bezier.t array, [> `InvalidPath]) Result.t
  = fun ?connexion0 ?connexion1 points ->

    let points' = match connexion0 with
      | None -> points
      | Some pt -> pt::points in

    let arr_points = match connexion1 with
      | None -> Array.of_list points'
      | Some pt ->
        let arr = Array.make (1 + (List.length points')) pt in
        List.iteri points'
          ~f:(fun i value -> Array.set arr i value);
        arr in

    let* bspline_points = from_points arr_points in

    let start = match connexion0 with
      | None -> 1
      | Some _ -> 2
    and end_ = match connexion1 with
      | None -> (Array.length bspline_points) - 1
      | Some _ -> (Array.length bspline_points) - 2 in

    let result = Array.init (end_ - start + 1) ~f:(fun i ->

        let i = i + start in

        let prev_b = Array.get bspline_points (i - 1)
        and bpoint = Array.get bspline_points i
        and prev_p = Array.get arr_points (i - 1)
        and point = Array.get arr_points i in
        let ctrl0 = Gg.V2.(mix prev_b bpoint (1. /. 3.))
        and ctrl1 = Gg.V2.(mix prev_b bpoint (2. /. 3.)) in

        let bezier =
          { Bezier.p0 = prev_p
          ; Bezier.p1 = point
          ; Bezier.ctrl0
          ; Bezier.ctrl1 } in

        bezier

      ) in
    Result.Ok result