Merge branch 'main' of https://github.com/BenoitGeslain/Visuo-haptic-…

…Illusions-Toolkit

Visuo-haptic Toolkit/Assets/Visuo-Haptic Toolkit/Scripts/Utils/InverseWeightedDistance.cs

@@ -0,0 +1,24 @@
+using System.Linq;
+using UnityEngine;
+using VHToolkit;
+
+namespace VHToolkit
+{
+    public static class InverseWeightedDistance
+    {
+   public static Vector3 Interpolate(Vector3[] x, Vector3[] target, float p, Vector3 position) {
+            var weights = new float[x.Length];
+            int idx = 0;
+            foreach (var (origin, targ) in x.Zip(target)) {
+                var d = Vector3.Distance(origin, position);
+                if (Mathf.Approximately(d, 0)) {
+                    return targ;
+                }
+                else {
+                    weights[idx++] =  Mathf.Pow(d, -p);
+                }
+            }
+            return target.Zip(weights, (t, w) => w * t).Aggregate(Vector3.zero, (a, b) => a + b) / weights.Sum();
+        }
+    }
+}

Visuo-haptic Toolkit/Assets/Visuo-Haptic Toolkit/Scripts/Utils/ThinPlateSpline.cs

@@ -103,5 +103,21 @@ public static Func<Vector3, Vector3> SabooSmoothedDisplacementField(Vector3[] x,
 
             return point => new(components[0](point), components[1](point), components[2](point));
         }
+
+        public static Func<Vector3, float> Solve(Vector3[] x, float[] y) => SolveSmoothed(x, y, 0f);
+
+        public static Func<Vector3, float> SolveSmoothed(Vector3[] x, float[] y, float lambda)
+        {
+            static float GreenFunction3D(Vector3 a, Vector3 b) => Vector3.Distance(a, b);
+            var Mat = Matrix<float>.Build;
+
+            var yVec = Vector<float>.Build.DenseOfArray(y);
+            var N = Mat.DenseOfRowArrays(x.Select(xx => new[] { 1, xx.x, xx.y, xx.z }));
+            var Minv = (Mat.Dense(x.Length, x.Length, (i, j) => GreenFunction3D(x[i], x[j])) + lambda * Mat.DenseIdentity(x.Length)).Inverse();
+            var b = (N.Transpose() * Minv * N).Inverse() * N.Transpose() * Minv * yVec;
+            var w = Minv * (yVec - N * b);
+
+            return (point) => x.Select(xx => GreenFunction3D(xx, point)).Zip(w, (ww, g) => ww * g).Sum() + b[0] + b[1] * point.x + b[2] * point.y + b[3] * point.z;
+        }
     }
 }