From 329af6602f2732672f6b32ca82cebd36f5416edc Mon Sep 17 00:00:00 2001
From: "Val Erastov (xibyte)" <xibyte@gmail.com>
Date: Wed, 11 Mar 2020 16:12:46 -0700
Subject: [PATCH] bezier curve tangent constraint improved

---
 web/app/brep/geom/impl/nurbs-ext.js      | 15 +++++--
 web/app/sketcher/constr/ANConstraints.js | 57 +++++++++++++++++++++---
 web/app/sketcher/constr/AlgNumSystem.js  |  9 +++-
 3 files changed, 70 insertions(+), 11 deletions(-)

diff --git a/web/app/brep/geom/impl/nurbs-ext.js b/web/app/brep/geom/impl/nurbs-ext.js
index eae1f1db..4ed9ac41 100644
--- a/web/app/brep/geom/impl/nurbs-ext.js
+++ b/web/app/brep/geom/impl/nurbs-ext.js
@@ -4,11 +4,17 @@ import {eqEps, TOLERANCE, TOLERANCE_01, TOLERANCE_SQ} from '../tolerance';
 import {fmin_bfgs} from "../../../math/optim";
 
 export function curveStep(curve, u, tessTol, scale) {
-  tessTol = tessTol || 1;
-  scale = scale || 1;
+
   let ders = verb.eval.Eval.rationalCurveDerivatives( curve, u, 2 );
-  let r1 = ders[1];
-  let r2 = ders[2];
+  let d1 = ders[1];
+  let d2 = ders[2];
+
+  return genericCurveStep(d1, d2, tessTol, scale);
+}
+
+export function genericCurveStep(d1, d2, tessTol = 1, scale = 1) {
+  let r1 = d1;
+  let r2 = d2;
 
   let r1lsq = vec.lengthSq(r1);
   let r1l = Math.sqrt(r1lsq);
@@ -20,6 +26,7 @@ export function curveStep(curve, u, tessTol, scale) {
   return step;
 }
 
+
 export function curveDomain(curve) {
   return [curve.knots[0], curve.knots[curve.knots.length - 1]];
 }
diff --git a/web/app/sketcher/constr/ANConstraints.js b/web/app/sketcher/constr/ANConstraints.js
index 1cb2f663..b13a7ebd 100644
--- a/web/app/sketcher/constr/ANConstraints.js
+++ b/web/app/sketcher/constr/ANConstraints.js
@@ -6,8 +6,10 @@ import {COS_FN, Polynomial, POW_1_FN, POW_2_FN, POW_3_FN, SIN_FN} from "./polyno
 import {Types} from "../io";
 
 import Vector from "math/vector";
-import {cubicBezierDer2, cubicBezierPoint} from "../../brep/geom/curves/bezierCubic";
+import {cubicBezierDer1, cubicBezierDer2, cubicBezierPoint} from "../../brep/geom/curves/bezierCubic";
 import {greaterThanConstraint, lessThanConstraint} from "./barriers";
+import {genericCurveStep} from "../../brep/geom/impl/nurbs-ext";
+import {_normalize} from "../../math/vec";
 
 export const ConstraintDefinitions = {
 
@@ -151,16 +153,53 @@ export const ConstraintDefinitions = {
     id: 'TangentLineBezier',
     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()];
+
+      let t = 0;
+      let bestT = 0.5;
+      let best = -1;
+      while (t <= 1) {
+
+        const d1 = cubicBezierDer1(p0, p1, p2, p3, t);
+        const d2 = cubicBezierDer2(p0, p1, p2, p3, t);
+
+        t = Math.min(1, t + (genericCurveStep(d1, d2)||0.1));
+        _normalize(d2);
+
+        const measure = Math.abs(d1[0] * Math.cos(ang) + d1[1] * Math.sin(ang));
+        if (measure > best) {
+          best = measure;
+          bestT = t;
+        }
+
+        if (t === 1) {
+          break;
+        }
+      }
+
+      _t.set(bestT);
+      const [_px, _py] = cubicBezierPoint(p0, p1, p2, p3, bestT);
+      px.set(_px);
+      px.set(_py);
+    },
+
     defineParamsScope: ([segment, curve], callback) => {
       const t0 = new Param(0.5, 't');
       t0.constraints = [greaterThanConstraint(0), lessThanConstraint(1)];
-      const [p0, p1, p2, p3] = [curve.p0, curve.p1, curve.p2, curve.p3].map(p => p.toVectorArray());
-      const [x0, y0] = cubicBezierPoint(p0, p1, p2, p3, t0.get());
-      const [nx0, ny0] = cubicBezierDer2(p0, p1, p2, p3, t0.get());
+
+      const px = new Param(0, 'X');
+      const py = new Param(0, 'Y');
+
       curve.visitParams(callback);
       callback(t0);
-      callback(new Param(x0, 'X'));
-      callback(new Param(y0, 'Y'));
+      callback(px);
+      callback(py);
       callback(new Param(nx0, 'X'));
       callback(new Param(ny0, 'Y'));
       callback(segment.params.ang);
@@ -945,5 +984,11 @@ export class AlgNumConstraint {
     }
   }
 
+  initialGuess() {
+    if (this.schema.initialGuess) {
+      this.schema.initialGuess(this.params);
+    }
+  }
+
 }
 
diff --git a/web/app/sketcher/constr/AlgNumSystem.js b/web/app/sketcher/constr/AlgNumSystem.js
index 9b08c92d..daa8462a 100644
--- a/web/app/sketcher/constr/AlgNumSystem.js
+++ b/web/app/sketcher/constr/AlgNumSystem.js
@@ -458,9 +458,13 @@ class Isolation {
     this.system = system;
     this.polynomials = polynomials;
     this.beingSolvedParams = new Set();
+    this.beingSolvedConstraints = new Set();
     const residuals = [];
 
-    this.polynomials.forEach(p => residuals.push(p.asResidual()));
+    this.polynomials.forEach(p => {
+      residuals.push(p.asResidual());
+      this.beingSolvedConstraints.add(system.polyToConstr.get(p));
+    });
 
     for (let residual of residuals) {
       residual.params.forEach(solverParam => {
@@ -495,6 +499,9 @@ class Isolation {
   }
 
   solve(rough) {
+
+    this.beingSolvedConstraints.forEach(c => c.initialGuess())
+
     this.beingSolvedParams.forEach(solverParam => {
       let val = solverParam.objectParam.get();