mirror of
https://github.com/xibyte/jsketcher
synced 2025-12-06 16:33:15 +01:00
multi solve
This commit is contained in:
Val Erastov
parent
3dadb83248
commit
cf891ba43e
14 changed files with 536 additions and 332 deletions
11
misc/perp
11
misc/perp
|
|
@ -2,21 +2,22 @@ pkg load optim;
|
|||
|
||||
function y = f (x)
|
||||
|
||||
y(1) = (
|
||||
y = [
|
||||
(
|
||||
(x(3) - x(1)) * (x(7) - x(5)) +
|
||||
(x(4) - x(2)) * (x(8) - x(6))
|
||||
) ^ 2;
|
||||
y(2) = (
|
||||
) ^ 2,
|
||||
(
|
||||
(x(11) - x(9)) * (x(7) - x(5)) +
|
||||
(x(12) - x(10)) * (x(8) - x(6))
|
||||
) ^ 2;
|
||||
) ^ 2];
|
||||
y(3) = 0;y(4) = 0;y(5) = 0;y(6) = 0;y(7) = 0;y(8) = 0;
|
||||
y(9) = 0;y(10) = 0;y(11) = 0;y(12) = 0;
|
||||
endfunction
|
||||
|
||||
x0 = [100, 100, 600, 600, 700, 600, 900, 100, 1100, 100, 1600, 600];
|
||||
#x = fminunc(@f, reshape(x0, 12,1));
|
||||
x = bfgsmin('f', {reshape(x0, 12,1)}); #WORKS!
|
||||
x = fsolve(@f, x0); #WORKS!
|
||||
|
||||
#x = x0;
|
||||
l1 = [x(1), x(2); x(3), x(4)];
|
||||
|
|
|
|||
|
|
@ -1,46 +1,42 @@
|
|||
package cad.fx;
|
||||
|
||||
import cad.gcs.Constraint;
|
||||
import cad.gcs.GradientDescent;
|
||||
import cad.gcs.GradientDescent2;
|
||||
import cad.gcs.GradientDescent3;
|
||||
import cad.gcs.Figures;
|
||||
import cad.gcs.Param;
|
||||
import cad.gcs.Solver;
|
||||
import cad.gcs.constr.Constraint2;
|
||||
import cad.gcs.constr.P2LDistance;
|
||||
import cad.gcs.constr.Parallel;
|
||||
import cad.gcs.constr.Perpendicular;
|
||||
import cad.gcs.constr.Perpendicular2;
|
||||
import cad.gcs.constr.X;
|
||||
import cad.gcs.constr.XY;
|
||||
import cad.math.Vector;
|
||||
import gnu.trove.list.TDoubleList;
|
||||
import javafx.application.Platform;
|
||||
import javafx.event.ActionEvent;
|
||||
import javafx.fxml.Initializable;
|
||||
import javafx.scene.Group;
|
||||
import javafx.scene.control.Button;
|
||||
import javafx.scene.layout.Pane;
|
||||
import javafx.scene.shape.Line;
|
||||
import org.apache.commons.math3.analysis.MultivariateMatrixFunction;
|
||||
import org.apache.commons.math3.analysis.MultivariateVectorFunction;
|
||||
import org.apache.commons.math3.analysis.function.Max;
|
||||
import org.apache.commons.math3.linear.Array2DRowRealMatrix;
|
||||
import org.apache.commons.math3.linear.MatrixUtils;
|
||||
import org.apache.commons.math3.linear.RealMatrix;
|
||||
import org.apache.commons.math3.optim.ConvergenceChecker;
|
||||
import org.apache.commons.math3.optim.InitialGuess;
|
||||
import org.apache.commons.math3.optim.MaxEval;
|
||||
import org.apache.commons.math3.optim.MaxIter;
|
||||
import org.apache.commons.math3.optim.PointVectorValuePair;
|
||||
import org.apache.commons.math3.optim.nonlinear.vector.ModelFunction;
|
||||
import org.apache.commons.math3.optim.nonlinear.vector.ModelFunctionJacobian;
|
||||
import org.apache.commons.math3.optim.nonlinear.vector.Target;
|
||||
import org.apache.commons.math3.optim.nonlinear.vector.Weight;
|
||||
import org.apache.commons.math3.optim.nonlinear.vector.jacobian.GaussNewtonOptimizer;
|
||||
import org.apache.commons.math3.optim.nonlinear.vector.jacobian.LevenbergMarquardtOptimizer;
|
||||
import org.jacop.constraints.Sum;
|
||||
|
||||
import java.net.URL;
|
||||
import java.util.ArrayList;
|
||||
import java.util.Arrays;
|
||||
import java.util.List;
|
||||
import java.util.ResourceBundle;
|
||||
import java.util.concurrent.ExecutorService;
|
||||
import java.util.concurrent.Executors;
|
||||
import java.util.concurrent.ThreadPoolExecutor;
|
||||
|
||||
import static java.util.Arrays.asList;
|
||||
|
||||
public class App2DCtrl implements Initializable {
|
||||
|
||||
|
|
@ -48,6 +44,7 @@ public class App2DCtrl implements Initializable {
|
|||
|
||||
public Pane viewer;
|
||||
public Button solve;
|
||||
public Button square;
|
||||
|
||||
@Override
|
||||
public void initialize(URL location, ResourceBundle resources) {
|
||||
|
|
@ -62,6 +59,10 @@ public class App2DCtrl implements Initializable {
|
|||
|
||||
content.getChildren().addAll(l1, l2, l3);
|
||||
|
||||
square.setOnAction(event -> {
|
||||
solveFigure(Figures.square(100));
|
||||
});
|
||||
|
||||
|
||||
solve.setOnAction(event -> {
|
||||
|
||||
|
|
@ -86,6 +87,11 @@ public class App2DCtrl implements Initializable {
|
|||
Param l3p2x = new Param(l3.getEndX());
|
||||
Param l3p2y = new Param(l3.getEndY());
|
||||
|
||||
// l2p2x.setLocked(true);
|
||||
// l2p2y.setLocked(true);
|
||||
// l2p1x.setLocked(true);
|
||||
// l2p1y.setLocked(true);
|
||||
|
||||
|
||||
Perpendicular perpendicular = new Perpendicular(
|
||||
l1p1x,
|
||||
|
|
@ -109,34 +115,25 @@ public class App2DCtrl implements Initializable {
|
|||
l2p2y
|
||||
);
|
||||
|
||||
|
||||
XY xy = new XY(as, new Vector(100, 100));
|
||||
|
||||
X x = new X(
|
||||
l1p1x,
|
||||
l1p1y,
|
||||
l1p2x,
|
||||
l1p2y,
|
||||
l2p1x,
|
||||
l2p1y,
|
||||
l2p2x,
|
||||
l2p2y
|
||||
P2LDistance p2l1 = new P2LDistance(
|
||||
10,
|
||||
l3p1x, l3p1y,
|
||||
l2p1x, l2p1y,
|
||||
l2p2x, l2p2y
|
||||
);
|
||||
|
||||
List<Constraint> constrs = Arrays.<Constraint>asList(perpendicular);
|
||||
Solver.SubSystem subSystem = new Solver.SubSystem(constrs);
|
||||
// Solver.optimize(subSystem);
|
||||
//
|
||||
P2LDistance p2l2 = new P2LDistance(
|
||||
10,
|
||||
l1p2x, l1p2y,
|
||||
l2p1x, l2p1y,
|
||||
l2p2x, l2p2y
|
||||
);
|
||||
|
||||
|
||||
// while (subSystem.error() > 0.0001 ) {
|
||||
Solver.solve_LM(subSystem);
|
||||
// }
|
||||
|
||||
// solveGC(subSystem);
|
||||
|
||||
java.lang.System.out.println("ANGLE |- :" + perpendicular.angle());
|
||||
java.lang.System.out.println("ANGLE || :" + parallel.angle());
|
||||
Runnable update = () -> {
|
||||
System.out.println("ANGLE |- : " + perpendicular.angle());
|
||||
System.out.println("ANGLE || : " + parallel.angle());
|
||||
System.out.println("DISTANCE : " + p2l1.error());
|
||||
|
||||
// Constraint2 constr = xy;
|
||||
|
||||
|
|
@ -175,7 +172,77 @@ public class App2DCtrl implements Initializable {
|
|||
// scale(l1);
|
||||
// scale(l2);
|
||||
// scale(l3);
|
||||
};
|
||||
|
||||
|
||||
List<Constraint> constrs = Arrays.<Constraint>asList(parallel);
|
||||
// List<Constraint> constrs = Arrays.<Constraint>asList(p2l1);
|
||||
Solver.SubSystem subSystem = new Solver.SubSystem(constrs);
|
||||
// Solver.optimize(subSystem);
|
||||
//
|
||||
|
||||
ExecutorService executor = Executors.newSingleThreadExecutor();
|
||||
executor.execute(() -> {
|
||||
globalSolve(subSystem, () -> Platform.runLater(update));
|
||||
if (true) return;
|
||||
while (subSystem.error() > 0.0001) {
|
||||
Solver.solve_LM(subSystem);
|
||||
// solveLM_COMMONS(subSystem);
|
||||
// Solver.solve_DL(subSystem);
|
||||
// Solver.solve_BFGS(subSystem, true);
|
||||
Platform.runLater(update);
|
||||
}
|
||||
});
|
||||
});
|
||||
}
|
||||
|
||||
private void solveFigure(Figures.Figure square) {
|
||||
|
||||
Solver.SubSystem subSystem = new Solver.SubSystem(square.constraints);
|
||||
|
||||
List<Line> lines = new ArrayList<>();
|
||||
|
||||
for (Param[] line : square.lines) {
|
||||
Line fxLine = new Line();
|
||||
fxLine.setStartX(line[0].get());
|
||||
fxLine.setStartY(line[1].get());
|
||||
fxLine.setEndX(line[2].get());
|
||||
fxLine.setEndY(line[3].get());
|
||||
lines.add(fxLine);
|
||||
}
|
||||
|
||||
ExecutorService executor = Executors.newSingleThreadExecutor();
|
||||
executor.execute(() -> {
|
||||
globalSolve(subSystem, () -> Platform.runLater(() -> {
|
||||
for (int i = 0; i < square.lines.length; i++) {
|
||||
Param[] line = square.lines[i];
|
||||
Line fxLine = lines.get(i);
|
||||
fxLine.setStartX(line[0].get());
|
||||
fxLine.setStartY(line[1].get());
|
||||
fxLine.setEndX(line[2].get());
|
||||
fxLine.setEndY(line[3].get());
|
||||
}
|
||||
}));
|
||||
});
|
||||
}
|
||||
|
||||
private void globalSolve(Solver.SubSystem subSystem, Runnable linearSolvedCallback) {
|
||||
|
||||
double eps = 0.0001;
|
||||
while (subSystem.error() > eps) {
|
||||
solveLM_COMMONS(subSystem);
|
||||
// Solver.solve_LM(subSystem);
|
||||
TDoubleList residuals = subSystem.calcResidual();
|
||||
double worseValue = residuals.max();
|
||||
if (Math.abs(worseValue) > eps) {
|
||||
int worseId = residuals.indexOf(worseValue);
|
||||
Solver.SubSystem worse = new Solver.SubSystem(asList(subSystem.constraints.get(worseId)));
|
||||
solveLM_COMMONS(worse);
|
||||
// Solver.solve_LM(worse);
|
||||
System.out.println("WORSE FIXED ERROR:" + worse.error());
|
||||
}
|
||||
linearSolvedCallback.run();
|
||||
}
|
||||
}
|
||||
|
||||
double xxx = 100;
|
||||
|
|
@ -191,18 +258,12 @@ public class App2DCtrl implements Initializable {
|
|||
l.setEndY(l.getStartY() + v.y);
|
||||
}
|
||||
|
||||
private void solveGC(final Solver.SubSystem subSystem) {
|
||||
GaussNewtonOptimizer optimizer = new GaussNewtonOptimizer((iteration, previous, current) -> {
|
||||
return subSystem.value() < 0.00001;
|
||||
}) {
|
||||
private void solveLM_COMMONS(final Solver.SubSystem subSystem) {
|
||||
double eps = 1e-10, eps1 = 1e-80;
|
||||
double tau = 1e-3;
|
||||
|
||||
@Override
|
||||
protected double[] computeResiduals(double[] objectiveValue) {
|
||||
TDoubleList residual = subSystem.calcResidual();
|
||||
return residual.toArray();
|
||||
}
|
||||
LevenbergMarquardtOptimizer optimizer = new LevenbergMarquardtOptimizer(eps, eps, eps1);
|
||||
|
||||
};
|
||||
double[] wieght = new double[subSystem.cSize()];
|
||||
Arrays.fill(wieght, 1);
|
||||
optimizer.optimize(
|
||||
|
|
|
|||
|
|
@ -15,6 +15,7 @@
|
|||
<Region styleClass="spacer"/>
|
||||
<HBox styleClass="segmented-button-bar">
|
||||
<Button text="solve" fx:id="solve"/>
|
||||
<Button text="square" fx:id="square"/>
|
||||
</HBox>
|
||||
</ToolBar>
|
||||
|
||||
|
|
|
|||
|
|
@ -3,7 +3,4 @@ package cad.gcs;
|
|||
public interface Constraint extends System {
|
||||
|
||||
double error();
|
||||
|
||||
void set(double[] input);
|
||||
|
||||
}
|
||||
|
|
|
|||
58
src/cad/gcs/Figures.java
Normal file
58
src/cad/gcs/Figures.java
Normal file
|
|
@ -0,0 +1,58 @@
|
|||
package cad.gcs;
|
||||
|
||||
import cad.gcs.constr.Equals;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
|
||||
public class Figures {
|
||||
|
||||
public static final int X1 = 0;
|
||||
public static final int Y1 = 1;
|
||||
public static final int X2 = 2;
|
||||
public static final int Y2 = 3;
|
||||
public static final int X3 = 4;
|
||||
public static final int Y3 = 5;
|
||||
public static final int X4 = 6;
|
||||
public static final int Y4 = 7;
|
||||
|
||||
public static Figure square(double size) {
|
||||
|
||||
List<Constraint> constrs = new ArrayList<>();
|
||||
|
||||
Param[] l1 = line();
|
||||
Param[] l2 = line();
|
||||
Param[] l3 = line();
|
||||
Param[] l4 = line();
|
||||
|
||||
constrs.add(new Equals(l1[X1], l4[X2]));
|
||||
constrs.add(new Equals(l1[Y1], l4[Y2]));
|
||||
constrs.add(new Equals(l2[X1], l1[X2]));
|
||||
constrs.add(new Equals(l2[Y1], l1[Y2]));
|
||||
constrs.add(new Equals(l3[X1], l2[X2]));
|
||||
constrs.add(new Equals(l3[Y1], l2[Y2]));
|
||||
constrs.add(new Equals(l4[X1], l4[X2]));
|
||||
constrs.add(new Equals(l4[Y1], l4[Y2]));
|
||||
constrs.add(new Equals(l1[Y1], l1[Y2]));
|
||||
constrs.add(new Equals(l3[Y1], l1[Y2]));
|
||||
constrs.add(new Equals(l2[X1], l1[X2]));
|
||||
constrs.add(new Equals(l4[X1], l1[X2]));
|
||||
|
||||
return new Figure(new Param[][]{l1, l2, l3, l4}, constrs);
|
||||
}
|
||||
|
||||
private static Param[] line() {
|
||||
return new Param[]{new Param(10), new Param(10), new Param(100), new Param(100)};
|
||||
}
|
||||
|
||||
public static class Figure {
|
||||
|
||||
public final Param[][] lines;
|
||||
public final List<Constraint> constraints;
|
||||
|
||||
public Figure(Param[][] lines, List<Constraint> constraints) {
|
||||
this.lines = lines;
|
||||
this.constraints = constraints;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -10,43 +10,43 @@ public class GradientDescent {
|
|||
|
||||
public static void solve(Constraint constr) {
|
||||
|
||||
|
||||
double last = value(constr);
|
||||
|
||||
double alpha = 10;
|
||||
int pSize = constr.pSize();
|
||||
|
||||
RealVector steps = new ArrayRealVector(pSize);
|
||||
steps.set(10);
|
||||
|
||||
for (int i = 0; i < 1000000; i++) {
|
||||
|
||||
|
||||
double[] gradData = new double[pSize];
|
||||
constr.gradient(gradData);
|
||||
ArrayRealVector grad = new ArrayRealVector(gradData);
|
||||
|
||||
RealVector dir = grad.mapDivide(grad.getNorm());
|
||||
dir = dir.mapMultiply( alpha);
|
||||
java.lang.System.out.println(dir.getNorm());
|
||||
|
||||
|
||||
ArrayRealVector params = new ArrayRealVector(constr.params());
|
||||
params = params.add(dir);
|
||||
constr.set(params.toArray());
|
||||
java.lang.System.out.println(((Perpendicular) constr).angle());
|
||||
// constr.step(alpha);
|
||||
double err = value(constr);
|
||||
|
||||
if (err < last) {
|
||||
|
||||
} else if (alpha < EPS) {
|
||||
return;
|
||||
} else {
|
||||
alpha /= 3;
|
||||
}
|
||||
last = err;
|
||||
}
|
||||
//
|
||||
// double last = value(constr);
|
||||
//
|
||||
// double alpha = 10;
|
||||
// int pSize = constr.pSize();
|
||||
//
|
||||
// RealVector steps = new ArrayRealVector(pSize);
|
||||
// steps.set(10);
|
||||
//
|
||||
// for (int i = 0; i < 1000000; i++) {
|
||||
//
|
||||
//
|
||||
// double[] gradData = new double[pSize];
|
||||
// constr.gradient(gradData);
|
||||
// ArrayRealVector grad = new ArrayRealVector(gradData);
|
||||
//
|
||||
// RealVector dir = grad.mapDivide(grad.getNorm());
|
||||
// dir = dir.mapMultiply( alpha);
|
||||
// java.lang.System.out.println(dir.getNorm());
|
||||
//
|
||||
//
|
||||
// ArrayRealVector params = new ArrayRealVector(constr.params());
|
||||
// params = params.add(dir);
|
||||
// constr.set(params.toArray());
|
||||
// java.lang.System.out.println(((Perpendicular) constr).angle());
|
||||
//// constr.step(alpha);
|
||||
// double err = value(constr);
|
||||
//
|
||||
// if (err < last) {
|
||||
//
|
||||
// } else if (alpha < EPS) {
|
||||
// return;
|
||||
// } else {
|
||||
// alpha /= 3;
|
||||
// }
|
||||
// last = err;
|
||||
// }
|
||||
}
|
||||
|
||||
private static double value(Constraint constr) {
|
||||
|
|
|
|||
|
|
@ -3,6 +3,7 @@ package cad.gcs;
|
|||
public class Param {
|
||||
|
||||
public double value;
|
||||
public boolean locked;
|
||||
|
||||
public Param(double value) {
|
||||
this.value = value;
|
||||
|
|
@ -15,4 +16,12 @@ public class Param {
|
|||
public double set(double value) {
|
||||
return this.value = value;
|
||||
}
|
||||
|
||||
public boolean isLocked() {
|
||||
return locked;
|
||||
}
|
||||
|
||||
public void setLocked(boolean locked) {
|
||||
this.locked = locked;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -68,6 +68,7 @@ public class Solver {
|
|||
double alpha = 0.;
|
||||
double nu = 2.;
|
||||
int iter = 0, stop = 0, reduce = 0;
|
||||
double mu = 1e-8;
|
||||
while (stop == 0) {
|
||||
|
||||
// check if finished
|
||||
|
|
@ -86,9 +87,38 @@ public class Solver {
|
|||
alpha = squaredNorm(g) / squaredNorm((Jx.multiply(g)));
|
||||
h_sd = g.scalarMultiply(alpha);
|
||||
|
||||
RealMatrix A = Jx.transpose().multiply(Jx);
|
||||
RealMatrix gg = Jx.transpose().multiply(fx.scalarMultiply(-1));
|
||||
double[] diag_A = diagonal(A);
|
||||
|
||||
double mu_increase_factor_ = 10.0;
|
||||
do {
|
||||
for (int i = 0; i < xsize; ++i) {
|
||||
A.addToEntry(i, i, mu);
|
||||
}
|
||||
|
||||
boolean success = true;
|
||||
try {
|
||||
h_gn = new LUDecomposition(A).getSolver().solve(gg);
|
||||
} catch (Exception ssse) {
|
||||
java.lang.System.out.println(ssse.getMessage());
|
||||
success = false;
|
||||
}
|
||||
if (success) {
|
||||
break;
|
||||
}
|
||||
mu *= mu_increase_factor_;
|
||||
} while (mu < 1.0);
|
||||
|
||||
// get the gauss-newton step
|
||||
h_gn = lu(Jx, fx.scalarMultiply(-1));
|
||||
double rel_error = (Jx.transpose().multiply(h_gn).add(fx)).getFrobeniusNorm() / fx.getFrobeniusNorm();
|
||||
// h_gn = new LUDecomposition(new Array2DRowRealMatrix(makeSquare(Jx.getData()))).getSolver().solve(fx.scalarMultiply(-1));
|
||||
|
||||
|
||||
for (int i = 0; i < xsize; ++i) // restore diagonal J^T J entries
|
||||
{
|
||||
A.setEntry(i, i, diag_A[i]);
|
||||
}
|
||||
double rel_error = (Jx.multiply(h_gn).add(fx)).getFrobeniusNorm() / fx.getFrobeniusNorm();
|
||||
if (rel_error > 1e15)
|
||||
break;
|
||||
|
||||
|
|
@ -142,9 +172,9 @@ public class Solver {
|
|||
double rho = dL / dF;
|
||||
|
||||
if (dF > 0 && dL > 0) {
|
||||
x = x_new;
|
||||
Jx = Jx_new;
|
||||
fx = fx_new;
|
||||
x = x_new.copy();
|
||||
Jx = Jx_new.copy();
|
||||
fx = fx_new.copy();
|
||||
err = err_new;
|
||||
|
||||
g = Jx.transpose().multiply(fx.scalarMultiply(-1));
|
||||
|
|
@ -401,14 +431,22 @@ public class Solver {
|
|||
|
||||
//solve augmented functions A*h=-g
|
||||
|
||||
for (int _ = 0; _ < 1000; _++) {
|
||||
try {
|
||||
h = new LUDecomposition(A).getSolver().solve(g);
|
||||
} catch (Exception ssse) {
|
||||
ssse.printStackTrace();
|
||||
mu *= 1./3.;
|
||||
for (int i = 0; i < xsize; ++i) {
|
||||
A.setEntry(i, i, diag_A[i] * mu);
|
||||
}
|
||||
if (_ == 999) {
|
||||
return SolveStatus.Success;
|
||||
}
|
||||
continue;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
;
|
||||
double rel_error = (A.multiply(h).subtract(g)).getFrobeniusNorm() / g.getFrobeniusNorm();
|
||||
|
||||
// check if solving works
|
||||
|
|
@ -690,7 +728,7 @@ public class Solver {
|
|||
|
||||
public static class SubSystem {
|
||||
|
||||
private final List<Constraint> constraints;
|
||||
public final List<Constraint> constraints;
|
||||
private final LinkedHashMap<Param, ParamInfo> params = new LinkedHashMap<>();
|
||||
|
||||
public SubSystem(List<Constraint> constraints) {
|
||||
|
|
@ -790,7 +828,7 @@ public class Solver {
|
|||
for (int p = 0; p < cParams.length; p++) {
|
||||
Param param = cParams[p];
|
||||
int j = params.get(param).id;
|
||||
jacobi.setEntry(i,j, grad[p]);
|
||||
jacobi.setEntry(i,j, param.isLocked() ? 0 : grad[p]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -803,7 +841,7 @@ public class Solver {
|
|||
|
||||
|
||||
public void setParams(RealMatrix params) {
|
||||
setParams(params.getData()[0]);
|
||||
setParams(params.getColumn(0));
|
||||
}
|
||||
|
||||
public void setParams(double[] arr) {
|
||||
|
|
|
|||
|
|
@ -4,8 +4,6 @@ public interface System {
|
|||
|
||||
Param[] getParams();
|
||||
|
||||
double[] params();
|
||||
|
||||
void gradient(double[] out);
|
||||
|
||||
int pSize();
|
||||
|
|
|
|||
34
src/cad/gcs/constr/Equals.java
Normal file
34
src/cad/gcs/constr/Equals.java
Normal file
|
|
@ -0,0 +1,34 @@
|
|||
package cad.gcs.constr;
|
||||
|
||||
import cad.gcs.Constraint;
|
||||
import cad.gcs.Param;
|
||||
|
||||
public class Equals implements Constraint {
|
||||
|
||||
private final Param[] params;
|
||||
|
||||
public Equals(Param p1, Param p2) {
|
||||
this.params = new Param[]{p1, p2};
|
||||
}
|
||||
|
||||
@Override
|
||||
public double error() {
|
||||
return params[0].get() - params[1].get();
|
||||
}
|
||||
|
||||
@Override
|
||||
public Param[] getParams() {
|
||||
return params;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void gradient(double[] out) {
|
||||
out[0] = 1;
|
||||
out[1] = -1;
|
||||
}
|
||||
|
||||
@Override
|
||||
public int pSize() {
|
||||
return params.length;
|
||||
}
|
||||
}
|
||||
193
src/cad/gcs/constr/P2LDistance.java
Normal file
193
src/cad/gcs/constr/P2LDistance.java
Normal file
|
|
@ -0,0 +1,193 @@
|
|||
package cad.gcs.constr;
|
||||
|
||||
import cad.gcs.Constraint;
|
||||
import cad.gcs.Param;
|
||||
import cad.math.Vector;
|
||||
|
||||
import static java.lang.Math.abs;
|
||||
import static java.lang.Math.sqrt;
|
||||
|
||||
public class P2LDistance implements Constraint {
|
||||
|
||||
private final Param[] params = new Param[6];
|
||||
|
||||
public static final int tx = 0;
|
||||
public static final int ty = 1;
|
||||
public static final int lp1x = 2;
|
||||
public static final int lp1y = 3;
|
||||
public static final int lp2x = 4;
|
||||
public static final int lp2y = 5;
|
||||
|
||||
private final double distance;
|
||||
|
||||
public P2LDistance(double distance, Param...params) {
|
||||
this.distance = distance;
|
||||
System.arraycopy(params, 0, this.params, 0, params.length);
|
||||
}
|
||||
|
||||
public double error() {
|
||||
double x0 = p0x(), x1 = p1x(), x2 = p2x();
|
||||
double y0 = p0y(), y1 = p1y(), y2 = p2y();
|
||||
double dist = distance();
|
||||
double dx = x2 - x1;
|
||||
double dy = y2 - y1;
|
||||
double d = sqrt(dx * dx + dy * dy);
|
||||
double area = abs
|
||||
(-x0 * dy + y0 * dx + x1 * y2 - x2 * y1); // = x1y2 - x2y1 - x0y2 + x2y0 + x0y1 - x1y0 = 2*(triangle area)
|
||||
return (area / d - dist);
|
||||
|
||||
}
|
||||
|
||||
|
||||
public double error2() {
|
||||
// //Basis
|
||||
double dx = params[lp2x].get() - params[lp1x].get();
|
||||
double dy = params[lp2y].get() - params[lp1y].get();
|
||||
Vector n = new Vector(-dy, dx).normalize();
|
||||
Vector target = new Vector(params[tx].get() - params[lp1x].get(), params[ty].get() - params[lp1y].get());
|
||||
return distance - target.dot(n);
|
||||
|
||||
}
|
||||
|
||||
private double distance() {
|
||||
return distance;
|
||||
}
|
||||
|
||||
private double p1x() {
|
||||
return params[lp1x].get();
|
||||
}
|
||||
|
||||
private double p1y() {
|
||||
return params[lp1y].get();
|
||||
}
|
||||
|
||||
private double p2x() {
|
||||
return params[lp2x].get();
|
||||
}
|
||||
|
||||
private double p2y() {
|
||||
return params[lp2y].get();
|
||||
}
|
||||
|
||||
private double p0x() {
|
||||
return params[tx].get();
|
||||
}
|
||||
|
||||
private double p0y() {
|
||||
return params[ty].get();
|
||||
}
|
||||
|
||||
public void gradient1(double[] out) {
|
||||
double x1 = params[lp1x].get();
|
||||
double x3 = params[lp2x].get();
|
||||
double x2 = params[lp1y].get();
|
||||
double x4 = params[lp2y].get();
|
||||
double x5 = params[tx].get();
|
||||
double x6 = params[ty].get();
|
||||
|
||||
// double dx = x3 - x1;
|
||||
// double dy = x4 - x2;
|
||||
// Vector n = new Vector(-dy, dx).normalize();
|
||||
// Vector target = new Vector(x5 - x1, x6 - x2);
|
||||
//
|
||||
// double nx = (x2 - x4) / sqrt( (x2 - x4)^2 + (x3 - x1)^2 );
|
||||
// double ny = (x4 - x2) / sqrt( (x2 - x4)^2 + (x3 - x1)^2 );
|
||||
//
|
||||
// double dot = (x5 - x1)*nx + (x6 - x2)*ny;
|
||||
// g(x1, x2, x3, x4)=sqrt( (x2 - x4)^2 + (x3 - x1)^2 );
|
||||
// f(x1, x2, x3, x4, x5, x6) = distance - (x5 - x1)*(x2 - x4) / g(x1, x2, x3, x4) + (x6 - x2) * (x4 - x2) / g(x1, x2, x3, x4);
|
||||
//
|
||||
// f(x1) = distance - (x5 - x1)*(x2 - x4) / sqrt( (x2 - x4)^2 + (x3 - x1)^2 ) + (x6 - x2) * (x4 - x2) / sqrt( (x2 - x4)^2 + (x3 - x1)^2 );
|
||||
|
||||
|
||||
//MAXIMA
|
||||
// diff(distance - (x5 - x1)*(x2 - x4) / sqrt( (x2 - x4)^2 + (x3 - x1)^2 ) + (
|
||||
// x6 - x2) * (x4 - x2) / sqrt( (x2 - x4)^2 + (x3 - x1)^2 ), x1);
|
||||
|
||||
|
||||
|
||||
|
||||
// (x3-x1)*(x4-x2)*(x6-x2) / Math.pow(())
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
public void gradient(double[] out) {
|
||||
double x0 = p0x(), x1 = p1x(), x2 = p2x();
|
||||
double y0 = p0y(), y1 = p1y(), y2 = p2y();
|
||||
double dx = x2 - x1;
|
||||
double dy = y2 - y1;
|
||||
double d2 = dx * dx + dy * dy;
|
||||
double d = sqrt(d2);
|
||||
double area = -x0 * dy + y0 * dx + x1 * y2 - x2 * y1;
|
||||
out[tx] = (y1-y2)*(x1*y2-x0*y2-x2*y1+x0*y1+x2*y0-x1*y0)/(sqrt(sq(y2-y1)+sq(x2-x1))
|
||||
*abs(x1*y2-x0*y2-x2*y1+x0*y1+x2*y0-x1*y0));
|
||||
|
||||
out[ty] = (x2-x1)*(x1*y2-x0*y2-x2*y1+x0*y1+x2*y0-x1*y0)/(sqrt(sq(y2-y1)+sq(x2-x1))
|
||||
*abs(x1*y2-x0*y2-x2*y1+x0*y1+x2*y0-x1*y0));
|
||||
|
||||
out[lp1x] = (x2-x1)* abs(x1 * y2 - x0 * y2 - x2 * y1 + x0 * y1 + x2 * y0 - x1 * y0)/p(sq(y2-y1)+sq(x2-x1), 3/2)
|
||||
+(y2-y0)*(x1*y2-x0*y2-x2*y1+x0*y1+x2*y0-x1*y0)/(sqrt(sq(y2-y1)+sq(x2-x1))*Math.abs(x1 * y2 - x0 * y2 - x2 * y1 + x0 * y1 + x2 * y0 - x1 * y0));
|
||||
|
||||
out[lp1y] = (y2-y1)*abs(x1*y2-x0*y2-x2*y1+x0*y1+x2*y0-x1*y0)/p(sq(y2-y1)+sq(x2-x1),3/2)
|
||||
+(x0-x2)*(x1*y2-x0*y2-x2*y1+x0*y1+x2*y0-x1*y0)/(sqrt(sq(y2-y1)+sq(x2-x1))*
|
||||
abs(x1*y2-x0*y2-x2*y1+x0*y1+x2*y0-x1*y0));
|
||||
|
||||
out[lp2x] = (y0-y1)*(x1*y2-x0*y2-x2*y1+x0*y1+x2*y0-x1*y0)/(sqrt(sq(y2-y1)+sq(x2-x1))
|
||||
*abs(x1*y2-x0*y2-x2*y1+x0*y1+x2*y0-x1*y0))-(x2-x1)*abs(x1*y2-x0*y2-x2*y1+x0*y1
|
||||
+x2*y0-x1*y0)/p(sq(y2-y1)+sq(x2-x1), 3/2);
|
||||
|
||||
out[lp2y] = (x1-x0)*(x1*y2-x0*y2-x2*y1+x0*y1+x2*y0-x1*y0)/(sqrt(sq(y2-y1)+sq(x2-x1))
|
||||
*abs(x1*y2-x0*y2-x2*y1+x0*y1+x2*y0-x1*y0))-(y2-y1)*abs(x1*y2-x0*y2-x2*y1+x0*y1
|
||||
+x2*y0-x1*y0)/p(sq(y2-y1)+sq(x2-x1),3/2);
|
||||
|
||||
// if (area < 0) {
|
||||
// for (int i = 0; i < 6; i++) {
|
||||
// out[i] *= -1;
|
||||
// }
|
||||
// }
|
||||
}
|
||||
|
||||
private double p(double v, int i) {
|
||||
return Math.pow(v, i);
|
||||
}
|
||||
|
||||
private double sq(double a) {
|
||||
return a*a;
|
||||
}
|
||||
|
||||
public void gradient2(double[] out) {
|
||||
double x0 = p0x(), x1 = p1x(), x2 = p2x();
|
||||
double y0 = p0y(), y1 = p1y(), y2 = p2y();
|
||||
double dx = x2 - x1;
|
||||
double dy = y2 - y1;
|
||||
double d2 = dx * dx + dy * dy;
|
||||
double d = sqrt(d2);
|
||||
double area = -x0 * dy + y0 * dx + x1 * y2 - x2 * y1;
|
||||
out[tx] = ((y1 - y2) / d);
|
||||
out[ty] = ((x2 - x1) / d);
|
||||
out[lp1x] = (((y2 - y0) * d + (dx / d) * area) / d2);
|
||||
out[lp1y] = (((x0 - x2) * d + (dy / d) * area) / d2);
|
||||
out[lp2x] = (((y0 - y1) * d - (dx / d) * area) / d2);
|
||||
out[lp2y] = (((x1 - x0) * d - (dy / d) * area) / d2);
|
||||
|
||||
if (area < 0) {
|
||||
for (int i = 0; i < 6; i++) {
|
||||
out[i] *= -1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public Param[] getParams() {
|
||||
return params;
|
||||
}
|
||||
|
||||
@Override
|
||||
public int pSize() {
|
||||
return 6;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
|
@ -53,20 +53,6 @@ public class Parallel implements Constraint {
|
|||
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());
|
||||
|
|
@ -141,7 +127,6 @@ public class Parallel implements Constraint {
|
|||
return 8;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void set(double[] input) {
|
||||
params[l1p1x].set(input[l1p1x]);
|
||||
params[l1p1y].set(input[l1p1y]);
|
||||
|
|
|
|||
|
|
@ -53,20 +53,6 @@ public class Perpendicular implements Constraint {
|
|||
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());
|
||||
|
|
@ -178,7 +164,6 @@ public class Perpendicular implements Constraint {
|
|||
return 8;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void set(double[] input) {
|
||||
params[l1p1x].set(input[l1p1x]);
|
||||
params[l1p1y].set(input[l1p1y]);
|
||||
|
|
|
|||
|
|
@ -1,156 +0,0 @@
|
|||
package cad.gcs.constr;
|
||||
|
||||
import cad.gcs.Constraint;
|
||||
import cad.gcs.Param;
|
||||
import cad.math.Vector;
|
||||
|
||||
public class X 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 X(
|
||||
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();
|
||||
}
|
||||
|
||||
public double valueX() {
|
||||
return (params[l2p2x].get() - params[l2p1x].get()) * .5 - (params[l1p2x].get() - params[l1p1x].get()) * .5;
|
||||
}
|
||||
|
||||
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];
|
||||
_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 v = valueX() + valueY();
|
||||
return v;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void gradient(double[] out) {
|
||||
|
||||
double valueX = valueX();
|
||||
double valueY = valueY();
|
||||
|
||||
Vector p1 = new Vector();
|
||||
Vector p2 = new Vector();
|
||||
Vector p3 = new Vector();
|
||||
Vector p4 = new Vector();
|
||||
|
||||
out(p1, p2, p3, p4);
|
||||
|
||||
Vector da = p2.minus(p1);
|
||||
Vector db = p4.minus(p3);
|
||||
|
||||
Vector g1 = p1.multi(valueX, valueY, 0);
|
||||
Vector g2 = p2.multi(valueX, valueY, 0).multi(-1);
|
||||
Vector g3 = p3.multi(valueX, valueY, 0).multi(-1);
|
||||
Vector g4 = p4.multi(valueX, valueY, 0);
|
||||
|
||||
out[l1p1x] = g1.x; // = dx2
|
||||
out[l1p1y] = g1.y; // = dx2
|
||||
|
||||
out[l1p2x] = g2.x;
|
||||
out[l1p2y] = g2.y;
|
||||
|
||||
out[l2p1x] = g3.x;
|
||||
out[l2p1y] = g3.y;
|
||||
|
||||
out[l2p2x] = g4.x;
|
||||
out[l2p2y] = g4.y;
|
||||
}
|
||||
|
||||
|
||||
public double angle() {
|
||||
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());
|
||||
//dot product shows how the lines off to be perpendicular
|
||||
double xl = Math.abs(dx1 * dx1 - dx2 * dx2);
|
||||
double yl = Math.abs(dy1*dy1 - dy2*dy2);
|
||||
double off = (dx1 * dx2 + dy1 * dy2) / (xl*yl);
|
||||
|
||||
return Math.acos(off) / Math.PI * 180;
|
||||
}
|
||||
|
||||
private void step(int px, int py, double gx, double gy, double alpha) {
|
||||
Vector dd = new Vector(gx, gy).normalize().multi(alpha);
|
||||
Vector n = new Vector(params[px].get(), params[py].get()).plus(dd);
|
||||
params[px].set(n.x);
|
||||
params[py].set(n.y);
|
||||
}
|
||||
|
||||
@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]);
|
||||
}
|
||||
|
||||
}
|
||||
Loading…
Reference in a new issue