From 3dadb83248ed23d441fb7751e760ed34d536d7a2 Mon Sep 17 00:00:00 2001 From: Val Erastov Date: Sat, 20 Sep 2014 21:00:35 -0700 Subject: [PATCH] parallel constraint --- src/cad/fx/App2DCtrl.java | 53 +++++++-- src/cad/gcs/Constraint.java | 2 - src/cad/gcs/Solver.java | 119 ++++++++------------ src/cad/gcs/System.java | 4 +- src/cad/gcs/constr/Parallel.java | 156 ++++++++++++++++++++++++++ src/cad/gcs/constr/Perpendicular.java | 25 ++--- src/cad/gcs/constr/X.java | 19 +--- 7 files changed, 268 insertions(+), 110 deletions(-) create mode 100644 src/cad/gcs/constr/Parallel.java diff --git a/src/cad/fx/App2DCtrl.java b/src/cad/fx/App2DCtrl.java index 17b26c62..234af8c9 100644 --- a/src/cad/fx/App2DCtrl.java +++ b/src/cad/fx/App2DCtrl.java @@ -7,6 +7,7 @@ import cad.gcs.GradientDescent3; import cad.gcs.Param; import cad.gcs.Solver; import cad.gcs.constr.Constraint2; +import cad.gcs.constr.Parallel; import cad.gcs.constr.Perpendicular; import cad.gcs.constr.Perpendicular2; import cad.gcs.constr.X; @@ -57,9 +58,9 @@ public class App2DCtrl implements Initializable { Line l1 = new Line(100, 100, 300, 600); Line l2 = new Line(400, 600, 600, 100); - content.getChildren().addAll(l1, l2); - + Line l3 = new Line(650, 100, 800, 600); + content.getChildren().addAll(l1, l2, l3); solve.setOnAction(event -> { @@ -74,13 +75,16 @@ public class App2DCtrl implements Initializable { Param l1p1y = new Param(l1.getStartY()); Param l1p2x = new Param(l1.getEndX()); Param l1p2y = new Param(l1.getEndY()); + Param l2p1x = new Param(l2.getStartX()); Param l2p1y = new Param(l2.getStartY()); Param l2p2x = new Param(l2.getEndX()); Param l2p2y = new Param(l2.getEndY()); - - Perpendicular2 perpendicular2 = new Perpendicular2(as, ae, bs, be); + Param l3p1x = new Param(l3.getStartX()); + Param l3p1y = new Param(l3.getStartY()); + Param l3p2x = new Param(l3.getEndX()); + Param l3p2y = new Param(l3.getEndY()); Perpendicular perpendicular = new Perpendicular( @@ -94,6 +98,17 @@ public class App2DCtrl implements Initializable { l2p2y ); + Parallel parallel = new Parallel( + l3p1x, + l3p1y, + l3p2x, + l3p2y, + l2p1x, + l2p1y, + l2p2x, + l2p2y + ); + XY xy = new XY(as, new Vector(100, 100)); @@ -120,9 +135,9 @@ public class App2DCtrl implements Initializable { // solveGC(subSystem); - java.lang.System.out.println(perpendicular.angle()); + java.lang.System.out.println("ANGLE |- :" + perpendicular.angle()); + java.lang.System.out.println("ANGLE || :" + parallel.angle()); - Constraint2 constr = perpendicular2; // Constraint2 constr = xy; // Constraint constr = perpendicular; @@ -146,17 +161,39 @@ public class App2DCtrl implements Initializable { l1.setStartY(l1p1y.get()); l1.setEndX(l1p2x.get()); l1.setEndY(l1p2y.get()); + l2.setStartX(l2p1x.get()); l2.setStartY(l2p1y.get()); l2.setEndX(l2p2x.get()); l2.setEndY(l2p2y.get()); - }); + l3.setStartX(l3p1x.get()); + l3.setStartY(l3p1y.get()); + l3.setEndX(l3p2x.get()); + l3.setEndY(l3p2y.get()); + +// scale(l1); +// scale(l2); +// scale(l3); + }); + } + + double xxx = 100; + + private void scale(Line l) { + + Vector v = new Vector(l.getEndX() - l.getStartX(), l.getEndY() - l.getStartY()); + v = v.normalize().multi(200); + l.setStartX(xxx += 100.); + l.setStartY(500.); + + l.setEndX(l.getStartX() + v.x); + l.setEndY(l.getStartY() + v.y); } private void solveGC(final Solver.SubSystem subSystem) { GaussNewtonOptimizer optimizer = new GaussNewtonOptimizer((iteration, previous, current) -> { - return subSystem.value() < 0.00000001; + return subSystem.value() < 0.00001; }) { @Override diff --git a/src/cad/gcs/Constraint.java b/src/cad/gcs/Constraint.java index 46a93aeb..72b3a041 100644 --- a/src/cad/gcs/Constraint.java +++ b/src/cad/gcs/Constraint.java @@ -4,8 +4,6 @@ public interface Constraint extends System { double error(); - void step(double alpha); - void set(double[] input); } diff --git a/src/cad/gcs/Solver.java b/src/cad/gcs/Solver.java index 704fa5aa..b970004a 100644 --- a/src/cad/gcs/Solver.java +++ b/src/cad/gcs/Solver.java @@ -15,6 +15,9 @@ import org.apache.commons.math3.linear.QRDecomposition; import org.apache.commons.math3.linear.RealMatrix; import org.apache.commons.math3.linear.SingularMatrixException; +import java.util.ArrayList; +import java.util.Iterator; +import java.util.LinkedHashMap; import java.util.List; public class Solver { @@ -401,7 +404,7 @@ public class Solver { try { h = new LUDecomposition(A).getSolver().solve(g); } catch (Exception ssse) { - java.lang.System.out.println( ""); + ssse.printStackTrace(); return SolveStatus.Success; } @@ -675,20 +678,38 @@ public class Solver { return new Array2DRowRealMatrix(rsize, csize); } + static class ParamInfo { + + final int id; + final List constraints = new ArrayList<>(); + + ParamInfo(int id) { + this.id = id; + } + } + public static class SubSystem { private final List constraints; + private final LinkedHashMap params = new LinkedHashMap<>(); public SubSystem(List constraints) { this.constraints = constraints; + for (Constraint c : constraints) { + + for (Param p : c.getParams()) { + ParamInfo paramInfo = params.get(p); + if (paramInfo == null) { + paramInfo = new ParamInfo(params.size()); + params.put(p, paramInfo); + } + paramInfo.constraints.add(c); + } + } } public int pSize() { - int s = 0; - for (Constraint c : constraints) { - s += c.params().length; - } - return s; + return params.size(); } public int cSize() { @@ -697,40 +718,23 @@ public class Solver { public void fillParams(RealMatrix x) { - int i = 0; - TDoubleList params = new TDoubleArrayList(); - for (Constraint c : constraints) { - params.add(c.params()); - } - x.setColumn(0, params.toArray()); + x.setColumn(0, getParams().toArray()); } - public TDoubleList getParams() { - int i = 0; - TDoubleList params = new TDoubleArrayList(); - for (Constraint c : constraints) { - params.add(c.params()); + TDoubleList params_ = new TDoubleArrayList(); + for (Param p : params.keySet()) { + params_.add(p.get()); } - return params; + return params_; } - public TDoubleList getValues2() { - TDoubleList params = new TDoubleArrayList(); - for (Constraint c : constraints) { - double err = c.error(); - params.add(err); - } - return params; - } - - public TDoubleList getValues() { - TDoubleList params = new TDoubleArrayList(); + TDoubleList values = new TDoubleArrayList(); for (Constraint c : constraints) { - params.add(c.error()); + values.add(c.error()); } - return params; + return values; } public double calcResidual(RealMatrix r) { @@ -778,51 +782,34 @@ public class Solver { } for (int i=0; i < constraints.size(); i++) { Constraint c = constraints.get(i); - double[] grad = new double[c.params().length]; + + Param[] cParams = c.getParams(); + double[] grad = new double[cParams.length]; c.gradient(grad); - for (int j=0; j < grad.length; j++) { - jacobi.setEntry(i,j, grad[j]); + + for (int p = 0; p < cParams.length; p++) { + Param param = cParams[p]; + int j = params.get(param).id; + jacobi.setEntry(i,j, grad[p]); } } } public RealMatrix makeJacobi() { RealMatrix jacobi = new Array2DRowRealMatrix(cSize(), pSize()); - for (int j=0; j < pSize(); j++) { - for (int i=0; i < constraints.size(); i++) { - jacobi.setEntry(i, j, 0); - } - } - for (int i=0; i < constraints.size(); i++) { - Constraint c = constraints.get(i); - double[] grad = new double[c.params().length]; - c.gradient(grad); - for (int j=0; j < grad.length; j++) { - jacobi.setEntry(i,j, grad[j]); - } - } + calcJacobi(jacobi); return jacobi; } public void setParams(RealMatrix params) { - int off = 0; - double[] arr = params.getColumn(0); - for (Constraint c : constraints) { - int l = c.params().length; - double[] cp = new double[l]; - java.lang.System.arraycopy(arr, off, cp, 0, l); - c.set(cp); - } + setParams(params.getData()[0]); } public void setParams(double[] arr) { - int off = 0; - for (Constraint c : constraints) { - int l = c.params().length; - double[] cp = new double[l]; - java.lang.System.arraycopy(arr, off, cp, 0, l); - c.set(cp); + Iterator pit = params.keySet().iterator(); + for (double v : arr) { + pit.next().set(v); } } @@ -831,15 +818,7 @@ public class Solver { } public void calcGrad(RealMatrix out) { - - int cc = 0; - for (Constraint c : constraints) { - double[] grad = new double[c.params().length]; - c.gradient(grad); - for (double aGrad : grad) { - out.setEntry(cc++, 0, aGrad); - } - } + throw new UnsupportedOperationException("men at work"); } } diff --git a/src/cad/gcs/System.java b/src/cad/gcs/System.java index e0c756af..56a2b5e9 100644 --- a/src/cad/gcs/System.java +++ b/src/cad/gcs/System.java @@ -1,7 +1,9 @@ package cad.gcs; public interface System { - + + Param[] getParams(); + double[] params(); void gradient(double[] out); diff --git a/src/cad/gcs/constr/Parallel.java b/src/cad/gcs/constr/Parallel.java new file mode 100644 index 00000000..95e38cde --- /dev/null +++ b/src/cad/gcs/constr/Parallel.java @@ -0,0 +1,156 @@ +package cad.gcs.constr; + +import cad.gcs.Constraint; +import cad.gcs.Param; +import cad.math.Vector; + +public class Parallel implements Constraint { + + public static final int l1p1x = 0; + public static final int l1p1y = 1; + public static final int l1p2x = 2; + public static final int l1p2y = 3; + public static final int l2p1x = 4; + public static final int l2p1y = 5; + public static final int l2p2x = 6; + public static final int l2p2y = 7; + + private final Param[] params = new Param[8]; + + public Parallel( + Param _l1p1x, + Param _l1p1y, + Param _l1p2x, + Param _l1p2y, + Param _l2p1x, + Param _l2p1y, + Param _l2p2x, + Param _l2p2y + ) { + params[l1p1x] = _l1p1x; + params[l1p1y] = _l1p1y; + params[l1p2x] = _l1p2x; + params[l1p2y] = _l1p2y; + params[l2p1x] = _l2p1x; + params[l2p1y] = _l2p1y; + params[l2p2x] = _l2p2x; + params[l2p2y] = _l2p2y; + } + + public void out(Vector p1, Vector p2, Vector p3, Vector p4) { + p1.x = params[l1p1x].get(); + p1.y = params[l1p1y].get(); + p2.x = params[l1p2x].get(); + p2.y = params[l1p2y].get(); + p3.x = params[l2p1x].get(); + p3.y = params[l2p1y].get(); + p4.x = params[l2p2x].get(); + p4.y = params[l2p2y].get(); + } + + @Override + public Param[] getParams() { + return params; + } + + @Override + public double[] params() { + double[] _params = new double[8]; + _params[l1p1x] = params[l1p1x].get(); + _params[l1p1y] = params[l1p1y].get(); + _params[l1p2x] = params[l1p2x].get(); + _params[l1p2y] = params[l1p2y].get(); + _params[l2p1x] = params[l2p1x].get(); + _params[l2p1y] = params[l2p1y].get(); + _params[l2p2x] = params[l2p2x].get(); + _params[l2p2y] = params[l2p2y].get(); + return _params; + } + + @Override + public double error() { + double dx1 = (params[l1p2x].get() - params[l1p1x].get()); + double dy1 = (params[l1p2y].get() - params[l1p1y].get()); + double dx2 = -(params[l2p2y].get() - params[l2p1y].get()); + double dy2 = (params[l2p2x].get() - params[l2p1x].get()); + //dot product shows how the lines off to be perpendicular + double off = dx1 * dx2 + dy1 * dy2; + return off * off; + } + + + //derivative of ((x-a1)*a2 + a3)^2 + public double partDerivative1(double a1, double a2, double a3, double x) { + return 2*a2*(-a1*a2 + a2*x+a3); + } + + //derivative of ((a1-x)*a2 + a3)^2 + public double partDerivative2(double a1, double a2, double a3, double x) { + return -2*a2*(a1*a2 - a2*x+a3); + } + + @Override + public void gradient(double[] out) { + + double x1 = params[l1p1x].get(); + double x2 = params[l1p1y].get(); + double x3 = params[l1p2x].get(); + double x4 = params[l1p2y].get(); + double x6 = params[l2p1x].get(); + double x5 = - params[l2p1y].get(); + double x8 = params[l2p2x].get(); + double x7 = - params[l2p2y].get(); + + + double c1 = x3 - x1; + double c2 = x7 - x5; + double c3 = x4 - x2; + double c4 = x8 - x6; +// + //f(x) = ( (x3 - x1) * ( x7 - x5) + (x4 - x2) * (x8 - x6) ) ^ 2 => + //f(x) = ( (x3 - x1) * (-x8 + x6) + (x4 - x2) * (x7 - x5) ) ^ 2 + + out[l1p1x] = partDerivative2(x3, c2, c3 * c4, x1); + out[l1p1y] = partDerivative2(x4, c4, c1 * c2, x2); + + out[l1p2x] = partDerivative1(x1, c2, c3 * c4, x3); + out[l1p2y] = partDerivative1(x2, c4, c1 * c2, x4); + + out[l2p1x] = partDerivative2(x7, c1, c3 * c4, x5); + out[l2p1y] = partDerivative2(x8, c3, c1 * c2, x6); + + out[l2p2x] = partDerivative1(x5, c1, c3 * c4, x7); + out[l2p2y] = partDerivative1(x6, c3, c1 * c2, x8); + } + + public double angle() { + double dx1 = (params[l1p2x].get() - params[l1p1x].get()); + double dy1 = (params[l1p2y].get() - params[l1p1y].get()); + double dx2 = (params[l2p2x].get() - params[l2p1x].get()); + double dy2 = (params[l2p2y].get() - params[l2p1y].get()); + + Vector d1 = new Vector(dx1, dy1); + Vector d2 = new Vector(dx2, dy2); + + + return Math.acos(d1.normalize().dot(d2.normalize())) / Math.PI * 180; + } + + @Override + public int pSize() { + return 8; + } + + @Override + public void set(double[] input) { + params[l1p1x].set(input[l1p1x]); + params[l1p1y].set(input[l1p1y]); + params[l1p2x].set(input[l1p2x]); + params[l1p2y].set(input[l1p2y]); + params[l2p1x].set(input[l2p1x]); + params[l2p1y].set(input[l2p1y]); + params[l2p2x].set(input[l2p2x]); + params[l2p2y].set(input[l2p2y]); + } + +} diff --git a/src/cad/gcs/constr/Perpendicular.java b/src/cad/gcs/constr/Perpendicular.java index c8a2fd5f..72aaa938 100644 --- a/src/cad/gcs/constr/Perpendicular.java +++ b/src/cad/gcs/constr/Perpendicular.java @@ -48,6 +48,11 @@ public class Perpendicular implements Constraint { p4.y = params[l2p2y].get(); } + @Override + public Param[] getParams() { + return params; + } + @Override public double[] params() { double[] _params = new double[8]; @@ -64,10 +69,10 @@ public class Perpendicular implements Constraint { @Override public double error() { - double dx1 = (params[l1p1x].get() - params[l1p2x].get()); - double dy1 = (params[l1p1y].get() - params[l1p2y].get()); - double dx2 = (params[l2p1x].get() - params[l2p2x].get()); - double dy2 = (params[l2p1y].get() - params[l2p2y].get()); + double dx1 = (params[l1p2x].get() - params[l1p1x].get()); + double dy1 = (params[l1p2y].get() - params[l1p1y].get()); + double dx2 = (params[l2p2x].get() - params[l2p1x].get()); + double dy2 = (params[l2p2y].get() - params[l2p1y].get()); //dot product shows how the lines off to be perpendicular double off = dx1 * dx2 + dy1 * dy2; return off * off; @@ -148,18 +153,6 @@ public class Perpendicular implements Constraint { } - @Override - public void step(double alpha) { - - System.out.println(angle()); - - step(l1p1x, l1p1y, params[l2p1x].get() - params[l2p2x].get(), params[l2p1y].get() - params[l2p2y].get(), alpha); - step(l1p2x, l1p2y, -(params[l2p1x].get() - params[l2p2x].get()), -(params[l2p1y].get() - params[l2p2y].get()), alpha); - - step(l2p1x, l2p1y, (params[l1p1x].get() - params[l1p2x].get()), (params[l1p1y].get() - params[l1p2y].get()), alpha); - step(l2p2x, l2p2y, -(params[l1p1x].get() - params[l1p2x].get()), -(params[l1p1y].get() - params[l1p2y].get()), alpha); - } - public double angle() { double dx1 = (params[l1p2x].get() - params[l1p1x].get()); double dy1 = (params[l1p2y].get() - params[l1p1y].get()); diff --git a/src/cad/gcs/constr/X.java b/src/cad/gcs/constr/X.java index 5a069450..7c72b985 100644 --- a/src/cad/gcs/constr/X.java +++ b/src/cad/gcs/constr/X.java @@ -55,7 +55,12 @@ public class X implements Constraint { public double valueY() { return (params[l2p2y].get() - params[l2p1y].get()) * .5 - (params[l1p2y].get() - params[l1p1y].get()) * .5; } - + + @Override + public Param[] getParams() { + return params; + } + @Override public double[] params() { double[] _params = new double[8]; @@ -111,18 +116,6 @@ public class X implements Constraint { } - @Override - public void step(double alpha) { - - System.out.println(angle()); - - step(l1p1x, l1p1y, params[l2p1x].get() - params[l2p2x].get(), params[l2p1y].get() - params[l2p2y].get(), alpha); - step(l1p2x, l1p2y, -(params[l2p1x].get() - params[l2p2x].get()), -(params[l2p1y].get() - params[l2p2y].get()), alpha); - - step(l2p1x, l2p1y, (params[l1p1x].get() - params[l1p2x].get()), (params[l1p1y].get() - params[l1p2y].get()), alpha); - step(l2p2x, l2p2y, -(params[l1p1x].get() - params[l1p2x].get()), -(params[l1p1y].get() - params[l1p2y].get()), alpha); - } - public double angle() { double dx1 = (params[l1p1x].get() - params[l1p2x].get()); double dy1 = (params[l1p1y].get() - params[l1p2y].get());