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-2016 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 = darray_size(self->mode->info->axes_w);
 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 #ifdef DEBUG
 151         fprintf(stdout, "Initial starting point: \n");
 152         fprintf(stdout, "x: ");
 153         for(i=0; i<len; ++i)
 154                 fprintf(stdout, " %.7f", s->x->data[i]);
 155         fprintf(stdout, "\nf: ");
 156         for(i=0; i<len; ++i)
 157                 fprintf(stdout, " %.7f", s->f->data[i]);
 158 #endif
 159
 160         /* iterate to find the solution */
 161         do {
 162                 ++iter;
 163                 status = gsl_multiroot_fsolver_iterate(s);
 164 #ifdef DEBUG
 165                 fprintf(stdout, "\nstatus : %d iter : %d\n", status, iter);
 166 #endif
 167                 if (status || (iter % 300) == 0) {
 168                         /* Restart from another point. */
 169                         for(i=0; i<len; ++i)
 170                                 x_data[i] = (double)rand() / RAND_MAX / 180. * M_PI;
 171                         gsl_multiroot_fsolver_set(s, f, x);
 172                         gsl_multiroot_fsolver_iterate(s);
 173 #ifdef DEBUG
 174                         fprintf(stdout, "randomize the starting point: \n");
 175                         fprintf(stdout, "x: ");
 176                         for(i=0; i<len; ++i)
 177                                 fprintf(stdout, " %.7f", s->x->data[i]);
 178                         fprintf(stdout, "\nf: ");
 179                         for(i=0; i<len; ++i)
 180                                 fprintf(stdout, " %.7f", s->f->data[i]);
 181 #endif
 182                 }
 183                 status = gsl_multiroot_test_residual (s->f, HKL_EPSILON / 10.);
 184 #ifdef DEBUG
 185                 fprintf(stdout, "\nstatus : %d iter : %d", status, iter);
 186                 for(i=0; i<len; ++i)
 187                         fprintf(stdout, " %.7f", s->f->data[i]);
 188                 fprintf(stdout, "\n");
 189 #endif
 190
 191         } while (status == GSL_CONTINUE && iter < 2000);
 192
 193 #ifdef DEBUG
 194         fprintf(stdout, "\nstatus : %d iter : %d", status, iter);
 195         for(i=0; i<len; ++i)
 196                 fprintf(stdout, " %.7f", s->f->data[i]);
 197         fprintf(stdout, "\n");
 198 #endif
 199
 200         if (status != GSL_CONTINUE) {
 201                 find_degenerated_axes(self, f, s->x, s->f, degenerated);
 202
 203 #ifdef DEBUG
 204                 /* print the test header */
 205                 fprintf(stdout, "\n");
 206                 for(i=0; i<len; ++i)
 207                         fprintf(stdout, "\t f(%d)", i);
 208                 darray_foreach(axis, self->axes){
 209                         fprintf(stdout, "\t \"%s\"", (*axis)->name);
 210                 }
 211 #endif
 212                 /* set the geometry from the gsl_vector */
 213                 /* in a futur version the geometry must contain a gsl_vector */
 214                 /* to avoid this. */
 215                 x_data = (double *)s->x->data;
 216                 i = 0;
 217                 darray_foreach(axis, self->axes){
 218                         hkl_parameter_value_set(*axis,
 219                                                 degenerated[i] ? x_data0[i] : x_data[i],
 220                                                 HKL_UNIT_DEFAULT,
 221                                                 NULL);
 222                         ++i;
 223                 }
 224
 225                 hkl_geometry_update(self->geometry);
 226                 res = TRUE;
 227         }
 228
 229         /* release memory */
 230         gsl_vector_free(x);
 231         gsl_multiroot_fsolver_free(s);
 232
 233         return res;
 234 }
 235
 236 /**
 237  * @brief This private method change the sector of angles.
 238  *
 239  * @param x The vector of changed angles.
 240  * @param x0 The vector of angles to change.
 241  * @param sector the sector vector operation.
 242  * @param n the size of all vectors.
 243  *
 244  * 0 -> no change
 245  * 1 -> pi - angle
 246  * 2 -> pi + angle
 247  * 3 -> -angle
 248  */
 249 static void change_sector(double x[], double const x0[],
 250                           int const sector[], size_t n)
 251 {
 252         size_t i;
 253
 254         for(i=0; i<n; ++i) {
 255                 switch (sector[i]) {
 256                 case 0:
 257                         x[i] = x0[i];
 258                         break;
 259                 case 1:
 260                         x[i] = M_PI - x0[i];
 261                         break;
 262                 case 2:
 263                         x[i] = M_PI + x0[i];
 264                         break;
 265                 case 3:
 266                         x[i] = -x0[i];
 267                         break;
 268                 }
 269         }
 270 }
 271
 272 /**
 273  * @brief Test if an angle combination is compatible with q function.
 274  *
 275  * @param x The vector of angles to test.
 276  * @param function The gsl_multiroot_function used for the test.
 277  * @param f a gsl_vector use to compute the result (optimization)
 278  */
 279 static int test_sector(gsl_vector const *x,
 280                        gsl_multiroot_function *function,
 281                        gsl_vector *f)
 282 {
 283         int res = TRUE;
 284         size_t i;
 285         double *f_data = f->data;
 286
 287         function->f(x, function->params, f);
 288
 289         for(i=0; i<f->size; ++i)
 290                 if (fabs(f_data[i]) > HKL_EPSILON){
 291                         res = FALSE;
 292                         break;
 293                 }
 294
 295 #ifdef DEBUG
 296         fprintf(stdout, "\n");
 297         for(i=0; i<f->size; ++i)
 298                 if(fabs(f_data[i]) < HKL_EPSILON)
 299                         fprintf(stdout, "\t%f *", f_data[i]);
 300                 else
 301                         fprintf(stdout, "\t%f", f_data[i]);
 302         for(i=0; i<f->size; ++i)
 303                 fprintf(stdout, "\t%f", gsl_sf_angle_restrict_symm(x->data[i]) * HKL_RADTODEG);
 304
 305         if(res == FALSE)
 306                 fprintf(stdout, "\t FAIL");
 307         else
 308                 fprintf(stdout, "\t SUCCESS");
 309 #endif
 310
 311         return res;
 312 }
 313
 314 /**
 315  * @brief compute the permutation and test its validity.
 316  *
 317  * @param axes_len number of axes
 318  * @param op_len number of operation per axes. (4 for now)
 319  * @param p The vector containing the current permutation.
 320  * @param axes_idx The index of the axes we are permution.
 321  * @param op the current operation to set.
 322  * @param f The function for the validity test.
 323  * @param x0 The starting point of all geometry permutations.
 324  * @param _x a gsl_vector use to compute the sectors (optimization)
 325  * @param _f a gsl_vector use during the sector test (optimization)
 326  */
 327 static void perm_r(size_t axes_len, size_t op_len[], int p[], size_t axes_idx,
 328                    int op, gsl_multiroot_function *f, double x0[],
 329                    gsl_vector *_x, gsl_vector *_f)
 330 {
 331         size_t i;
 332
 333         p[axes_idx++] = op;
 334         if (axes_idx == axes_len) {
 335                 double *x_data = _x->data;
 336                 change_sector(x_data, x0, p, axes_len);
 337                 if (test_sector(_x, f, _f))
 338                         hkl_engine_add_geometry(f->params, x_data);
 339         } else
 340                 for (i=0; i<op_len[axes_idx]; ++i)
 341                         perm_r(axes_len, op_len, p, axes_idx, i, f, x0, _x, _f);
 342 }
 343
 344 /**
 345  * @brief Find all numerical solutions of a mode.
 346  *
 347  * @param self the current HklEngine
 348  * @param function The mode function
 349  *
 350  * @return TRUE or FALSE
 351  *
 352  * This method find a first solution with a numerical method from the
 353  * GSL library (the multi root solver hybrid). Then it multiplicates the
 354  * solutions from this starting point using cosinus/sinus properties.
 355  * It addes all valid solutions to the self->geometries.
 356  */
 357 static int solve_function(HklEngine *self,
 358                           const HklFunction *function)
 359 {
 360
 361         size_t i;
 362         int p[function->size];
 363         double x0[function->size];
 364         int degenerated[function->size];
 365         size_t op_len[function->size];
 366         int res;
 367         gsl_vector *_x; /* use to compute sectors in perm_r (avoid copy) */
 368         gsl_vector *_f; /* use to test sectors in perm_r (avoid copy) */
 369         gsl_multiroot_function f;
 370         HklParameter **axis;
 371
 372         _x = gsl_vector_alloc(function->size);
 373         _f = gsl_vector_alloc(function->size);
 374
 375         f.f = function->function;
 376         f.n = function->size;
 377         f.params = self;
 378
 379         res = find_first_geometry(self, &f, degenerated);
 380         if (res) {
 381                 memset(p, 0, sizeof(p));
 382                 /* use first solution as starting point for permutations */
 383                 i = 0;
 384                 darray_foreach(axis, self->axes){
 385                         x0[i] = (*axis)->_value;
 386                         op_len[i] = degenerated[i] ? 1 : 4;
 387                         ++i;
 388                 }
 389                 for (i=0; i<op_len[0]; ++i)
 390                         perm_r(function->size, op_len, p, 0, i, &f, x0, _x, _f);
 391         }
 392
 393         gsl_vector_free(_f);
 394         gsl_vector_free(_x);
 395         return res;
 396 }
 397
 398 /* check that the number of axis of the mode is the right number of variables expected by mode functions */
 399 static inline void check_validity(const HklModeAutoInfo *auto_info)
 400 {
 401         const HklFunction **function;
 402         darray_foreach(function, auto_info->functions)
 403                 hkl_assert((*function)->size == darray_size(auto_info->info.axes_w));
 404 }
 405
 406 HklMode *hkl_mode_auto_new(const HklModeAutoInfo *auto_info,
 407                            const HklModeOperations *ops,
 408                            int initialized)
 409 {
 410         check_validity(auto_info);
 411
 412         return hkl_mode_new(&auto_info->info, ops, initialized);
 413
 414 }
 415
 416 void hkl_mode_auto_init(HklMode *self,
 417                         const HklModeAutoInfo *auto_info,
 418                         const HklModeOperations *ops,
 419                         int initialized)
 420 {
 421         check_validity(auto_info);
 422
 423         hkl_mode_init(self, &auto_info->info, ops, initialized);
 424
 425 }
 426
 427 int hkl_mode_auto_set_real(HklMode *self,
 428                            HklEngine *engine,
 429                            HklGeometry *geometry,
 430                            HklDetector *detector,
 431                            HklSample *sample,
 432                            GError **error)
 433 {
 434         int ok = FALSE;
 435         HklModeAutoInfo *auto_info = container_of(self->info, HklModeAutoInfo, info);
 436         const HklFunction **function;
 437
 438         hkl_error (error == NULL || *error == NULL);
 439
 440         if(!self || !engine || !geometry || !detector || !sample){
 441                 g_set_error(error,
 442                             HKL_MODE_AUTO_ERROR,
 443                             HKL_MODE_AUTO_ERROR_SET,
 444                             "Internal error");
 445                 return FALSE;
 446         }
 447
 448         darray_foreach(function, auto_info->functions)
 449                 ok |= solve_function(engine, *function);
 450
 451         if(!ok){
 452                 g_set_error(error,
 453                             HKL_MODE_AUTO_ERROR,
 454                             HKL_MODE_AUTO_ERROR_SET,
 455                             "none of the functions were solved !!!");
 456                 return FALSE;
 457         }
 458
 459 #ifdef DEBUG
 460         hkl_engine_fprintf(stdout, engine);
 461 #endif
 462
 463         return TRUE;
 464 }
 465
 466 HklMode *hkl_mode_auto_with_init_new(const HklModeAutoInfo *auto_info,
 467                                      const HklModeOperations *ops,
 468                                      int initialized)
 469 {
 470         HklModeAutoWithInit *self = HKL_MALLOC(HklModeAutoWithInit);
 471
 472         hkl_mode_auto_init(&self->mode, auto_info, ops, initialized);
 473
 474         self->geometry = NULL;
 475         self->detector = NULL;
 476         self->sample = NULL;
 477
 478         return &self->mode;
 479 }