penalty function to restrict circle radius

web/app/math/optim.js

@@ -315,10 +315,10 @@ var dog_leg = function (subsys, rough) {
   function lsolve(A, b) {
     if (csize < xsize) {
       var At = n.transpose(A);
-      var sol = n.solve(n.dot(A, At), b, true);
+      var sol = lu_solve(n.dot(A, At), b, true);
       return n.dot(At, sol);
     } else {
-      return n.solve(A, b, false);
+      return lu_solve(A, b, false);
     }
   }
 
@@ -351,7 +351,7 @@ var dog_leg = function (subsys, rough) {
       returnCode = INVALID_STATE;
     }
 
-    if (returnCode != 0) {
+    if (returnCode !== 0) {
       break;
     }
 
@@ -462,6 +462,143 @@ var dog_leg = function (subsys, rough) {
   return _result(iter, err, returnCode);
 };
 
+function gaussElimination(A) {
+  let h = 0;
+  let k = 0;
+  const m = A.length;
+  const n = A[0].length;
+
+  while (h < m && k  < n) {
+
+    let i_max = h;
+
+    for (let i = h + 1; i < m; i++) {
+      if (Math.abs(A[i][k]) > Math.abs(A[i_max][k]) ) {
+        i_max = i;
+      }
+    }
+
+    if (A[i_max][k] === 0) {
+      /* No pivot in this column, pass to next column */
+      k ++;
+    } else {
+      let t = A[h];
+      A[h] = A[i_max];
+      A[i_max] = t;
+
+
+      for (let i = h + 1; i < m; i++) {
+        let f = A[i][k] / A[h][k]; //it cant be 0 here see condition up
+        A[i][k] = 0;
+        for (let j = k + 1; j < n; j++) {
+          A[i][j] = A[i][j] - A[h][j] * f;
+        }
+      }
+      h ++;
+      k ++;
+    }
+  }
+
+}
+
+function LU(A, fast) {
+  fast = fast || false;
+
+  var abs = Math.abs;
+  var i, j, k, absAjk, Akk, Ak, Pk, Ai;
+  var max;
+  var n = A.length, n1 = n-1;
+  var P = new Array(n);
+  if(!fast) A = numeric.clone(A);
+
+  for (k = 0; k < n; ++k) {
+    Pk = k;
+    Ak = A[k];
+    max = abs(Ak[k]);
+    for (j = k + 1; j < n; ++j) {
+      absAjk = abs(A[j][k]);
+      if (max < absAjk) {
+        max = absAjk;
+        Pk = j;
+      }
+    }
+    P[k] = Pk;
+
+    if (Pk != k) {
+      A[k] = A[Pk];
+      A[Pk] = Ak;
+      Ak = A[k];
+    }
+
+    Akk = Ak[k];
+
+    if (Akk === 0) {
+      Akk = 0.0000001;
+    }
+
+    for (i = k + 1; i < n; ++i) {
+      A[i][k] /= Akk;
+    }
+
+    for (i = k + 1; i < n; ++i) {
+      Ai = A[i];
+      for (j = k + 1; j < n1; ++j) {
+        Ai[j] -= Ai[k] * Ak[j];
+        ++j;
+        Ai[j] -= Ai[k] * Ak[j];
+      }
+      if(j===n1) Ai[j] -= Ai[k] * Ak[j];
+    }
+  }
+
+  return {
+    LU: A,
+    P:  P
+  };
+}
+
+function LUsolve(LUP, b) {
+  var i, j;
+  var LU = LUP.LU;
+  var n   = LU.length;
+  var x = numeric.clone(b);
+  var P   = LUP.P;
+  var Pi, LUi, LUii, tmp;
+
+  for (i=n-1;i!==-1;--i) x[i] = b[i];
+  for (i = 0; i < n; ++i) {
+    Pi = P[i];
+    if (P[i] !== i) {
+      tmp = x[i];
+      x[i] = x[Pi];
+      x[Pi] = tmp;
+    }
+
+    LUi = LU[i];
+    for (j = 0; j < i; ++j) {
+      x[i] -= x[j] * LUi[j];
+    }
+  }
+
+  for (i = n - 1; i >= 0; --i) {
+    LUi = LU[i];
+    for (j = i + 1; j < n; ++j) {
+      x[i] -= x[j] * LUi[j];
+    }
+    if (LUi[i] !== 0) { // We want it because it 99% of the time happens when penalty function returns ZERO gradient, so we just ignore zero-rows
+      x[i] /= LUi[i];
+    }
+  }
+
+  return x;
+}
+
+function lu_solve(A, b, fast) {
+  const lu = LU(A,fast);
+  return LUsolve(lu, b);
+}
+
+
 var cg = function(A, x, b, tol, maxIt) {
 
   var _ = numeric;

web/app/sketcher/actions/constraintActions.js

@@ -39,7 +39,7 @@ export default [
       const constraint = new AlgNumConstraint(ConstraintDefinitions.TangentLC, [line, circle]);
       constraint.initConstants();
       const pm = viewer.parametricManager;
-      pm.addConstraint(constraint);
+      pm.algnNumSystem.addConstraint(constraint);
       pm.refresh();
     }
 

web/app/sketcher/constr/AlgNumSystem.js

@@ -5,6 +5,7 @@ import {GCCircle, GCPoint} from "./constractibles";
 import {eqEps, eqTol} from "../../brep/geom/tolerance";
 import {Polynomial, POW_1_FN} from "./polynomial";
 import {compositeFn, NOOP} from "../../../../modules/gems/func";
+import {sq} from "../../math/math";
 
 export class AlgNumSubSystem {
 
@@ -19,7 +20,7 @@ export class AlgNumSubSystem {
 
   generatedParams = new Set();
 
-  beingSolvedParams = new Set();
+  paramToIsolation = new Map();
 
   eliminatedParams = new Map();
 
@@ -37,8 +38,14 @@ export class AlgNumSubSystem {
 
   dof = 0;
 
-  SubSystem(generator) {
+  constructor(generator = NOOP) {
     this.generator = generator;
+
+    this.solveStatus = {
+      error: 0,
+      success: true
+    }
+
   }
 
 
@@ -122,8 +129,8 @@ export class AlgNumSubSystem {
     this.polynomials = [];
     this.substitutedParams = [];
     this.eliminatedParams.clear();
-    this.beingSolvedParams.clear();
     this.polyToConstr.clear();
+    this.paramToIsolation.clear();
   }
 
   evaluatePolynomials() {
@@ -235,71 +242,78 @@ export class AlgNumSubSystem {
 
     this.evaluatePolynomials();
 
-    // this.polynomialClusters = splitByIsolatedClusters(this.polynomials);
+    this.polynomialIsolations = this.splitByIsolatedClusters(this.polynomials);
+    this.polynomialIsolations.forEach(iso => {
+      iso.beingSolvedParams.forEach(solverParam => this.paramToIsolation.set(solverParam.objectParam, iso))
+    });
 
     console.log('solving system:');
-    this.polynomials.forEach(p => console.log(p.toString()));
+    this.polynomialIsolations.forEach((iso, i) => {
+      console.log(i + ". ISOLATION, DOF: " + iso.dof);
+      iso.polynomials.forEach(p => console.log(p.toString()));
+    });
+
     console.log('with respect to:');
     this.substitutedParams.forEach(([x, expr]) => console.log('X' + x.id  + ' = ' + expr.toString()));
-
-    const residuals = [];
-
-    this.polynomials.forEach(p => residuals.push(p.asResidual()));
-
-    for (let residual of residuals) {
-      residual.params.forEach(solverParam => {
-        if (!this.beingSolvedParams.has(solverParam)) {
-          solverParam.reset(solverParam.objectParam.get());
-          this.beingSolvedParams.add(solverParam);
-        }
-      });
-    }
-    this.numericalSolver = prepare(residuals);
   }
 
   splitByIsolatedClusters(polynomials) {
 
-    const graph = [polynomials.length];
-    const params = [];
-    const paramMap = new Map();
-    polynomials.forEach((pl, i) => pl.visitParams(p => {
-      let pIndex = paramMap.get(p);
-      if (pIndex === undefined) {
-        pIndex = paramMap.size;
-        paramMap.set(p, pIndex);
-        params.push(p);
+
+    const graph = new Map();
+
+    function link(a, b) {
+      let list = graph.get(a);
+      if (!list) {
+        list = [];
+        graph.set(a, list);
       }
-      graph[i][pIndex] = 1;
-    }));
+      list.push(b);
+    }
 
     const visited = new Set();
+
+    polynomials.forEach(pl => {
+      visited.clear();
+      pl.visitParams(p => {
+        if (visited.has(p)) {
+          return;
+        }
+        visited.add(p);
+        link(p, pl);
+        link(pl, p);
+      })
+    });
+
+    visited.clear();
+
     const clusters = [];
 
-    for (let param of params) {
-      if (visited.has(param)) {
+    for (let initPl of polynomials) {
+      if (visited.has(initPl)) {
         continue
       }
-      const stack = [param];
+      const stack = [initPl];
       const isolation = [];
       while (stack.length) {
-        const p = stack.pop();
-        if (visited.has(p)) {
+        const pl = stack.pop();
+        if (visited.has(pl)) {
           continue;
         }
-        isolation.push(p);
-        const index = paramMap.get(p);
-        for (let i = 0; i < graph.length; ++i) {
-          if (graph[i][index] === 1) {
-            for (let j = 0; j < params.length; j ++) {
-              if (j !== index) {
-                stack.push(params[j]);
-              }
+        isolation.push(pl);
+        visited.add(pl);
+        const params = graph.get(pl);
+        for (let p of params) {
+          let linkedPolynomials = graph.get(p);
+          for (let linkedPolynomial of linkedPolynomials) {
+            if (linkedPolynomial !== pl) {
+              stack.push(linkedPolynomial);
             }
           }
         }
       }
       if (isolation.length) {
-        clusters.push(new Cluster(isolation));
+        clusters.push(new Isolation(isolation));
       }
     }
 
@@ -318,23 +332,27 @@ export class AlgNumSubSystem {
   solve(rough) {
     this.generator();
 
-    this.beingSolvedParams.forEach(solverParam => {
-      solverParam.set(solverParam.objectParam.get());
+    this.polynomialIsolations.forEach(iso => {
+      iso.solve(rough);
     });
 
-    const solveStatus = this.numericalSolver.solveSystem(rough);
     if (!rough) {
-      this.solveStatus = solveStatus;
+
+      this.solveStatus.error = 0;
+      this.solveStatus.success = true;
+
+      this.polynomialIsolations.forEach(iso => {
+        this.solveStatus.error = Math.max(this.solveStatus.error, iso.solveStatus.error);
+        this.solveStatus.success = this.solveStatus.success && iso.solveStatus.success;
+      });
+
+      console.log('numerical result: ' + this.solveStatus.success);
     }
-    console.log('numerical result: ' + solveStatus.success);
 
     for (let [p, val] of this.eliminatedParams) {
       p.set(val);
     }
 
-    this.beingSolvedParams.forEach(solverParam => {
-      solverParam.objectParam.set(solverParam.get());
-    });
     for (let i = this.substitutedParams.length - 1; i >= 0; i--) {
       let [param, expression] = this.substitutedParams[i];
       param.set(expression.value());
@@ -344,6 +362,8 @@ export class AlgNumSubSystem {
   updateFullyConstrainedObjects() {
 
 
+    const substitutedParamsLookup =  new Set();
+    this.substitutedParams.forEach(([p]) => substitutedParamsLookup.add(p));
 
     this.validConstraints(c => {
 
@@ -353,7 +373,10 @@ export class AlgNumSubSystem {
 
         obj.visitParams(p => {
 
-          if (!this.eliminatedParams.has(p)) {
+          const eliminated = this.eliminatedParams.has(p);
+          const substituted = substitutedParamsLookup.has(p);
+          const iso = this.paramToIsolation.get(p);
+          if (!eliminated && !substituted && (!iso || !iso.fullyConstrained)) {
             allLocked = false;
           }
         });
@@ -377,7 +400,7 @@ export class AlgNumSubSystem {
 }
 
 
-class Cluster {
+class Isolation {
 
   constructor(polynomials) {
     this.polynomials = polynomials;
@@ -394,7 +417,25 @@ class Cluster {
         }
       });
     }
+    this.dof = this.beingSolvedParams.size - polynomials.length;
 
+    let penaltyFunction = new PolynomialResidual();
+    this.beingSolvedParams.forEach(sp => {
+      const param = sp.objectParam;
+      if (param.constraints) {
+        param.constraints.forEach(pc => penaltyFunction.add(sp, pc))
+      }
+    });
+
+    if (penaltyFunction.params.length) {
+      residuals.push(penaltyFunction);
+    }
+
+    this.numericalSolver = prepare(residuals);
+  }
+
+  get fullyConstrained() {
+    return this.dof === 0;
   }
 
   solve(rough) {
@@ -411,26 +452,31 @@ class Cluster {
 
 }
 
+class PolynomialResidual {
 
-export class AlgNumSystem {
-  constraints = [];
-  generators = [];
-  locked = new Set();
-  constantParams = new Set();
+  params = [];
+  functions = [];
+
+  add(param, fn) {
+    this.params.push(param);
+    this.functions.push(fn);
 
-  constructor(visitAllObjects) {
-    this.visitAllObjects = visitAllObjects;
   }
 
-  addConstraint(constraint) {
-    this.constraints.push(constraint);
-    if (constraint.schema.generator) {
+  error() {
+    let err = 0;
+    for (let i = 0 ; i < this.params.length; ++i) {
+      err += this.functions[i].d0(this.params[i].get());
+    }
 
+    return err;
+  }
+
+  gradient(out) {
+    for (let i = 0 ; i < this.params.length; ++i) {
+      out[i] = this.functions[i].d1(this.params[i].get());
     }
   }
 
-  startTransaction(interactiveLock = []) {
-    this.systemTransaction.prepare(interactiveLock);
-    return this.systemTransaction;
-  }
 }
+

web/app/sketcher/constr/barriers.js

@@ -0,0 +1,17 @@
+import {sq} from "../../math/math";
+
+export function greaterThanConstraint(val) {
+  const K = 100;
+  return {
+    d0: x => K*sq(Math.min(0, x - val)),
+    d1: x => x < val ? K*(2*x - 2*val) : 0
+  }
+}
+
+export function lessThanConstraint(val) {
+  const K = 100;
+  return {
+    d0: x => K*sq(Math.max(0, x - val)),
+    d1: x => x <= val ? 0 : K*(2*x - 2*val)
+  }
+}

web/app/sketcher/constr/polynomial.js

@@ -1,6 +1,4 @@
 import {eqEps} from "../../brep/geom/tolerance";
-import {sq} from "../../math/math";
-import {polynomial} from "../../math/vec";
 import {compositeFn} from "../../../../modules/gems/func";
 
 export class Polynomial {

web/app/sketcher/shapes/circle.js

@@ -1,10 +1,9 @@
-import * as utils from '../../utils/utils';
 import * as math from '../../math/math';
-import {EditCircleTool} from '../tools/circle'
-
-import {EndPoint} from './point'
-import {Ref} from './ref'
 import {SketchObject} from './sketch-object'
+import {Param} from "./param";
+import {greaterThanConstraint} from "../constr/barriers";
+
+const MIN_RADIUS = 100;
 
 export class Circle extends SketchObject {
 
@@ -13,8 +12,8 @@ export class Circle extends SketchObject {
     this.c = c;
     c.parent = this;
     this.children.push(c);
-    this.r = new Ref(0);
-    this.r.obj = this;
+    this.r = new Param(MIN_RADIUS + 0.001);
+    this.r.constraints = [greaterThanConstraint(MIN_RADIUS)];
   }
 
   visitParams(callback) {

web/app/sketcher/shapes/sketch-object.js

@@ -87,11 +87,11 @@ export class SketchObject extends Shape {
     if (this.marked != null) {
       ctx.save();
       viewer.setStyle(this.marked, ctx);
-    }
-    if (this.fullyConstrained) {
+    } else if (this.fullyConstrained) {
       ctx.save();
       viewer.setStyle(Styles.FULLY_CONSTRAINED, ctx);
     }
+
     this.drawImpl(ctx, scale, viewer);
     if (this.marked != null || this.fullyConstrained) ctx.restore();
   }

web/app/sketcher/styles.js

@@ -41,8 +41,8 @@ export const Styles = {
 
   FULLY_CONSTRAINED: {
     lineWidth : 2,
-    strokeStyle : "#d7e6ff",
-    fillStyle : "#424e56"
+    strokeStyle : "#9cd4ff",
+    fillStyle : "#2c44bc"
   },
 
   CONSTRUCTION : {