From e1deaa670e2eec783a21a7d081f82e91ff294c56 Mon Sep 17 00:00:00 2001
From: "Val Erastov (xibyte)" <xibyte@gmail.com>
Date: Wed, 11 Mar 2020 17:37:16 -0700
Subject: [PATCH] improving penalty functions

---
 web/app/sketcher/constr/ANConstraints.js | 41 ++++++++++++++++--------
 web/app/sketcher/constr/AlgNumSystem.js  | 38 +++++++++++++---------
 2 files changed, 50 insertions(+), 29 deletions(-)

diff --git a/web/app/sketcher/constr/ANConstraints.js b/web/app/sketcher/constr/ANConstraints.js
index b13a7ebd..8e8579bc 100644
--- a/web/app/sketcher/constr/ANConstraints.js
+++ b/web/app/sketcher/constr/ANConstraints.js
@@ -134,6 +134,16 @@ export const ConstraintDefinitions = {
     id: 'PointOnBezier',
     name: 'Point On Bezier Curve',
 
+    initialGuess: ([p0x,p0y, p3x,p3y, p1x,p1y, p2x,p2y, t, px, py]) => {
+      const _t = t.get();
+      if (_t < 0.001) {
+        t.set(0);
+      }
+      if (_t > 0.999) {
+        t.set(1);
+      }
+    },
+
     defineParamsScope: ([pt, curve], callback) => {
       const t = new Param(0.5, 't');
       t.constraints = [greaterThanConstraint(0), lessThanConstraint(1)];
@@ -154,12 +164,11 @@ export const ConstraintDefinitions = {
     name: 'Line & Bezier Tangency',
 
     initialGuess([p0x,p0y, p3x,p3y, p1x,p1y, p2x,p2y, _t, px,py, nx,ny, _ang, ax,ay]) {
-
       const ang = _ang.get();
-      const p0 = [p0x.get(), p0y.get()];
-      const p1 = [p1x.get(),p1y.get()];
-      const p2 = [p2x.get(),p2y.get()];
-      const p3 = [p3x.get(),p3y.get()];
+      const p0 = [p0x.get(), p0y.get(), 0];
+      const p1 = [p1x.get(),p1y.get(), 0];
+      const p2 = [p2x.get(),p2y.get(), 0];
+      const p3 = [p3x.get(),p3y.get(), 0];
 
       let t = 0;
       let bestT = 0.5;
@@ -183,25 +192,31 @@ export const ConstraintDefinitions = {
         }
       }
 
+      //otherwise it gets stuck in the straight areas
+      if (Math.abs(bestT - _t.get()) < 0.2) {
+        return;
+      }
+
       _t.set(bestT);
       const [_px, _py] = cubicBezierPoint(p0, p1, p2, p3, bestT);
+      const [_nx, _ny] = cubicBezierDer1(p0, p1, p2, p3, bestT);
       px.set(_px);
-      px.set(_py);
+      py.set(_py);
+
+      nx.set(_nx);
+      ny.set(_ny);
     },
 
     defineParamsScope: ([segment, curve], callback) => {
       const t0 = new Param(0.5, 't');
       t0.constraints = [greaterThanConstraint(0), lessThanConstraint(1)];
 
-      const px = new Param(0, 'X');
-      const py = new Param(0, 'Y');
-
       curve.visitParams(callback);
       callback(t0);
-      callback(px);
-      callback(py);
-      callback(new Param(nx0, 'X'));
-      callback(new Param(ny0, 'Y'));
+      callback(new Param(0, 'X'));
+      callback(new Param(0, 'Y'));
+      callback(new Param(0, 'X'));
+      callback(new Param(0, 'Y'));
       callback(segment.params.ang);
       segment.a.visitParams(callback);
     },
diff --git a/web/app/sketcher/constr/AlgNumSystem.js b/web/app/sketcher/constr/AlgNumSystem.js
index daa8462a..950722e3 100644
--- a/web/app/sketcher/constr/AlgNumSystem.js
+++ b/web/app/sketcher/constr/AlgNumSystem.js
@@ -475,21 +475,17 @@ class Isolation {
       });
     }
     this.dof = this.beingSolvedParams.size - polynomials.length;
-
+    const penaltyFunction = new PolynomialResidual();
     this.beingSolvedParams.forEach(sp => {
       const param = sp.objectParam;
       if (param.constraints) {
-        param.constraints.forEach(pc => {
-          let penaltyFunction = new PolynomialResidual();
-          penaltyFunction.add(sp, pc);
-          residuals.push(penaltyFunction);
-        })
+        penaltyFunction.add(sp, param.constraints);
       }
     });
 
-    // if (penaltyFunction.params.length) {
-    //   residuals.push(penaltyFunction);
-    // }
+    if (penaltyFunction.params.length) {
+      residuals.push(penaltyFunction);
+    }
 
     this.numericalSolver = prepare(residuals);
   }
@@ -500,7 +496,7 @@ class Isolation {
 
   solve(rough) {
 
-    this.beingSolvedConstraints.forEach(c => c.initialGuess())
+    this.beingSolvedConstraints.forEach(c => c.initialGuess());
 
     this.beingSolvedParams.forEach(solverParam => {
       let val = solverParam.objectParam.get();
@@ -527,24 +523,34 @@ class PolynomialResidual {
   params = [];
   functions = [];
 
-  add(param, fn) {
+  add(param, fns) {
     this.params.push(param);
-    this.functions.push(fn);
-
+    this.functions.push(fns);
   }
 
   error() {
     let err = 0;
     for (let i = 0 ; i < this.params.length; ++i) {
-      err += this.functions[i].d0(this.params[i].get());
+      const val = this.params[i].get();
+      const paramFunctions = this.functions[i];
+      for (let fn of paramFunctions) {
+        const d0 = fn.d0(val);
+        err += d0;// * d0;
+      }
     }
 
-    return err;
+    return err;//0.5 * err;
   }
 
   gradient(out) {
     for (let i = 0 ; i < this.params.length; ++i) {
-      out[i] = this.functions[i].d1(this.params[i].get());
+      const val = this.params[i].get();
+      const paramFunctions = this.functions[i];
+      for (let fn of paramFunctions) {
+        // const d0 = fn.d0(val);
+        const d1 = fn.d1(val);
+        out[i] += d1; //d0 * d1; //degenerated chain rule
+      }
     }
   }