diff --git a/Makefile b/Makefile
index 0dd68743680321e38d4a27eeaa586a16fb2d62a7..c4eb8a2e389cd3b3ed536f6d9081240de5774406 100644
--- a/Makefile
+++ b/Makefile
@@ -1,17 +1,19 @@
CC=g++
-CFLAGS=-I. -Wall \
--fpermissive -lpthread -std=c++11 -I~/.progs/armadillo-6.400.3/include -DARMA_DONT_USE_WRAPPER -larmadillo #-llapack -lblas
+CFLAGS = -g -O2 -Wall -W -pedantic-errors
+# Change the following to include current directory to armadillo.../include:
+CFLAGS += -Wmissing-braces -Wparentheses -Wold-style-cast -I/home/martinka/miscellaneous/armadillo-6.400.3/include -DARMA_DONT_USE_WRAPPER -lblas -llapack
+CFLAGS += -std=c++11
LFLAGS=-lrt
-#LIBS=-lm -llapack -lblas
-OBJSTATIC = traj2dmp.o file2mat.o\
-optpart.o solver.o cvxgen/matrix_support.o cvxgen/util.o cvxgen/ldl.o cuttraj.o dmp.o
+LIBS=-lm -llapack -lblas
+OBJSTATIC = traj2dmp.o plotMat3.o plotgnu3.o\
+optpart.o cvxgen/solver.o cvxgen/matrix_support.o cvxgen/util.o cvxgen/ldl.o cuttraj.o dmp.o
all: example_main
%: %.c
$(CC) -o $@ $(CFLAGS) $^ $(LIBS)
-%.o: %.cpp $(DEPS)
+%.o: %.cc $(DEPS)
$(CC) $(CFLAGS) -c -o $@ $<
diff --git a/cuttraj.cc b/cuttraj.cc
index 8a7767a7319346471abf0f3ec494f1030e987197..03ee964aa66a4e2271a74bfa60dcdbbf64397b2b 100644
--- a/cuttraj.cc
+++ b/cuttraj.cc
@@ -33,7 +33,6 @@ void cuttraj(const mat& evalTraj, const mat& corrTraj, mat& traj1, mat& traj2) {
mat dist2cutnew = dists.col(cutnew);
double mindist2cutnew = as_scalar(min(dist2cutnew));
- cout << mindist2cutnew << endl;
double cutold = as_scalar(find(dist2cutnew == mindist2cutnew, 1, "first")) + 1;
diff --git a/dmp.cc b/dmp.cc
index 5ac0f125f3dac86a905433a1f3e100153d0361d1..dcf109ab44a91fabc3e313126a368052d7f7d932 100644
--- a/dmp.cc
+++ b/dmp.cc
@@ -4,7 +4,19 @@
using namespace arma;
using namespace std;
-Dmp::Dmp(mat w, mat g, double t) : weights(w), goal(g), tau(t) {}
+Dmp::Dmp(mat w, mat g, double t) : weights(w), goal(g), tau(t) {
+ nDims = w.n_cols;
+ x = ones<mat>(nDims,1);
+ v = zeros<mat>(nDims,1);
+ z = zeros<mat>(nDims,1);
+ int n_rfs = w.n_rows; // number of weight functions
+ mat arr = linspace<mat>(0, 1, n_rfs)*0.5;
+ c = (1+alpha_z/2*arr)%exp(-alpha_z/2*arr);
+ D = pow((diff(c)*0.55),2);
+ mat Dlast(1,1);
+ Dlast = D(D.n_rows - 1);
+ D = 1/join_vert(D, Dlast);
+}
mat Dmp::getW() const {
return weights;
@@ -18,8 +30,8 @@ double Dmp::getT() const {
return tau;
}
-void Dmp::setG(mat g) {
- goal = g;
+void Dmp::setG(mat gIn) {
+ goal = gIn;
}
void Dmp::setT(double t) {
@@ -27,12 +39,12 @@ void Dmp::setT(double t) {
}
Dmp& Dmp::doubleSpeed() {
- tau = tau/2;
+ tau = 2*tau;
return *this;
}
Dmp& Dmp::speedupTimes(int x) {
- tau = tau/x;
+ tau = x*tau;
return *this;
}
@@ -46,3 +58,68 @@ ostream& operator<<(ostream& os, const Dmp& dmp) {
return os;
}
+void Dmp::resetState() {
+ firstSample = true;
+ x = ones<mat>(nDims,1);
+ v = zeros<mat>(nDims,1);
+ z = zeros<mat>(nDims,1);
+ g = zeros<mat>(nDims,1);
+}
+
+
+mat Dmp::getVel(const mat& posIn, const double& dt) {
+ mat pos = posIn.t();
+ if (firstSample) {
+ g = pos;
+ firstSample = false;
+ }
+ mat vel = zeros(nDims,1);
+
+ for (int j = 0; j < nDims; ++j) {
+ //mat psi = zeros(d.n_rows, d.n_cols);
+ mat psi = exp(-0.5*pow((x(j)-c),2) % D);
+ double f = as_scalar(sum(v(j)*weights.col(j)%psi)/sum(psi+1.e-10));
+ // Differential equations
+ double vd = (alpha_v*(beta_v*(0-x(j))-v(j)))*tau;
+ double xd = v(j)*tau;
+ double zd = (alpha_z*(beta_z*(g(j)-pos(j))-z(j))+f)*tau;
+ double yd = z(j)*tau;
+ double gd = alpha_g*(goal(j)-g(j));
+ // State update:
+ x(j) = xd*dt+x(j);
+ v(j) = vd*dt+v(j);
+ z(j) = zd*dt+z(j);
+ g(j) = gd*dt+g(j);
+ vel(j) = yd;
+
+ }
+ return vel.t();
+}
+
+
+/*
+for j = 1:7
+ psi = exp(-0.5*((x(j)-c).^2).*D);
+ f = sum(v(j)*w(:,j).*psi)/sum(psi+1.e-10);
+
+
+
+ % The dynamical systems:
+ vd = (alpha_v*(beta_v*(0-x(j))-v(j)))*tau;
+ xd = v(j)*tau;
+
+ zd = (alpha_z*(beta_z*(g(j)-yact(j))-z(j))+f)*tau;
+
+ yd = z(j)*tau;
+
+ gd = alpha_g*(G(j)-g(j));
+
+ % State update:
+ x(j) = xd*dt+x(j);
+ v(j) = vd*dt+v(j);
+ z(j) = zd*dt+z(j);
+ g(j) = gd*dt+g(j);
+ yrefd(j) = yd;
+end
+*/
+
diff --git a/dmp.h b/dmp.h
index cd9b3ed8f446ffe6c0c37e9f74b7269198f1f46a..d91cf1678f5923bf5fe0002fcbf0130be3eb8a15 100644
--- a/dmp.h
+++ b/dmp.h
@@ -17,11 +17,12 @@ public:
void setT(double t);
Dmp& doubleSpeed();
Dmp& speedupTimes(int x);
+ mat getVel(const mat& pos, const double& dt);
+ void resetState();
private:
mat weights;
mat goal;
double tau;
-
//new, to be integrated:
string ID;
string GroupID;
@@ -38,6 +39,21 @@ private:
string startTolerance;
string endTolerance;
string goalObject;
+ ////////////////////////////////////////////
+ int nDims;
+ bool firstSample = true;
+ mat x;
+ mat v;
+ mat z;
+ mat g;
+ mat c;
+ mat D;
+ double alpha_g = 12.5;
+ double alpha_v = 25;
+ double alpha_z = 25;
+ double beta_v = 6.25;
+ //double beta_z = 6.25;
+ double beta_z = 8;
};
ostream& operator<<(ostream& os, const Dmp& dmp);
diff --git a/example_main b/example_main
index fd588890afddf3387fbeb937215f2e805064a2a9..f36e2e93a3d3d0788b25dc52f982b3cba17ed331 100755
Binary files a/example_main and b/example_main differ
diff --git a/example_main.cc b/example_main.cc
index ea5472a6ae79a1d799f1976ff9c5c90c433cddd0..a7f78f1ff62b41fd1abd365fa2d8d234a2a2964a 100644
--- a/example_main.cc
+++ b/example_main.cc
@@ -3,7 +3,6 @@
#include "traj2dmp.h"
#include "optpart.h"
#include "cuttraj.h"
-#include "file2mat.h"
#include <vector>
#include <netdb.h>
#include <string.h>
@@ -12,6 +11,8 @@
#include "dmp.h"
#include <unistd.h>
#include <armadillo>
+#include "plotMat3.h"
+
using namespace std;
@@ -21,23 +22,12 @@ using namespace arma;
int main(int argc, char *argv[]) {
// Initialize original trajectories:
- double dt = 0.200; // 5 Hz sampling rate, in this example
- //mat yDef;
- //file2mat("yDef.txt",yDef);
- //cout << yDef << endl;
- //cout << yDef.n_rows << endl;
- cout << "hej" << endl;
- mat yDef = ones<mat>(10,3);
- cout << "hej" << endl;
- mat yCorr = ones<mat>(10,3);
- yCorr = yCorr*2;
-
- //mat yCorr = file2mat("yCorr.txt");
-
- cout << "hej" << endl;
- cout << yCorr.n_rows << endl;
-
-
+ double dt = 0.040; // 25 Hz sampling rate, in this example
+ mat yDef;
+ yDef.load("yDef.txt", raw_ascii);
+ mat yCorr;
+ yCorr.load("yCorr.txt", raw_ascii);
+ int nDims = yDef.n_cols; // Number of trajectory dimensions
mat yDefKeep; // kept part of deficient trajectory
mat yCorrKeep; // kept part of corrective trajectory
@@ -45,30 +35,56 @@ int main(int argc, char *argv[]) {
cuttraj(yDef, yCorr, yDefKeep, yCorrKeep); //Determine which part of trajectories to keep
cout << "cuttraj done " << endl;
- cout << yDefKeep << endl;
- cout << yDef.n_rows << endl;
- cout << yDefKeep.n_rows << endl;
// extrapolate/intrapolate to length 100 (for CVXGEN solver):
vec x = linspace<vec>(0,1,yDefKeep.n_rows);
- cout << "hej " << endl;
+
mat xx = linspace<vec>(0,1,100);
mat yDefKeepExtrap;
- cout << "hej " << endl;
- mat yMod = zeros(100, yDefKeep.n_cols);
+ mat yMod = zeros(100, nDims);
// loop over dimensions j and solve optimization problem:
cout << "extrap done " << endl;
- for (int j = 0; j < yDef.n_cols; ++j) {
+ for (int j = 0; j < nDims; ++j) {
interp1(x, yDefKeep.col(j), xx, yDefKeepExtrap);
yMod.col(j) = optpart(yDefKeepExtrap, yCorrKeep.col(j));
}
- cout << "optimization done " << endl;
-
- Dmp resDmp1 = traj2dmp(yMod, dt*yMod.n_rows/yMod.n_rows).speedupTimes(1);
-
+ cout << "optimization done " << endl;
+
+ Dmp resDmp1 = traj2dmp(yMod, dt*yDefKeep.n_rows/yMod.n_rows).speedupTimes(1);
Dmp resDmp2 = traj2dmp(yCorrKeep, dt).speedupTimes(1);
cout << "First resulting DMP: " << endl << resDmp1 << endl;
cout << "Second resulting DMP: " << endl << resDmp2 << endl;
-
+
+
+ int simSamples1 = 500;
+ mat yRes1 = yDef.row(0);
+ mat vel = zeros(1,nDims);
+ mat yResNext = zeros(1,nDims);
+ for (int i = 0; i < simSamples1; ++i) { // Simulate trajectory from resDmp1
+ vel = resDmp1.getVel(yRes1.row(yRes1.n_rows-1), dt);
+ for (int j = 0; j < nDims; ++j) {
+ yResNext(j) = yRes1(yRes1.n_rows-1,j) + vel(j) * dt;
+ }
+ yRes1 = join_vert(yRes1, yResNext);
+ }
+
+ mat yRes2 = yCorrKeep.row(0);
+ for (int i = 0; i < simSamples1; ++i) { // Simulate trajectory from resDmp2
+ vel = resDmp2.getVel(yRes2.row(yRes2.n_rows-1), dt);
+ for (int j = 0; j < nDims; ++j) {
+ yResNext(j) = yRes2(yRes2.n_rows-1,j) + vel(j) * dt;
+ }
+ yRes2 = join_vert(yRes2, yResNext);
+ }
+
+
+ cout << "Start plotting" << endl;
+ plotMat3(yDef, "Deficient trajectory");
+ plotMat3(yCorr, "Corrective trajectory");
+ plotMat3(yMod, "Modified trajectory");
+ plotMat3(yRes1, "Resulting trajectory part 1");
+ plotMat3(yRes2, "Resulting trajectory part 2");
+
}
+
diff --git a/file2mat.cc b/file2mat.cc
deleted file mode 100644
index 28c6da79f49203caa3f02d4659d8d42332ef1344..0000000000000000000000000000000000000000
--- a/file2mat.cc
+++ /dev/null
@@ -1,29 +0,0 @@
-#include "cuttraj.h"
-#include <iostream>
-#include <armadillo>
-
-using namespace arma;
-using namespace std;
-
-
-void file2mat(const string& filename, mat& y) {
- mat yTemp;
- yTemp.load(filename, raw_ascii);
-
-
- y = zeros<mat>(yTemp.n_rows, yTemp.n_cols);
-
-
- double dTemp;
- for (int i = 0; i < y.n_rows; ++i) {
- for (int j = 0; j < y.n_cols; ++j) {
- dTemp = yTemp(i,j);
- y(i,j) = dTemp;
- }
- }
- cout << y.n_rows << endl;
- cout << y.n_cols << endl;
-
- y = ones<mat>(yTemp.n_rows, yTemp.n_cols);
-
-}
diff --git a/file2mat.h b/file2mat.h
deleted file mode 100644
index f5253e68313eb35daa74000d102f847ce84a5009..0000000000000000000000000000000000000000
--- a/file2mat.h
+++ /dev/null
@@ -1,9 +0,0 @@
-#include <armadillo>
-using namespace arma;
-#ifndef FILE2MAT_H
-#define FILE2MAT_H
-using namespace std;
-using namespace arma;
-void file2mat(const string& filename, mat& y);
-
-#endif
diff --git a/optpart.cc b/optpart.cc
index 32df4acdad7cd05b608f3540e103ce6652947048..7025be58f03b7b45aae89c2268a7bf31b51884ed 100644
--- a/optpart.cc
+++ b/optpart.cc
@@ -6,7 +6,7 @@
/* Description: Basic test harness for solver.c. */
-#include "solver.h"
+#include "cvxgen/solver.h"
#include "optpart.h"
#include <iostream>
#include <armadillo>
@@ -19,16 +19,12 @@ Settings settings;
mat optpart(const mat& traj1, const mat& traj2) {
- set_defaults();
- setup_indexing();
- load_default_data(traj1, traj2);
-
- settings.verbose = 1;
+ set_defaults();
+ setup_indexing();
+ load_default_data(traj1, traj2);
solve();
-
mat restraj = zeros<mat>(100,1);
for (int i = 1; i < 101; ++i) {
- //cout << *vars.x[i] << endl;
restraj(i-1) = *vars.x[i];
}
return restraj;
@@ -37,8 +33,7 @@ void load_default_data(const mat& traj1, const mat& traj2) {
for (int i = 1; i < 101; ++i) {
params.traj1[i][0] = traj1(i-1);
}
- //cout << "hej" << endl;
- params.lambda[0] = 10^12;
+ params.lambda[0] = 10000;
params.traj2_first[0] = traj2[0];
params.traj2_second[0] = traj2[1];
}
diff --git a/optpart.h b/optpart.h
index 0c707cc20b56f6097e1b1c05170f216caeefb400..9d7f11e6eb34e1eee7feb0b9ec6fee36709a2d69 100644
--- a/optpart.h
+++ b/optpart.h
@@ -6,7 +6,7 @@
/* Filename: testsolver.c. */
/* Description: Basic test harness for solver.c. */
-#include "solver.h"
+#include "cvxgen/solver.h"
#include <iostream>
#include <armadillo>
using namespace arma;
diff --git a/solver.c b/solver.c
deleted file mode 100644
index 8c990bcd863205e7a95d2348e4abd011c4ed9e71..0000000000000000000000000000000000000000
--- a/solver.c
+++ /dev/null
@@ -1,692 +0,0 @@
-/* Produced by CVXGEN, 2016-01-17 08:15:52 -0500. */
-/* CVXGEN is Copyright (C) 2006-2012 Jacob Mattingley, jem@cvxgen.com. */
-/* The code in this file is Copyright (C) 2006-2012 Jacob Mattingley. */
-/* CVXGEN, or solvers produced by CVXGEN, cannot be used for commercial */
-/* applications without prior written permission from Jacob Mattingley. */
-
-/* Filename: solver.c. */
-/* Description: Main solver file. */
-#include "solver.h"
-double eval_gap(void) {
- int i;
- double gap;
- gap = 0;
- for (i = 0; i < 0; i++)
- gap += work.z[i]*work.s[i];
- return gap;
-}
-void set_defaults(void) {
- settings.resid_tol = 1e-6;
- settings.eps = 1e-4;
- settings.max_iters = 25;
- settings.refine_steps = 1;
- settings.s_init = 1;
- settings.z_init = 1;
- settings.debug = 0;
- settings.verbose = 1;
- settings.verbose_refinement = 0;
- settings.better_start = 1;
- settings.kkt_reg = 1e-7;
-}
-void setup_pointers(void) {
- work.y = work.x + 198;
- work.s = work.x + 298;
- work.z = work.x + 298;
- vars.x_1 = work.x + 98;
- vars.x_2 = work.x + 99;
- vars.x_3 = work.x + 100;
- vars.x_4 = work.x + 101;
- vars.x_5 = work.x + 102;
- vars.x_6 = work.x + 103;
- vars.x_7 = work.x + 104;
- vars.x_8 = work.x + 105;
- vars.x_9 = work.x + 106;
- vars.x_10 = work.x + 107;
- vars.x_11 = work.x + 108;
- vars.x_12 = work.x + 109;
- vars.x_13 = work.x + 110;
- vars.x_14 = work.x + 111;
- vars.x_15 = work.x + 112;
- vars.x_16 = work.x + 113;
- vars.x_17 = work.x + 114;
- vars.x_18 = work.x + 115;
- vars.x_19 = work.x + 116;
- vars.x_20 = work.x + 117;
- vars.x_21 = work.x + 118;
- vars.x_22 = work.x + 119;
- vars.x_23 = work.x + 120;
- vars.x_24 = work.x + 121;
- vars.x_25 = work.x + 122;
- vars.x_26 = work.x + 123;
- vars.x_27 = work.x + 124;
- vars.x_28 = work.x + 125;
- vars.x_29 = work.x + 126;
- vars.x_30 = work.x + 127;
- vars.x_31 = work.x + 128;
- vars.x_32 = work.x + 129;
- vars.x_33 = work.x + 130;
- vars.x_34 = work.x + 131;
- vars.x_35 = work.x + 132;
- vars.x_36 = work.x + 133;
- vars.x_37 = work.x + 134;
- vars.x_38 = work.x + 135;
- vars.x_39 = work.x + 136;
- vars.x_40 = work.x + 137;
- vars.x_41 = work.x + 138;
- vars.x_42 = work.x + 139;
- vars.x_43 = work.x + 140;
- vars.x_44 = work.x + 141;
- vars.x_45 = work.x + 142;
- vars.x_46 = work.x + 143;
- vars.x_47 = work.x + 144;
- vars.x_48 = work.x + 145;
- vars.x_49 = work.x + 146;
- vars.x_50 = work.x + 147;
- vars.x_51 = work.x + 148;
- vars.x_52 = work.x + 149;
- vars.x_53 = work.x + 150;
- vars.x_54 = work.x + 151;
- vars.x_55 = work.x + 152;
- vars.x_56 = work.x + 153;
- vars.x_57 = work.x + 154;
- vars.x_58 = work.x + 155;
- vars.x_59 = work.x + 156;
- vars.x_60 = work.x + 157;
- vars.x_61 = work.x + 158;
- vars.x_62 = work.x + 159;
- vars.x_63 = work.x + 160;
- vars.x_64 = work.x + 161;
- vars.x_65 = work.x + 162;
- vars.x_66 = work.x + 163;
- vars.x_67 = work.x + 164;
- vars.x_68 = work.x + 165;
- vars.x_69 = work.x + 166;
- vars.x_70 = work.x + 167;
- vars.x_71 = work.x + 168;
- vars.x_72 = work.x + 169;
- vars.x_73 = work.x + 170;
- vars.x_74 = work.x + 171;
- vars.x_75 = work.x + 172;
- vars.x_76 = work.x + 173;
- vars.x_77 = work.x + 174;
- vars.x_78 = work.x + 175;
- vars.x_79 = work.x + 176;
- vars.x_80 = work.x + 177;
- vars.x_81 = work.x + 178;
- vars.x_82 = work.x + 179;
- vars.x_83 = work.x + 180;
- vars.x_84 = work.x + 181;
- vars.x_85 = work.x + 182;
- vars.x_86 = work.x + 183;
- vars.x_87 = work.x + 184;
- vars.x_88 = work.x + 185;
- vars.x_89 = work.x + 186;
- vars.x_90 = work.x + 187;
- vars.x_91 = work.x + 188;
- vars.x_92 = work.x + 189;
- vars.x_93 = work.x + 190;
- vars.x_94 = work.x + 191;
- vars.x_95 = work.x + 192;
- vars.x_96 = work.x + 193;
- vars.x_97 = work.x + 194;
- vars.x_98 = work.x + 195;
- vars.x_99 = work.x + 196;
- vars.x_100 = work.x + 197;
-}
-void setup_indexed_params(void) {
- /* In CVXGEN, you can say */
- /* parameters */
- /* A[i] (5,3), i=1..4 */
- /* end */
- /* This function sets up A[2] to be a pointer to A_2, which is a length-15 */
- /* vector of doubles. */
- /* If you access parameters that you haven't defined in CVXGEN, the result */
- /* is undefined. */
- params.traj1[1] = params.traj1_1;
- params.traj1[2] = params.traj1_2;
- params.traj1[3] = params.traj1_3;
- params.traj1[4] = params.traj1_4;
- params.traj1[5] = params.traj1_5;
- params.traj1[6] = params.traj1_6;
- params.traj1[7] = params.traj1_7;
- params.traj1[8] = params.traj1_8;
- params.traj1[9] = params.traj1_9;
- params.traj1[10] = params.traj1_10;
- params.traj1[11] = params.traj1_11;
- params.traj1[12] = params.traj1_12;
- params.traj1[13] = params.traj1_13;
- params.traj1[14] = params.traj1_14;
- params.traj1[15] = params.traj1_15;
- params.traj1[16] = params.traj1_16;
- params.traj1[17] = params.traj1_17;
- params.traj1[18] = params.traj1_18;
- params.traj1[19] = params.traj1_19;
- params.traj1[20] = params.traj1_20;
- params.traj1[21] = params.traj1_21;
- params.traj1[22] = params.traj1_22;
- params.traj1[23] = params.traj1_23;
- params.traj1[24] = params.traj1_24;
- params.traj1[25] = params.traj1_25;
- params.traj1[26] = params.traj1_26;
- params.traj1[27] = params.traj1_27;
- params.traj1[28] = params.traj1_28;
- params.traj1[29] = params.traj1_29;
- params.traj1[30] = params.traj1_30;
- params.traj1[31] = params.traj1_31;
- params.traj1[32] = params.traj1_32;
- params.traj1[33] = params.traj1_33;
- params.traj1[34] = params.traj1_34;
- params.traj1[35] = params.traj1_35;
- params.traj1[36] = params.traj1_36;
- params.traj1[37] = params.traj1_37;
- params.traj1[38] = params.traj1_38;
- params.traj1[39] = params.traj1_39;
- params.traj1[40] = params.traj1_40;
- params.traj1[41] = params.traj1_41;
- params.traj1[42] = params.traj1_42;
- params.traj1[43] = params.traj1_43;
- params.traj1[44] = params.traj1_44;
- params.traj1[45] = params.traj1_45;
- params.traj1[46] = params.traj1_46;
- params.traj1[47] = params.traj1_47;
- params.traj1[48] = params.traj1_48;
- params.traj1[49] = params.traj1_49;
- params.traj1[50] = params.traj1_50;
- params.traj1[51] = params.traj1_51;
- params.traj1[52] = params.traj1_52;
- params.traj1[53] = params.traj1_53;
- params.traj1[54] = params.traj1_54;
- params.traj1[55] = params.traj1_55;
- params.traj1[56] = params.traj1_56;
- params.traj1[57] = params.traj1_57;
- params.traj1[58] = params.traj1_58;
- params.traj1[59] = params.traj1_59;
- params.traj1[60] = params.traj1_60;
- params.traj1[61] = params.traj1_61;
- params.traj1[62] = params.traj1_62;
- params.traj1[63] = params.traj1_63;
- params.traj1[64] = params.traj1_64;
- params.traj1[65] = params.traj1_65;
- params.traj1[66] = params.traj1_66;
- params.traj1[67] = params.traj1_67;
- params.traj1[68] = params.traj1_68;
- params.traj1[69] = params.traj1_69;
- params.traj1[70] = params.traj1_70;
- params.traj1[71] = params.traj1_71;
- params.traj1[72] = params.traj1_72;
- params.traj1[73] = params.traj1_73;
- params.traj1[74] = params.traj1_74;
- params.traj1[75] = params.traj1_75;
- params.traj1[76] = params.traj1_76;
- params.traj1[77] = params.traj1_77;
- params.traj1[78] = params.traj1_78;
- params.traj1[79] = params.traj1_79;
- params.traj1[80] = params.traj1_80;
- params.traj1[81] = params.traj1_81;
- params.traj1[82] = params.traj1_82;
- params.traj1[83] = params.traj1_83;
- params.traj1[84] = params.traj1_84;
- params.traj1[85] = params.traj1_85;
- params.traj1[86] = params.traj1_86;
- params.traj1[87] = params.traj1_87;
- params.traj1[88] = params.traj1_88;
- params.traj1[89] = params.traj1_89;
- params.traj1[90] = params.traj1_90;
- params.traj1[91] = params.traj1_91;
- params.traj1[92] = params.traj1_92;
- params.traj1[93] = params.traj1_93;
- params.traj1[94] = params.traj1_94;
- params.traj1[95] = params.traj1_95;
- params.traj1[96] = params.traj1_96;
- params.traj1[97] = params.traj1_97;
- params.traj1[98] = params.traj1_98;
- params.traj1[99] = params.traj1_99;
- params.traj1[100] = params.traj1_100;
-}
-void setup_indexed_optvars(void) {
- /* In CVXGEN, you can say */
- /* variables */
- /* x[i] (5), i=2..4 */
- /* end */
- /* This function sets up x[3] to be a pointer to x_3, which is a length-5 */
- /* vector of doubles. */
- /* If you access variables that you haven't defined in CVXGEN, the result */
- /* is undefined. */
- vars.x[1] = vars.x_1;
- vars.x[2] = vars.x_2;
- vars.x[3] = vars.x_3;
- vars.x[4] = vars.x_4;
- vars.x[5] = vars.x_5;
- vars.x[6] = vars.x_6;
- vars.x[7] = vars.x_7;
- vars.x[8] = vars.x_8;
- vars.x[9] = vars.x_9;
- vars.x[10] = vars.x_10;
- vars.x[11] = vars.x_11;
- vars.x[12] = vars.x_12;
- vars.x[13] = vars.x_13;
- vars.x[14] = vars.x_14;
- vars.x[15] = vars.x_15;
- vars.x[16] = vars.x_16;
- vars.x[17] = vars.x_17;
- vars.x[18] = vars.x_18;
- vars.x[19] = vars.x_19;
- vars.x[20] = vars.x_20;
- vars.x[21] = vars.x_21;
- vars.x[22] = vars.x_22;
- vars.x[23] = vars.x_23;
- vars.x[24] = vars.x_24;
- vars.x[25] = vars.x_25;
- vars.x[26] = vars.x_26;
- vars.x[27] = vars.x_27;
- vars.x[28] = vars.x_28;
- vars.x[29] = vars.x_29;
- vars.x[30] = vars.x_30;
- vars.x[31] = vars.x_31;
- vars.x[32] = vars.x_32;
- vars.x[33] = vars.x_33;
- vars.x[34] = vars.x_34;
- vars.x[35] = vars.x_35;
- vars.x[36] = vars.x_36;
- vars.x[37] = vars.x_37;
- vars.x[38] = vars.x_38;
- vars.x[39] = vars.x_39;
- vars.x[40] = vars.x_40;
- vars.x[41] = vars.x_41;
- vars.x[42] = vars.x_42;
- vars.x[43] = vars.x_43;
- vars.x[44] = vars.x_44;
- vars.x[45] = vars.x_45;
- vars.x[46] = vars.x_46;
- vars.x[47] = vars.x_47;
- vars.x[48] = vars.x_48;
- vars.x[49] = vars.x_49;
- vars.x[50] = vars.x_50;
- vars.x[51] = vars.x_51;
- vars.x[52] = vars.x_52;
- vars.x[53] = vars.x_53;
- vars.x[54] = vars.x_54;
- vars.x[55] = vars.x_55;
- vars.x[56] = vars.x_56;
- vars.x[57] = vars.x_57;
- vars.x[58] = vars.x_58;
- vars.x[59] = vars.x_59;
- vars.x[60] = vars.x_60;
- vars.x[61] = vars.x_61;
- vars.x[62] = vars.x_62;
- vars.x[63] = vars.x_63;
- vars.x[64] = vars.x_64;
- vars.x[65] = vars.x_65;
- vars.x[66] = vars.x_66;
- vars.x[67] = vars.x_67;
- vars.x[68] = vars.x_68;
- vars.x[69] = vars.x_69;
- vars.x[70] = vars.x_70;
- vars.x[71] = vars.x_71;
- vars.x[72] = vars.x_72;
- vars.x[73] = vars.x_73;
- vars.x[74] = vars.x_74;
- vars.x[75] = vars.x_75;
- vars.x[76] = vars.x_76;
- vars.x[77] = vars.x_77;
- vars.x[78] = vars.x_78;
- vars.x[79] = vars.x_79;
- vars.x[80] = vars.x_80;
- vars.x[81] = vars.x_81;
- vars.x[82] = vars.x_82;
- vars.x[83] = vars.x_83;
- vars.x[84] = vars.x_84;
- vars.x[85] = vars.x_85;
- vars.x[86] = vars.x_86;
- vars.x[87] = vars.x_87;
- vars.x[88] = vars.x_88;
- vars.x[89] = vars.x_89;
- vars.x[90] = vars.x_90;
- vars.x[91] = vars.x_91;
- vars.x[92] = vars.x_92;
- vars.x[93] = vars.x_93;
- vars.x[94] = vars.x_94;
- vars.x[95] = vars.x_95;
- vars.x[96] = vars.x_96;
- vars.x[97] = vars.x_97;
- vars.x[98] = vars.x_98;
- vars.x[99] = vars.x_99;
- vars.x[100] = vars.x_100;
-}
-void setup_indexing(void) {
- setup_pointers();
- setup_indexed_params();
- setup_indexed_optvars();
-}
-void set_start(void) {
- int i;
- for (i = 0; i < 198; i++)
- work.x[i] = 0;
- for (i = 0; i < 100; i++)
- work.y[i] = 0;
- for (i = 0; i < 0; i++)
- work.s[i] = (work.h[i] > 0) ? work.h[i] : settings.s_init;
- for (i = 0; i < 0; i++)
- work.z[i] = settings.z_init;
-}
-double eval_objv(void) {
- int i;
- double objv;
- /* Borrow space in work.rhs. */
- multbyP(work.rhs, work.x);
- objv = 0;
- for (i = 0; i < 198; i++)
- objv += work.x[i]*work.rhs[i];
- objv *= 0.5;
- for (i = 0; i < 198; i++)
- objv += work.q[i]*work.x[i];
- objv += work.quad_309377593344[0]+work.quad_170383695872[0]+work.quad_112192413696[0]+work.quad_271369404416[0]+work.quad_974379421696[0]+work.quad_880958541824[0]+work.quad_572094451712[0]+work.quad_789148684288[0]+work.quad_448304680960[0]+work.quad_196635672576[0]+work.quad_489733091328[0]+work.quad_802233995264[0]+work.quad_865562517504[0]+work.quad_910621253632[0]+work.quad_321513025536[0]+work.quad_997344780288[0]+work.quad_448780910592[0]+work.quad_115901485056[0]+work.quad_782888361984[0]+work.quad_609463140352[0]+work.quad_88437858304[0]+work.quad_984663347200[0]+work.quad_230162866176[0]+work.quad_416382242816[0]+work.quad_380873158656[0]+work.quad_556464291840[0]+work.quad_266292043776[0]+work.quad_970572640256[0]+work.quad_39497891840[0]+work.quad_701607514112[0]+work.quad_181586403328[0]+work.quad_260463075328[0]+work.quad_382856859648[0]+work.quad_199224414208[0]+work.quad_511656841216[0]+work.quad_171214155776[0]+work.quad_70639910912[0]+work.quad_463249276928[0]+work.quad_244412502016[0]+work.quad_258282409984[0]+work.quad_321028120576[0]+work.quad_30674518016[0]+work.quad_824505257984[0]+work.quad_484111179776[0]+work.quad_675570683904[0]+work.quad_386911121408[0]+work.quad_932702867456[0]+work.quad_274526810112[0]+work.quad_96616189952[0]+work.quad_711864590336[0]+work.quad_200392192000[0]+work.quad_150270558208[0]+work.quad_598460686336[0]+work.quad_831592636416[0]+work.quad_874189471744[0]+work.quad_632352092160[0]+work.quad_969116119040[0]+work.quad_610911072256[0]+work.quad_606707699712[0]+work.quad_449159319552[0]+work.quad_700922720256[0]+work.quad_911067136000[0]+work.quad_304706789376[0]+work.quad_530900680704[0]+work.quad_307691851776[0]+work.quad_875119529984[0]+work.quad_330167877632[0]+work.quad_106008100864[0]+work.quad_586821791744[0]+work.quad_62270459904[0]+work.quad_705160380416[0]+work.quad_566627430400[0]+work.quad_833211822080[0]+work.quad_28799266816[0]+work.quad_442252132352[0]+work.quad_713742774272[0]+work.quad_447903891456[0]+work.quad_655034785792[0]+work.quad_576686788608[0]+work.quad_288749699072[0]+work.quad_866096066560[0]+work.quad_345799389184[0]+work.quad_335292588032[0]+work.quad_292534816768[0]+work.quad_832380674048[0]+work.quad_934938374144[0]+work.quad_265476059136[0]+work.quad_478224822272[0]+work.quad_901249572864[0]+work.quad_216942473216[0]+work.quad_227493912576[0]+work.quad_458072092672[0]+work.quad_912331198464[0]+work.quad_307004768256[0]+work.quad_284125421568[0]+work.quad_704038281216[0]+work.quad_401987686400[0]+work.quad_592084643840[0]+work.quad_8875266048[0]+work.quad_270270570496[0];
- return objv;
-}
-void fillrhs_aff(void) {
- int i;
- double *r1, *r2, *r3, *r4;
- r1 = work.rhs;
- r2 = work.rhs + 198;
- r3 = work.rhs + 198;
- r4 = work.rhs + 198;
- /* r1 = -A^Ty - G^Tz - Px - q. */
- multbymAT(r1, work.y);
- multbymGT(work.buffer, work.z);
- for (i = 0; i < 198; i++)
- r1[i] += work.buffer[i];
- multbyP(work.buffer, work.x);
- for (i = 0; i < 198; i++)
- r1[i] -= work.buffer[i] + work.q[i];
- /* r2 = -z. */
- for (i = 0; i < 0; i++)
- r2[i] = -work.z[i];
- /* r3 = -Gx - s + h. */
- multbymG(r3, work.x);
- for (i = 0; i < 0; i++)
- r3[i] += -work.s[i] + work.h[i];
- /* r4 = -Ax + b. */
- multbymA(r4, work.x);
- for (i = 0; i < 100; i++)
- r4[i] += work.b[i];
-}
-void fillrhs_cc(void) {
- int i;
- double *r2;
- double *ds_aff, *dz_aff;
- double mu;
- double alpha;
- double sigma;
- double smu;
- double minval;
- r2 = work.rhs + 198;
- ds_aff = work.lhs_aff + 198;
- dz_aff = work.lhs_aff + 198;
- mu = 0;
- for (i = 0; i < 0; i++)
- mu += work.s[i]*work.z[i];
- /* Don't finish calculating mu quite yet. */
- /* Find min(min(ds./s), min(dz./z)). */
- minval = 0;
- for (i = 0; i < 0; i++)
- if (ds_aff[i] < minval*work.s[i])
- minval = ds_aff[i]/work.s[i];
- for (i = 0; i < 0; i++)
- if (dz_aff[i] < minval*work.z[i])
- minval = dz_aff[i]/work.z[i];
- /* Find alpha. */
- if (-1 < minval)
- alpha = 1;
- else
- alpha = -1/minval;
- sigma = 0;
- for (i = 0; i < 0; i++)
- sigma += (work.s[i] + alpha*ds_aff[i])*
- (work.z[i] + alpha*dz_aff[i]);
- sigma /= mu;
- sigma = sigma*sigma*sigma;
- /* Finish calculating mu now. */
- smu = sigma*mu;
- /* Fill-in the rhs. */
- for (i = 0; i < 198; i++)
- work.rhs[i] = 0;
- for (i = 198; i < 298; i++)
- work.rhs[i] = 0;
- for (i = 0; i < 0; i++)
- r2[i] = work.s_inv[i]*(smu - ds_aff[i]*dz_aff[i]);
-}
-void refine(double *target, double *var) {
- int i, j;
- double *residual = work.buffer;
- double norm2;
- double *new_var = work.buffer2;
- for (j = 0; j < settings.refine_steps; j++) {
- norm2 = 0;
- matrix_multiply(residual, var);
- for (i = 0; i < 298; i++) {
- residual[i] = residual[i] - target[i];
- norm2 += residual[i]*residual[i];
- }
-#ifndef ZERO_LIBRARY_MODE
- if (settings.verbose_refinement) {
- if (j == 0)
- printf("Initial residual before refinement has norm squared %.6g.\n", norm2);
- else
- printf("After refinement we get squared norm %.6g.\n", norm2);
- }
-#endif
- /* Solve to find new_var = KKT \ (target - A*var). */
- ldl_solve(residual, new_var);
- /* Update var += new_var, or var += KKT \ (target - A*var). */
- for (i = 0; i < 298; i++) {
- var[i] -= new_var[i];
- }
- }
-#ifndef ZERO_LIBRARY_MODE
- if (settings.verbose_refinement) {
- /* Check the residual once more, but only if we're reporting it, since */
- /* it's expensive. */
- norm2 = 0;
- matrix_multiply(residual, var);
- for (i = 0; i < 298; i++) {
- residual[i] = residual[i] - target[i];
- norm2 += residual[i]*residual[i];
- }
- if (j == 0)
- printf("Initial residual before refinement has norm squared %.6g.\n", norm2);
- else
- printf("After refinement we get squared norm %.6g.\n", norm2);
- }
-#endif
-}
-double calc_ineq_resid_squared(void) {
- /* Calculates the norm ||-Gx - s + h||. */
- double norm2_squared;
- int i;
- /* Find -Gx. */
- multbymG(work.buffer, work.x);
- /* Add -s + h. */
- for (i = 0; i < 0; i++)
- work.buffer[i] += -work.s[i] + work.h[i];
- /* Now find the squared norm. */
- norm2_squared = 0;
- for (i = 0; i < 0; i++)
- norm2_squared += work.buffer[i]*work.buffer[i];
- return norm2_squared;
-}
-double calc_eq_resid_squared(void) {
- /* Calculates the norm ||-Ax + b||. */
- double norm2_squared;
- int i;
- /* Find -Ax. */
- multbymA(work.buffer, work.x);
- /* Add +b. */
- for (i = 0; i < 100; i++)
- work.buffer[i] += work.b[i];
- /* Now find the squared norm. */
- norm2_squared = 0;
- for (i = 0; i < 100; i++)
- norm2_squared += work.buffer[i]*work.buffer[i];
- return norm2_squared;
-}
-void better_start(void) {
- /* Calculates a better starting point, using a similar approach to CVXOPT. */
- /* Not yet speed optimized. */
- int i;
- double *x, *s, *z, *y;
- double alpha;
- work.block_33[0] = -1;
- /* Make sure sinvz is 1 to make hijacked KKT system ok. */
- for (i = 0; i < 0; i++)
- work.s_inv_z[i] = 1;
- fill_KKT();
- ldl_factor();
- fillrhs_start();
- /* Borrow work.lhs_aff for the solution. */
- ldl_solve(work.rhs, work.lhs_aff);
- /* Don't do any refinement for now. Precision doesn't matter too much. */
- x = work.lhs_aff;
- s = work.lhs_aff + 198;
- z = work.lhs_aff + 198;
- y = work.lhs_aff + 198;
- /* Just set x and y as is. */
- for (i = 0; i < 198; i++)
- work.x[i] = x[i];
- for (i = 0; i < 100; i++)
- work.y[i] = y[i];
- /* Now complete the initialization. Start with s. */
- /* Must have alpha > max(z). */
- alpha = -1e99;
- for (i = 0; i < 0; i++)
- if (alpha < z[i])
- alpha = z[i];
- if (alpha < 0) {
- for (i = 0; i < 0; i++)
- work.s[i] = -z[i];
- } else {
- alpha += 1;
- for (i = 0; i < 0; i++)
- work.s[i] = -z[i] + alpha;
- }
- /* Now initialize z. */
- /* Now must have alpha > max(-z). */
- alpha = -1e99;
- for (i = 0; i < 0; i++)
- if (alpha < -z[i])
- alpha = -z[i];
- if (alpha < 0) {
- for (i = 0; i < 0; i++)
- work.z[i] = z[i];
- } else {
- alpha += 1;
- for (i = 0; i < 0; i++)
- work.z[i] = z[i] + alpha;
- }
-}
-void fillrhs_start(void) {
- /* Fill rhs with (-q, 0, h, b). */
- int i;
- double *r1, *r2, *r3, *r4;
- r1 = work.rhs;
- r2 = work.rhs + 198;
- r3 = work.rhs + 198;
- r4 = work.rhs + 198;
- for (i = 0; i < 198; i++)
- r1[i] = -work.q[i];
- for (i = 0; i < 0; i++)
- r2[i] = 0;
- for (i = 0; i < 0; i++)
- r3[i] = work.h[i];
- for (i = 0; i < 100; i++)
- r4[i] = work.b[i];
-}
-long solve(void) {
- int i;
- int iter;
- double *dx, *ds, *dy, *dz;
- double minval;
- double alpha;
- work.converged = 0;
- setup_pointers();
- pre_ops();
-#ifndef ZERO_LIBRARY_MODE
- if (settings.verbose)
- printf("iter objv gap |Ax-b| |Gx+s-h| step\n");
-#endif
- fillq();
- fillh();
- fillb();
- if (settings.better_start)
- better_start();
- else
- set_start();
- for (iter = 0; iter < settings.max_iters; iter++) {
- for (i = 0; i < 0; i++) {
- work.s_inv[i] = 1.0 / work.s[i];
- work.s_inv_z[i] = work.s_inv[i]*work.z[i];
- }
- work.block_33[0] = 0;
- fill_KKT();
- ldl_factor();
- /* Affine scaling directions. */
- fillrhs_aff();
- ldl_solve(work.rhs, work.lhs_aff);
- refine(work.rhs, work.lhs_aff);
- /* Centering plus corrector directions. */
- fillrhs_cc();
- ldl_solve(work.rhs, work.lhs_cc);
- refine(work.rhs, work.lhs_cc);
- /* Add the two together and store in aff. */
- for (i = 0; i < 298; i++)
- work.lhs_aff[i] += work.lhs_cc[i];
- /* Rename aff to reflect its new meaning. */
- dx = work.lhs_aff;
- ds = work.lhs_aff + 198;
- dz = work.lhs_aff + 198;
- dy = work.lhs_aff + 198;
- /* Find min(min(ds./s), min(dz./z)). */
- minval = 0;
- for (i = 0; i < 0; i++)
- if (ds[i] < minval*work.s[i])
- minval = ds[i]/work.s[i];
- for (i = 0; i < 0; i++)
- if (dz[i] < minval*work.z[i])
- minval = dz[i]/work.z[i];
- /* Find alpha. */
- if (-0.99 < minval)
- alpha = 1;
- else
- alpha = -0.99/minval;
- /* Update the primal and dual variables. */
- for (i = 0; i < 198; i++)
- work.x[i] += alpha*dx[i];
- for (i = 0; i < 0; i++)
- work.s[i] += alpha*ds[i];
- for (i = 0; i < 0; i++)
- work.z[i] += alpha*dz[i];
- for (i = 0; i < 100; i++)
- work.y[i] += alpha*dy[i];
- work.gap = eval_gap();
- work.eq_resid_squared = calc_eq_resid_squared();
- work.ineq_resid_squared = calc_ineq_resid_squared();
-#ifndef ZERO_LIBRARY_MODE
- if (settings.verbose) {
- work.optval = eval_objv();
- printf("%3d %10.3e %9.2e %9.2e %9.2e % 6.4f\n",
- iter+1, work.optval, work.gap, sqrt(work.eq_resid_squared),
- sqrt(work.ineq_resid_squared), alpha);
- }
-#endif
- /* Test termination conditions. Requires optimality, and satisfied */
- /* constraints. */
- if ( (work.gap < settings.eps)
- && (work.eq_resid_squared <= settings.resid_tol*settings.resid_tol)
- && (work.ineq_resid_squared <= settings.resid_tol*settings.resid_tol)
- ) {
- work.converged = 1;
- work.optval = eval_objv();
- return iter+1;
- }
- }
- return iter;
-}
diff --git a/solver.h b/solver.h
deleted file mode 100644
index 56b08df97f6adb69d57563020c7357ea567db650..0000000000000000000000000000000000000000
--- a/solver.h
+++ /dev/null
@@ -1,540 +0,0 @@
-/* Produced by CVXGEN, 2016-01-17 08:15:56 -0500. */
-/* CVXGEN is Copyright (C) 2006-2012 Jacob Mattingley, jem@cvxgen.com. */
-/* The code in this file is Copyright (C) 2006-2012 Jacob Mattingley. */
-/* CVXGEN, or solvers produced by CVXGEN, cannot be used for commercial */
-/* applications without prior written permission from Jacob Mattingley. */
-
-/* Filename: solver.h. */
-/* Description: Header file with relevant definitions. */
-#ifndef SOLVER_H
-#define SOLVER_H
-/* Uncomment the next line to remove all library dependencies. */
-/*#define ZERO_LIBRARY_MODE */
-#ifdef MATLAB_MEX_FILE
-/* Matlab functions. MATLAB_MEX_FILE will be defined by the mex compiler. */
-/* If you are not using the mex compiler, this functionality will not intrude, */
-/* as it will be completely disabled at compile-time. */
-#include "mex.h"
-#else
-#ifndef ZERO_LIBRARY_MODE
-#include <stdio.h>
-#endif
-#endif
-/* Space must be allocated somewhere (testsolver.c, csolve.c or your own */
-/* program) for the global variables vars, params, work and settings. */
-/* At the bottom of this file, they are externed. */
-#ifndef ZERO_LIBRARY_MODE
-#include <math.h>
-#define pm(A, m, n) printmatrix(#A, A, m, n, 1)
-#endif
-
-#include <armadillo>
-using namespace arma;
-typedef struct Params_t {
- double traj1_1[1];
- double traj1_2[1];
- double traj1_3[1];
- double traj1_4[1];
- double traj1_5[1];
- double traj1_6[1];
- double traj1_7[1];
- double traj1_8[1];
- double traj1_9[1];
- double traj1_10[1];
- double traj1_11[1];
- double traj1_12[1];
- double traj1_13[1];
- double traj1_14[1];
- double traj1_15[1];
- double traj1_16[1];
- double traj1_17[1];
- double traj1_18[1];
- double traj1_19[1];
- double traj1_20[1];
- double traj1_21[1];
- double traj1_22[1];
- double traj1_23[1];
- double traj1_24[1];
- double traj1_25[1];
- double traj1_26[1];
- double traj1_27[1];
- double traj1_28[1];
- double traj1_29[1];
- double traj1_30[1];
- double traj1_31[1];
- double traj1_32[1];
- double traj1_33[1];
- double traj1_34[1];
- double traj1_35[1];
- double traj1_36[1];
- double traj1_37[1];
- double traj1_38[1];
- double traj1_39[1];
- double traj1_40[1];
- double traj1_41[1];
- double traj1_42[1];
- double traj1_43[1];
- double traj1_44[1];
- double traj1_45[1];
- double traj1_46[1];
- double traj1_47[1];
- double traj1_48[1];
- double traj1_49[1];
- double traj1_50[1];
- double traj1_51[1];
- double traj1_52[1];
- double traj1_53[1];
- double traj1_54[1];
- double traj1_55[1];
- double traj1_56[1];
- double traj1_57[1];
- double traj1_58[1];
- double traj1_59[1];
- double traj1_60[1];
- double traj1_61[1];
- double traj1_62[1];
- double traj1_63[1];
- double traj1_64[1];
- double traj1_65[1];
- double traj1_66[1];
- double traj1_67[1];
- double traj1_68[1];
- double traj1_69[1];
- double traj1_70[1];
- double traj1_71[1];
- double traj1_72[1];
- double traj1_73[1];
- double traj1_74[1];
- double traj1_75[1];
- double traj1_76[1];
- double traj1_77[1];
- double traj1_78[1];
- double traj1_79[1];
- double traj1_80[1];
- double traj1_81[1];
- double traj1_82[1];
- double traj1_83[1];
- double traj1_84[1];
- double traj1_85[1];
- double traj1_86[1];
- double traj1_87[1];
- double traj1_88[1];
- double traj1_89[1];
- double traj1_90[1];
- double traj1_91[1];
- double traj1_92[1];
- double traj1_93[1];
- double traj1_94[1];
- double traj1_95[1];
- double traj1_96[1];
- double traj1_97[1];
- double traj1_98[1];
- double traj1_99[1];
- double traj1_100[1];
- double lambda[1];
- double traj2_first[1];
- double traj2_second[1];
- double *traj1[101];
-} Params;
-typedef struct Vars_t {
- double *x_1; /* 1 rows. */
- double *x_2; /* 1 rows. */
- double *x_3; /* 1 rows. */
- double *x_4; /* 1 rows. */
- double *x_5; /* 1 rows. */
- double *x_6; /* 1 rows. */
- double *x_7; /* 1 rows. */
- double *x_8; /* 1 rows. */
- double *x_9; /* 1 rows. */
- double *x_10; /* 1 rows. */
- double *x_11; /* 1 rows. */
- double *x_12; /* 1 rows. */
- double *x_13; /* 1 rows. */
- double *x_14; /* 1 rows. */
- double *x_15; /* 1 rows. */
- double *x_16; /* 1 rows. */
- double *x_17; /* 1 rows. */
- double *x_18; /* 1 rows. */
- double *x_19; /* 1 rows. */
- double *x_20; /* 1 rows. */
- double *x_21; /* 1 rows. */
- double *x_22; /* 1 rows. */
- double *x_23; /* 1 rows. */
- double *x_24; /* 1 rows. */
- double *x_25; /* 1 rows. */
- double *x_26; /* 1 rows. */
- double *x_27; /* 1 rows. */
- double *x_28; /* 1 rows. */
- double *x_29; /* 1 rows. */
- double *x_30; /* 1 rows. */
- double *x_31; /* 1 rows. */
- double *x_32; /* 1 rows. */
- double *x_33; /* 1 rows. */
- double *x_34; /* 1 rows. */
- double *x_35; /* 1 rows. */
- double *x_36; /* 1 rows. */
- double *x_37; /* 1 rows. */
- double *x_38; /* 1 rows. */
- double *x_39; /* 1 rows. */
- double *x_40; /* 1 rows. */
- double *x_41; /* 1 rows. */
- double *x_42; /* 1 rows. */
- double *x_43; /* 1 rows. */
- double *x_44; /* 1 rows. */
- double *x_45; /* 1 rows. */
- double *x_46; /* 1 rows. */
- double *x_47; /* 1 rows. */
- double *x_48; /* 1 rows. */
- double *x_49; /* 1 rows. */
- double *x_50; /* 1 rows. */
- double *x_51; /* 1 rows. */
- double *x_52; /* 1 rows. */
- double *x_53; /* 1 rows. */
- double *x_54; /* 1 rows. */
- double *x_55; /* 1 rows. */
- double *x_56; /* 1 rows. */
- double *x_57; /* 1 rows. */
- double *x_58; /* 1 rows. */
- double *x_59; /* 1 rows. */
- double *x_60; /* 1 rows. */
- double *x_61; /* 1 rows. */
- double *x_62; /* 1 rows. */
- double *x_63; /* 1 rows. */
- double *x_64; /* 1 rows. */
- double *x_65; /* 1 rows. */
- double *x_66; /* 1 rows. */
- double *x_67; /* 1 rows. */
- double *x_68; /* 1 rows. */
- double *x_69; /* 1 rows. */
- double *x_70; /* 1 rows. */
- double *x_71; /* 1 rows. */
- double *x_72; /* 1 rows. */
- double *x_73; /* 1 rows. */
- double *x_74; /* 1 rows. */
- double *x_75; /* 1 rows. */
- double *x_76; /* 1 rows. */
- double *x_77; /* 1 rows. */
- double *x_78; /* 1 rows. */
- double *x_79; /* 1 rows. */
- double *x_80; /* 1 rows. */
- double *x_81; /* 1 rows. */
- double *x_82; /* 1 rows. */
- double *x_83; /* 1 rows. */
- double *x_84; /* 1 rows. */
- double *x_85; /* 1 rows. */
- double *x_86; /* 1 rows. */
- double *x_87; /* 1 rows. */
- double *x_88; /* 1 rows. */
- double *x_89; /* 1 rows. */
- double *x_90; /* 1 rows. */
- double *x_91; /* 1 rows. */
- double *x_92; /* 1 rows. */
- double *x_93; /* 1 rows. */
- double *x_94; /* 1 rows. */
- double *x_95; /* 1 rows. */
- double *x_96; /* 1 rows. */
- double *x_97; /* 1 rows. */
- double *x_98; /* 1 rows. */
- double *x_99; /* 1 rows. */
- double *x_100; /* 1 rows. */
- double *t_01; /* 1 rows. */
- double *t_02; /* 1 rows. */
- double *t_03; /* 1 rows. */
- double *t_04; /* 1 rows. */
- double *t_05; /* 1 rows. */
- double *t_06; /* 1 rows. */
- double *t_07; /* 1 rows. */
- double *t_08; /* 1 rows. */
- double *t_09; /* 1 rows. */
- double *t_10; /* 1 rows. */
- double *t_11; /* 1 rows. */
- double *t_12; /* 1 rows. */
- double *t_13; /* 1 rows. */
- double *t_14; /* 1 rows. */
- double *t_15; /* 1 rows. */
- double *t_16; /* 1 rows. */
- double *t_17; /* 1 rows. */
- double *t_18; /* 1 rows. */
- double *t_19; /* 1 rows. */
- double *t_20; /* 1 rows. */
- double *t_21; /* 1 rows. */
- double *t_22; /* 1 rows. */
- double *t_23; /* 1 rows. */
- double *t_24; /* 1 rows. */
- double *t_25; /* 1 rows. */
- double *t_26; /* 1 rows. */
- double *t_27; /* 1 rows. */
- double *t_28; /* 1 rows. */
- double *t_29; /* 1 rows. */
- double *t_30; /* 1 rows. */
- double *t_31; /* 1 rows. */
- double *t_32; /* 1 rows. */
- double *t_33; /* 1 rows. */
- double *t_34; /* 1 rows. */
- double *t_35; /* 1 rows. */
- double *t_36; /* 1 rows. */
- double *t_37; /* 1 rows. */
- double *t_38; /* 1 rows. */
- double *t_39; /* 1 rows. */
- double *t_40; /* 1 rows. */
- double *t_41; /* 1 rows. */
- double *t_42; /* 1 rows. */
- double *t_43; /* 1 rows. */
- double *t_44; /* 1 rows. */
- double *t_45; /* 1 rows. */
- double *t_46; /* 1 rows. */
- double *t_47; /* 1 rows. */
- double *t_48; /* 1 rows. */
- double *t_49; /* 1 rows. */
- double *t_50; /* 1 rows. */
- double *t_51; /* 1 rows. */
- double *t_52; /* 1 rows. */
- double *t_53; /* 1 rows. */
- double *t_54; /* 1 rows. */
- double *t_55; /* 1 rows. */
- double *t_56; /* 1 rows. */
- double *t_57; /* 1 rows. */
- double *t_58; /* 1 rows. */
- double *t_59; /* 1 rows. */
- double *t_60; /* 1 rows. */
- double *t_61; /* 1 rows. */
- double *t_62; /* 1 rows. */
- double *t_63; /* 1 rows. */
- double *t_64; /* 1 rows. */
- double *t_65; /* 1 rows. */
- double *t_66; /* 1 rows. */
- double *t_67; /* 1 rows. */
- double *t_68; /* 1 rows. */
- double *t_69; /* 1 rows. */
- double *t_70; /* 1 rows. */
- double *t_71; /* 1 rows. */
- double *t_72; /* 1 rows. */
- double *t_73; /* 1 rows. */
- double *t_74; /* 1 rows. */
- double *t_75; /* 1 rows. */
- double *t_76; /* 1 rows. */
- double *t_77; /* 1 rows. */
- double *t_78; /* 1 rows. */
- double *t_79; /* 1 rows. */
- double *t_80; /* 1 rows. */
- double *t_81; /* 1 rows. */
- double *t_82; /* 1 rows. */
- double *t_83; /* 1 rows. */
- double *t_84; /* 1 rows. */
- double *t_85; /* 1 rows. */
- double *t_86; /* 1 rows. */
- double *t_87; /* 1 rows. */
- double *t_88; /* 1 rows. */
- double *t_89; /* 1 rows. */
- double *t_90; /* 1 rows. */
- double *t_91; /* 1 rows. */
- double *t_92; /* 1 rows. */
- double *t_93; /* 1 rows. */
- double *t_94; /* 1 rows. */
- double *t_95; /* 1 rows. */
- double *t_96; /* 1 rows. */
- double *t_97; /* 1 rows. */
- double *t_98; /* 1 rows. */
- double *x[101];
-} Vars;
-typedef struct Workspace_t {
- double *h;
- double *s_inv;
- double *s_inv_z;
- double b[100];
- double q[198];
- double rhs[298];
- double x[298];
- double *s;
- double *z;
- double *y;
- double lhs_aff[298];
- double lhs_cc[298];
- double buffer[298];
- double buffer2[298];
- double KKT[593];
- double L[493];
- double d[298];
- double v[298];
- double d_inv[298];
- double gap;
- double optval;
- double ineq_resid_squared;
- double eq_resid_squared;
- double block_33[1];
- /* Pre-op symbols. */
- double quad_309377593344[1];
- double quad_170383695872[1];
- double quad_112192413696[1];
- double quad_271369404416[1];
- double quad_974379421696[1];
- double quad_880958541824[1];
- double quad_572094451712[1];
- double quad_789148684288[1];
- double quad_448304680960[1];
- double quad_196635672576[1];
- double quad_489733091328[1];
- double quad_802233995264[1];
- double quad_865562517504[1];
- double quad_910621253632[1];
- double quad_321513025536[1];
- double quad_997344780288[1];
- double quad_448780910592[1];
- double quad_115901485056[1];
- double quad_782888361984[1];
- double quad_609463140352[1];
- double quad_88437858304[1];
- double quad_984663347200[1];
- double quad_230162866176[1];
- double quad_416382242816[1];
- double quad_380873158656[1];
- double quad_556464291840[1];
- double quad_266292043776[1];
- double quad_970572640256[1];
- double quad_39497891840[1];
- double quad_701607514112[1];
- double quad_181586403328[1];
- double quad_260463075328[1];
- double quad_382856859648[1];
- double quad_199224414208[1];
- double quad_511656841216[1];
- double quad_171214155776[1];
- double quad_70639910912[1];
- double quad_463249276928[1];
- double quad_244412502016[1];
- double quad_258282409984[1];
- double quad_321028120576[1];
- double quad_30674518016[1];
- double quad_824505257984[1];
- double quad_484111179776[1];
- double quad_675570683904[1];
- double quad_386911121408[1];
- double quad_932702867456[1];
- double quad_274526810112[1];
- double quad_96616189952[1];
- double quad_711864590336[1];
- double quad_200392192000[1];
- double quad_150270558208[1];
- double quad_598460686336[1];
- double quad_831592636416[1];
- double quad_874189471744[1];
- double quad_632352092160[1];
- double quad_969116119040[1];
- double quad_610911072256[1];
- double quad_606707699712[1];
- double quad_449159319552[1];
- double quad_700922720256[1];
- double quad_911067136000[1];
- double quad_304706789376[1];
- double quad_530900680704[1];
- double quad_307691851776[1];
- double quad_875119529984[1];
- double quad_330167877632[1];
- double quad_106008100864[1];
- double quad_586821791744[1];
- double quad_62270459904[1];
- double quad_705160380416[1];
- double quad_566627430400[1];
- double quad_833211822080[1];
- double quad_28799266816[1];
- double quad_442252132352[1];
- double quad_713742774272[1];
- double quad_447903891456[1];
- double quad_655034785792[1];
- double quad_576686788608[1];
- double quad_288749699072[1];
- double quad_866096066560[1];
- double quad_345799389184[1];
- double quad_335292588032[1];
- double quad_292534816768[1];
- double quad_832380674048[1];
- double quad_934938374144[1];
- double quad_265476059136[1];
- double quad_478224822272[1];
- double quad_901249572864[1];
- double quad_216942473216[1];
- double quad_227493912576[1];
- double quad_458072092672[1];
- double quad_912331198464[1];
- double quad_307004768256[1];
- double quad_284125421568[1];
- double quad_704038281216[1];
- double quad_401987686400[1];
- double quad_592084643840[1];
- double quad_8875266048[1];
- double quad_270270570496[1];
- int converged;
-} Workspace;
-typedef struct Settings_t {
- double resid_tol;
- double eps;
- int max_iters;
- int refine_steps;
- int better_start;
- /* Better start obviates the need for s_init and z_init. */
- double s_init;
- double z_init;
- int verbose;
- /* Show extra details of the iterative refinement steps. */
- int verbose_refinement;
- int debug;
- /* For regularization. Minimum value of abs(D_ii) in the kkt D factor. */
- double kkt_reg;
-} Settings;
-extern Vars vars;
-extern Params params;
-extern Workspace work;
-extern Settings settings;
-/* Function definitions in ldl.c: */
-void ldl_solve(double *target, double *var);
-void ldl_factor(void);
-double check_factorization(void);
-void matrix_multiply(double *result, double *source);
-double check_residual(double *target, double *multiplicand);
-void fill_KKT(void);
-
-/* Function definitions in matrix_support.c: */
-void multbymA(double *lhs, double *rhs);
-void multbymAT(double *lhs, double *rhs);
-void multbymG(double *lhs, double *rhs);
-void multbymGT(double *lhs, double *rhs);
-void multbyP(double *lhs, double *rhs);
-void fillq(void);
-void fillh(void);
-void fillb(void);
-void pre_ops(void);
-
-/* Function definitions in solver.c: */
-double eval_gap(void);
-void set_defaults(void);
-void setup_pointers(void);
-void setup_indexed_params(void);
-void setup_indexed_optvars(void);
-void setup_indexing(void);
-void set_start(void);
-double eval_objv(void);
-void fillrhs_aff(void);
-void fillrhs_cc(void);
-void refine(double *target, double *var);
-double calc_ineq_resid_squared(void);
-double calc_eq_resid_squared(void);
-void better_start(void);
-void fillrhs_start(void);
-long solve(void);
-
-/* Function definitions in testsolver.c: */
-int main(int argc, char **argv);
-void load_default_data(const mat& traj1, const mat& traj2);
-
-/* Function definitions in util.c: */
-void tic(void);
-float toc(void);
-float tocq(void);
-void printmatrix(char *name, double *A, int m, int n, int sparse);
-double unif(double lower, double upper);
-float ran1(long*idum, int reset);
-float randn_internal(long *idum, int reset);
-double randn(void);
-void reset_rand(void);
-
-#endif
diff --git a/traj2dmp.cc b/traj2dmp.cc
index 6b2ef24cef76bc09bc5ba0921b40727a8b98281f..efc056b61583da861b65e8752b3665036bb64a9b 100644
--- a/traj2dmp.cc
+++ b/traj2dmp.cc
@@ -13,15 +13,16 @@ mat smartDiff(const mat& inMat) { //now supports col vecs only
}
Dmp traj2dmp(const mat& trajectory, const double& dt) {
-
+ int nbrDims = trajectory.n_cols; // Number of dimensions
double tau;;
- mat g = zeros<mat>(7,1);
- mat w = zeros<mat>(15,7);
+ mat g = zeros<mat>(nbrDims,1);
+ mat w = zeros<mat>(15,nbrDims);
static double alpha_g = 12.5;
static double alpha_v = 25;
static double alpha_z = 25;
static double beta_v = 6.25;
- static double beta_z = 6.25;
+ //static double beta_z = 6.25;
+ static double beta_z = 8;
static double n_rfs = 15; // number of weight functions
mat arr = linspace<mat>(0, 1, n_rfs)*0.5;
mat c = (1+alpha_z/2*arr)%exp(-alpha_z/2*arr);
@@ -31,7 +32,7 @@ Dmp traj2dmp(const mat& trajectory, const double& dt) {
D = 1/join_vert(D, Dlast);
- for (int j = 0; j < trajectory.n_cols; ++j) {
+ for (int j = 0; j < nbrDims; ++j) {
mat T = trajectory.col(j);
mat TD = smartDiff(T)/dt;
mat TDD = smartDiff(TD)/dt;
@@ -74,7 +75,8 @@ Dmp traj2dmp(const mat& trajectory, const double& dt) {
mat sxtd = sum(((V%Ft)*ones(1,c.n_rows))%PSI).t();
w.col(j) = sxtd/(sx2+1.e-10);
}
- tau = trajectory.n_rows * dt;
+ tau = 0.5 / (trajectory.n_rows * dt);
return Dmp(w, g, tau);
}
+
diff --git a/yCorr.txt b/yCorr.txt
index 3f4185c3cbb91b37bded6169227d02896fd85333..1fa85dd452fed8d7e04e9e301ed70d54e7f16b99 100644
--- a/yCorr.txt
+++ b/yCorr.txt
@@ -1,76 +1,360 @@
- 3.0223695e+02 -3.9834919e+01 -2.0297419e+02
- 3.0223591e+02 -3.9835325e+01 -2.0297487e+02
- 3.0223686e+02 -3.9832731e+01 -2.0297521e+02
- 3.0223692e+02 -3.9834239e+01 -2.0297520e+02
- 3.0223657e+02 -3.9834502e+01 -2.0297487e+02
- 3.0223689e+02 -3.9834719e+01 -2.0297465e+02
- 3.0223708e+02 -3.9834659e+01 -2.0297420e+02
- 3.0223711e+02 -3.9834843e+01 -2.0297545e+02
- 3.0223638e+02 -3.9834673e+01 -2.0297430e+02
- 3.0223525e+02 -3.9835508e+01 -2.0297332e+02
- 3.0223721e+02 -3.9833826e+01 -2.0297526e+02
- 3.0224879e+02 -3.9822272e+01 -2.0298614e+02
- 3.0223990e+02 -3.9829889e+01 -2.0297635e+02
- 3.0237299e+02 -3.9899177e+01 -2.0283473e+02
- 3.0349354e+02 -4.0989845e+01 -2.0087493e+02
- 3.0380491e+02 -4.1538497e+01 -1.9983147e+02
- 3.0416867e+02 -4.1684898e+01 -1.9870055e+02
- 3.0418522e+02 -4.0523534e+01 -1.9825814e+02
- 3.0503675e+02 -4.1026825e+01 -1.9523339e+02
- 3.0510448e+02 -4.1000287e+01 -1.9390557e+02
- 3.0526585e+02 -3.8165719e+01 -1.9025976e+02
- 3.0460684e+02 -3.6666290e+01 -1.8461684e+02
- 3.0489587e+02 -3.5777473e+01 -1.7983821e+02
- 3.0458161e+02 -3.3167132e+01 -1.7710945e+02
- 3.0398068e+02 -2.9601744e+01 -1.7398172e+02
- 3.0272250e+02 -2.5806459e+01 -1.7204405e+02
- 3.0211820e+02 -2.5272499e+01 -1.7205352e+02
- 3.0183699e+02 -2.5026116e+01 -1.7250083e+02
- 3.0209857e+02 -2.4415831e+01 -1.7427006e+02
- 3.0249621e+02 -2.4170410e+01 -1.7598460e+02
- 3.0175846e+02 -2.6807308e+01 -1.7736552e+02
- 3.0210448e+02 -2.7008256e+01 -1.8170757e+02
- 3.0257423e+02 -2.8706595e+01 -1.8470767e+02
- 3.0138933e+02 -2.8727643e+01 -1.8820684e+02
- 3.0229662e+02 -2.8698888e+01 -1.9049696e+02
- 3.0168589e+02 -3.0408833e+01 -1.9155020e+02
- 3.0073978e+02 -3.0545191e+01 -1.9295977e+02
- 3.0105268e+02 -2.9967830e+01 -1.9488827e+02
- 3.0128969e+02 -2.8819625e+01 -1.9731451e+02
- 3.0152293e+02 -2.9056384e+01 -1.9773305e+02
- 3.0292551e+02 -2.8930579e+01 -1.9894957e+02
- 3.0282002e+02 -2.9994016e+01 -1.9935546e+02
- 3.0183690e+02 -2.9714251e+01 -2.0109038e+02
- 3.0155707e+02 -3.0923319e+01 -2.0111282e+02
- 3.0114863e+02 -3.1262125e+01 -2.0161330e+02
- 3.0080948e+02 -3.1330917e+01 -2.0199235e+02
- 3.0066369e+02 -3.1904731e+01 -2.0230181e+02
- 3.0089914e+02 -3.2239725e+01 -2.0257610e+02
- 3.0086561e+02 -3.1910751e+01 -2.0378762e+02
- 3.0126695e+02 -3.2142638e+01 -2.0411564e+02
- 3.0170184e+02 -3.2050087e+01 -2.0456446e+02
- 3.0142283e+02 -3.2177461e+01 -2.0513109e+02
- 3.0050868e+02 -3.2393142e+01 -2.0609014e+02
- 2.9994504e+02 -3.2161979e+01 -2.0740855e+02
- 2.9986788e+02 -3.2332645e+01 -2.0759383e+02
- 2.9952583e+02 -3.2648627e+01 -2.0849387e+02
- 2.9985118e+02 -3.2264215e+01 -2.1134744e+02
- 3.0033634e+02 -3.2168657e+01 -2.1179252e+02
- 3.0065482e+02 -3.2054062e+01 -2.1209720e+02
- 3.0105754e+02 -3.1678614e+01 -2.1257322e+02
- 3.0118408e+02 -3.1253063e+01 -2.1317607e+02
- 3.0135382e+02 -3.1052778e+01 -2.1352918e+02
- 3.0125728e+02 -3.1329922e+01 -2.1393510e+02
- 3.0074174e+02 -3.1504056e+01 -2.1459154e+02
- 3.0066282e+02 -3.1426728e+01 -2.1549018e+02
- 3.0100472e+02 -3.1193307e+01 -2.1647271e+02
- 3.0054018e+02 -3.1543460e+01 -2.1851332e+02
- 3.0066610e+02 -3.1176024e+01 -2.1964827e+02
- 3.0095334e+02 -3.1299501e+01 -2.1974256e+02
- 3.0136688e+02 -3.1178770e+01 -2.2002438e+02
- 3.0155365e+02 -3.1113655e+01 -2.2031908e+02
- 3.0198413e+02 -3.0930672e+01 -2.2081381e+02
- 3.0232691e+02 -3.0838242e+01 -2.2156517e+02
- 3.0253697e+02 -3.0778464e+01 -2.2211745e+02
- 3.0276447e+02 -3.0753085e+01 -2.2316257e+02
- 3.0294690e+02 -3.0679013e+01 -2.2429655e+02
+ 4.3605988e+02 -6.7619013e+01 -2.4550054e+02
+ 4.3606081e+02 -6.7615225e+01 -2.4549979e+02
+ 4.3606090e+02 -6.7616406e+01 -2.4549926e+02
+ 4.3605886e+02 -6.7616217e+01 -2.4550194e+02
+ 4.3606006e+02 -6.7619367e+01 -2.4550061e+02
+ 4.3605941e+02 -6.7616390e+01 -2.4550051e+02
+ 4.3605941e+02 -6.7618121e+01 -2.4550117e+02
+ 4.3606003e+02 -6.7617203e+01 -2.4550038e+02
+ 4.3605939e+02 -6.7618223e+01 -2.4550052e+02
+ 4.3605933e+02 -6.7618592e+01 -2.4550104e+02
+ 4.3606036e+02 -6.7618074e+01 -2.4549795e+02
+ 4.3605990e+02 -6.7617547e+01 -2.4550052e+02
+ 4.3606019e+02 -6.7614628e+01 -2.4550018e+02
+ 4.3605966e+02 -6.7617531e+01 -2.4550162e+02
+ 4.3605823e+02 -6.7619106e+01 -2.4550301e+02
+ 4.3605839e+02 -6.7618217e+01 -2.4550310e+02
+ 4.3606094e+02 -6.7617744e+01 -2.4549946e+02
+ 4.3605980e+02 -6.7616020e+01 -2.4550110e+02
+ 4.3605917e+02 -6.7620927e+01 -2.4550279e+02
+ 4.3606006e+02 -6.7617227e+01 -2.4550049e+02
+ 4.3605987e+02 -6.7619014e+01 -2.4550063e+02
+ 4.3605993e+02 -6.7614385e+01 -2.4549944e+02
+ 4.3605894e+02 -6.7619588e+01 -2.4550128e+02
+ 4.3607628e+02 -6.7660451e+01 -2.4548194e+02
+ 4.3608962e+02 -6.7694019e+01 -2.4546488e+02
+ 4.3608972e+02 -6.7691529e+01 -2.4546631e+02
+ 4.3608885e+02 -6.7690711e+01 -2.4546788e+02
+ 4.3609051e+02 -6.7693604e+01 -2.4546605e+02
+ 4.3608902e+02 -6.7691013e+01 -2.4546678e+02
+ 4.3608831e+02 -6.7695123e+01 -2.4546922e+02
+ 4.3609029e+02 -6.7693803e+01 -2.4546547e+02
+ 4.3608767e+02 -6.7696083e+01 -2.4546889e+02
+ 4.3608919e+02 -6.7694221e+01 -2.4546807e+02
+ 4.3608888e+02 -6.7695031e+01 -2.4546915e+02
+ 4.3608852e+02 -6.7690927e+01 -2.4546819e+02
+ 4.3608944e+02 -6.7695656e+01 -2.4546782e+02
+ 4.3609005e+02 -6.7689771e+01 -2.4546571e+02
+ 4.3608936e+02 -6.7695146e+01 -2.4546808e+02
+ 4.3608942e+02 -6.7695456e+01 -2.4546636e+02
+ 4.3609018e+02 -6.7693278e+01 -2.4546503e+02
+ 4.3609139e+02 -6.7693090e+01 -2.4546280e+02
+ 4.3609106e+02 -6.7692733e+01 -2.4546511e+02
+ 4.3609070e+02 -6.7691824e+01 -2.4546380e+02
+ 4.3609023e+02 -6.7693077e+01 -2.4546251e+02
+ 4.3609165e+02 -6.7696469e+01 -2.4545627e+02
+ 4.3609223e+02 -6.7694318e+01 -2.4545757e+02
+ 4.3609033e+02 -6.7694969e+01 -2.4546006e+02
+ 4.3609168e+02 -6.7696258e+01 -2.4545757e+02
+ 4.3609462e+02 -6.7694137e+01 -2.4545434e+02
+ 4.3609503e+02 -6.7702260e+01 -2.4545431e+02
+ 4.3609700e+02 -6.7705858e+01 -2.4545343e+02
+ 4.3610132e+02 -6.7726055e+01 -2.4544761e+02
+ 4.3616497e+02 -6.7881866e+01 -2.4537644e+02
+ 4.3623984e+02 -6.8073951e+01 -2.4528919e+02
+ 4.3626126e+02 -6.8124503e+01 -2.4526542e+02
+ 4.3629900e+02 -6.8230802e+01 -2.4522412e+02
+ 4.3641881e+02 -6.8539642e+01 -2.4508818e+02
+ 4.3655002e+02 -6.8878403e+01 -2.4493589e+02
+ 4.3665956e+02 -6.9157974e+01 -2.4481106e+02
+ 4.3680204e+02 -6.9532447e+01 -2.4464552e+02
+ 4.3700604e+02 -7.0062758e+01 -2.4441018e+02
+ 4.3723692e+02 -7.0629976e+01 -2.4405025e+02
+ 4.3745276e+02 -7.0876607e+01 -2.4313999e+02
+ 4.3764455e+02 -7.0954967e+01 -2.4208196e+02
+ 4.3778063e+02 -7.0993802e+01 -2.4125888e+02
+ 4.3789328e+02 -7.1104861e+01 -2.4073629e+02
+ 4.3793746e+02 -7.1169331e+01 -2.4057396e+02
+ 4.3795223e+02 -7.1202150e+01 -2.4052049e+02
+ 4.3794428e+02 -7.1189597e+01 -2.4049388e+02
+ 4.3794056e+02 -7.1205183e+01 -2.4049563e+02
+ 4.3793520e+02 -7.1214725e+01 -2.4048081e+02
+ 4.3794438e+02 -7.1279566e+01 -2.4043753e+02
+ 4.3793022e+02 -7.1551078e+01 -2.4040192e+02
+ 4.3783439e+02 -7.1868868e+01 -2.4043284e+02
+ 4.3772817e+02 -7.2214586e+01 -2.4050302e+02
+ 4.3765516e+02 -7.2419774e+01 -2.4054675e+02
+ 4.3762785e+02 -7.2492960e+01 -2.4050207e+02
+ 4.3761670e+02 -7.2481231e+01 -2.4039291e+02
+ 4.3762435e+02 -7.2542074e+01 -2.4016308e+02
+ 4.3770229e+02 -7.2824700e+01 -2.3992245e+02
+ 4.3778919e+02 -7.3225683e+01 -2.3973844e+02
+ 4.3776226e+02 -7.3338957e+01 -2.3956653e+02
+ 4.3771858e+02 -7.3275548e+01 -2.3907586e+02
+ 4.3770184e+02 -7.2901370e+01 -2.3805057e+02
+ 4.3769303e+02 -7.2469084e+01 -2.3697582e+02
+ 4.3758744e+02 -7.2256074e+01 -2.3610730e+02
+ 4.3748367e+02 -7.2500519e+01 -2.3596512e+02
+ 4.3728555e+02 -7.3072209e+01 -2.3614562e+02
+ 4.3709358e+02 -7.3418075e+01 -2.3626432e+02
+ 4.3697462e+02 -7.3743222e+01 -2.3619639e+02
+ 4.3691095e+02 -7.3750641e+01 -2.3576467e+02
+ 4.3687527e+02 -7.3651451e+01 -2.3514997e+02
+ 4.3683923e+02 -7.3753493e+01 -2.3481500e+02
+ 4.3678663e+02 -7.4063214e+01 -2.3469873e+02
+ 4.3672447e+02 -7.4508593e+01 -2.3462010e+02
+ 4.3669359e+02 -7.4824563e+01 -2.3448497e+02
+ 4.3667374e+02 -7.5032032e+01 -2.3406589e+02
+ 4.3661055e+02 -7.4954784e+01 -2.3309387e+02
+ 4.3660905e+02 -7.4830989e+01 -2.3232538e+02
+ 4.3656857e+02 -7.4698109e+01 -2.3162142e+02
+ 4.3650661e+02 -7.4695751e+01 -2.3104395e+02
+ 4.3641653e+02 -7.4795227e+01 -2.3092178e+02
+ 4.3617606e+02 -7.4862809e+01 -2.3068826e+02
+ 4.3604971e+02 -7.4777756e+01 -2.2984775e+02
+ 4.3604007e+02 -7.4753749e+01 -2.2868407e+02
+ 4.3597193e+02 -7.4546014e+01 -2.2744058e+02
+ 4.3603815e+02 -7.4461440e+01 -2.2651740e+02
+ 4.3610014e+02 -7.4529802e+01 -2.2613006e+02
+ 4.3618403e+02 -7.4754566e+01 -2.2600595e+02
+ 4.3630603e+02 -7.5093506e+01 -2.2584363e+02
+ 4.3637316e+02 -7.5266973e+01 -2.2572408e+02
+ 4.3638057e+02 -7.5275438e+01 -2.2566746e+02
+ 4.3638473e+02 -7.5255586e+01 -2.2560551e+02
+ 4.3638556e+02 -7.5240383e+01 -2.2558155e+02
+ 4.3639186e+02 -7.5232213e+01 -2.2556426e+02
+ 4.3639581e+02 -7.5220019e+01 -2.2555305e+02
+ 4.3639892e+02 -7.5209182e+01 -2.2554928e+02
+ 4.3641403e+02 -7.5153524e+01 -2.2553834e+02
+ 4.3643851e+02 -7.5071635e+01 -2.2552278e+02
+ 4.3647081e+02 -7.4967233e+01 -2.2549856e+02
+ 4.3655044e+02 -7.4706268e+01 -2.2543898e+02
+ 4.3667000e+02 -7.4304602e+01 -2.2535351e+02
+ 4.3682311e+02 -7.3799970e+01 -2.2523648e+02
+ 4.3704173e+02 -7.3411246e+01 -2.2504696e+02
+ 4.3739680e+02 -7.3510148e+01 -2.2482338e+02
+ 4.3767729e+02 -7.3555828e+01 -2.2468040e+02
+ 4.3786108e+02 -7.3393541e+01 -2.2476718e+02
+ 4.3798703e+02 -7.3089456e+01 -2.2489794e+02
+ 4.3816613e+02 -7.2815749e+01 -2.2529385e+02
+ 4.3838522e+02 -7.2552412e+01 -2.2599695e+02
+ 4.3858922e+02 -7.2238666e+01 -2.2628347e+02
+ 4.3876455e+02 -7.2074280e+01 -2.2636488e+02
+ 4.3895018e+02 -7.1935247e+01 -2.2639859e+02
+ 4.3930574e+02 -7.1847381e+01 -2.2642521e+02
+ 4.3964662e+02 -7.1712967e+01 -2.2657483e+02
+ 4.3984794e+02 -7.1320402e+01 -2.2680812e+02
+ 4.4005134e+02 -7.0913760e+01 -2.2707746e+02
+ 4.4030856e+02 -7.0640746e+01 -2.2762879e+02
+ 4.4067022e+02 -7.0443799e+01 -2.2826565e+02
+ 4.4119066e+02 -7.0585318e+01 -2.2888245e+02
+ 4.4178430e+02 -7.0645601e+01 -2.2940340e+02
+ 4.4236276e+02 -7.0674022e+01 -2.2980610e+02
+ 4.4294527e+02 -7.0442735e+01 -2.3009266e+02
+ 4.4327663e+02 -6.9913969e+01 -2.3016691e+02
+ 4.4353320e+02 -6.9412747e+01 -2.3042787e+02
+ 4.4383510e+02 -6.9135953e+01 -2.3096591e+02
+ 4.4410977e+02 -6.9064410e+01 -2.3111094e+02
+ 4.4433897e+02 -6.9107651e+01 -2.3097396e+02
+ 4.4446520e+02 -6.9037109e+01 -2.3111772e+02
+ 4.4462202e+02 -6.9067297e+01 -2.3193858e+02
+ 4.4480255e+02 -6.8904959e+01 -2.3285547e+02
+ 4.4503807e+02 -6.8580409e+01 -2.3384480e+02
+ 4.4522354e+02 -6.8292194e+01 -2.3456304e+02
+ 4.4538496e+02 -6.7980820e+01 -2.3488799e+02
+ 4.4559498e+02 -6.7787902e+01 -2.3526606e+02
+ 4.4587147e+02 -6.7754636e+01 -2.3559870e+02
+ 4.4599055e+02 -6.7842588e+01 -2.3605457e+02
+ 4.4601376e+02 -6.8004810e+01 -2.3663200e+02
+ 4.4603502e+02 -6.8107554e+01 -2.3695877e+02
+ 4.4608729e+02 -6.7986758e+01 -2.3700851e+02
+ 4.4615918e+02 -6.7791622e+01 -2.3705662e+02
+ 4.4627401e+02 -6.7457552e+01 -2.3708261e+02
+ 4.4646556e+02 -6.7196789e+01 -2.3717902e+02
+ 4.4686351e+02 -6.6992311e+01 -2.3731733e+02
+ 4.4722026e+02 -6.6628141e+01 -2.3744638e+02
+ 4.4747009e+02 -6.6275285e+01 -2.3771029e+02
+ 4.4760340e+02 -6.6071448e+01 -2.3806835e+02
+ 4.4776849e+02 -6.6194495e+01 -2.3865237e+02
+ 4.4798479e+02 -6.6412726e+01 -2.3906429e+02
+ 4.4816220e+02 -6.6549757e+01 -2.3920418e+02
+ 4.4824454e+02 -6.6469248e+01 -2.3930417e+02
+ 4.4833660e+02 -6.6373442e+01 -2.3946839e+02
+ 4.4845855e+02 -6.6416625e+01 -2.4002734e+02
+ 4.4859287e+02 -6.6410307e+01 -2.4057024e+02
+ 4.4876690e+02 -6.6252178e+01 -2.4069958e+02
+ 4.4900502e+02 -6.6099802e+01 -2.4107743e+02
+ 4.4926655e+02 -6.6056307e+01 -2.4177172e+02
+ 4.4939053e+02 -6.5802197e+01 -2.4189089e+02
+ 4.4944599e+02 -6.5625890e+01 -2.4185673e+02
+ 4.4951250e+02 -6.5422623e+01 -2.4187590e+02
+ 4.4961608e+02 -6.5142314e+01 -2.4202101e+02
+ 4.4971000e+02 -6.5006159e+01 -2.4258829e+02
+ 4.4976810e+02 -6.4990343e+01 -2.4321976e+02
+ 4.4980650e+02 -6.4907210e+01 -2.4329551e+02
+ 4.4985126e+02 -6.4904801e+01 -2.4325957e+02
+ 4.5003806e+02 -6.5042080e+01 -2.4306774e+02
+ 4.5014779e+02 -6.5085592e+01 -2.4298232e+02
+ 4.5016420e+02 -6.5056378e+01 -2.4306015e+02
+ 4.5019751e+02 -6.4993114e+01 -2.4309654e+02
+ 4.5024177e+02 -6.4932666e+01 -2.4316069e+02
+ 4.5029559e+02 -6.4999220e+01 -2.4358632e+02
+ 4.5040785e+02 -6.5140239e+01 -2.4446254e+02
+ 4.5045477e+02 -6.5260783e+01 -2.4515952e+02
+ 4.5054164e+02 -6.5343620e+01 -2.4573588e+02
+ 4.5066904e+02 -6.5289829e+01 -2.4595720e+02
+ 4.5092975e+02 -6.5235677e+01 -2.4575296e+02
+ 4.5120911e+02 -6.5452009e+01 -2.4563379e+02
+ 4.5145537e+02 -6.5649030e+01 -2.4562097e+02
+ 4.5166232e+02 -6.5923641e+01 -2.4562678e+02
+ 4.5173865e+02 -6.6131787e+01 -2.4584357e+02
+ 4.5186987e+02 -6.6185771e+01 -2.4610758e+02
+ 4.5209398e+02 -6.5988142e+01 -2.4621076e+02
+ 4.5236034e+02 -6.5825738e+01 -2.4627123e+02
+ 4.5260724e+02 -6.5678796e+01 -2.4637867e+02
+ 4.5285430e+02 -6.5578953e+01 -2.4664594e+02
+ 4.5299803e+02 -6.5410539e+01 -2.4698862e+02
+ 4.5303097e+02 -6.5340672e+01 -2.4723311e+02
+ 4.5305562e+02 -6.5264636e+01 -2.4728045e+02
+ 4.5306594e+02 -6.5247004e+01 -2.4732519e+02
+ 4.5306405e+02 -6.5264496e+01 -2.4742607e+02
+ 4.5304472e+02 -6.5397836e+01 -2.4782421e+02
+ 4.5301322e+02 -6.5620756e+01 -2.4845155e+02
+ 4.5299279e+02 -6.5809958e+01 -2.4919277e+02
+ 4.5299431e+02 -6.5890315e+01 -2.4967726e+02
+ 4.5301099e+02 -6.5888175e+01 -2.4996206e+02
+ 4.5303029e+02 -6.5875616e+01 -2.5027983e+02
+ 4.5303933e+02 -6.5921322e+01 -2.5072798e+02
+ 4.5305895e+02 -6.5882937e+01 -2.5099112e+02
+ 4.5308636e+02 -6.5804778e+01 -2.5109610e+02
+ 4.5310461e+02 -6.5751077e+01 -2.5113044e+02
+ 4.5311672e+02 -6.5712112e+01 -2.5116566e+02
+ 4.5312965e+02 -6.5681314e+01 -2.5123475e+02
+ 4.5312968e+02 -6.5677515e+01 -2.5154169e+02
+ 4.5317532e+02 -6.5612120e+01 -2.5207506e+02
+ 4.5327657e+02 -6.5284186e+01 -2.5216013e+02
+ 4.5330303e+02 -6.5190016e+01 -2.5216508e+02
+ 4.5331565e+02 -6.5155558e+01 -2.5216246e+02
+ 4.5333801e+02 -6.5080263e+01 -2.5217843e+02
+ 4.5336334e+02 -6.4999102e+01 -2.5218027e+02
+ 4.5337978e+02 -6.4940435e+01 -2.5218500e+02
+ 4.5338259e+02 -6.4937747e+01 -2.5220608e+02
+ 4.5338485e+02 -6.4934983e+01 -2.5220713e+02
+ 4.5338358e+02 -6.4933019e+01 -2.5220869e+02
+ 4.5338219e+02 -6.4931190e+01 -2.5221045e+02
+ 4.5338271e+02 -6.4934712e+01 -2.5220752e+02
+ 4.5337501e+02 -6.4951187e+01 -2.5221272e+02
+ 4.5336891e+02 -6.4951670e+01 -2.5221831e+02
+ 4.5336754e+02 -6.4944503e+01 -2.5221621e+02
+ 4.5336443e+02 -6.4935635e+01 -2.5221662e+02
+ 4.5336564e+02 -6.4936344e+01 -2.5221407e+02
+ 4.5336670e+02 -6.4938077e+01 -2.5221080e+02
+ 4.5336610e+02 -6.4940106e+01 -2.5221049e+02
+ 4.5336498e+02 -6.4937594e+01 -2.5221181e+02
+ 4.5336420e+02 -6.4939112e+01 -2.5221336e+02
+ 4.5336439e+02 -6.4935320e+01 -2.5221199e+02
+ 4.5336260e+02 -6.4936576e+01 -2.5221503e+02
+ 4.5336522e+02 -6.4935999e+01 -2.5221241e+02
+ 4.5336555e+02 -6.4934017e+01 -2.5221204e+02
+ 4.5336550e+02 -6.4931021e+01 -2.5221162e+02
+ 4.5336551e+02 -6.4931536e+01 -2.5221021e+02
+ 4.5336492e+02 -6.4929382e+01 -2.5221042e+02
+ 4.5336590e+02 -6.4931413e+01 -2.5221013e+02
+ 4.5336506e+02 -6.4930182e+01 -2.5221022e+02
+ 4.5336695e+02 -6.4927169e+01 -2.5220452e+02
+ 4.5336485e+02 -6.4925919e+01 -2.5221042e+02
+ 4.5336693e+02 -6.4930863e+01 -2.5220815e+02
+ 4.5336578e+02 -6.4928643e+01 -2.5220895e+02
+ 4.5336597e+02 -6.4929302e+01 -2.5220874e+02
+ 4.5336549e+02 -6.4930967e+01 -2.5220982e+02
+ 4.5336576e+02 -6.4930669e+01 -2.5220952e+02
+ 4.5336596e+02 -6.4928373e+01 -2.5220802e+02
+ 4.5336710e+02 -6.4929208e+01 -2.5220647e+02
+ 4.5336471e+02 -6.4925760e+01 -2.5220930e+02
+ 4.5336641e+02 -6.4926177e+01 -2.5220759e+02
+ 4.5336555e+02 -6.4930922e+01 -2.5220799e+02
+ 4.5336629e+02 -6.4928618e+01 -2.5220860e+02
+ 4.5336688e+02 -6.4931693e+01 -2.5220755e+02
+ 4.5336692e+02 -6.4928975e+01 -2.5220785e+02
+ 4.5336553e+02 -6.4931582e+01 -2.5220955e+02
+ 4.5336633e+02 -6.4927163e+01 -2.5220832e+02
+ 4.5336762e+02 -6.4930368e+01 -2.5220499e+02
+ 4.5336652e+02 -6.4932318e+01 -2.5220990e+02
+ 4.5336615e+02 -6.4924892e+01 -2.5220680e+02
+ 4.5336600e+02 -6.4929465e+01 -2.5220808e+02
+ 4.5336667e+02 -6.4933060e+01 -2.5220833e+02
+ 4.5336776e+02 -6.4924386e+01 -2.5220619e+02
+ 4.5336541e+02 -6.4929126e+01 -2.5220871e+02
+ 4.5336640e+02 -6.4929148e+01 -2.5220556e+02
+ 4.5336650e+02 -6.4928535e+01 -2.5220549e+02
+ 4.5336726e+02 -6.4927171e+01 -2.5220615e+02
+ 4.5336645e+02 -6.4928644e+01 -2.5220648e+02
+ 4.5336676e+02 -6.4926939e+01 -2.5220672e+02
+ 4.5336697e+02 -6.4928301e+01 -2.5220619e+02
+ 4.5336641e+02 -6.4926327e+01 -2.5220830e+02
+ 4.5336658e+02 -6.4932604e+01 -2.5220413e+02
+ 4.5336556e+02 -6.4928771e+01 -2.5220757e+02
+ 4.5336742e+02 -6.4927043e+01 -2.5220484e+02
+ 4.5336694e+02 -6.4926921e+01 -2.5220639e+02
+ 4.5336574e+02 -6.4923366e+01 -2.5220614e+02
+ 4.5336533e+02 -6.4924804e+01 -2.5220769e+02
+ 4.5336668e+02 -6.4924963e+01 -2.5220547e+02
+ 4.5336515e+02 -6.4923417e+01 -2.5220751e+02
+ 4.5336700e+02 -6.4924754e+01 -2.5220582e+02
+ 4.5336668e+02 -6.4927534e+01 -2.5220372e+02
+ 4.5336741e+02 -6.4923318e+01 -2.5220340e+02
+ 4.5336659e+02 -6.4924670e+01 -2.5220548e+02
+ 4.5336720e+02 -6.4924474e+01 -2.5219705e+02
+ 4.5336549e+02 -6.4922881e+01 -2.5220140e+02
+ 4.5336509e+02 -6.4918678e+01 -2.5220039e+02
+ 4.5336770e+02 -6.4921534e+01 -2.5219675e+02
+ 4.5336648e+02 -6.4925208e+01 -2.5219926e+02
+ 4.5336584e+02 -6.4921114e+01 -2.5219988e+02
+ 4.5336751e+02 -6.4918846e+01 -2.5219674e+02
+ 4.5336594e+02 -6.4917603e+01 -2.5219816e+02
+ 4.5336560e+02 -6.4921296e+01 -2.5219954e+02
+ 4.5336629e+02 -6.4920930e+01 -2.5219812e+02
+ 4.5336559e+02 -6.4921054e+01 -2.5219831e+02
+ 4.5336810e+02 -6.4920005e+01 -2.5219735e+02
+ 4.5336553e+02 -6.4919440e+01 -2.5219734e+02
+ 4.5336640e+02 -6.4918152e+01 -2.5219746e+02
+ 4.5336676e+02 -6.4921776e+01 -2.5219585e+02
+ 4.5336739e+02 -6.4918274e+01 -2.5219608e+02
+ 4.5336405e+02 -6.4918037e+01 -2.5219984e+02
+ 4.5336633e+02 -6.4923198e+01 -2.5219886e+02
+ 4.5336529e+02 -6.4920268e+01 -2.5219931e+02
+ 4.5336546e+02 -6.4919335e+01 -2.5219925e+02
+ 4.5336606e+02 -6.4921074e+01 -2.5219906e+02
+ 4.5336718e+02 -6.4920141e+01 -2.5219649e+02
+ 4.5336657e+02 -6.4917883e+01 -2.5219460e+02
+ 4.5336583e+02 -6.4919618e+01 -2.5219775e+02
+ 4.5336623e+02 -6.4921718e+01 -2.5219752e+02
+ 4.5336597e+02 -6.4920605e+01 -2.5219816e+02
+ 4.5336714e+02 -6.4917770e+01 -2.5219728e+02
+ 4.5336662e+02 -6.4920252e+01 -2.5219703e+02
+ 4.5336580e+02 -6.4922223e+01 -2.5219848e+02
+ 4.5336538e+02 -6.4921635e+01 -2.5220007e+02
+ 4.5336691e+02 -6.4921705e+01 -2.5219757e+02
+ 4.5336630e+02 -6.4922936e+01 -2.5219934e+02
+ 4.5336482e+02 -6.4920539e+01 -2.5219872e+02
+ 4.5336667e+02 -6.4922156e+01 -2.5219829e+02
+ 4.5336591e+02 -6.4917084e+01 -2.5219986e+02
+ 4.5336753e+02 -6.4919667e+01 -2.5219760e+02
+ 4.5336635e+02 -6.4918881e+01 -2.5219815e+02
+ 4.5336594e+02 -6.4919811e+01 -2.5219675e+02
+ 4.5336655e+02 -6.4921335e+01 -2.5219609e+02
+ 4.5336441e+02 -6.4918611e+01 -2.5220002e+02
+ 4.5336576e+02 -6.4920603e+01 -2.5219826e+02
+ 4.5336648e+02 -6.4919723e+01 -2.5219704e+02
+ 4.5336552e+02 -6.4919398e+01 -2.5219813e+02
+ 4.5336494e+02 -6.4919208e+01 -2.5219808e+02
+ 4.5336595e+02 -6.4919125e+01 -2.5219804e+02
+ 4.5336656e+02 -6.4919787e+01 -2.5219698e+02
+ 4.5336628e+02 -6.4918607e+01 -2.5219646e+02
+ 4.5336514e+02 -6.4919659e+01 -2.5219885e+02
+ 4.5336388e+02 -6.4919170e+01 -2.5220170e+02
+ 4.5336823e+02 -6.4921747e+01 -2.5219482e+02
+ 4.5336515e+02 -6.4921390e+01 -2.5219951e+02
+ 4.5336608e+02 -6.4920417e+01 -2.5219756e+02
+ 4.5336644e+02 -6.4919597e+01 -2.5219705e+02
+ 4.5336648e+02 -6.4918769e+01 -2.5219702e+02
+ 4.5336669e+02 -6.4917250e+01 -2.5219616e+02
+ 4.5336492e+02 -6.4921413e+01 -2.5219793e+02
+ 4.5336470e+02 -6.4922470e+01 -2.5220019e+02
+ 4.5336628e+02 -6.4922024e+01 -2.5219810e+02
+ 4.5336534e+02 -6.4920044e+01 -2.5220037e+02
+ 4.5336703e+02 -6.4920123e+01 -2.5219776e+02
+ 4.5336494e+02 -6.4917917e+01 -2.5219985e+02
+ 4.5336670e+02 -6.4920167e+01 -2.5219724e+02
+ 4.5336595e+02 -6.4921194e+01 -2.5219805e+02
+ 4.5336595e+02 -6.4921194e+01 -2.5219805e+02
diff --git a/yDef.txt b/yDef.txt
index ea67eebcdd31437cb45dbfa0ba0eae44683c2aba..aef346ddc8c35422e36e0850f42193b1df3cea9e 100644
--- a/yDef.txt
+++ b/yDef.txt
@@ -1,44 +1,241 @@
- 3.2768525e+02 7.8363243e+01 -8.5946192e+01
- 3.2774126e+02 7.8335016e+01 -8.5837661e+01
- 3.2779447e+02 7.8308290e+01 -8.5750800e+01
- 3.2783042e+02 7.8299708e+01 -8.5704856e+01
- 3.2784690e+02 7.8300467e+01 -8.5686742e+01
- 3.2785531e+02 7.8311779e+01 -8.5685360e+01
- 3.2786113e+02 7.8312944e+01 -8.5679364e+01
- 3.2786538e+02 7.8311501e+01 -8.5674790e+01
- 3.2784013e+02 7.8106486e+01 -8.5776253e+01
- 3.2549070e+02 6.9241715e+01 -9.3386761e+01
- 3.2021972e+02 5.0200306e+01 -1.0814421e+02
- 3.1480022e+02 2.8189001e+01 -1.2433614e+02
- 3.1139499e+02 9.3181056e+00 -1.4142745e+02
- 3.0932249e+02 -5.9714685e+00 -1.5852857e+02
- 3.0765599e+02 -1.8951405e+01 -1.7256487e+02
- 3.0577346e+02 -2.8161469e+01 -1.8451982e+02
- 3.0425340e+02 -3.3984474e+01 -1.9393052e+02
- 3.0324620e+02 -3.7471751e+01 -2.0013148e+02
- 3.0252194e+02 -3.9213062e+01 -2.0329289e+02
- 3.0230601e+02 -3.9907045e+01 -2.0398257e+02
- 3.0222986e+02 -3.9907444e+01 -2.0404027e+02
- 3.0206323e+02 -3.9716149e+01 -2.0395877e+02
- 3.0200094e+02 -3.9636141e+01 -2.0372995e+02
- 3.0202264e+02 -3.9665638e+01 -2.0349725e+02
- 3.0210856e+02 -3.9767849e+01 -2.0324613e+02
- 3.0215245e+02 -3.9823732e+01 -2.0310492e+02
- 3.0218207e+02 -3.9837033e+01 -2.0304172e+02
- 3.0221128e+02 -3.9839423e+01 -2.0299456e+02
- 3.0222661e+02 -3.9839144e+01 -2.0296808e+02
- 3.0223228e+02 -3.9840421e+01 -2.0295388e+02
- 3.0223492e+02 -3.9839228e+01 -2.0294787e+02
- 3.0224060e+02 -3.9837916e+01 -2.0294521e+02
- 3.0224135e+02 -3.9835714e+01 -2.0294698e+02
- 3.0224117e+02 -3.9834511e+01 -2.0294841e+02
- 3.0224059e+02 -3.9835500e+01 -2.0295001e+02
- 3.0223719e+02 -3.9836645e+01 -2.0295103e+02
- 3.0223875e+02 -3.9834847e+01 -2.0295698e+02
- 3.0223426e+02 -3.9834094e+01 -2.0296371e+02
- 3.0222926e+02 -3.9835312e+01 -2.0296789e+02
- 3.0223012e+02 -3.9836794e+01 -2.0296991e+02
- 3.0223208e+02 -3.9837415e+01 -2.0297179e+02
- 3.0223656e+02 -3.9835058e+01 -2.0297407e+02
- 3.0223670e+02 -3.9835128e+01 -2.0297373e+02
- 3.0223695e+02 -3.9834919e+01 -2.0297419e+02
+ 3.1891354e+02 -1.8728151e+02 -1.1170299e+02
+ 3.1893083e+02 -1.8716699e+02 -1.1176664e+02
+ 3.1905090e+02 -1.8682610e+02 -1.1185550e+02
+ 3.1961230e+02 -1.8610299e+02 -1.1156605e+02
+ 3.2056792e+02 -1.8510694e+02 -1.1089480e+02
+ 3.2165550e+02 -1.8379752e+02 -1.1034154e+02
+ 3.2255946e+02 -1.8244033e+02 -1.1016678e+02
+ 3.2340125e+02 -1.8088006e+02 -1.1030512e+02
+ 3.2439132e+02 -1.7897202e+02 -1.1066604e+02
+ 3.2554385e+02 -1.7654090e+02 -1.1109324e+02
+ 3.2675391e+02 -1.7402735e+02 -1.1158615e+02
+ 3.2789717e+02 -1.7164232e+02 -1.1204024e+02
+ 3.2903201e+02 -1.6942593e+02 -1.1259538e+02
+ 3.3017807e+02 -1.6718421e+02 -1.1332037e+02
+ 3.3128794e+02 -1.6504778e+02 -1.1451571e+02
+ 3.3260614e+02 -1.6280632e+02 -1.1579368e+02
+ 3.3405841e+02 -1.6049640e+02 -1.1700134e+02
+ 3.3556899e+02 -1.5817293e+02 -1.1794644e+02
+ 3.3711541e+02 -1.5588773e+02 -1.1908081e+02
+ 3.3867670e+02 -1.5367180e+02 -1.2025212e+02
+ 3.4007586e+02 -1.5172772e+02 -1.2140439e+02
+ 3.4132246e+02 -1.4979438e+02 -1.2247257e+02
+ 3.4250441e+02 -1.4792186e+02 -1.2334300e+02
+ 3.4363371e+02 -1.4596341e+02 -1.2404606e+02
+ 3.4470245e+02 -1.4394426e+02 -1.2462320e+02
+ 3.4580692e+02 -1.4204775e+02 -1.2510434e+02
+ 3.4696467e+02 -1.4026670e+02 -1.2580829e+02
+ 3.4816573e+02 -1.3832741e+02 -1.2675058e+02
+ 3.4954010e+02 -1.3659561e+02 -1.2784336e+02
+ 3.5114943e+02 -1.3503251e+02 -1.2916562e+02
+ 3.5302662e+02 -1.3350077e+02 -1.3072422e+02
+ 3.5491513e+02 -1.3198197e+02 -1.3242468e+02
+ 3.5681460e+02 -1.3042521e+02 -1.3420711e+02
+ 3.5865915e+02 -1.2884798e+02 -1.3630404e+02
+ 3.6042164e+02 -1.2698679e+02 -1.3819228e+02
+ 3.6255136e+02 -1.2526188e+02 -1.4030053e+02
+ 3.6470897e+02 -1.2301685e+02 -1.4220202e+02
+ 3.6680134e+02 -1.2102569e+02 -1.4406875e+02
+ 3.6881174e+02 -1.1890852e+02 -1.4582428e+02
+ 3.7075315e+02 -1.1725604e+02 -1.4774741e+02
+ 3.7253025e+02 -1.1572531e+02 -1.4957755e+02
+ 3.7422953e+02 -1.1429259e+02 -1.5131268e+02
+ 3.7596558e+02 -1.1276607e+02 -1.5287091e+02
+ 3.7767464e+02 -1.1093115e+02 -1.5435644e+02
+ 3.7931836e+02 -1.0877285e+02 -1.5576237e+02
+ 3.8107894e+02 -1.0680911e+02 -1.5738644e+02
+ 3.8293294e+02 -1.0474051e+02 -1.5945289e+02
+ 3.8463643e+02 -1.0278821e+02 -1.6176530e+02
+ 3.8633218e+02 -1.0109875e+02 -1.6439861e+02
+ 3.8829733e+02 -9.9466592e+01 -1.6705065e+02
+ 3.9017610e+02 -9.7941644e+01 -1.6979108e+02
+ 3.9194780e+02 -9.6612437e+01 -1.7278456e+02
+ 3.9378020e+02 -9.5183225e+01 -1.7600718e+02
+ 3.9566172e+02 -9.3757972e+01 -1.7876817e+02
+ 3.9740142e+02 -9.2412597e+01 -1.8095401e+02
+ 3.9911721e+02 -9.1129826e+01 -1.8305519e+02
+ 4.0061914e+02 -8.9918681e+01 -1.8488156e+02
+ 4.0188646e+02 -8.9164347e+01 -1.8676272e+02
+ 4.0305545e+02 -8.8848863e+01 -1.8892092e+02
+ 4.0424969e+02 -8.8613330e+01 -1.9100387e+02
+ 4.0534858e+02 -8.8229997e+01 -1.9287830e+02
+ 4.0632823e+02 -8.7726501e+01 -1.9470230e+02
+ 4.0724704e+02 -8.7038748e+01 -1.9655798e+02
+ 4.0821602e+02 -8.6080628e+01 -1.9829472e+02
+ 4.0927366e+02 -8.4713935e+01 -1.9982436e+02
+ 4.1042879e+02 -8.3218124e+01 -2.0145505e+02
+ 4.1167345e+02 -8.2148658e+01 -2.0302699e+02
+ 4.1292177e+02 -8.1277123e+01 -2.0469670e+02
+ 4.1403515e+02 -8.0208125e+01 -2.0613284e+02
+ 4.1485726e+02 -7.8996168e+01 -2.0727110e+02
+ 4.1580176e+02 -7.7119201e+01 -2.0816282e+02
+ 4.1684022e+02 -7.4845934e+01 -2.0897035e+02
+ 4.1783898e+02 -7.2706888e+01 -2.0982845e+02
+ 4.1876650e+02 -7.0912010e+01 -2.1082105e+02
+ 4.1969826e+02 -6.9463347e+01 -2.1233966e+02
+ 4.2056962e+02 -6.8366317e+01 -2.1418368e+02
+ 4.2130398e+02 -6.7636017e+01 -2.1583291e+02
+ 4.2195103e+02 -6.7167187e+01 -2.1727470e+02
+ 4.2254508e+02 -6.6753775e+01 -2.1864673e+02
+ 4.2319482e+02 -6.6350748e+01 -2.2015068e+02
+ 4.2403678e+02 -6.6147252e+01 -2.2175327e+02
+ 4.2492634e+02 -6.6152170e+01 -2.2329521e+02
+ 4.2560610e+02 -6.6066465e+01 -2.2473186e+02
+ 4.2609805e+02 -6.5892533e+01 -2.2584835e+02
+ 4.2676071e+02 -6.5633425e+01 -2.2637709e+02
+ 4.2776172e+02 -6.5384700e+01 -2.2670184e+02
+ 4.2853003e+02 -6.5178877e+01 -2.2708663e+02
+ 4.2922178e+02 -6.5576389e+01 -2.2767038e+02
+ 4.2979196e+02 -6.6375911e+01 -2.2853653e+02
+ 4.3025529e+02 -6.7372405e+01 -2.2953308e+02
+ 4.3072220e+02 -6.8357599e+01 -2.3041329e+02
+ 4.3109125e+02 -6.9396674e+01 -2.3177585e+02
+ 4.3135069e+02 -7.0183411e+01 -2.3342212e+02
+ 4.3145496e+02 -7.0763862e+01 -2.3515335e+02
+ 4.3158644e+02 -7.1120084e+01 -2.3669190e+02
+ 4.3180940e+02 -7.1099745e+01 -2.3804487e+02
+ 4.3210437e+02 -7.0762651e+01 -2.3912818e+02
+ 4.3243401e+02 -7.0236159e+01 -2.4014555e+02
+ 4.3266302e+02 -6.9749489e+01 -2.4065923e+02
+ 4.3278414e+02 -6.9589115e+01 -2.4098668e+02
+ 4.3291125e+02 -6.9607442e+01 -2.4131286e+02
+ 4.3314207e+02 -6.9509703e+01 -2.4155115e+02
+ 4.3347486e+02 -6.9265854e+01 -2.4205434e+02
+ 4.3408956e+02 -6.9123747e+01 -2.4346268e+02
+ 4.3456727e+02 -6.8838695e+01 -2.4566541e+02
+ 4.3533932e+02 -6.8458596e+01 -2.4545104e+02
+ 4.3564500e+02 -6.7923964e+01 -2.4560567e+02
+ 4.3576521e+02 -6.7609935e+01 -2.4580167e+02
+ 4.3580495e+02 -6.7530955e+01 -2.4582379e+02
+ 4.3581609e+02 -6.7538741e+01 -2.4581776e+02
+ 4.3581788e+02 -6.7551044e+01 -2.4582894e+02
+ 4.3581780e+02 -6.7551321e+01 -2.4582860e+02
+ 4.3581858e+02 -6.7558663e+01 -2.4582781e+02
+ 4.3581880e+02 -6.7558672e+01 -2.4582929e+02
+ 4.3581780e+02 -6.7559466e+01 -2.4582964e+02
+ 4.3581662e+02 -6.7560176e+01 -2.4583029e+02
+ 4.3581472e+02 -6.7565913e+01 -2.4583085e+02
+ 4.3580951e+02 -6.7560573e+01 -2.4583328e+02
+ 4.3580930e+02 -6.7563355e+01 -2.4583210e+02
+ 4.3580824e+02 -6.7555368e+01 -2.4582887e+02
+ 4.3580447e+02 -6.7549170e+01 -2.4582739e+02
+ 4.3580387e+02 -6.7545448e+01 -2.4581980e+02
+ 4.3580292e+02 -6.7507055e+01 -2.4574121e+02
+ 4.3580066e+02 -6.7460679e+01 -2.4565955e+02
+ 4.3579670e+02 -6.7465795e+01 -2.4567355e+02
+ 4.3579544e+02 -6.7463781e+01 -2.4567402e+02
+ 4.3579469e+02 -6.7458020e+01 -2.4567534e+02
+ 4.3579323e+02 -6.7464697e+01 -2.4567675e+02
+ 4.3579197e+02 -6.7461963e+01 -2.4567842e+02
+ 4.3579134e+02 -6.7461141e+01 -2.4567689e+02
+ 4.3579109e+02 -6.7460871e+01 -2.4567780e+02
+ 4.3578997e+02 -6.7461057e+01 -2.4567906e+02
+ 4.3578925e+02 -6.7462137e+01 -2.4568226e+02
+ 4.3578900e+02 -6.7459409e+01 -2.4568143e+02
+ 4.3578748e+02 -6.7460407e+01 -2.4568326e+02
+ 4.3578710e+02 -6.7457437e+01 -2.4568291e+02
+ 4.3578523e+02 -6.7461951e+01 -2.4568494e+02
+ 4.3578571e+02 -6.7457928e+01 -2.4568505e+02
+ 4.3578330e+02 -6.7460604e+01 -2.4568863e+02
+ 4.3578277e+02 -6.7458609e+01 -2.4568836e+02
+ 4.3578339e+02 -6.7460146e+01 -2.4568897e+02
+ 4.3578274e+02 -6.7458170e+01 -2.4568962e+02
+ 4.3578126e+02 -6.7459094e+01 -2.4568913e+02
+ 4.3577934e+02 -6.7460117e+01 -2.4569243e+02
+ 4.3578105e+02 -6.7456988e+01 -2.4569184e+02
+ 4.3577862e+02 -6.7459970e+01 -2.4569347e+02
+ 4.3577901e+02 -6.7457212e+01 -2.4569214e+02
+ 4.3577890e+02 -6.7459156e+01 -2.4569432e+02
+ 4.3577916e+02 -6.7458260e+01 -2.4569370e+02
+ 4.3577554e+02 -6.7458675e+01 -2.4569669e+02
+ 4.3577592e+02 -6.7460535e+01 -2.4569724e+02
+ 4.3577372e+02 -6.7458688e+01 -2.4569921e+02
+ 4.3577428e+02 -6.7460392e+01 -2.4569918e+02
+ 4.3577363e+02 -6.7456143e+01 -2.4569930e+02
+ 4.3577065e+02 -6.7458383e+01 -2.4570303e+02
+ 4.3577376e+02 -6.7458344e+01 -2.4569907e+02
+ 4.3577221e+02 -6.7456988e+01 -2.4570162e+02
+ 4.3576949e+02 -6.7456470e+01 -2.4570427e+02
+ 4.3576902e+02 -6.7457412e+01 -2.4570491e+02
+ 4.3576956e+02 -6.7456951e+01 -2.4570597e+02
+ 4.3576600e+02 -6.7456040e+01 -2.4571012e+02
+ 4.3576758e+02 -6.7459513e+01 -2.4570760e+02
+ 4.3576451e+02 -6.7454782e+01 -2.4571175e+02
+ 4.3576250e+02 -6.7455322e+01 -2.4571464e+02
+ 4.3576054e+02 -6.7458992e+01 -2.4571726e+02
+ 4.3574604e+02 -6.7454507e+01 -2.4573285e+02
+ 4.3573953e+02 -6.7451781e+01 -2.4573959e+02
+ 4.3574076e+02 -6.7453630e+01 -2.4573929e+02
+ 4.3573893e+02 -6.7455818e+01 -2.4574138e+02
+ 4.3573923e+02 -6.7454150e+01 -2.4574054e+02
+ 4.3573797e+02 -6.7454532e+01 -2.4574392e+02
+ 4.3574037e+02 -6.7454929e+01 -2.4574027e+02
+ 4.3573931e+02 -6.7453694e+01 -2.4573992e+02
+ 4.3573694e+02 -6.7452049e+01 -2.4574454e+02
+ 4.3573957e+02 -6.7451242e+01 -2.4574089e+02
+ 4.3573839e+02 -6.7453348e+01 -2.4574196e+02
+ 4.3573915e+02 -6.7453863e+01 -2.4574118e+02
+ 4.3573992e+02 -6.7453302e+01 -2.4574059e+02
+ 4.3574046e+02 -6.7453773e+01 -2.4574076e+02
+ 4.3573868e+02 -6.7453345e+01 -2.4574184e+02
+ 4.3573982e+02 -6.7455406e+01 -2.4574053e+02
+ 4.3573860e+02 -6.7453154e+01 -2.4574143e+02
+ 4.3573991e+02 -6.7454674e+01 -2.4573887e+02
+ 4.3573895e+02 -6.7454196e+01 -2.4574100e+02
+ 4.3573977e+02 -6.7455402e+01 -2.4574080e+02
+ 4.3574030e+02 -6.7453936e+01 -2.4574001e+02
+ 4.3573837e+02 -6.7452793e+01 -2.4574261e+02
+ 4.3573838e+02 -6.7453245e+01 -2.4574207e+02
+ 4.3574031e+02 -6.7452732e+01 -2.4574102e+02
+ 4.3573832e+02 -6.7451854e+01 -2.4574003e+02
+ 4.3573941e+02 -6.7454453e+01 -2.4574046e+02
+ 4.3573984e+02 -6.7454896e+01 -2.4574056e+02
+ 4.3573970e+02 -6.7453315e+01 -2.4574009e+02
+ 4.3574005e+02 -6.7455884e+01 -2.4574111e+02
+ 4.3573815e+02 -6.7454666e+01 -2.4574192e+02
+ 4.3573707e+02 -6.7454055e+01 -2.4574280e+02
+ 4.3574013e+02 -6.7452342e+01 -2.4574037e+02
+ 4.3574018e+02 -6.7455222e+01 -2.4573857e+02
+ 4.3574094e+02 -6.7450721e+01 -2.4573902e+02
+ 4.3573836e+02 -6.7455710e+01 -2.4574234e+02
+ 4.3573724e+02 -6.7453171e+01 -2.4574204e+02
+ 4.3573889e+02 -6.7454377e+01 -2.4573994e+02
+ 4.3573854e+02 -6.7452820e+01 -2.4574064e+02
+ 4.3573755e+02 -6.7451119e+01 -2.4574343e+02
+ 4.3573786e+02 -6.7452755e+01 -2.4574275e+02
+ 4.3573934e+02 -6.7456613e+01 -2.4573994e+02
+ 4.3573778e+02 -6.7452892e+01 -2.4574307e+02
+ 4.3573969e+02 -6.7455067e+01 -2.4573908e+02
+ 4.3573933e+02 -6.7453814e+01 -2.4574122e+02
+ 4.3573944e+02 -6.7455847e+01 -2.4574002e+02
+ 4.3573937e+02 -6.7454909e+01 -2.4574136e+02
+ 4.3574107e+02 -6.7453451e+01 -2.4573877e+02
+ 4.3573823e+02 -6.7452979e+01 -2.4574259e+02
+ 4.3573903e+02 -6.7452894e+01 -2.4574148e+02
+ 4.3573889e+02 -6.7453996e+01 -2.4574040e+02
+ 4.3573969e+02 -6.7452094e+01 -2.4574215e+02
+ 4.3573783e+02 -6.7454377e+01 -2.4574367e+02
+ 4.3573862e+02 -6.7455455e+01 -2.4574125e+02
+ 4.3573936e+02 -6.7453288e+01 -2.4573863e+02
+ 4.3573775e+02 -6.7454846e+01 -2.4574375e+02
+ 4.3573904e+02 -6.7456352e+01 -2.4574211e+02
+ 4.3573920e+02 -6.7455135e+01 -2.4574146e+02
+ 4.3573767e+02 -6.7455317e+01 -2.4574373e+02
+ 4.3573828e+02 -6.7453405e+01 -2.4574279e+02
+ 4.3574018e+02 -6.7454657e+01 -2.4573844e+02
+ 4.3573972e+02 -6.7455281e+01 -2.4573956e+02
+ 4.3573989e+02 -6.7452902e+01 -2.4573908e+02
+ 4.3573949e+02 -6.7455714e+01 -2.4574086e+02
+ 4.3573863e+02 -6.7455536e+01 -2.4574159e+02
+ 4.3573896e+02 -6.7455015e+01 -2.4574176e+02
+ 4.3573797e+02 -6.7456343e+01 -2.4574161e+02
+ 4.3573963e+02 -6.7456408e+01 -2.4574075e+02
+ 4.3573924e+02 -6.7453925e+01 -2.4573962e+02
+ 4.3574031e+02 -6.7455286e+01 -2.4573983e+02
+ 4.3573819e+02 -6.7453162e+01 -2.4574155e+02
+ 4.3573800e+02 -6.7453586e+01 -2.4574168e+02
+ 4.3573925e+02 -6.7453055e+01 -2.4574153e+02
+ 4.3573919e+02 -6.7452946e+01 -2.4573986e+02
+ 4.3574134e+02 -6.7452017e+01 -2.4573906e+02
+ 4.3573936e+02 -6.7455111e+01 -2.4574056e+02
+ 4.3573936e+02 -6.7455111e+01 -2.4574056e+02