mirror of
https://github.com/xibyte/jsketcher
synced 2025-12-24 01:15:25 +01:00
some success with non linear optimization
This commit is contained in:
Val Erastov
parent
c4a79d2078
commit
789d9f0aee
23 changed files with 1725 additions and 76 deletions
26
misc/perp
Normal file
26
misc/perp
Normal file
|
|
@ -0,0 +1,26 @@
|
|||
pkg load optim;
|
||||
|
||||
function y = f (x)
|
||||
|
||||
y(1) = (
|
||||
(x(3) - x(1)) * (x(7) - x(5)) +
|
||||
(x(4) - x(2)) * (x(8) - x(6))
|
||||
) ^ 2;
|
||||
y(2) = (
|
||||
(x(11) - x(9)) * (x(7) - x(5)) +
|
||||
(x(12) - x(10)) * (x(8) - x(6))
|
||||
) ^ 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 = x0;
|
||||
l1 = [x(1), x(2); x(3), x(4)];
|
||||
l2 = [x(5), x(6); x(7), x(8)];
|
||||
l3 = [x(9), x(10); x(11), x(12)];
|
||||
|
||||
plot(l1(:,1), l1(:,2), l2(:,1), l2(:,2), l3(:,1), l3(:,2),'-');
|
||||
31
misc/simple.m
Normal file
31
misc/simple.m
Normal file
|
|
@ -0,0 +1,31 @@
|
|||
function y = angle (x)
|
||||
|
||||
a = [x(3) - x(1); x(4) - x(2)];
|
||||
b = [x(7) - x(5); x(8) - x(6)];
|
||||
|
||||
y = acos(dot(a, b) / (norm(a) * norm(b))) / pi * 180;
|
||||
|
||||
endfunction
|
||||
|
||||
function y = f (x)
|
||||
y(1) = (
|
||||
(x(3) - x(1)) * (x(7) - x(5)) +
|
||||
(x(4) - x(2)) * (x(8) - x(6))
|
||||
) ^ 2;
|
||||
endfunction
|
||||
|
||||
x0 = [100, 100, 600, 600, 700, 600, 900, 100];
|
||||
x = sqp(x0, @f);
|
||||
|
||||
#pkg load optim;
|
||||
#x = bfgsmin('f', {reshape(x0, 8,1)}); #WORKS!
|
||||
|
||||
|
||||
l1 = [x(1), x(2); x(3), x(4)];
|
||||
l2 = [x(5), x(6); x(7), x(8)];
|
||||
|
||||
plot(l1(:,1), l1(:,2), l2(:,1), l2(:,2), '-');
|
||||
|
||||
|
||||
#d = (x(3) - x(1)) * (x(7) - x(5)) + (x(4) - x(2)) * (x(8) - x(6)) ;
|
||||
disp("Angle: "), disp(angle(x));
|
||||
|
|
@ -1,15 +1,40 @@
|
|||
package cad.fx;
|
||||
|
||||
import cad.gcs.Constraint;
|
||||
import cad.gcs.GradientDescent;
|
||||
import cad.gcs.GradientDescent2;
|
||||
import cad.gcs.GradientDescent3;
|
||||
import cad.gcs.Param;
|
||||
import cad.gcs.Solver;
|
||||
import cad.gcs.constr.Constraint2;
|
||||
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.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 java.net.URL;
|
||||
import java.util.Arrays;
|
||||
|
|
@ -35,33 +60,135 @@ public class App2DCtrl implements Initializable {
|
|||
content.getChildren().addAll(l1, l2);
|
||||
|
||||
|
||||
|
||||
|
||||
solve.setOnAction(event -> {
|
||||
|
||||
Vector a1 = new Vector(l1.getStartX(), l1.getStartY());
|
||||
Vector b1 = new Vector(l1.getEndX(), l1.getEndY());
|
||||
Vector a2 = new Vector(l2.getStartX(), l2.getStartY());
|
||||
Vector b2 = new Vector(l2.getEndX(), l2.getEndY());
|
||||
Perpendicular perpendicular = new Perpendicular(a1, b1, a2, b2);
|
||||
Vector as = new Vector(l1.getStartX(), l1.getStartY());
|
||||
Vector ae = new Vector(l1.getEndX(), l1.getEndY());
|
||||
Vector bs = new Vector(l2.getStartX(), l2.getStartY());
|
||||
Vector be = new Vector(l2.getEndX(), l2.getEndY());
|
||||
|
||||
List<Constraint> parallels = Arrays.<Constraint>asList(perpendicular);
|
||||
Solver.SubSystem subSystem = new Solver.SubSystem(parallels);
|
||||
Solver.solve_DL(subSystem);
|
||||
|
||||
perpendicular.out(a1, b1, a2, b2);
|
||||
Param l1p1x = new Param(l1.getStartX());
|
||||
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());
|
||||
|
||||
l1.setStartX(a1.x);
|
||||
l1.setStartY(a1.y);
|
||||
l1.setEndX(b1.x);
|
||||
l1.setEndY(b1.y);
|
||||
|
||||
l2.setStartX(a2.x);
|
||||
l2.setStartY(a2.y);
|
||||
l2.setEndX(b2.x);
|
||||
l2.setEndY(b2.y);
|
||||
Perpendicular2 perpendicular2 = new Perpendicular2(as, ae, bs, be);
|
||||
|
||||
|
||||
Perpendicular perpendicular = new Perpendicular(
|
||||
l1p1x,
|
||||
l1p1y,
|
||||
l1p2x,
|
||||
l1p2y,
|
||||
l2p1x,
|
||||
l2p1y,
|
||||
l2p2x,
|
||||
l2p2y
|
||||
);
|
||||
|
||||
|
||||
XY xy = new XY(as, new Vector(100, 100));
|
||||
|
||||
X x = new X(
|
||||
l1p1x,
|
||||
l1p1y,
|
||||
l1p2x,
|
||||
l1p2y,
|
||||
l2p1x,
|
||||
l2p1y,
|
||||
l2p2x,
|
||||
l2p2y
|
||||
);
|
||||
|
||||
List<Constraint> constrs = Arrays.<Constraint>asList(perpendicular);
|
||||
Solver.SubSystem subSystem = new Solver.SubSystem(constrs);
|
||||
// Solver.optimize(subSystem);
|
||||
//
|
||||
|
||||
|
||||
// while (subSystem.error() > 0.0001 ) {
|
||||
Solver.solve_LM(subSystem);
|
||||
// }
|
||||
|
||||
// solveGC(subSystem);
|
||||
|
||||
java.lang.System.out.println(perpendicular.angle());
|
||||
|
||||
Constraint2 constr = perpendicular2;
|
||||
// Constraint2 constr = xy;
|
||||
|
||||
// Constraint constr = perpendicular;
|
||||
// GradientDescent.solve(constr);
|
||||
// perpendicular.out(a1, b1, a2, b2);
|
||||
|
||||
// GradientDescent2.solve(constr);
|
||||
|
||||
|
||||
// l1.setStartX(as.x);
|
||||
// l1.setStartY(as.y);
|
||||
// l1.setEndX(ae.x);
|
||||
// l1.setEndY(ae.y);
|
||||
//
|
||||
// l2.setStartX(bs.x);
|
||||
// l2.setStartY(bs.y);
|
||||
// l2.setEndX(be.x);
|
||||
// l2.setEndY(be.y);
|
||||
|
||||
l1.setStartX(l1p1x.get());
|
||||
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());
|
||||
});
|
||||
|
||||
}
|
||||
|
||||
private void solveGC(final Solver.SubSystem subSystem) {
|
||||
GaussNewtonOptimizer optimizer = new GaussNewtonOptimizer((iteration, previous, current) -> {
|
||||
return subSystem.value() < 0.00000001;
|
||||
}) {
|
||||
|
||||
@Override
|
||||
protected double[] computeResiduals(double[] objectiveValue) {
|
||||
TDoubleList residual = subSystem.calcResidual();
|
||||
return residual.toArray();
|
||||
}
|
||||
|
||||
};
|
||||
double[] wieght = new double[subSystem.cSize()];
|
||||
Arrays.fill(wieght, 1);
|
||||
optimizer.optimize(
|
||||
new MaxEval(10000),
|
||||
new MaxIter(10000),
|
||||
new InitialGuess(subSystem.getParams().toArray()),
|
||||
new Target(new double[subSystem.cSize()]),
|
||||
new Weight(wieght),
|
||||
new ModelFunctionJacobian(point -> {
|
||||
subSystem.setParams(point);
|
||||
return subSystem.makeJacobi().getData();
|
||||
}),
|
||||
new ModelFunction(new MultivariateVectorFunction() {
|
||||
@Override
|
||||
public double[] value(double[] point) throws IllegalArgumentException {
|
||||
subSystem.setParams(point);
|
||||
return subSystem.getValues().toArray();
|
||||
}
|
||||
})
|
||||
|
||||
);
|
||||
}
|
||||
|
||||
private void solve(ActionEvent e) {
|
||||
|
||||
// UnconstrainedLeastSquares opt = FactoryOptimization.leastSquaresTrustRegion(100, RegionStepType.DOG_LEG_FTF, false);
|
||||
|
|
|
|||
|
|
@ -112,7 +112,7 @@ public class CadContext {
|
|||
}
|
||||
|
||||
Sketch sketch = selection.csgNode.getSketch(selection.poly);
|
||||
Vector dir = sketch.owner.normal.scale(height);
|
||||
Vector dir = sketch.owner.normal.multi(height);
|
||||
for (List<Vector> polygon : sketch.polygons) {
|
||||
if (polygon.isEmpty()) {
|
||||
continue;
|
||||
|
|
|
|||
|
|
@ -149,7 +149,7 @@ public class Utils3D {
|
|||
|
||||
public static List<Polygon> createCube(double width) {
|
||||
Polygon square = createSquare(width);
|
||||
return Polygon.extrude(square, square.normal.scale(width));
|
||||
return Polygon.extrude(square, square.normal.multi(width));
|
||||
}
|
||||
|
||||
public static Polygon createSquare(double width) {
|
||||
|
|
|
|||
|
|
@ -1,11 +1,11 @@
|
|||
package cad.gcs;
|
||||
|
||||
import java.util.List;
|
||||
|
||||
public interface Constraint extends System {
|
||||
|
||||
double error();
|
||||
|
||||
void step(double alpha);
|
||||
|
||||
void set(double[] input);
|
||||
|
||||
}
|
||||
|
|
|
|||
56
src/cad/gcs/GradientDescent.java
Normal file
56
src/cad/gcs/GradientDescent.java
Normal file
|
|
@ -0,0 +1,56 @@
|
|||
package cad.gcs;
|
||||
|
||||
import cad.gcs.constr.Perpendicular;
|
||||
import org.apache.commons.math3.linear.ArrayRealVector;
|
||||
import org.apache.commons.math3.linear.RealVector;
|
||||
|
||||
public class GradientDescent {
|
||||
|
||||
static double EPS = 0.0000001;
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
private static double value(Constraint constr) {
|
||||
double err = constr.error();
|
||||
return err * err;
|
||||
}
|
||||
}
|
||||
54
src/cad/gcs/GradientDescent2.java
Normal file
54
src/cad/gcs/GradientDescent2.java
Normal file
|
|
@ -0,0 +1,54 @@
|
|||
package cad.gcs;
|
||||
|
||||
import cad.gcs.constr.Constraint2;
|
||||
import cad.gcs.constr.Perpendicular2;
|
||||
import cad.math.Vector;
|
||||
|
||||
import java.lang.*;
|
||||
import java.util.List;
|
||||
|
||||
public class GradientDescent2 {
|
||||
|
||||
private static final double DBL_EPSILON = Double.MIN_VALUE;
|
||||
|
||||
static double EPS = 0.0000000001;
|
||||
|
||||
public static void solve(Constraint2 constr) {
|
||||
|
||||
|
||||
double last = value(constr);
|
||||
|
||||
double alpha = .01;
|
||||
|
||||
List<Vector> params = constr.params();
|
||||
for (int i = 0; i < 100000; i++) {
|
||||
|
||||
|
||||
List<Vector> grad = constr.gradient();
|
||||
for (int j = 0; j < grad.size(); j++) {
|
||||
Vector gr = grad.get(j);
|
||||
Vector param = params.get(j);
|
||||
Vector step = gr.normalize().multi(alpha);
|
||||
param._plus(step);
|
||||
}
|
||||
double err = value(constr);
|
||||
|
||||
java.lang.System.out.println(constr.debug() + "===" + err + "====>" + alpha );
|
||||
if (err < last) {
|
||||
|
||||
} else {
|
||||
if (alpha < EPS) {
|
||||
return;
|
||||
} else {
|
||||
alpha /= 3;
|
||||
}
|
||||
}
|
||||
last = err;
|
||||
}
|
||||
}
|
||||
|
||||
private static double value(Constraint2 constr) {
|
||||
double err = constr.error();
|
||||
return Math.abs(err);
|
||||
}
|
||||
}
|
||||
78
src/cad/gcs/GradientDescent3.java
Normal file
78
src/cad/gcs/GradientDescent3.java
Normal file
|
|
@ -0,0 +1,78 @@
|
|||
package cad.gcs;
|
||||
|
||||
import cad.gcs.constr.Constraint2;
|
||||
import cad.gcs.constr.Perpendicular2;
|
||||
import cad.math.Vector;
|
||||
import gnu.trove.list.TDoubleList;
|
||||
import gnu.trove.list.array.TDoubleArrayList;
|
||||
import gnu.trove.map.hash.TObjectDoubleHashMap;
|
||||
|
||||
import java.lang.*;
|
||||
import java.util.HashMap;
|
||||
import java.util.List;
|
||||
|
||||
public class GradientDescent3 {
|
||||
|
||||
private static final double DBL_EPSILON = Double.MIN_VALUE;
|
||||
|
||||
static double EPS = 0.0000001;
|
||||
|
||||
public static void solve(Constraint2... constrs) {
|
||||
|
||||
TObjectDoubleHashMap<Vector> alphas = new TObjectDoubleHashMap<>();
|
||||
double[] values = new double[constrs.length];
|
||||
double[] calphas = new double[constrs.length];
|
||||
|
||||
for (int k = 0; k < constrs.length; k++) {
|
||||
Constraint2 constr = constrs[k];
|
||||
for (Vector p : constr.params()) {
|
||||
alphas.put(p, 1);
|
||||
}
|
||||
values[k] = (value(constr));
|
||||
}
|
||||
|
||||
|
||||
for (int i = 0; i < 100000; i++) {
|
||||
for (int k = 0; k < constrs.length; k++) {
|
||||
Constraint2 constr = constrs[k];
|
||||
List<Vector> params = constr.params();
|
||||
double calpha = calphas[k];
|
||||
List<Vector> grad = constr.gradient();
|
||||
for (int j = 0; j < grad.size(); j++) {
|
||||
Vector gr = grad.get(j);
|
||||
Vector param = params.get(j);
|
||||
double alpha = alphas.get(param);
|
||||
Vector step = gr.normalize().multi(alpha);
|
||||
param._plus(step);
|
||||
}
|
||||
double err = value(constr);
|
||||
double last = values[k];
|
||||
// java.lang.System.out.println(constr.debug() + "===" + err + "====>" + alpha );
|
||||
if (err < last) {
|
||||
} else {
|
||||
boolean divergence = true;
|
||||
for (double a : calphas) {
|
||||
if (a >= EPS) {
|
||||
divergence = false;
|
||||
}
|
||||
}
|
||||
if (divergence) {
|
||||
return;
|
||||
} else {
|
||||
calpha /= 3;
|
||||
calphas[k] = calpha;
|
||||
for (Vector param : params) {
|
||||
alphas.put(param, Math.min(alphas.get(param), calpha));
|
||||
}
|
||||
}
|
||||
}
|
||||
values[k] = err;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static double value(Constraint2 constr) {
|
||||
double err = constr.error();
|
||||
return Math.abs(err);
|
||||
}
|
||||
}
|
||||
256
src/cad/gcs/LM.java
Normal file
256
src/cad/gcs/LM.java
Normal file
|
|
@ -0,0 +1,256 @@
|
|||
// levenberg-marquardt in java
|
||||
//
|
||||
// To use this, implement the functions in the LMfunc interface.
|
||||
//
|
||||
// This library uses simple matrix routines from the JAMA java matrix package,
|
||||
// which is in the public domain. Reference:
|
||||
// http://math.nist.gov/javanumerics/jama/
|
||||
// (JAMA has a matrix object class. An earlier library JNL, which is no longer
|
||||
// available, represented matrices as low-level arrays. Several years
|
||||
// ago the performance of JNL matrix code was better than that of JAMA,
|
||||
// though improvements in java compilers may have fixed this by now.)
|
||||
//
|
||||
// One further recommendation would be to use an inverse based
|
||||
// on Choleski decomposition, which is easy to implement and
|
||||
// suitable for the symmetric inverse required here. There is a choleski
|
||||
// routine at idiom.com/~zilla.
|
||||
//
|
||||
// If you make an improved version, please consider adding your
|
||||
// name to it ("modified by ...") and send it back to me
|
||||
// (and put it on the web).
|
||||
//
|
||||
// ----------------------------------------------------------------
|
||||
//
|
||||
// This library is free software; you can redistribute it and/or
|
||||
// modify it under the terms of the GNU Library General Public
|
||||
// License as published by the Free Software Foundation; either
|
||||
// version 2 of the License, or (at your option) any later version.
|
||||
//
|
||||
// This library is distributed in the hope that it will be useful,
|
||||
// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
// Library General Public License for more details.
|
||||
//
|
||||
// You should have received a copy of the GNU Library General Public
|
||||
// License along with this library; if not, write to the
|
||||
// Free Software Foundation, Inc., 59 Temple Place - Suite 330,
|
||||
// Boston, MA 02111-1307, USA.
|
||||
//
|
||||
// initial author contact info:
|
||||
// jplewis www.idiom.com/~zilla zilla # computer.org, #=at
|
||||
//
|
||||
// Improvements by:
|
||||
// dscherba www.ncsa.uiuc.edu/~dscherba
|
||||
// Jonathan Jackson j.jackson # ucl.ac.uk
|
||||
|
||||
|
||||
package cad.gcs;
|
||||
|
||||
// see comment above
|
||||
|
||||
import Jama.*;
|
||||
|
||||
/**
|
||||
* Levenberg-Marquardt, implemented from the general description
|
||||
* in Numerical Recipes (NR), then tweaked slightly to mostly
|
||||
* match the results of their code.
|
||||
* Use for nonlinear least squares assuming Gaussian errors.
|
||||
* <p>
|
||||
* TODO this holds some parameters fixed by simply not updating them.
|
||||
* this may be ok if the number if fixed parameters is small,
|
||||
* but if the number of varying parameters is larger it would
|
||||
* be more efficient to make a smaller hessian involving only
|
||||
* the variables.
|
||||
* <p>
|
||||
* The NR code assumes a statistical context, e.g. returns
|
||||
* covariance of parameter errors; we do not do this.
|
||||
*/
|
||||
public final class LM {
|
||||
|
||||
/**
|
||||
* calculate the current sum-squared-error
|
||||
* (Chi-squared is the distribution of squared Gaussian errors,
|
||||
* thus the name)
|
||||
*/
|
||||
static double chiSquared(double[] a, double[] y, double[] s,
|
||||
LMfunc f) {
|
||||
int npts = y.length;
|
||||
double sum = 0.;
|
||||
|
||||
double[] val = f.val(a);
|
||||
for (int i = 0; i < npts; i++) {
|
||||
double d = y[i] - val[i];
|
||||
d = d / s[i];
|
||||
sum = sum + (d * d);
|
||||
}
|
||||
|
||||
return sum;
|
||||
} //chiSquared
|
||||
|
||||
|
||||
/**
|
||||
* Minimize E = sum {(y[k] - f(x[k],a)) / s[k]}^2
|
||||
* The individual errors are optionally scaled by s[k].
|
||||
* Note that LMfunc implements the value and gradient of f(x,a),
|
||||
* NOT the value and gradient of E with respect to a!
|
||||
*
|
||||
* @param y corresponding array of values
|
||||
* @param a the parameters/state of the model
|
||||
* @param vary false to indicate the corresponding a[k] is to be held fixed
|
||||
* @param s sigma^2 for point i
|
||||
* @param lambda blend between steepest descent (lambda high) and
|
||||
* jump to bottom of quadratic (lambda zero).
|
||||
* Start with 0.001.
|
||||
* @param termepsilon termination accuracy (0.01)
|
||||
* @param maxiter stop and return after this many iterations if not done
|
||||
* @param verbose set to zero (no prints), 1, 2
|
||||
* @return the new lambda for future iterations.
|
||||
* Can use this and maxiter to interleave the LM descent with some other
|
||||
* task, setting maxiter to something small.
|
||||
*/
|
||||
public static double solve(double[] a, double[] y, double[] s,
|
||||
boolean[] vary, LMfunc f,
|
||||
double lambda, double termepsilon, int maxiter,
|
||||
int verbose)
|
||||
throws Exception {
|
||||
int npts = y.length;
|
||||
int nparm = a.length;
|
||||
assert s.length == npts;
|
||||
if (verbose > 0) {
|
||||
out().print(" a[" + a.length + "]");
|
||||
out().println(" y[" + y.length + "]");
|
||||
}
|
||||
|
||||
double e0 = chiSquared(a, y, s, f);
|
||||
//double lambda = 0.001;
|
||||
boolean done = false;
|
||||
|
||||
// g = gradient, H = hessian, d = step to minimum
|
||||
// H d = -g, solve for d
|
||||
double[][] H = new double[nparm][nparm];
|
||||
double[] g = new double[nparm];
|
||||
//double[] d = new double[nparm];
|
||||
|
||||
double[] oos2 = new double[s.length];
|
||||
for (int i = 0; i < npts; i++) {
|
||||
oos2[i] = 1. / (s[i] * s[i]);
|
||||
}
|
||||
|
||||
int iter = 0;
|
||||
int term = 0; // termination count test
|
||||
|
||||
do {
|
||||
++iter;
|
||||
|
||||
// hessian approximation
|
||||
for (int r = 0; r < nparm; r++) {
|
||||
for (int c = 0; c < nparm; c++) {
|
||||
for (int i = 0; i < npts; i++) {
|
||||
if (i == 0) {
|
||||
H[r][c] = 0.;
|
||||
}
|
||||
double[] grad = f.grad(a);
|
||||
H[r][c] += (oos2[i] * grad[r] * grad[c]);
|
||||
} //npts
|
||||
} //c
|
||||
} //r
|
||||
|
||||
// boost diagonal towards gradient descent
|
||||
for (int r = 0; r < nparm; r++) {
|
||||
H[r][r] *= (1. + lambda);
|
||||
}
|
||||
|
||||
// gradient
|
||||
for (int r = 0; r < nparm; r++) {
|
||||
for (int i = 0; i < npts; i++) {
|
||||
if (i == 0) {
|
||||
g[r] = 0.;
|
||||
}
|
||||
double[] grad = f.grad(a);
|
||||
double[] val = f.val(a);
|
||||
g[r] += (oos2[i] * (y[i] - val[i]) * grad[r]);
|
||||
}
|
||||
} //npts
|
||||
|
||||
// scale (for consistency with NR, not necessary)
|
||||
if (false) {
|
||||
for (int r = 0; r < nparm; r++) {
|
||||
g[r] = -0.5 * g[r];
|
||||
for (int c = 0; c < nparm; c++) {
|
||||
H[r][c] *= 0.5;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// solve H d = -g, evaluate error at new location
|
||||
//double[] d = DoubleMatrix.solve(H, g);
|
||||
double[] d = (new Matrix(H)).lu().solve(new Matrix(g, nparm)).getRowPackedCopy();
|
||||
//double[] na = DoubleVector.add(a, d);
|
||||
double[] na = (new Matrix(a, nparm)).plus(new Matrix(d, nparm)).getRowPackedCopy();
|
||||
double e1 = chiSquared(na, y, s, f);
|
||||
|
||||
if (verbose > 0) {
|
||||
out().println("\n\niteration " + iter + " lambda = " + lambda);
|
||||
out().print("a = ");
|
||||
(new Matrix(a, nparm)).print(10, 2);
|
||||
if (verbose > 1) {
|
||||
out().print("H = ");
|
||||
(new Matrix(H)).print(10, 2);
|
||||
out().print("g = ");
|
||||
(new Matrix(g, nparm)).print(10, 2);
|
||||
out().print("d = ");
|
||||
(new Matrix(d, nparm)).print(10, 2);
|
||||
}
|
||||
out().print("e0 = " + e0 + ": ");
|
||||
out().print("moved from ");
|
||||
(new Matrix(a, nparm)).print(10, 2);
|
||||
out().print("e1 = " + e1 + ": ");
|
||||
if (e1 < e0) {
|
||||
out().print("to ");
|
||||
(new Matrix(na, nparm)).print(10, 2);
|
||||
} else {
|
||||
out().println("move rejected");
|
||||
}
|
||||
}
|
||||
|
||||
// termination test (slightly different than NR)
|
||||
if (Math.abs(e1 - e0) > termepsilon) {
|
||||
term = 0;
|
||||
} else {
|
||||
term++;
|
||||
if (term == 4) {
|
||||
out().println("terminating after " + iter + " iterations");
|
||||
done = true;
|
||||
}
|
||||
}
|
||||
if (iter >= maxiter) {
|
||||
done = true;
|
||||
}
|
||||
|
||||
// in the C++ version, found that changing this to e1 >= e0
|
||||
// was not a good idea. See comment there.
|
||||
//
|
||||
if (e1 > e0 || Double.isNaN(e1)) { // new location worse than before
|
||||
lambda *= 10.;
|
||||
} else { // new location better, accept new parameters
|
||||
lambda *= 0.1;
|
||||
e0 = e1;
|
||||
// simply assigning a = na will not get results copied back to caller
|
||||
for (int i = 0; i < nparm; i++) {
|
||||
if (vary[i]) {
|
||||
a[i] = na[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} while (!done);
|
||||
|
||||
return lambda;
|
||||
} //solve
|
||||
|
||||
private static java.io.PrintStream out() {
|
||||
return java.lang.System.out;
|
||||
}
|
||||
|
||||
//----------------------------------------------------------------
|
||||
} //LM
|
||||
26
src/cad/gcs/LMfunc.java
Normal file
26
src/cad/gcs/LMfunc.java
Normal file
|
|
@ -0,0 +1,26 @@
|
|||
// LMfunc.java
|
||||
|
||||
package cad.gcs;
|
||||
|
||||
/**
|
||||
* Caller implement this interface to specify the
|
||||
* function to be minimized and its gradient.
|
||||
*
|
||||
* Optionally return an initial guess and some test data,
|
||||
* though the LM.java only uses this in its optional main() test program.
|
||||
* Return null if these are not needed.
|
||||
*/
|
||||
public interface LMfunc
|
||||
{
|
||||
|
||||
/**
|
||||
* x is a single point, but domain may be mulidimensional
|
||||
*/
|
||||
double[] val(double[] a);
|
||||
|
||||
/**
|
||||
* return the kth component of the gradient df(x,a)/da_k
|
||||
*/
|
||||
double[] grad(double[] a);
|
||||
|
||||
} //LMfunc
|
||||
|
|
@ -1,12 +1,18 @@
|
|||
package cad.gcs;
|
||||
|
||||
public abstract class Param {
|
||||
public class Param {
|
||||
|
||||
public final Constraint constraint;
|
||||
public double value;
|
||||
|
||||
protected Param(Constraint constraint) {
|
||||
this.constraint = constraint;
|
||||
public Param(double value) {
|
||||
this.value = value;
|
||||
}
|
||||
|
||||
public abstract double value();
|
||||
public double get() {
|
||||
return value;
|
||||
}
|
||||
|
||||
public double set(double value) {
|
||||
return this.value = value;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -2,15 +2,26 @@ package cad.gcs;
|
|||
|
||||
import gnu.trove.list.TDoubleList;
|
||||
import gnu.trove.list.array.TDoubleArrayList;
|
||||
import org.apache.commons.math3.exception.ConvergenceException;
|
||||
import org.apache.commons.math3.exception.DimensionMismatchException;
|
||||
import org.apache.commons.math3.exception.MathInternalError;
|
||||
import org.apache.commons.math3.exception.util.LocalizedFormats;
|
||||
import org.apache.commons.math3.linear.Array2DRowRealMatrix;
|
||||
import org.apache.commons.math3.linear.ArrayRealVector;
|
||||
import org.apache.commons.math3.linear.BlockRealMatrix;
|
||||
import org.apache.commons.math3.linear.DecompositionSolver;
|
||||
import org.apache.commons.math3.linear.LUDecomposition;
|
||||
import org.apache.commons.math3.linear.QRDecomposition;
|
||||
import org.apache.commons.math3.linear.RealMatrix;
|
||||
import org.apache.commons.math3.linear.SingularMatrixException;
|
||||
|
||||
import java.util.List;
|
||||
|
||||
public class Solver {
|
||||
|
||||
public static final boolean useLU = true;
|
||||
private static final double DBL_EPSILON = Double.MIN_VALUE;
|
||||
|
||||
enum SolveStatus {
|
||||
Success, // Found a solution zeroing the error function
|
||||
Converged, // Found a solution minimizing the error function
|
||||
|
|
@ -164,12 +175,108 @@ public class Solver {
|
|||
|
||||
private static RealMatrix lu(RealMatrix jx, RealMatrix fx) {
|
||||
|
||||
return new cad.gcs.LUDecomposition(jx).solve(fx);
|
||||
|
||||
return new org.apache.commons.math3.linear.LUDecomposition(new Array2DRowRealMatrix(makeSquare(jx.getData()))).getSolver().solve(fx);
|
||||
|
||||
// DoubleMatrix2D solve = new LUDecomposition(new DenseDoubleMatrix2D(jx.getData()))
|
||||
// .solve(new DenseDoubleMatrix2D(fx.getData()));
|
||||
}
|
||||
|
||||
public static double[][] makeSquare(double[][] m) {
|
||||
if (m.length > m[0].length) {
|
||||
for (int r = 0; r < m.length; r++) {
|
||||
double[] row = m[r];
|
||||
m[r] = new double[m.length];
|
||||
java.lang.System.arraycopy(row, 0, m[r], 0, row.length);
|
||||
}
|
||||
} else {
|
||||
double[][] _m = new double[m[0].length][];
|
||||
for (int r = 0; r < m.length; r++) {
|
||||
_m[r] = m[r];
|
||||
}
|
||||
for (int r = m.length; r < _m.length; r++) {
|
||||
_m[r] = new double[m[0].length];
|
||||
}
|
||||
m = _m;
|
||||
}
|
||||
return m;
|
||||
}
|
||||
|
||||
|
||||
public static void optimize(SubSystem subSystem) {
|
||||
|
||||
final int cSize = subSystem.cSize();
|
||||
|
||||
final double[] currentPoint = subSystem.getParams().toArray();
|
||||
final int pSize = currentPoint.length;
|
||||
|
||||
// iterate until convergence is reached
|
||||
double[] current = null;
|
||||
int iter = 0;
|
||||
for (boolean converged = false; !converged; ) {
|
||||
++iter;
|
||||
|
||||
// evaluate the objective function and its jacobian
|
||||
// Value of the objective function at "currentPoint".
|
||||
final double[] currentResiduals = subSystem.calcResidual().toArray();
|
||||
final RealMatrix jacobian = new Array2DRowRealMatrix(makeSquare(subSystem.makeJacobi().getData()));
|
||||
|
||||
// build the linear problem
|
||||
final double[] b = new double[pSize];
|
||||
final double[][] a = new double[pSize][pSize];
|
||||
for (int i = 0; i < cSize; ++i) {
|
||||
|
||||
final double[] grad = jacobian.getRow(i);
|
||||
final double weight = 1;
|
||||
final double residual = currentResiduals[i];
|
||||
|
||||
// compute the normal equation
|
||||
final double wr = weight * residual;
|
||||
for (int j = 0; j < pSize; ++j) {
|
||||
b[j] += wr * grad[j];
|
||||
}
|
||||
|
||||
// build the contribution matrix for measurement i
|
||||
for (int k = 0; k < pSize; ++k) {
|
||||
double[] ak = a[k];
|
||||
double wgk = weight * grad[k];
|
||||
for (int l = 0; l < pSize; ++l) {
|
||||
ak[l] += wgk * grad[l];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
try {
|
||||
// solve the linearized least squares problem
|
||||
RealMatrix mA = new BlockRealMatrix(a);
|
||||
DecompositionSolver solver = useLU ?
|
||||
new org.apache.commons.math3.linear.LUDecomposition(jacobian).getSolver() :
|
||||
new QRDecomposition(jacobian).getSolver();
|
||||
final double[] dX = solver.solve(new ArrayRealVector(currentPoint).mapMultiply(-1)).toArray();
|
||||
// update the estimated parameters
|
||||
for (int i = 0; i < pSize; ++i) {
|
||||
currentPoint[i] += dX[i];
|
||||
}
|
||||
subSystem.setParams(currentPoint);
|
||||
} catch (SingularMatrixException e) {
|
||||
throw new ConvergenceException(LocalizedFormats.UNABLE_TO_SOLVE_SINGULAR_PROBLEM);
|
||||
}
|
||||
|
||||
// Check convergence.
|
||||
if (iter != 0) {
|
||||
// converged = checker.converged(iter, previous, current);
|
||||
// if (converged) {
|
||||
// setCost(computeCost(currentResiduals));
|
||||
// return current;
|
||||
// }
|
||||
if (subSystem.value() < 0.0001) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
// Must never happen.
|
||||
throw new MathInternalError();
|
||||
}
|
||||
|
||||
public RealMatrix solve(RealMatrix b, int[] pivot, double[][] lu) {
|
||||
|
||||
final int m = pivot.length;
|
||||
|
|
@ -218,7 +325,334 @@ public class Solver {
|
|||
|
||||
return new Array2DRowRealMatrix(bp, false);
|
||||
}
|
||||
|
||||
|
||||
public static SolveStatus solve_LM(SubSystem subsys) {
|
||||
int xsize = subsys.pSize();
|
||||
int csize = subsys.cSize();
|
||||
|
||||
if (xsize == 0) {
|
||||
return SolveStatus.Success;
|
||||
}
|
||||
|
||||
RealMatrix e = mtrx(csize), e_new = mtrx(csize); // vector of all function errors (every constraint is one function)
|
||||
RealMatrix J = mtrx(csize, xsize); // Jacobi of the subsystem
|
||||
RealMatrix A = mtrx(xsize, xsize);
|
||||
RealMatrix x = mtrx(xsize), h = mtrx(xsize), x_new = mtrx(xsize), g = mtrx(xsize);
|
||||
double[] diag_A;
|
||||
|
||||
// subsys.redirectParams();
|
||||
|
||||
subsys.fillParams(x);
|
||||
subsys.calcResidual(e);
|
||||
e = e.scalarMultiply(-1);
|
||||
|
||||
int maxIterNumber = MaxIterations * xsize;
|
||||
double divergingLim = 1e6 * squaredNorm(e) + 1e12;
|
||||
|
||||
double eps = 1e-10, eps1 = 1e-80;
|
||||
double tau = 1e-3;
|
||||
double nu = 2, mu = 0;
|
||||
int iter = 0, stop = 0;
|
||||
for (iter = 0; iter < maxIterNumber && stop == 0; ++iter) {
|
||||
|
||||
// check error
|
||||
double err = squaredNorm(e);
|
||||
if (err <= eps) { // error is small, Success
|
||||
stop = 1;
|
||||
break;
|
||||
} else if (err > divergingLim || err != err) { // check for diverging and NaN
|
||||
stop = 6;
|
||||
break;
|
||||
}
|
||||
|
||||
// J^T J, J^T e
|
||||
subsys.calcJacobi(J);
|
||||
;
|
||||
|
||||
A = J.transpose().multiply(J);
|
||||
g = J.transpose().multiply(e);
|
||||
|
||||
// Compute ||J^T e||_inf
|
||||
double g_inf = infinityNorm(g);
|
||||
diag_A = diagonal(A); // save diagonal entries so that augmentation can be later canceled
|
||||
|
||||
// check for convergence
|
||||
if (g_inf <= eps1) {
|
||||
stop = 2;
|
||||
break;
|
||||
}
|
||||
|
||||
// compute initial damping factor
|
||||
if (iter == 0) {
|
||||
mu = tau * new ArrayRealVector(diag_A).getLInfNorm() ;
|
||||
}
|
||||
|
||||
// determine increment using adaptive damping
|
||||
int k = 0;
|
||||
while (k < 50) {
|
||||
// augment normal equations A = A+uI
|
||||
for (int i = 0; i < xsize; ++i) {
|
||||
A.addToEntry(i, i, mu);
|
||||
}
|
||||
|
||||
//solve augmented functions A*h=-g
|
||||
|
||||
try {
|
||||
h = new LUDecomposition(A).getSolver().solve(g);
|
||||
} catch (Exception ssse) {
|
||||
java.lang.System.out.println( "");
|
||||
return SolveStatus.Success;
|
||||
}
|
||||
|
||||
;
|
||||
double rel_error = (A.multiply(h).subtract(g)).getFrobeniusNorm() / g.getFrobeniusNorm();
|
||||
|
||||
// check if solving works
|
||||
if (rel_error < 1e-5) {
|
||||
|
||||
// restrict h according to maxStep
|
||||
// double scale = subsys.maxStep(h);
|
||||
// if (scale < 1.) {
|
||||
// h = h.scalarMultiply(scale);
|
||||
// }
|
||||
|
||||
// compute par's new estimate and ||d_par||^2
|
||||
x_new = x.add(h);
|
||||
double h_norm = squaredNorm(h);
|
||||
|
||||
if (h_norm <= eps1 * eps1 * x.getFrobeniusNorm()) { // relative change in p is small, stop
|
||||
stop = 3;
|
||||
break;
|
||||
} else if (h_norm >= (x.getFrobeniusNorm() + eps1) / (DBL_EPSILON * DBL_EPSILON)) { // almost singular
|
||||
stop = 4;
|
||||
break;
|
||||
}
|
||||
|
||||
subsys.setParams(x_new);
|
||||
subsys.calcResidual(e_new);
|
||||
e_new = e_new.scalarMultiply(-1);
|
||||
|
||||
double dF = squaredNorm(e) - squaredNorm(e_new);
|
||||
double dL = dot(h, (h.scalarMultiply(mu).add(g)));
|
||||
|
||||
if (dF > 0. && dL > 0.) { // reduction in error, increment is accepted
|
||||
double tmp = 2 * dF / dL - 1.;
|
||||
mu *= Math.max(1. / 3., 1. - tmp * tmp * tmp);
|
||||
nu = 2;
|
||||
|
||||
// update par's estimate
|
||||
x = x_new.copy();
|
||||
e = e_new.copy();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// if this point is reached, either the linear system could not be solved or
|
||||
// the error did not reduce; in any case, the increment must be rejected
|
||||
|
||||
mu *= nu;
|
||||
nu *= 2.0;
|
||||
for (int i = 0; i < xsize; ++i) // restore diagonal J^T J entries
|
||||
{
|
||||
A.setEntry(i, i, diag_A[i]);
|
||||
}
|
||||
|
||||
k++;
|
||||
}
|
||||
if (k > 50) {
|
||||
stop = 7;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (iter >= maxIterNumber) {
|
||||
stop = 5;
|
||||
}
|
||||
|
||||
// subsys.revertParams();
|
||||
|
||||
return (stop == 1) ? SolveStatus.Success : SolveStatus.Failed;
|
||||
}
|
||||
|
||||
private static void identity(RealMatrix m) {
|
||||
for (int i = 0; i < m.getColumnDimension() && i < m.getRowDimension(); i++) {
|
||||
m.setEntry(i, i, 1.0);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static double lineSearch(SubSystem subsys, RealMatrix xdir) {
|
||||
double f1, f2, f3, alpha1, alpha2, alpha3, alphaStar;
|
||||
|
||||
double alphaMax = 1;//subsys.maxStep(xdir);
|
||||
|
||||
int pSize = subsys.pSize();
|
||||
RealMatrix x0 = mtrx(pSize), x = mtrx(pSize);
|
||||
|
||||
//Save initial values
|
||||
subsys.fillParams(x0);
|
||||
|
||||
//Start at the initial position alpha1 = 0
|
||||
alpha1 = 0.;
|
||||
f1 = subsys.error();
|
||||
|
||||
//Take a step of alpha2 = 1
|
||||
alpha2 = 1.;
|
||||
x = x0.add(xdir.scalarMultiply(alpha2));
|
||||
subsys.setParams(x);
|
||||
f2 = subsys.error();
|
||||
|
||||
//Take a step of alpha3 = 2*alpha2
|
||||
alpha3 = alpha2 * 2;
|
||||
x = x0.add(xdir.scalarMultiply(alpha3));
|
||||
subsys .setParams(x);
|
||||
f3 = subsys . error();
|
||||
|
||||
//Now reduce or lengthen alpha2 and alpha3 until the minimum is
|
||||
//Bracketed by the triplet f1>f2<f3
|
||||
while (f2 > f1 || f2 > f3) {
|
||||
if (f2 > f1) {
|
||||
//If f2 is greater than f1 then we shorten alpha2 and alpha3 closer to f1
|
||||
//Effectively both are shortened by a factor of two.
|
||||
alpha3 = alpha2;
|
||||
f3 = f2;
|
||||
alpha2 = alpha2 / 2;
|
||||
x = x0.add( xdir.scalarMultiply(alpha2 ));
|
||||
subsys . setParams(x);
|
||||
f2 = subsys . error();
|
||||
} else if (f2 > f3) {
|
||||
if (alpha3 >= alphaMax) {
|
||||
break;
|
||||
}
|
||||
//If f2 is greater than f3 then we increase alpha2 and alpha3 away from f1
|
||||
//Effectively both are lengthened by a factor of two.
|
||||
alpha2 = alpha3;
|
||||
f2 = f3;
|
||||
alpha3 = alpha3 * 2;
|
||||
x = x0.add( xdir.scalarMultiply(alpha3));
|
||||
subsys . setParams(x);
|
||||
f3 = subsys . error();
|
||||
}
|
||||
}
|
||||
//Get the alpha for the minimum f of the quadratic approximation
|
||||
alphaStar = alpha2 + ((alpha2 - alpha1) * (f1 - f3)) / (3 * (f1 - 2 * f2 + f3));
|
||||
|
||||
//Guarantee that the new alphaStar is within the bracket
|
||||
if (alphaStar >= alpha3 || alphaStar <= alpha1) {
|
||||
alphaStar = alpha2;
|
||||
}
|
||||
|
||||
if (alphaStar > alphaMax) {
|
||||
alphaStar = alphaMax;
|
||||
}
|
||||
|
||||
if (alphaStar != alphaStar) {
|
||||
alphaStar = 0.;
|
||||
}
|
||||
|
||||
//Take a final step to alphaStar
|
||||
x = x0 .add( xdir.scalarMultiply( alphaStar ) );
|
||||
subsys . setParams(x);
|
||||
|
||||
return alphaStar;
|
||||
}
|
||||
|
||||
public static SolveStatus solve_BFGS(SubSystem subsys, boolean isFine) {
|
||||
int xsize = subsys.pSize();
|
||||
if (xsize == 0) {
|
||||
return SolveStatus.Success;
|
||||
}
|
||||
|
||||
|
||||
RealMatrix D = new Array2DRowRealMatrix(xsize, xsize);
|
||||
identity(D);
|
||||
//
|
||||
RealMatrix x = new Array2DRowRealMatrix(xsize, 1);
|
||||
RealMatrix xdir = new Array2DRowRealMatrix(xsize, 1);
|
||||
RealMatrix grad = new Array2DRowRealMatrix(xsize, 1);
|
||||
RealMatrix h = new Array2DRowRealMatrix(xsize, 1);
|
||||
RealMatrix y = new Array2DRowRealMatrix(xsize, 1);
|
||||
RealMatrix Dy = new Array2DRowRealMatrix(xsize, 1);
|
||||
|
||||
// Initial unknowns vector and initial gradient vector
|
||||
subsys.fillParams(x);
|
||||
subsys.calcGrad(grad);
|
||||
|
||||
// Initial search direction oposed to gradient (steepest-descent)
|
||||
xdir = grad.scalarMultiply(-1);
|
||||
lineSearch(subsys, xdir);
|
||||
double err = subsys.error();
|
||||
|
||||
h = new Array2DRowRealMatrix(x.getData());
|
||||
subsys.fillParams(x);
|
||||
h = x.subtract(h); // = x - xold
|
||||
|
||||
double convergence = isFine ? XconvergenceFine : XconvergenceRough;
|
||||
int maxIterNumber = MaxIterations * xsize;
|
||||
double divergingLim = 1e6 * err + 1e12;
|
||||
|
||||
for (int iter = 1; iter < maxIterNumber; iter++) {
|
||||
|
||||
if (h.getFrobeniusNorm() <= convergence || err <= smallF) {
|
||||
break;
|
||||
}
|
||||
if (err > divergingLim || err != err) // check for diverging and NaN
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
y = new Array2DRowRealMatrix(grad.getData());
|
||||
subsys.calcGrad(grad);
|
||||
y = grad.subtract(y); // = grad - gradold
|
||||
|
||||
double hty = dotProduct(h, y);
|
||||
//make sure that hty is never 0
|
||||
if (hty == 0) {
|
||||
hty = .0000000001;
|
||||
}
|
||||
|
||||
Dy = D.multiply(y);
|
||||
|
||||
double ytDy = dotProduct(y, Dy);
|
||||
|
||||
//Now calculate the BFGS update on D
|
||||
D = D.add(h.scalarMultiply((1. + ytDy / hty) / hty).multiply(h.transpose()));
|
||||
D = D.subtract((
|
||||
h.multiply(Dy.transpose())
|
||||
.add(Dy.multiply(h.transpose()))
|
||||
).scalarMultiply(1. / hty)
|
||||
);
|
||||
|
||||
xdir = D.scalarMultiply(-1).multiply(grad);
|
||||
lineSearch(subsys, xdir);
|
||||
err = subsys.error();
|
||||
|
||||
h = new Array2DRowRealMatrix(x.getData());
|
||||
subsys.fillParams(x);
|
||||
h = x.subtract(h); // = x - xold
|
||||
}
|
||||
|
||||
// subsys.revertParams();
|
||||
|
||||
if (err <= smallF) {
|
||||
return SolveStatus.Success;
|
||||
}
|
||||
if (h.getFrobeniusNorm() <= convergence) {
|
||||
return SolveStatus.Converged;
|
||||
}
|
||||
return SolveStatus.Failed;
|
||||
}
|
||||
|
||||
|
||||
private static double[] diagonal(RealMatrix a) {
|
||||
int s = Math.min(a.getColumnDimension(), a.getRowDimension());
|
||||
double[] d = new double[s];
|
||||
for (int i = 0; i < s; i++) {
|
||||
d[i] = a.getEntry(i, i);
|
||||
}
|
||||
return d;
|
||||
}
|
||||
|
||||
private static double dot(RealMatrix m1, RealMatrix m2) {
|
||||
return new ArrayRealVector(m1.getData()[0]).dotProduct(new ArrayRealVector(m2.getData()[0]));
|
||||
|
|
@ -271,6 +705,34 @@ public class Solver {
|
|||
x.setColumn(0, params.toArray());
|
||||
}
|
||||
|
||||
|
||||
public TDoubleList getParams() {
|
||||
int i = 0;
|
||||
TDoubleList params = new TDoubleArrayList();
|
||||
for (Constraint c : constraints) {
|
||||
params.add(c.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();
|
||||
for (Constraint c : constraints) {
|
||||
params.add(c.error());
|
||||
}
|
||||
return params;
|
||||
}
|
||||
|
||||
public double calcResidual(RealMatrix r) {
|
||||
double err = 0.;
|
||||
int i = 0;
|
||||
|
|
@ -283,6 +745,30 @@ public class Solver {
|
|||
return err;
|
||||
}
|
||||
|
||||
public TDoubleList calcResidual() {
|
||||
TDoubleList r = new TDoubleArrayList();
|
||||
double err = 0.;
|
||||
int i = 0;
|
||||
for (Constraint c : constraints) {
|
||||
double v = c.error();
|
||||
r.add(v);
|
||||
err += v * v;
|
||||
}
|
||||
err *= 0.5;
|
||||
return r;
|
||||
}
|
||||
|
||||
public double value() {
|
||||
double err = 0.;
|
||||
for (Constraint c : constraints) {
|
||||
double v = c.error();
|
||||
err += v * v;
|
||||
}
|
||||
err *= 0.5;
|
||||
return err;
|
||||
}
|
||||
|
||||
|
||||
public void calcJacobi(RealMatrix jacobi) {
|
||||
// jacobi.setZero(csize, params.size());
|
||||
for (int j=0; j < pSize(); j++) {
|
||||
|
|
@ -300,6 +786,25 @@ public class Solver {
|
|||
}
|
||||
}
|
||||
|
||||
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]);
|
||||
}
|
||||
}
|
||||
return jacobi;
|
||||
}
|
||||
|
||||
|
||||
public void setParams(RealMatrix params) {
|
||||
int off = 0;
|
||||
double[] arr = params.getColumn(0);
|
||||
|
|
@ -310,5 +815,40 @@ public class Solver {
|
|||
c.set(cp);
|
||||
}
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
public double error() {
|
||||
return value();
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static double dotProduct(RealMatrix m1, RealMatrix m2) {
|
||||
return new ArrayRealVector(m1.getData()[0]).dotProduct(new ArrayRealVector(m2.getData()[0]));
|
||||
}
|
||||
|
||||
|
||||
static double XconvergenceRough = 1e-8;
|
||||
static double XconvergenceFine = 1e-10;
|
||||
static double smallF = 1e-20;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -5,4 +5,6 @@ public interface System {
|
|||
double[] params();
|
||||
|
||||
void gradient(double[] out);
|
||||
|
||||
int pSize();
|
||||
}
|
||||
|
|
|
|||
9
src/cad/gcs/constr/AbstractConstraint.java
Normal file
9
src/cad/gcs/constr/AbstractConstraint.java
Normal file
|
|
@ -0,0 +1,9 @@
|
|||
package cad.gcs.constr;
|
||||
|
||||
import cad.gcs.Constraint;
|
||||
|
||||
/**
|
||||
* Created by verastov
|
||||
*/
|
||||
public abstract class AbstractConstraint implements Constraint {
|
||||
}
|
||||
15
src/cad/gcs/constr/Constraint2.java
Normal file
15
src/cad/gcs/constr/Constraint2.java
Normal file
|
|
@ -0,0 +1,15 @@
|
|||
package cad.gcs.constr;
|
||||
|
||||
import cad.math.Vector;
|
||||
|
||||
import java.util.List;
|
||||
|
||||
public interface Constraint2 {
|
||||
double error();
|
||||
|
||||
List<Vector> params();
|
||||
|
||||
List<Vector> gradient();
|
||||
|
||||
Object debug();
|
||||
}
|
||||
17
src/cad/gcs/constr/PRef.java
Normal file
17
src/cad/gcs/constr/PRef.java
Normal file
|
|
@ -0,0 +1,17 @@
|
|||
package cad.gcs.constr;
|
||||
|
||||
import cad.gcs.Param;
|
||||
|
||||
/**
|
||||
* Created by verastov
|
||||
*/
|
||||
public class PRef {
|
||||
|
||||
private final Param x;
|
||||
private final Param y;
|
||||
|
||||
public PRef(Param x, Param y) {
|
||||
this.x = x;
|
||||
this.y = y;
|
||||
}
|
||||
}
|
||||
|
|
@ -4,8 +4,6 @@ import cad.gcs.Constraint;
|
|||
import cad.gcs.Param;
|
||||
import cad.math.Vector;
|
||||
|
||||
import java.util.List;
|
||||
|
||||
public class Perpendicular implements Constraint {
|
||||
|
||||
public static final int l1p1x = 0;
|
||||
|
|
@ -16,66 +14,187 @@ public class Perpendicular implements Constraint {
|
|||
public static final int l2p1y = 5;
|
||||
public static final int l2p2x = 6;
|
||||
public static final int l2p2y = 7;
|
||||
|
||||
private final double[] params = new double[8];
|
||||
|
||||
public Perpendicular(Vector a1, Vector a2, Vector b1, Vector b2) {
|
||||
params[l1p1x] = a1.x;
|
||||
params[l1p1y] = a1.y;
|
||||
params[l1p2x] = b1.x;
|
||||
params[l1p2y] = b1.y;
|
||||
params[l2p1x] = a2.x;
|
||||
params[l2p1y] = a2.y;
|
||||
params[l2p2x] = b2.x;
|
||||
params[l2p2y] = b2.y;
|
||||
private final Param[] params = new Param[8];
|
||||
|
||||
public Perpendicular(
|
||||
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 a1, Vector a2, Vector b1, Vector b2) {
|
||||
a1.x = params[l1p1x];
|
||||
a1.y = params[l1p1y];
|
||||
b1.x = params[l1p2x];
|
||||
b1.y = params[l1p2y];
|
||||
a2.x = params[l2p1x];
|
||||
a2.y = params[l2p1y];
|
||||
b2.x = params[l2p2x];
|
||||
b2.y = params[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 double[] params() {
|
||||
return 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[l1p1x] - params[l1p2x]);
|
||||
double dy1 = (params[l1p1y] - params[l1p2y]);
|
||||
double dx2 = (params[l2p1x] - params[l2p2x]);
|
||||
double dy2 = (params[l2p1y] - params[l2p2y]);
|
||||
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 off = dx1 * dx2 + dy1 * dy2;
|
||||
return off;
|
||||
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) {
|
||||
out[l1p1x] = (params[l2p1x] - params[l2p2x]); // = dx2
|
||||
out[l1p2x] = -(params[l2p1x] - params[l2p2x]); // = -dx2
|
||||
out[l1p1y] = (params[l2p1y] - params[l2p2y]); // = dy2
|
||||
out[l1p2y] = -(params[l2p1y] - params[l2p2y]); // = -dy2
|
||||
out[l2p1x] = (params[l1p1x] - params[l1p2x]); // = dx1
|
||||
out[l2p2x] = -(params[l1p1x] - params[l1p2x]); // = -dx1
|
||||
out[l2p1y] = (params[l1p1y] - params[l1p2y]); // = dy1
|
||||
out[l2p2y] = -(params[l1p1y] - params[l1p2y]); // = -dy1
|
||||
|
||||
// for (int i = 0; i < out.length; i++) {
|
||||
// out[i] *= err;
|
||||
// }
|
||||
double x1 = params[l1p1x].get();
|
||||
double x2 = params[l1p1y].get();
|
||||
double x3 = params[l1p2x].get();
|
||||
double x4 = params[l1p2y].get();
|
||||
double x5 = params[l2p1x].get();
|
||||
double x6 = params[l2p1y].get();
|
||||
double x7 = params[l2p2x].get();
|
||||
double x8 = 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
|
||||
|
||||
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 void gradient2(double[] out) {
|
||||
|
||||
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);
|
||||
|
||||
double k = (da.dot(db) * 2);
|
||||
|
||||
Vector g1 = p1.multi(db.x, db.y, db.z).multi(-k);
|
||||
Vector g2 = p2.multi(db.x, db.y, db.z).multi(k);
|
||||
Vector g3 = p3.multi(da.x, da.y, da.z).multi(-k);
|
||||
Vector g4 = p4.multi(da.x, da.y, da.z).multi(k);
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
@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());
|
||||
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 xl = Math.sqrt(dx1 * dx1 + dx2 * dx2);
|
||||
double yl = Math.sqrt(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) {
|
||||
System.arraycopy(input, 0, params, 0, params.length);
|
||||
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]);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
74
src/cad/gcs/constr/Perpendicular2.java
Normal file
74
src/cad/gcs/constr/Perpendicular2.java
Normal file
|
|
@ -0,0 +1,74 @@
|
|||
package cad.gcs.constr;
|
||||
|
||||
import cad.math.Vector;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.Arrays;
|
||||
import java.util.List;
|
||||
|
||||
public class Perpendicular2 implements Constraint2 {
|
||||
|
||||
public final Vector a1;
|
||||
public final Vector a2;
|
||||
public final Vector b1;
|
||||
public final Vector b2;
|
||||
private double target;
|
||||
|
||||
public Perpendicular2(Vector a1, Vector a2, Vector b1, Vector b2) {
|
||||
this.a1 = a1;
|
||||
this.a2 = a2;
|
||||
this.b1 = b1;
|
||||
this.b2 = b2;
|
||||
this.target = target;
|
||||
}
|
||||
|
||||
@Override
|
||||
public double error() {
|
||||
return da().dot(db());
|
||||
}
|
||||
|
||||
@Override
|
||||
public List<Vector> params() {
|
||||
return Arrays.asList(a1, a2, b1, b2);
|
||||
}
|
||||
|
||||
@Override
|
||||
public List<Vector> gradient() {
|
||||
List<Vector> grad = new ArrayList<>(4);
|
||||
// Vector da = da();
|
||||
// Vector db = db();
|
||||
// double k = da().dot(db()) > 0 ? -1 : 1;
|
||||
//// double k = 1;
|
||||
// grad.add(db.multi(- k));
|
||||
// grad.add(db.multi( k));
|
||||
// grad.add(da.multi(- k));
|
||||
// grad.add(da.multi( k));
|
||||
// return grad;
|
||||
|
||||
double k = (da().dot(db()) * 2);
|
||||
|
||||
Vector da = da();
|
||||
Vector db = db();
|
||||
grad.add(a1.multi(db.x, db.y, db.z).multi(-1));
|
||||
grad.add(a2.multi(db.x, db.y, db.z));
|
||||
|
||||
grad.add(b1.multi(da.x, da.y, da.z).multi(-1));
|
||||
grad.add(b2.multi(da.x, da.y, da.z));
|
||||
|
||||
return grad;
|
||||
|
||||
}
|
||||
|
||||
private Vector db() {
|
||||
return b2.minus(b1);
|
||||
}
|
||||
|
||||
private Vector da() {
|
||||
return a2.minus(a1);
|
||||
}
|
||||
|
||||
@Override
|
||||
public Object debug() {
|
||||
return Math.acos(error() / (da().length() * db().length()) ) / Math.PI * 180;
|
||||
}
|
||||
}
|
||||
163
src/cad/gcs/constr/X.java
Normal file
163
src/cad/gcs/constr/X.java
Normal file
|
|
@ -0,0 +1,163 @@
|
|||
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 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;
|
||||
}
|
||||
|
||||
|
||||
@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());
|
||||
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]);
|
||||
}
|
||||
|
||||
}
|
||||
45
src/cad/gcs/constr/XY.java
Normal file
45
src/cad/gcs/constr/XY.java
Normal file
|
|
@ -0,0 +1,45 @@
|
|||
package cad.gcs.constr;
|
||||
|
||||
import cad.math.Vector;
|
||||
|
||||
import java.util.Collections;
|
||||
import java.util.List;
|
||||
|
||||
/**
|
||||
* Created by verastov
|
||||
*/
|
||||
public class XY implements Constraint2 {
|
||||
|
||||
private final Vector point;
|
||||
private final Vector lock;
|
||||
|
||||
public XY(Vector point, Vector lock) {
|
||||
this.point = point;
|
||||
this.lock = lock;
|
||||
}
|
||||
|
||||
public Vector diff() {
|
||||
return lock.minus(point);
|
||||
}
|
||||
|
||||
@Override
|
||||
public double error() {
|
||||
Vector diff = diff();
|
||||
return diff.x * diff.x + diff.y * diff.y;
|
||||
}
|
||||
|
||||
@Override
|
||||
public List<Vector> params() {
|
||||
return Collections.singletonList(point);
|
||||
}
|
||||
|
||||
@Override
|
||||
public List<Vector> gradient() {
|
||||
return Collections.singletonList(diff());
|
||||
}
|
||||
|
||||
@Override
|
||||
public Object debug() {
|
||||
return point;
|
||||
}
|
||||
}
|
||||
|
|
@ -1,11 +1,9 @@
|
|||
package cad.gl;
|
||||
|
||||
import cad.fx.Polygon;
|
||||
import cad.math.Vector;
|
||||
import com.jogamp.newt.event.MouseEvent;
|
||||
import com.jogamp.newt.event.MouseListener;
|
||||
import com.jogamp.newt.opengl.GLWindow;
|
||||
import com.sun.javafx.geom.*;
|
||||
import javafx.scene.input.PickResult;
|
||||
|
||||
import javax.media.opengl.GL2;
|
||||
|
|
@ -154,7 +152,7 @@ public class Scene implements GLEventListener, MouseListener {
|
|||
// Vector dir = pickRay[1].minus(pickRay[0]);
|
||||
|
||||
// pickRay[1] = pickRay[1].minus(pickRay[0]).scale(700);//.normalize().scale(55);
|
||||
pickRay[1] = pickRay[1].minus(pickRay[0]).normalize().scale(30);
|
||||
pickRay[1] = pickRay[1].minus(pickRay[0]).normalize().multi(30);
|
||||
}
|
||||
|
||||
public static float[] fixW(float[] v) {
|
||||
|
|
|
|||
|
|
@ -53,11 +53,11 @@ public class Vector {
|
|||
this.z = data[2];
|
||||
}
|
||||
|
||||
public Vector scale(double factor) {
|
||||
return scale(factor, factor, factor);
|
||||
public Vector multi(double factor) {
|
||||
return multi(factor, factor, factor);
|
||||
}
|
||||
|
||||
public Vector scale(double dx, double dy, double dz) {
|
||||
public Vector multi(double dx, double dy, double dz) {
|
||||
return new Vector(x * dx, y * dy, z * dz);
|
||||
}
|
||||
|
||||
|
|
@ -83,7 +83,14 @@ public class Vector {
|
|||
}
|
||||
|
||||
public Vector plus(Vector vector) {
|
||||
return new Vector(x + vector.x, y + vector.y, z + vector.z);
|
||||
return new Vector(this)._plus(vector);
|
||||
}
|
||||
|
||||
public Vector _plus(Vector vector) {
|
||||
x += vector.x;
|
||||
y += vector.y;
|
||||
z += vector.z;
|
||||
return this;
|
||||
}
|
||||
|
||||
public Vector plus(double dx, double dy, double dz) {
|
||||
|
|
@ -118,7 +125,7 @@ public class Vector {
|
|||
}
|
||||
|
||||
public Vector negate() {
|
||||
return scale(-1);
|
||||
return multi(-1);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in a new issue