From 4f262d6540281487f79870aff589ca92f5d2f6c6 Mon Sep 17 00:00:00 2001
From: Sébastien Dailly <sebastien@chimrod.com>
Date: Wed, 16 Dec 2020 14:39:42 +0100
Subject: Initial commit

---
 curves/bspline.ml | 149 ++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 149 insertions(+)
 create mode 100755 curves/bspline.ml

(limited to 'curves/bspline.ml')

diff --git a/curves/bspline.ml b/curves/bspline.ml
new file mode 100755
index 0000000..bb60227
--- /dev/null
+++ b/curves/bspline.ml
@@ -0,0 +1,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
+
-- 
cgit v1.2.3