mirror of
https://github.com/xibyte/jsketcher
synced 2025-12-15 12:53:52 +01:00
parallel constraint
This commit is contained in:
Val Erastov
parent
789d9f0aee
commit
3dadb83248
7 changed files with 268 additions and 110 deletions
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -4,8 +4,6 @@ public interface Constraint extends System {
|
|||
|
||||
double error();
|
||||
|
||||
void step(double alpha);
|
||||
|
||||
void set(double[] input);
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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<Constraint> constraints = new ArrayList<>();
|
||||
|
||||
ParamInfo(int id) {
|
||||
this.id = id;
|
||||
}
|
||||
}
|
||||
|
||||
public static class SubSystem {
|
||||
|
||||
private final List<Constraint> constraints;
|
||||
private final LinkedHashMap<Param, ParamInfo> params = new LinkedHashMap<>();
|
||||
|
||||
public SubSystem(List<Constraint> 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<Param> 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");
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -1,7 +1,9 @@
|
|||
package cad.gcs;
|
||||
|
||||
public interface System {
|
||||
|
||||
|
||||
Param[] getParams();
|
||||
|
||||
double[] params();
|
||||
|
||||
void gradient(double[] out);
|
||||
|
|
|
|||
156
src/cad/gcs/constr/Parallel.java
Normal file
156
src/cad/gcs/constr/Parallel.java
Normal file
|
|
@ -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]);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -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());
|
||||
|
|
|
|||
|
|
@ -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());
|
||||
|
|
|
|||
Loading…
Reference in a new issue