hkl/hkl-pseudoaxis-auto.c

   1 /* This file is part of the hkl library.
   2  *
   3  * The hkl library is free software: you can redistribute it and/or modify
   4  * it under the terms of the GNU General Public License as published by
   5  * the Free Software Foundation, either version 3 of the License, or
   6  * (at your option) any later version.
   7  *
   8  * The hkl library is distributed in the hope that it will be useful,
   9  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  11  * GNU General Public License for more details.
  12  *
  13  * You should have received a copy of the GNU General Public License
  14  * along with the hkl library.  If not, see <http://www.gnu.org/licenses/>.
  15  *
  16  * Copyright (C) 2003-2014 Synchrotron SOLEIL
  17  *                         L'Orme des Merisiers Saint-Aubin
  18  *                         BP 48 91192 GIF-sur-YVETTE CEDEX
  19  *
  20  * Authors: Picca Frédéric-Emmanuel <picca@synchrotron-soleil.fr>
  21  */
  22 #include <alloca.h>                     // for alloca
  23 #include <gsl/gsl_errno.h>              // for ::GSL_CONTINUE
  24 #include <gsl/gsl_machine.h>            // for GSL_SQRT_DBL_EPSILON
  25 #include <gsl/gsl_matrix_double.h>      // for gsl_matrix_alloc, etc
  26 #include <gsl/gsl_multiroots.h>         // for gsl_multiroot_function, etc
  27 #include <gsl/gsl_vector_double.h>      // for gsl_vector, etc
  28 #include <math.h>                       // for fabs, M_PI
  29 #include <stddef.h>                     // for size_t
  30 #include <stdlib.h>                     // for rand, RAND_MAX
  31 #include <string.h>                     // for NULL, memset, memcpy
  32 #include <sys/types.h>                  // for uint
  33 #include "hkl-geometry-private.h"       // for hkl_geometry_update
  34 #include "hkl-macros-private.h"         // for HKL_MALLOC, hkl_assert, etc
  35 #include "hkl-parameter-private.h"      // for _HklParameter
  36 #include "hkl-pseudoaxis-auto-private.h"  // for HklModeAutoInfo, etc
  37 #include "hkl-pseudoaxis-private.h"     // for _HklEngine, HklModeInfo, etc
  38 #include "hkl.h"                        // for HklEngine, HklMode, etc
  39 #include "hkl/ccan/container_of/container_of.h"  // for container_of
  40 #include "hkl/ccan/darray/darray.h"     // for darray_foreach
  41
  42 /* #define DEBUG */
  43
  44 #define HKL_MODE_AUTO_ERROR hkl_mode_auto_error_quark ()
  45
  46 static GQuark hkl_mode_auto_error_quark (void)
  47 {
  48         return g_quark_from_static_string ("hkl-mode-auto-error-quark");
  49 }
  50
  51 typedef enum {
  52         HKL_MODE_AUTO_ERROR_SET, /* can not set the engine */
  53 } HklModeAutoError;
  54
  55 /*********************************************/
  56 /* methods use to solve numerical pseudoAxes */
  57 /*********************************************/
  58
  59 /**
  60  * @brief This private method find the degenerated axes.
  61  *
  62  * @param func the gsl_multiroopt_function to test
  63  * @param x the starting point
  64  * @param f the result of the function evaluation.
  65  *
  66  * with this method we can see if an axis is degenerated or not.
  67  * A degenerated axis is an axis with no effect on the function evaluation.
  68  * In the Jacobian matrix all elements of a columnn is null.
  69  * Once we know this the axis is mark as degenerated and we do not need to
  70  * change is sector.
  71  */
  72 static void find_degenerated_axes(HklEngine *self,
  73                                   gsl_multiroot_function *func,
  74                                   gsl_vector const *x, gsl_vector const *f,
  75                                   int degenerated[])
  76 {
  77         gsl_matrix *J;
  78         size_t i, j;
  79
  80         memset(degenerated, 0, x->size * sizeof(int));
  81         J = gsl_matrix_alloc(x->size, f->size);
  82
  83         gsl_multiroot_fdjacobian(func, x, f, GSL_SQRT_DBL_EPSILON, J);
  84         for(j=0; j<x->size && !degenerated[j]; ++j) {
  85                 for(i=0; i<f->size; ++i)
  86                         if (fabs(gsl_matrix_get(J, i, j)) > HKL_EPSILON)
  87                                 break;
  88                 if (i == f->size)
  89                         degenerated[j] = 1;
  90         }
  91
  92 #ifdef DEBUG
  93         fprintf(stdout, "\nLooks for degenerated axes\n");
  94         for(i=0; i<x->size; ++i)
  95                 fprintf(stdout, " %d", degenerated[i]);
  96         for(i=0;i<x->size;++i) {
  97                 fprintf(stdout, "\n   ");
  98                 for(j=0;j<f->size;++j)
  99                         fprintf(stdout, " %f", gsl_matrix_get(J, i, j));
 100         }
 101         fprintf(stdout, "\n");
 102 #endif
 103         gsl_matrix_free(J);
 104 }
 105
 106 /**
 107  * @brief this private method try to find the first solution
 108  *
 109  * @param self the current HklPseudoAxeEngine.
 110  * @param f The function to use for the computation.
 111  *
 112  * If a solution was found it also check for degenerated axes.
 113  * A degenerated axes is an Axes with no effect on the function.
 114  * @see find_degenerated
 115  * @return TRUE or FALSE.
 116  */
 117 static int find_first_geometry(HklEngine *self,
 118                                gsl_multiroot_function *f,
 119                                int degenerated[])
 120 {
 121         gsl_multiroot_fsolver_type const *T;
 122         gsl_multiroot_fsolver *s;
 123         gsl_vector *x;
 124         size_t len = self->mode->info->n_axes;
 125         double *x_data;
 126         double *x_data0 = alloca(len * sizeof(*x_data0));
 127         size_t iter = 0;
 128         int status;
 129         int res = FALSE;
 130         size_t i;
 131         HklParameter **axis;
 132
 133         /* get the starting point from the geometry */
 134         /* must be put in the auto_set method */
 135         x = gsl_vector_alloc(len);
 136         x_data = (double *)x->data;
 137         i = 0;
 138         darray_foreach(axis, self->axes){
 139                 x_data[i++] = (*axis)->_value;
 140         }
 141
 142         /* keep a copy of the first axes positions to deal with degenerated axes */
 143         memcpy(x_data0, x_data, len * sizeof(double));
 144
 145         /* Initialize method  */
 146         T = gsl_multiroot_fsolver_hybrid;
 147         s = gsl_multiroot_fsolver_alloc (T, len);
 148         gsl_multiroot_fsolver_set (s, f, x);
 149
 150         /* iterate to find the solution */
 151         do {
 152                 ++iter;
 153                 status = gsl_multiroot_fsolver_iterate(s);
 154                 if (status || iter % 300 == 0) {
 155                         /* Restart from another point. */
 156                         for(i=0; i<len; ++i)
 157                                 x_data[i] = (double)rand() / RAND_MAX * 180. / M_PI;
 158                         gsl_multiroot_fsolver_set(s, f, x);
 159                         gsl_multiroot_fsolver_iterate(s);
 160                 }
 161                 status = gsl_multiroot_test_residual (s->f, HKL_EPSILON / 10.);
 162         } while (status == GSL_CONTINUE && iter < 2000);
 163
 164 #ifdef DEBUG
 165         fprintf(stdout, "\nstatus : %d iter : %d", status, iter);
 166         for(i=0; i<len; ++i)
 167                 fprintf(stdout, " %.7f", s->f->data[i]);
 168         fprintf(stdout, "\n");
 169 #endif
 170
 171         if (status != GSL_CONTINUE) {
 172                 find_degenerated_axes(self, f, s->x, s->f, degenerated);
 173
 174 #ifdef DEBUG
 175                 /* print the test header */
 176                 fprintf(stdout, "\n");
 177                 for(i=0; i<len; ++i)
 178                         fprintf(stdout, "\t f(%d)", i);
 179                 darray_foreach(axis, self->axes){
 180                         fprintf(stdout, "\t \"%s\"", (*axis)->name);
 181                 }
 182 #endif
 183                 /* set the geometry from the gsl_vector */
 184                 /* in a futur version the geometry must contain a gsl_vector */
 185                 /* to avoid this. */
 186                 x_data = (double *)s->x->data;
 187                 i = 0;
 188                 darray_foreach(axis, self->axes){
 189                         hkl_parameter_value_set(*axis,
 190                                                 degenerated[i] ? x_data0[i] : x_data[i],
 191                                                 HKL_UNIT_DEFAULT,
 192                                                 NULL);
 193                         ++i;
 194                 }
 195
 196                 hkl_geometry_update(self->geometry);
 197                 res = TRUE;
 198         }
 199
 200         /* release memory */
 201         gsl_vector_free(x);
 202         gsl_multiroot_fsolver_free(s);
 203
 204         return res;
 205 }
 206
 207 /**
 208  * @brief This private method change the sector of angles.
 209  *
 210  * @param x The vector of changed angles.
 211  * @param x0 The vector of angles to change.
 212  * @param sector the sector vector operation.
 213  * @param n the size of all vectors.
 214  *
 215  * 0 -> no change
 216  * 1 -> pi - angle
 217  * 2 -> pi + angle
 218  * 3 -> -angle
 219  */
 220 static void change_sector(double x[], double const x0[],
 221                           int const sector[], size_t n)
 222 {
 223         size_t i;
 224
 225         for(i=0; i<n; ++i) {
 226                 switch (sector[i]) {
 227                 case 0:
 228                         x[i] = x0[i];
 229                         break;
 230                 case 1:
 231                         x[i] = M_PI - x0[i];
 232                         break;
 233                 case 2:
 234                         x[i] = M_PI + x0[i];
 235                         break;
 236                 case 3:
 237                         x[i] = -x0[i];
 238                         break;
 239                 }
 240         }
 241 }
 242
 243 /**
 244  * @brief Test if an angle combination is compatible with q function.
 245  *
 246  * @param x The vector of angles to test.
 247  * @param function The gsl_multiroot_function used for the test.
 248  * @param f a gsl_vector use to compute the result (optimization)
 249  */
 250 static int test_sector(gsl_vector const *x,
 251                        gsl_multiroot_function *function,
 252                        gsl_vector *f)
 253 {
 254         int res = TRUE;
 255         size_t i;
 256         double *f_data = f->data;
 257
 258         function->f(x, function->params, f);
 259
 260         for(i=0; i<f->size; ++i)
 261                 if (fabs(f_data[i]) > HKL_EPSILON){
 262                         res = FALSE;
 263                         break;
 264                 }
 265
 266 #ifdef DEBUG
 267         fprintf(stdout, "\n");
 268         for(i=0; i<f->size; ++i)
 269                 if(fabs(f_data[i]) < HKL_EPSILON)
 270                         fprintf(stdout, "\t%f *", f_data[i]);
 271                 else
 272                         fprintf(stdout, "\t%f", f_data[i]);
 273         for(i=0; i<f->size; ++i)
 274                 fprintf(stdout, "\t%f", gsl_sf_angle_restrict_symm(x->data[i]) * HKL_RADTODEG);
 275
 276         if(res == FALSE)
 277                 fprintf(stdout, "\t FAIL");
 278         else
 279                 fprintf(stdout, "\t SUCCESS");
 280 #endif
 281
 282         return res;
 283 }
 284
 285 /**
 286  * @brief compute the permutation and test its validity.
 287  *
 288  * @param axes_len number of axes
 289  * @param op_len number of operation per axes. (4 for now)
 290  * @param p The vector containing the current permutation.
 291  * @param axes_idx The index of the axes we are permution.
 292  * @param op the current operation to set.
 293  * @param f The function for the validity test.
 294  * @param x0 The starting point of all geometry permutations.
 295  * @param _x a gsl_vector use to compute the sectors (optimization)
 296  * @param _f a gsl_vector use during the sector test (optimization)
 297  */
 298 static void perm_r(size_t axes_len, size_t op_len[], int p[], size_t axes_idx,
 299                    int op, gsl_multiroot_function *f, double x0[],
 300                    gsl_vector *_x, gsl_vector *_f)
 301 {
 302         size_t i;
 303
 304         p[axes_idx++] = op;
 305         if (axes_idx == axes_len) {
 306                 double *x_data = _x->data;
 307                 change_sector(x_data, x0, p, axes_len);
 308                 if (test_sector(_x, f, _f))
 309                         hkl_engine_add_geometry(f->params, x_data);
 310         } else
 311                 for (i=0; i<op_len[axes_idx]; ++i)
 312                         perm_r(axes_len, op_len, p, axes_idx, i, f, x0, _x, _f);
 313 }
 314
 315 /**
 316  * @brief Find all numerical solutions of a mode.
 317  *
 318  * @param self the current HklEngine
 319  * @param function The mode function
 320  *
 321  * @return TRUE or FALSE
 322  *
 323  * This method find a first solution with a numerical method from the
 324  * GSL library (the multi root solver hybrid). Then it multiplicates the
 325  * solutions from this starting point using cosinus/sinus properties.
 326  * It addes all valid solutions to the self->geometries.
 327  */
 328 static int solve_function(HklEngine *self,
 329                           const HklFunction *function)
 330 {
 331
 332         size_t i;
 333         int p[function->size];
 334         double x0[function->size];
 335         int degenerated[function->size];
 336         size_t op_len[function->size];
 337         int res;
 338         gsl_vector *_x; /* use to compute sectors in perm_r (avoid copy) */
 339         gsl_vector *_f; /* use to test sectors in perm_r (avoid copy) */
 340         gsl_multiroot_function f;
 341         HklParameter **axis;
 342
 343         _x = gsl_vector_alloc(function->size);
 344         _f = gsl_vector_alloc(function->size);
 345
 346         f.f = function->function;
 347         f.n = function->size;
 348         f.params = self;
 349
 350         res = find_first_geometry(self, &f, degenerated);
 351         if (res) {
 352                 memset(p, 0, sizeof(p));
 353                 /* use first solution as starting point for permutations */
 354                 i = 0;
 355                 darray_foreach(axis, self->axes){
 356                         x0[i] = (*axis)->_value;
 357                         op_len[i] = degenerated[i] ? 1 : 4;
 358                         ++i;
 359                 }
 360                 for (i=0; i<op_len[0]; ++i)
 361                         perm_r(function->size, op_len, p, 0, i, &f, x0, _x, _f);
 362         }
 363
 364         gsl_vector_free(_f);
 365         gsl_vector_free(_x);
 366         return res;
 367 }
 368
 369 /* check that the number of axis of the mode is the right number of variables expected by mode functions */
 370 static inline void check_validity(const HklModeAutoInfo *info)
 371 {
 372         for(uint i=0; i<info->n_functions; ++i)
 373                 hkl_assert(info->functions[i]->size == info->mode.n_axes);
 374 }
 375
 376 HklMode *hkl_mode_auto_new(const HklModeAutoInfo *info,
 377                            const HklModeOperations *ops)
 378 {
 379         check_validity(info);
 380
 381         return hkl_mode_new(&info->mode, ops);
 382
 383 }
 384
 385 void hkl_mode_auto_init(HklMode *self,
 386                         const HklModeAutoInfo *info,
 387                         const HklModeOperations *ops)
 388 {
 389         check_validity(info);
 390
 391         hkl_mode_init(self, &info->mode, ops);
 392
 393 }
 394
 395 int hkl_mode_auto_set_real(HklMode *self,
 396                            HklEngine *engine,
 397                            HklGeometry *geometry,
 398                            HklDetector *detector,
 399                            HklSample *sample,
 400                            GError **error)
 401 {
 402         size_t i;
 403         int ok = FALSE;
 404         HklModeAutoInfo *info = container_of(self->info, HklModeAutoInfo, mode);
 405
 406         hkl_return_val_if_fail (error == NULL || *error == NULL, FALSE);
 407
 408         if(!self || !engine || !geometry || !detector || !sample){
 409                 g_set_error(error,
 410                             HKL_MODE_AUTO_ERROR,
 411                             HKL_MODE_AUTO_ERROR_SET,
 412                             "Internal error");
 413                 return FALSE;
 414         }
 415
 416         for(i=0;i<info->n_functions;++i)
 417                 ok |= solve_function(engine, info->functions[i]);
 418
 419         if(!ok){
 420                 g_set_error(error,
 421                             HKL_MODE_AUTO_ERROR,
 422                             HKL_MODE_AUTO_ERROR_SET,
 423                             "none of the functions were solved !!!");
 424                 return FALSE;
 425         }
 426
 427 #ifdef DEBUG
 428         hkl_engine_fprintf(stdout, engine);
 429 #endif
 430
 431         return TRUE;
 432 }
 433
 434 HklMode *hkl_mode_auto_with_init_new(const HklModeAutoInfo *info,
 435                                      const HklModeOperations *ops)
 436 {
 437         HklModeAutoWithInit *self = HKL_MALLOC(HklModeAutoWithInit);
 438
 439         hkl_mode_auto_init(&self->mode, info, ops);
 440         self->geometry = NULL;
 441         self->detector = NULL;
 442         self->sample = NULL;
 443
 444         return &self->mode;
 445 }