From b2a0790e1bdbd29e48117ea68b9e5e88c102484a Mon Sep 17 00:00:00 2001
From: nejc <kandelabr@gmail.com>
Date: Fri, 2 Feb 2024 15:02:51 +0100
Subject: [PATCH] Bugfix: change clamps to follow domain scale

Radial and Line clamps work with angle or parameter from 0 to 1
but clamps are moved according to junction.delta which works with
cell sizes.

Clamps are now changed so that angle is translated to actual movement
(bigger radii-bigger junction.delta and v.v.) and lines' parameter
is actual displacement instead of interval 0...1.
---
 src/classy_blocks/modify/clamps/curve.py | 29 ++++++++++++++++++++----
 src/classy_blocks/modify/optimizer.py    |  5 ++--
 src/classy_blocks/util/functions.py      | 10 ++++++++
 3 files changed, 36 insertions(+), 8 deletions(-)

diff --git a/src/classy_blocks/modify/clamps/curve.py b/src/classy_blocks/modify/clamps/curve.py
index 056e1065..d32a8800 100644
--- a/src/classy_blocks/modify/clamps/curve.py
+++ b/src/classy_blocks/modify/clamps/curve.py
@@ -1,7 +1,8 @@
 import copy
 from typing import List, Optional, Tuple
 
-from classy_blocks.construct.curves.analytic import LineCurve
+import numpy as np
+
 from classy_blocks.construct.curves.curve import CurveBase
 from classy_blocks.items.vertex import Vertex
 from classy_blocks.modify.clamps.clamp import ClampBase
@@ -43,10 +44,20 @@ class LineClamp(ClampBase):
     Parameter 't' goes from 0 at point_1 to 1 at point_2
     (and beyond if different bounds are specified)."""
 
-    def __init__(self, vertex: Vertex, point_1: PointType, point_2: PointType, bounds: Tuple[float, float] = (0, 1)):
-        curve = LineCurve(point_1, point_2, bounds)
+    def __init__(
+        self, vertex: Vertex, point_1: PointType, point_2: PointType, bounds: Optional[Tuple[float, float]] = None
+    ):
+        # curve = LineCurve(point_1, point_2, bounds)
+        point_1 = np.array(point_1)
+        point_2 = np.array(point_2)
+
+        def function(t):
+            return point_1 + t[0] * f.unit_vector(point_2 - point_1)
+
+        if bounds is None:
+            bounds = (0, f.norm(point_2 - point_1))
 
-        super().__init__(vertex, lambda t: curve.get_point(t[0]), [list(bounds)])
+        super().__init__(vertex, function, [list(bounds)])
 
     @property
     def initial_guess(self) -> List[float]:
@@ -71,7 +82,15 @@ def __init__(self, vertex: Vertex, center: PointType, normal: VectorType, bounds
         else:
             clamp_bounds = None
 
-        super().__init__(vertex, lambda params: f.rotate(initial_point, params[0], normal, center), clamp_bounds)
+        # Clamps that move points linearly have a clear connection
+        # <delta_params> - <delta_position>.
+        # With rotation, this strongly depends on the radius of the point.
+        # To conquer that, divide params by radius
+        radius = f.norm(np.cross(vertex.position - center, normal)) / f.norm(normal)
+
+        super().__init__(
+            vertex, lambda params: f.rotate(initial_point, params[0] / radius, normal, center), clamp_bounds
+        )
 
     @property
     def initial_guess(self):
diff --git a/src/classy_blocks/modify/optimizer.py b/src/classy_blocks/modify/optimizer.py
index 4efb5b57..5f972769 100644
--- a/src/classy_blocks/modify/optimizer.py
+++ b/src/classy_blocks/modify/optimizer.py
@@ -159,15 +159,14 @@ def fquality(params):
 
             if clamp.is_linked:
                 return self.grid.quality
-            else:
-                return junction.quality
+            return junction.quality
 
         scipy.optimize.minimize(
             fquality,
             clamp.params,
             bounds=clamp.bounds,
             method="L-BFGS-B",
-            options={"maxiter": 20, "ftol": 1, "eps": junction.delta / 10},
+            options={"maxiter": 20, "ftol": 1, "eps": junction.delta / 10 / (iteration.index + 1)},
         )
         # alas, works well with this kind of problem but does not support bounds
         # method="COBYLA",
diff --git a/src/classy_blocks/util/functions.py b/src/classy_blocks/util/functions.py
index a8ce09f0..8e35547e 100644
--- a/src/classy_blocks/util/functions.py
+++ b/src/classy_blocks/util/functions.py
@@ -1,4 +1,5 @@
 """Mathematical functions for general everyday household use"""
+
 from typing import Literal, Optional, Union
 
 import numpy as np
@@ -262,3 +263,12 @@ def is_point_on_plane(origin: PointType, normal: VectorType, point: PointType) -
         return True
 
     return abs(np.dot(unit_vector(point - origin), normal)) < constants.TOL
+
+
+def point_to_line_distance(origin: PointType, direction: VectorType, point: PointType) -> float:
+    """Calculates distance from a line, defined by a point and normal, and an arbitrary point in 3D space"""
+    origin = np.asarray(origin)
+    point = np.asarray(point)
+    direction = np.asarray(direction)
+
+    return norm(np.cross(point - origin, direction)) / norm(direction)