Introduce fall-off exponent

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

@@ -44,6 +44,9 @@ public static TSource MinBy<TSource, TKey>(this IEnumerable<TSource> source, Fun
 			}
 			return result;
 		}
+
+		public static IEnumerable<(T, T)> CyclicPairs<T>(this List<T> list) => list.Zip(list.Skip(1).Append(list.First()));
+
 	}
 
 }

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

@@ -2,6 +2,7 @@
 using System;
 using System.Collections.Generic;
 using System.Linq;
+using MathNet.Numerics.Distributions;
 
 namespace VHToolkit {
 	public static class MathTools {
@@ -34,11 +35,11 @@ public static Vector2 Gradient2(Func<Vector2, float> function, Vector2 x) {
 			) / (2 * eps);
 		}
 
-		public static Func<Vector3, float> RepulsivePotential3D(List<Collider> obstacles) =>
-			(x) => obstacles.Sum(o => 1 / Vector3.Distance(x, o.ClosestPoint(x)));
+		public static Func<Vector3, float> RepulsivePotential3D(List<Collider> obstacles, float exponent = 1f) =>
+			(x) => obstacles.Sum(o => 1 / Mathf.Pow(Vector3.Distance(x, o.ClosestPoint(x)), exponent));
 
-		public static Func<Vector3, float> RepulsivePotential3D(List<Vector3> obstacles) =>
-			(x) => obstacles.Sum(o => 1 / (x - o).sqrMagnitude);
+		public static Func<Vector3, float> RepulsivePotential3D(List<Vector3> obstacles, float exponent = 1f) =>
+			(x) => obstacles.Sum(o => 1 / Mathf.Pow(Vector3.Distance(x, o), exponent));
 
 
 		public static Vector2 Gradient2v2(Vector2 x, List<Collider2D> obstaclescolliders) {

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

@@ -3,50 +3,42 @@
 using UnityEngine;
 
 using VHToolkit;
-using VHToolkit.Redirection;
 
 public class PhysicalEnvironmentCalibration : MonoBehaviour {
 
-    [SerializeField] private float eps = 0.01f;
-    [SerializeField] private Transform user;
-    [SerializeField] private List<Vector3> bounds;
-
-    // Start is called before the first frame update
-    void Start() {
-
-    }
-
-    // Update is called once per frame
-    void Update() {
-        List<Vector3> obs = new();
-
-        var dir = (bounds.First() - bounds.Last()).normalized;
-        int n = (int)(Vector3.Distance(bounds.Last(), bounds.First()) / eps);
-        for (int j = 0; j < n; j++) {
-            obs.Add(bounds.Last() + (j + 0.5f) * eps * dir);
-        }
-        for (int i = 1; i < bounds.Count; i++) {
-            dir = (bounds[i] - bounds[i - 1]).normalized;
-            n = (int) (Vector3.Distance(bounds[i - 1], bounds[i]) / eps);
-            for (int j = 0; j < n; j++) {
-                obs.Add(bounds[i - 1] + (j + 0.5f) * eps * dir);
-            }
-        }
-        Debug.Log(obs.Count);
-        obs.ForEach(o => Debug.Log(o));
-
-        Debug.DrawRay(user.position, ComputeGradient(user, obs).normalized);
-    }
-
-    private void OnDrawGizmos() {
-        for (int i = 1; i < bounds.Count; i++) {
-            Gizmos.DrawLine(bounds[i - 1], bounds[i]);
-        }
-        Gizmos.DrawLine(bounds.Last(), bounds.First());
-    }
-
-    private Vector3 ComputeGradient(Transform user, List<Vector3> dividedObstacles) => Vector3.ProjectOnPlane(MathTools.Gradient3(
-                                        MathTools.RepulsivePotential3D(dividedObstacles),
-                                        MathTools.ProjectToHorizontalPlaneV3(user.position)
-                                    ), Vector3.up);
+	[SerializeField] private float eps = 0.01f;
+	[SerializeField] private Transform user;
+	[SerializeField] private List<Vector3> bounds;
+
+	// Start is called before the first frame update
+	void Start() {
+
+	}
+
+	// Update is called once per frame
+	void Update() {
+		var obs = bounds.CyclicPairs().SelectMany(
+				p => {
+					var dir = (p.Item1 - p.Item2).normalized;
+					var n = (int)(Vector3.Distance(p.Item1, p.Item2) / eps);
+					return Enumerable.Range(0, n).Select(j => p.Item1 + (j + 0.5f) * eps * dir);
+				}
+			).ToList();
+
+		Debug.Log(obs.Count);
+		obs.ForEach(o => Debug.Log(o));
+
+		Debug.DrawRay(user.position, ComputeGradient(user, obs).normalized);
+	}
+
+	private void OnDrawGizmos() {
+		foreach (var (first, second) in bounds.CyclicPairs()) {
+			Gizmos.DrawLine(first, second);
+		}
+	}
+
+	private Vector3 ComputeGradient(Transform user, List<Vector3> dividedObstacles) => Vector3.ProjectOnPlane(MathTools.Gradient3(
+										MathTools.RepulsivePotential3D(dividedObstacles),
+										MathTools.ProjectToHorizontalPlaneV3(user.position)
+									), Vector3.up);
 }