feat: Clough-Tocher interpolation

Seems to works, but ded slow

src/Evaluate.fs

@@ -1,10 +1,17 @@
 module Oceanbox.FvcomKit.Evaluate
 
 open Grid
+open Gradient
 
 let private getCell (grid: ExtendedGrid) (idx: ElemIdx) =
     (grid :> IGrid).getCell idx
 
+let private getVertex (grid: ExtendedGrid) (idx: NodeIdx) =
+    (grid :> IGrid).getVertex idx
+
+let private getCentroid (grid: ExtendedGrid) (idx: ElemIdx) =
+    (grid.getCentroids ())[idx]
+
 let private containsIndex (idx: ElemIdx) (cell: Cell) =
     let x, y, z = cell
     (idx = x) || (idx = y) || (idx = z)
@@ -21,112 +28,212 @@ let private countCommonIndices (c1: Cell) (c2: Cell) =
     let s2 = countIndex i2 c2
     s0 + s1 + s2
 
-// Elements have a shared edge if they have two common node indices
-let private sharingEdge (grid: ExtendedGrid) (i1: ElemIdx) (i0: ElemIdx) =
+// Elements are adjacent if they have a shared edge, i.e. have two common node indices
+let private areAdjacent (grid: ExtendedGrid) (i1: ElemIdx) (i0: ElemIdx) =
     let nCommon =
         countCommonIndices
         <| (getCell grid i0)
         <| (getCell grid i1)
     nCommon = 2
 
-let private iterQandS (C: ElemIdx -> single*single) (F: ElemIdx -> single) (df: single*single) (i0: ElemIdx) (i1: ElemIdx) =
-    let x0, y0 = C i0
-    let x1, y1 = C i1
-
-    let ex = x1 - x0
-    let ey = y1 - y0
-
-    let L = sqrt (ex*ex + ey*ey)
-    let L3 = L*L*L
-
-    let f0 = F i0
-    let f1 = F i1
-
-    let dfx, dfy = df
-    let df2 = -ex*dfx - ey*dfy
-
-    let q1 = 4f * ex * ex / L3
-    let q2 = 4f * ex * ey / L3
-    let q3 = 4f * ey * ey / L3
-
-    let s1 = (6f*(f0 - f1) - 2f*df2) * ex / L3
-    let s2 = (6f*(f0 - f1) - 2f*df2) * ey / L3
-
-    let Q = [| q1; q2; q2; q3 |]
-    let s = [| s1; s2 |]
-
-    Q, s
-
-let private calcQandS (C: ElemIdx -> single*single) (F: ElemIdx -> single) (neighbors: ElemIdx list) (i0: ElemIdx) (df: single*single) =
-    let (<+>) a b = Array.map2 (+) a b
-
-    // capture fixed parameters in upcoming loop
-    let iterQSclosure = iterQandS C F df i0
-
-    let rec accumulateQandS Q s nodes =
-        match nodes with
-        | [] -> Q, s
-        | x::xs ->
-            let Q', s' = iterQSclosure x
-            accumulateQandS (Q <+> Q') (s <+> s') xs
-
-    let Q0 = [| 0f; 0f; 0f; 0f |]
-    let s0 = [| 0f; 0f |]
-    accumulateQandS Q0 s0 neighbors
-
-let private calcGradient (grid: ExtendedGrid) (field: single[]) (i0: ElemIdx) (thrs: single) : single*single =
+// Picks out the three neighbors of an element that are sharing a common edge
+// (two neighbors for boundary elements)
+let private getAdjacentNeighbors (grid: ExtendedGrid) (idx: ElemIdx) =
     let neighbors =
-        grid.getElemsSurroundingElem i0
-        |> Array.filter (sharingEdge grid i0)
-        |> Array.toList
+        grid.getElemsSurroundingElem idx
+        |> Array.filter (areAdjacent grid idx)
 
+    // a valid centroid grid should always give two or three neighbors
     match neighbors.Length with
     | n when n < 2 -> failwith "Too few neighbors"
     | n when n > 3 -> failwith "Too many neighbors"
-    | _ -> ()
+    | _ -> neighbors
 
-    // define functions to return points and values on the grid
-    let C (idx: ElemIdx) : single*single = (grid.getCentroids ())[idx]
-    let F (idx: ElemIdx) : single = field[idx]
-
-    // capture fixed parameters in upcoming loop
-    let calcQSclosure = calcQandS C F neighbors i0
-
-    let rec convergeGradient (error: single) (df: single*single) =
-        if error < thrs then
-            df
-        else
-            let Q, s = calcQSclosure df
-            let detQ = Q[0]*Q[3] - Q[1]*Q[2]
-
-            if (abs detQ) < thrs*thrs then
-                df
-            else
-                let r0 = ( Q[3]*s[0] - Q[1]*s[1]) / detQ
-                let r1 = (-Q[2]*s[0] + Q[0]*s[1]) / detQ
-                let dfx, dfy = df
-                let change = max (abs (dfx + r0)) (abs (dfy + r1))
-                convergeGradient change (-r0, -r1)
-
-    let err0 = 1f
-    let grad0 = 0f, 0f
-    convergeGradient err0 grad0
-
-let evaluate (grid: ExtendedGrid) (F: Field) (P: Pos)  =
-    match grid.tryGetElement P with
+// Evaluate 2D field based on linear extrapolation from centroid field value,
+// using gradient estimated from values in the three adjacent neigboring elements
+let evaluateLinearCentroid (grid: ExtendedGrid) (field: Field) (p: Pos)  =
+    match grid.tryGetElement p with
         | None -> None
-        | Some e ->
-            let U = F |> Array.map fst
-            let V = F |> Array.map snd
+        | Some e0 ->
+            let centroids = grid.getCentroids ()
+
+            let c_0 = centroids[e0]
+            let u_0 = field[e0] |> fst
+            let v_0 = field[e0] |> snd
+
+            let neighbors = getAdjacentNeighbors grid e0
+            let c_n = neighbors |> Array.map (Array.get centroids)
+            let u_n = neighbors |> Array.map (Array.get field) |> Array.map fst
+            let v_n = neighbors |> Array.map (Array.get field) |> Array.map snd
 
             let thrs = 1e-6f
-            let ux, uy = calcGradient grid U e thrs
-            let vx, vy = calcGradient grid V e thrs
+            let du_x, du_y = calcGradientFromNeighbors c_0 u_0 c_n u_n thrs
+            let dv_x, dv_y = calcGradientFromNeighbors c_0 v_0 c_n v_n thrs
 
-            let x0, y0 = (grid.getCentroids ())[e]
-            let x, y = P
+            let x, y = p
+            let x_0, y_0 = c_0
 
-            let u0, v0 = F[e]
-            let u = u0 + ux*(x - x0) + uy*(y - y0)
-            let v = v0 + vx*(x - x0) + vy*(y - y0)
+            let u = u_0 + du_x*(x - x_0) + du_y*(y - y_0)
+            let v = v_0 + dv_x*(x - x_0) + dv_y*(y - y_0)
             Some (u, v)
+
+// Evaluate 2D field based on constant extrapolation from centroid field value
+let evaluateConstantCentroid (grid: ExtendedGrid) (field: Field) (p: Pos)  =
+    match grid.tryGetElement p with
+        | None -> None
+        | Some e0 -> Some (field[e0])
+
+
+
+
+
+// Given centroid 2D field values, compute 2D field value in single vertex
+// by a weighted average of values in all surrounding elements
+let private calcVertexValue (grid: ExtendedGrid) (field: Field) (idx: NodeIdx) : single*single =
+    // Fetch surrounding elements
+    let e_n = grid.getElemsSurroundingNode idx
+
+    // Fetch points
+    let p_0 = getVertex grid idx
+    let p_n = e_n |> Array.map (getCentroid grid)
+
+    // Fetch function values
+    let f_n = e_n |> Array.map (Array.get field)
+    let fx_n = f_n |> Array.map fst
+    let fy_n = f_n |> Array.map snd
+
+    // Helper functions
+    let (<->) (a1, a2) (b1, b2) = (a1 - b1, a2 - b2)
+    let inv_norm (px, py) = 1f / sqrt (px*px + py*py)
+
+    // Compute inverse distance weights
+    let w_n = p_n |> Array.map (fun p -> p <-> p_0) |> Array.map inv_norm
+    let W = w_n |> Array.sum
+
+    // Compute mean function value on vertex P_0
+    let fx_0 = (Array.map2 (*) w_n fx_n |> Array.sum) / W
+    let fy_0 = (Array.map2 (*) w_n fy_n |> Array.sum) / W
+    fx_0, fy_0
+
+// Transform centroid (Voronoi) 2D field values to vertex (Delaunay) 2D field values
+let centroidToVertexValues (grid: ExtendedGrid) (field: Field) : Field =
+    let N = ((grid :> IGrid).getVertices ()).Length
+    [| 0..(N-1) |] |> Array.Parallel.map (calcVertexValue grid field)
+
+
+
+
+
+// Cyclic access of array, (A[A.Length] = A[0]), (A[-1] = A[A.Length-1]), etc
+let private access (A: single[]) (idx: int) : single =
+    let l = A.Length
+    let i = idx % l
+    if i >= 0 then A[i] else A[l + i]
+
+// Clough-Tocher interpolation of 1D field inside single triangular cell
+// p_0: position to evaluate, should be within the triangle defined by P_n
+// p_n, f_n, df_n: Positions, values and gradients at cell vertices
+// Ref: C.L. Lawson "C1-Compatible Interpolation Over a Triangle" (1976)
+let private interpolateTriangleCT (p_n: Pos[]) (f_n: single[]) (df_n: Field) (p_0: Pos): single=
+    let idx = [| 0..2 |]
+
+    // Phase 1:
+    let x_n = p_n |> Array.map fst
+    let y_n = p_n |> Array.map snd
+
+    let u = idx |> Array.map (fun i -> (access x_n (i-1)) - (access x_n (i+1)))
+    let v = idx |> Array.map (fun i -> (access y_n (i-1)) - (access y_n (i+1)))
+    let l2 = Array.map2 (fun a b -> a*a + b*b) u v
+
+    let delta = u[0]*v[1] - v[0]*u[1]
+    if (abs delta) < 1.0e-12f then failwith "Co-linear vertices!"
+    let d_inv = 1f / delta
+
+    let x, y = p_0
+    let x_tilde = x - x_n[0]
+    let y_tilde = y - y_n[0]
+
+    let r_1 = d_inv * (u[1]*y_tilde - v[1]*x_tilde)
+    let r_2 = d_inv * (u[2]*y_tilde - v[2]*x_tilde)
+    let r_0 = 1f - (r_1 + r_2)
+    let r = [| r_0; r_1; r_2 |]
+
+    let exterior = r |> Array.exists (fun r_i -> r_i < 0f)
+    if exterior then failwith "Point is outside the triangle!"
+
+    let phi = idx |> Array.map (fun i -> (access r (i+1)) * (access r (i-1)))
+
+    // Phase 2 (Zienkievicz):
+    let rho = idx |> Array.map (fun i ->
+            let r_im1 = access r (i-1)
+            let r_ip1 = access r (i+1)
+            (r[i] * r_ip1*r_ip1 * r_im1*r_im1) / ((1f - r_ip1)*(1f - r_im1))
+        )
+
+    // Phase 3:
+    let fx_n = df_n |> Array.map fst
+    let fy_n = df_n |> Array.map snd
+
+    let h_tilde = idx |> Array.map (fun i ->
+            let fx_ip1 = access fx_n (i+1)
+            let fy_ip1 = access fy_n (i+1)
+            u[i]*fx_ip1 + v[i]*fy_ip1
+        )
+
+    let k_tilde = idx |> Array.map (fun i ->
+            let fx_im1 = access fx_n (i-1)
+            let fy_im1 = access fy_n (i-1)
+            u[i]*fx_im1 + v[i]*fy_im1
+        )
+
+    let g_tilde = idx |> Array.map (fun i ->
+            let r_ip1 = access r (i+1)
+            let r_im1 = access r (i-1)
+            let l2_ip1 = access l2 (i+1)
+            let l2_im1 = access l2 (i-1)
+            let rho_ip1 = access rho (i+1)
+            let rho_im1 = access rho (i-1)
+            (r_ip1 - r_im1) * phi[i] + (3f * rho[i]) * (l2_ip1 - l2_im1) / l2[i] - rho_ip1 + rho_im1
+        )
+
+    let w = idx |> Array.map (fun i ->
+            let f_im1 = access f_n (i-1)
+            let f_ip1 = access f_n (i+1)
+            f_n[i]*r[i] + 0.5f * (h_tilde[i] - k_tilde[i]) * phi[i] +
+            g_tilde[i] * (0.5f * (h_tilde[i] + k_tilde[i]) - f_im1 + f_ip1)
+        )
+    w |> Array.sum
+
+// Clough-Tocher interpolation on vertex 2D field
+let evaluateCloughTocherVertex (grid: ExtendedGrid) (field: Field) (p: Pos) =
+    match grid.tryGetElement p with
+        | None -> None
+        | Some e ->
+            let i0, i1, i2 = getCell grid e
+            let idx = [| i0; i1; i2 |]
+
+            // Split field
+            let U = field |> Array.map fst
+            let V = field |> Array.map snd
+
+            // Position of vertices
+            let p_n = idx |> Array.map (getVertex grid)
+
+            // Field values at vertices
+            let u_n = idx |> Array.map (Array.get U)
+            let v_n = idx |> Array.map (Array.get V)
+
+            // Gradient values at vertices
+            let thrs = 1e-6f
+            let du_n = idx |> Array.map (calcVertexGradient grid U thrs)
+            let dv_n = idx |> Array.map (calcVertexGradient grid V thrs)
+
+            let u1 = interpolateTriangleCT p_n u_n du_n p
+            let v1 = interpolateTriangleCT p_n v_n dv_n p
+            Some (u1, v1)
+
+// Clough-Tocher interpolation on centroid 2D field
+let evaluateCloughTocherCentroid (grid: ExtendedGrid) (field_c: Field) (p: Pos) =
+    let field_v = centroidToVertexValues grid field_c
+    evaluateCloughTocherVertex grid field_v p
+

src/Gradient.fs

@@ -0,0 +1,108 @@
+module Oceanbox.FvcomKit.Gradient
+
+open Grid
+
+let private getCell (grid: ExtendedGrid) (idx: ElemIdx) =
+    (grid :> IGrid).getCell idx
+
+let private getVertex (grid: ExtendedGrid) (idx: NodeIdx) =
+    (grid :> IGrid).getVertex idx
+
+// Compute contributions to Q and s from a single neighboring point, given current estimate of gradient df
+let private calcContributionQandS (df: single*single) (p0: Pos) (f0: single) (p1: Pos) (f1: single) =
+    let x0, y0 = p0
+    let x1, y1 = p1
+
+    let ex = x1 - x0
+    let ey = y1 - y0
+
+    let L = sqrt (ex*ex + ey*ey)
+    let L3 = L*L*L
+
+    let dfx, dfy = df
+    let df2 = -ex*dfx - ey*dfy
+
+    let q1 = 4f * ex * ex / L3
+    let q2 = 4f * ex * ey / L3
+    let q3 = 4f * ey * ey / L3
+
+    let s1 = (6f*(f0 - f1) - 2f*df2) * ex / L3
+    let s2 = (6f*(f0 - f1) - 2f*df2) * ey / L3
+
+    let Q = [| q1; q2; q2; q3 |]
+    let s = [| s1; s2 |]
+    Q, s
+
+// Set up linear system to solve (Qx = s), accumulated from N neighboring points
+let private setupQandS (p0: Pos) (f0: single) (p_n: Pos[]) (f_n: single[]) (df: single*single) =
+    let calcQS_n = calcContributionQandS df p0 f0
+
+    let (<+>) a b = Array.map2 (+) a b
+    let rec accumulateQandS Q s idx =
+        match idx with
+        | [] -> Q, s
+        | x::xs ->
+            let Q', s' = calcQS_n p_n[x] f_n[x]
+            accumulateQandS (Q <+> Q') (s <+> s') xs
+
+    let Q0 = [| 0f; 0f; 0f; 0f |]
+    let s0 = [| 0f; 0f |]
+    let idx = [ 0..(p_n.Length-1) ]
+    accumulateQandS Q0 s0 idx
+
+// Estimates gradient in point P_0 based on neighboring function values
+// p0: Point of interest
+// f0: Function value in POI
+// p_n: Neighboring points
+// f_n: Function values in neighboring points
+// thrs: Convergence threshold in iterative procedure
+//
+// Ref: Based on SciPy implementation which uses the following
+// G. Nielson "A method for interpolating scattered data based upon a minimum norm network" (1983)
+// R. J. Renka and A. K. Cline "A Triangle-based C1 interpolation method" (1984)
+let calcGradientFromNeighbors (p0: Pos) (f0: single) (p_n: Pos[]) (f_n: single[]) (thrs: single) : single*single =
+    let setupQS = setupQandS p0 f0 p_n f_n
+
+    let rec convergeGradient (error: single) (df: single*single) =
+        if error < thrs then
+            df
+        else
+            let Q, s = setupQS df
+            let detQ = Q[0]*Q[3] - Q[1]*Q[2]
+
+            if (abs detQ) < thrs*thrs then
+                df
+            else
+                let r0 = ( Q[3]*s[0] - Q[1]*s[1]) / detQ
+                let r1 = (-Q[2]*s[0] + Q[0]*s[1]) / detQ
+                let dfx, dfy = df
+                let change = max (abs (dfx + r0)) (abs (dfy + r1))
+                convergeGradient change (-r0, -r1)
+
+    let err0 = 1f
+    let grad0 = 0f, 0f
+    convergeGradient err0 grad0
+
+// Given vertex 1D field values, compute gradient in single vertex
+let calcVertexGradient (grid: ExtendedGrid) (field: single[]) (thrs: single) (idx: NodeIdx) : single*single =
+    let neighbors =
+        grid.getNodesSurroundingNode idx
+        |> Array.filter (fun i -> i <> idx)
+
+    // Get data from point of interest
+    let p0 = getVertex grid idx
+    let f0 = field[idx]
+
+    // Get data from neighbors of POI
+    let p_n = neighbors |> Array.map (getVertex grid)
+    let f_n = neighbors |> Array.map (Array.get field)
+
+    calcGradientFromNeighbors p0 f0 p_n f_n thrs
+
+// Given vertex 1D field values, compute gradients on entire grid
+let calcVertexGradients (grid: ExtendedGrid) (field: single[]) (thrs: single) : Field =
+    let N = ((grid :> IGrid).getVertices ()).Length
+    [| 0..(N-1) |] |> Array.map (calcVertexGradient grid field thrs)
+
+
+

src/Oceanbox.FvcomKit.fsproj

@@ -22,6 +22,7 @@
     <Compile Include="NorKyst.fs"/>
     <Compile Include="NorShelf.fs"/>
     <Compile Include="Smoothing.fs"/>
+    <Compile Include="Gradient.fs"/>
     <Compile Include="Evaluate.fs"/>
   </ItemGroup>
   <ItemGroup>