diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/ProjectionTangentialLSQ.H | 10 | ||||
| -rw-r--r-- | src/ProjectionTangentialLSQ.cxx | 128 | ||||
| -rw-r--r-- | src/lsq_funcs.mac | 3 |
3 files changed, 136 insertions, 5 deletions
diff --git a/src/ProjectionTangentialLSQ.H b/src/ProjectionTangentialLSQ.H index 492860d..9172836 100644 --- a/src/ProjectionTangentialLSQ.H +++ b/src/ProjectionTangentialLSQ.H @@ -12,7 +12,8 @@ class ProjectionTangentialLSQ : public Projection { private: - double comp_center_angle(double alph_a, double alph_b, double d1, double d2); + double comp_center_angle(double alph_a, double alph_b, + double d1, double d2); double comp_scale(double alph_a, double alph_b, double d1, double d2); @@ -20,10 +21,13 @@ class ProjectionTangentialLSQ : public Projection { int optimize(const Hill *m1, const Hill *m2, ViewParams *parms); + int lsq(Hills *m, ViewParams *parms); + public: - void get_coordinates(double a_view, double a_nick, const ViewParams *parms, - double *x, double *y); + void get_coordinates(double a_view, double a_nick, + const ViewParams *parms, double *x, double *y); int comp_params(const Hill *m1, const Hill *m2, ViewParams *parms); + }; #endif diff --git a/src/ProjectionTangentialLSQ.cxx b/src/ProjectionTangentialLSQ.cxx index f4dc44a..49bce8f 100644 --- a/src/ProjectionTangentialLSQ.cxx +++ b/src/ProjectionTangentialLSQ.cxx @@ -9,6 +9,12 @@ #include <string.h> #include <math.h> +#include <gsl/gsl_rng.h> +#include <gsl/gsl_randist.h> +#include <gsl/gsl_vector.h> +#include <gsl/gsl_blas.h> +#include <gsl/gsl_multifit_nlin.h> + #include "ProjectionTangentialLSQ.H" @@ -79,6 +85,122 @@ ProjectionTangentialLSQ::angle_dist(double a1, double a2) { return ret; } +struct data { + Hills *h; +}; + +#define CALL(A) A(c_view, c_nick, c_tilt, scale, k0, k1, m->a_view, m->a_nick) + +static int +lsq_f (const gsl_vector * x, void *data, gsl_vector * f) { + struct data *dat = (struct data *) data; + + double c_view = gsl_vector_get (x, 0); + double c_nick = gsl_vector_get (x, 1); + double c_tilt = gsl_vector_get (x, 2); + double scale = gsl_vector_get (x, 3); + double k0 = gsl_vector_get (x, 4); + double k1 = gsl_vector_get (x, 5); + + for (int i=0; i<dat->h->get_num(); i++) { + Hill *m = dat->h->get(i); + + double x = CALL(mac_x); + double y = CALL(mac_y); + + gsl_vector_set (f, i*2, x - m->x); + gsl_vector_set (f, i*2+1, y - m->y); + } + + return GSL_SUCCESS; +} + + +static int +lsq_df (const gsl_vector * x, void *data, gsl_matrix * J) { + struct data *dat = (struct data *) data; + + double c_view = gsl_vector_get (x, 0); + double c_nick = gsl_vector_get (x, 1); + double c_tilt = gsl_vector_get (x, 2); + double scale = gsl_vector_get (x, 3); + double k0 = gsl_vector_get (x, 4); + double k1 = gsl_vector_get (x, 5); + + for (int i=0; i<dat->h->get_num(); i++) { + Hill *m = dat->h->get(i); + + gsl_matrix_set (J, 2*i, 0, CALL(mac_x_dc_view)); + gsl_matrix_set (J, 2*i, 1, CALL(mac_x_dc_nick)); + gsl_matrix_set (J, 2*i, 2, CALL(mac_x_dc_tilt)); + gsl_matrix_set (J, 2*i, 3, CALL(mac_x_dscale)); + gsl_matrix_set (J, 2*i, 4, CALL(mac_x_dk0)); + gsl_matrix_set (J, 2*i, 5, CALL(mac_x_dk1)); + + gsl_matrix_set (J, 2*i+1, 0, CALL(mac_y_dc_view)); + gsl_matrix_set (J, 2*i+1, 1, CALL(mac_y_dc_nick)); + gsl_matrix_set (J, 2*i+1, 2, CALL(mac_y_dc_tilt)); + gsl_matrix_set (J, 2*i+1, 3, CALL(mac_y_dscale)); + gsl_matrix_set (J, 2*i+1, 4, CALL(mac_y_dk0)); + gsl_matrix_set (J, 2*i+1, 5, CALL(mac_y_dk1)); + } + + return GSL_SUCCESS; +} + +static int +lsq_fdf (const gsl_vector * x, void *data, gsl_vector * f, gsl_matrix * J) { + lsq_f (x, data, f); + lsq_df (x, data, J); + + return GSL_SUCCESS; +} + + +int +ProjectionTangentialLSQ::lsq(Hills *h, ViewParams *parms) { + const gsl_multifit_fdfsolver_type *T; + gsl_multifit_fdfsolver *s; + gsl_multifit_function_fdf f; + struct data dat; + double x_init[6]; + gsl_vector_view x; + int status; + + dat.h = h; + + x_init[0] = parms->a_center; + x_init[1] = parms->a_nick; + x_init[2] = parms->a_tilt; + x_init[3] = parms->scale; + x_init[4] = k0; + x_init[5] = k1; + + x = gsl_vector_view_array (x_init, 6); + + f.f = &lsq_f; + f.df = &lsq_df; + f.fdf = &lsq_fdf; + f.n = h->get_num() * 2; + f.p = 6; + f.params = &dat; + + T = gsl_multifit_fdfsolver_lmsder; + s = gsl_multifit_fdfsolver_alloc (T, h->get_num() * 2, 6); + gsl_multifit_fdfsolver_set (s, &f, &x.vector); + + + for (int i=0; i<20 && status == GSL_CONTINUE; i++) { + + status = gsl_multifit_fdfsolver_iterate (s); + + } + + + gsl_multifit_fdfsolver_free (s); + return 0; +} + int ProjectionTangentialLSQ::optimize(const Hill *m1, const Hill *m2, ViewParams *parms) { int i; @@ -146,8 +268,10 @@ void ProjectionTangentialLSQ::get_coordinates(double a_view, double a_nick, const ViewParams *parms, double *x, double *y) { - *x = mac_x(parms->a_center, parms->a_nick, parms->a_tilt, parms->scale, k0, k1, a_view, a_nick); - *y = mac_y(parms->a_center, parms->a_nick, parms->a_tilt, parms->scale, k0, k1, a_view, a_nick); + *x = mac_x(parms->a_center, parms->a_nick, parms->a_tilt, parms->scale, + k0, k1, a_view, a_nick); + *y = mac_y(parms->a_center, parms->a_nick, parms->a_tilt, parms->scale, + k0, k1, a_view, a_nick); } double diff --git a/src/lsq_funcs.mac b/src/lsq_funcs.mac index 3bd124e..0056185 100644 --- a/src/lsq_funcs.mac +++ b/src/lsq_funcs.mac @@ -25,13 +25,16 @@ printfunc(name, expression) := sprint("static double", name, args, "{ return ", printfunc("mac_x", x_expand); printfunc("mac_y", y_expand); + printfunc("mac_x_dc_view", diff(x_expand, c_view)); printfunc("mac_x_dc_nick", diff(x_expand, c_nick)); +printfunc("mac_x_dc_tilt", diff(x_expand, c_tilt)); printfunc("mac_x_dscale", diff(x_expand, scale)); printfunc("mac_x_dk0", diff(x_expand, k0)); printfunc("mac_x_dk1", diff(x_expand, k1)); printfunc("mac_y_dc_view", diff(y_expand, c_view)); printfunc("mac_y_dc_nick", diff(y_expand, c_nick)); +printfunc("mac_y_dc_tilt", diff(y_expand, c_tilt)); printfunc("mac_y_dscale", diff(y_expand, scale)); printfunc("mac_y_dk0", diff(y_expand, k0)); printfunc("mac_y_dk1", diff(y_expand, k1)); |