src/hkl-pseudoaxis-auto.c

   1 #include <gsl/gsl_multiroots.h>
   2 #include <gsl/gsl_sf_trig.h>
   3
   4 #include <hkl/hkl-pseudoaxis.h>
   5
   6 typedef struct
   7 {
   8         size_t n;
   9         unsigned int work_on_consign;
  10 }
  11 auto_state_t;
  12
  13 //forward declaration
  14 static int E4CV_bissector(const gsl_vector *x, void *params, gsl_vector *f);
  15 static int _auto_to_geometry(void *vstate, HklPseudoAxisEngine *engine);
  16
  17 static int auto_alloc(void *vstate, size_t n)
  18 {
  19         auto_state_t *state;
  20
  21         state = vstate;
  22
  23         return HKL_SUCCESS;
  24 }
  25
  26 static int auto_set(void *vstate, HklPseudoAxisEngine *engine)
  27 {
  28         auto_state_t *state;
  29
  30         state = vstate;
  31         state->n = engine->related_axes_idx->size;
  32         state->work_on_consign = 0;
  33
  34         return HKL_SUCCESS;
  35 }
  36
  37 static int auto_to_geometry(void *vstate, HklPseudoAxisEngine *engine)
  38 {
  39         auto_state_t *state;
  40
  41         state = vstate;
  42         state->work_on_consign = 0;
  43         _auto_to_geometry(vstate, engine);
  44         state->work_on_consign = 1;
  45         _auto_to_geometry(vstate, engine);
  46
  47         return HKL_SUCCESS;
  48 }
  49
  50 static int auto_to_pseudoAxes(void *vstate, HklPseudoAxisEngine *engine)
  51 {
  52         HklHolder *holder;
  53         HklMatrix RUB, RUBc;
  54         HklVector hkl, hklc, ki, Q, Qc;
  55         HklPseudoAxis *H, *K, *L;
  56
  57         // update the geometry internals
  58         hkl_geometry_update(engine->geom);
  59
  60         // R * UB
  61         // for now the 0 holder is the sample holder.
  62         holder = hkl_geometry_get_holder(engine->geom, 0);
  63         hkl_quaternion_to_smatrix(holder->q, &RUB);
  64         hkl_quaternion_to_smatrix(holder->qc, &RUBc);
  65         hkl_matrix_times_smatrix(&RUB, engine->sample->UB);
  66         hkl_matrix_times_smatrix(&RUBc, engine->sample->UB);
  67
  68         // kf - ki = Q
  69         hkl_source_get_ki(engine->geom->source, &ki);
  70         hkl_detector_get_kf(engine->det, engine->geom, &Q, &Qc);
  71         hkl_vector_minus_vector(&Q, &ki);
  72         hkl_vector_minus_vector(&Qc, &ki);
  73
  74         hkl_matrix_solve(&RUB, &hkl, &Q);
  75         hkl_matrix_solve(&RUBc, &hklc, &Qc);
  76
  77         // update the pseudoAxes current and consign parts
  78         H = hkl_list_get_by_idx(engine->pseudoAxes, 0);
  79         K = hkl_list_get_by_idx(engine->pseudoAxes, 1);
  80         L = hkl_list_get_by_idx(engine->pseudoAxes, 2);
  81         H->config.current = hkl.data[0];
  82         K->config.current = hkl.data[1];
  83         L->config.current = hkl.data[2];
  84         H->config.consign = hklc.data[0];
  85         K->config.consign = hklc.data[1];
  86         L->config.consign = hklc.data[2];
  87
  88         return HKL_SUCCESS;
  89 }
  90
  91 static void auto_free(void *state)
  92 {
  93 }
  94
  95 static int _auto_to_geometry(void *vstate, HklPseudoAxisEngine *engine)
  96 {
  97         auto_state_t *state;
  98         gsl_multiroot_fsolver_type const *T;
  99         gsl_multiroot_fsolver *s;
 100         gsl_vector *x;
 101         size_t iter = 0;
 102         int status;
 103         size_t i;
 104         unsigned int  idx;
 105         double d;
 106
 107         state = vstate;
 108         gsl_multiroot_function f = {&E4CV_bissector, state->n, engine};
 109
 110         // get the starting point from the geometry
 111         // must be put in the auto_set method
 112         x = gsl_vector_alloc(state->n);
 113         for(i=0; i<state->n; ++i) {
 114                 HklAxis const *axis;
 115
 116                 idx = gsl_vector_uint_get(engine->related_axes_idx, i);
 117                 axis = hkl_geometry_get_axis(engine->geom, idx);
 118                 if (state->work_on_consign)
 119                         gsl_vector_set(x, i, axis->config.consign);
 120                 else
 121                         gsl_vector_set(x, i, axis->config.current);
 122         }
 123
 124         // Initialize method
 125         T = gsl_multiroot_fsolver_hybrids;
 126         s = gsl_multiroot_fsolver_alloc (T, state->n);
 127         gsl_multiroot_fsolver_set (s, &f, x);
 128
 129         // iterate to find the solution
 130         do {
 131                 ++iter;
 132                 status = gsl_multiroot_fsolver_iterate(s);
 133                 if (status || iter % 1000 == 0) {
 134                         // Restart from another point.
 135                         for(i=0; i<state->n; ++i) {
 136                                 d = (double)rand() / RAND_MAX * 180. / M_PI;
 137                                 gsl_vector_set(s->x, i, d);
 138                         }
 139                         gsl_multiroot_fsolver_set(s, &f, s->x);
 140                         status = gsl_multiroot_fsolver_iterate(s);
 141                 }
 142                 status = gsl_multiroot_test_residual (s->f, HKL_EPSILON);
 143         } while (status == GSL_CONTINUE && iter < 10000);
 144
 145         // set the geometry from the gsl_vector
 146         // in a futur version the geometry must contain a gsl_vector
 147         // to avoid this.
 148         for(i=0; i<state->n; ++i) {
 149                 HklAxisConfig config;
 150                 HklAxis *axis;
 151
 152                 idx = gsl_vector_uint_get(engine->related_axes_idx, i);
 153                 axis = hkl_geometry_get_axis(engine->geom, idx);
 154                 hkl_axis_get_config(axis, &config);
 155                 d = gsl_vector_get(s->x, i);
 156                 gsl_sf_angle_restrict_pos_e(&d);
 157                 if (state->work_on_consign)
 158                         config.consign = d;
 159                 else
 160                         config.current = d;
 161                 hkl_axis_set_config(axis, &config);
 162         }
 163         hkl_geometry_update(engine->geom);
 164
 165         // release memory
 166         gsl_vector_free(x);
 167         gsl_multiroot_fsolver_free(s);
 168
 169         return HKL_SUCCESS;
 170 }
 171
 172 static int common_hkl_part(const gsl_vector *x, void *params, gsl_vector *f)
 173 {
 174         auto_state_t *state;
 175         HklVector Hkl;
 176         HklVector ki, dQ, shit;
 177         HklPseudoAxisEngine *engine;
 178         HklPseudoAxis *H, *K, *L;
 179         HklHolder *holder;
 180         unsigned int i;
 181
 182         engine = params;
 183         state = engine->state;
 184         H = hkl_pseudoAxisEngine_get_pseudoAxis(engine, 0);
 185         K = hkl_pseudoAxisEngine_get_pseudoAxis(engine, 1);
 186         L = hkl_pseudoAxisEngine_get_pseudoAxis(engine, 2);
 187
 188         // update the workspace from x;
 189         for(i=0; i<engine->related_axes_idx->size ; ++i) {
 190                 HklAxis *axis;
 191                 unsigned int idx;
 192                 HklAxisConfig config;
 193
 194                 idx = gsl_vector_uint_get(engine->related_axes_idx, i);
 195                 axis = hkl_geometry_get_axis(engine->geom, idx);
 196                 hkl_axis_get_config(axis, &config);
 197                 if (state->work_on_consign)
 198                         config.consign = gsl_vector_get(x, i);
 199                 else
 200                         config.current = gsl_vector_get(x, i);
 201
 202                 hkl_axis_set_config(axis, &config);
 203         }
 204         hkl_geometry_update(engine->geom);
 205
 206         if (state->work_on_consign)
 207                 hkl_vector_set(&Hkl, H->config.consign, K->config.consign,
 208                                 L->config.consign);
 209         else
 210                 hkl_vector_set(&Hkl, H->config.current, K->config.current,
 211                                 L->config.current);
 212
 213         // R * UB * h = Q
 214         // for now the 0 holder is the sample holder.
 215         holder = hkl_geometry_get_holder(engine->geom, 0);
 216         hkl_matrix_times_vector(engine->sample->UB, &Hkl);
 217         if (state->work_on_consign)
 218                 hkl_vector_rotated_quaternion(&Hkl, holder->qc);
 219         else
 220                 hkl_vector_rotated_quaternion(&Hkl, holder->q);
 221
 222         // kf - ki = Q
 223         hkl_source_get_ki(engine->geom->source, &ki);
 224         if (state->work_on_consign)
 225                 hkl_detector_get_kf(engine->det, engine->geom, &shit, &dQ);
 226         else
 227                 hkl_detector_get_kf(engine->det, engine->geom, &dQ, &shit);
 228         hkl_vector_minus_vector(&dQ, &ki);
 229
 230         hkl_vector_minus_vector(&dQ, &Hkl);
 231
 232         gsl_vector_set (f, 0, dQ.data[0]);
 233         gsl_vector_set (f, 1, dQ.data[1]);
 234         gsl_vector_set (f, 2, dQ.data[2]);
 235 }
 236
 237 static int E4CV_bissector(const gsl_vector *x, void *params, gsl_vector *f)
 238 {
 239         double omega, delta;
 240
 241         common_hkl_part(x, params, f);
 242
 243         omega = gsl_sf_angle_restrict_pos(gsl_vector_get(x, 0));
 244         delta = gsl_sf_angle_restrict_pos(gsl_vector_get(x, 3));
 245
 246         gsl_vector_set (f, 3, delta - 2 * omega);
 247
 248         return  GSL_SUCCESS;
 249 }
 250
 251 static HklPseudoAxisEngineType auto_type =
 252 {
 253         "auto",
 254         sizeof(auto_state_t),
 255         &auto_alloc,
 256         &auto_set,
 257         &auto_to_geometry,
 258         &auto_to_pseudoAxes,
 259         &auto_free
 260 };
 261
 262 const HklPseudoAxisEngineType *hkl_pseudoAxisEngine_type_auto = &auto_type;