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-2020, 2022 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                 darray_foreach(axis, self->axes){
 207                         fprintf(stdout, "\t \"%s\"", (*axis)->name);
 208                 }
 209                 for(i=0; i<len; ++i)
 210                         fprintf(stdout, "\t f(%d)", i);
 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 #ifdef DEBUG
 288         fprintf(stdout, "\n");
 289         for(i=0; i<f->size; ++i)
 290                 fprintf(stdout, "\t%f", x->data[i]);
 291         /* fprintf(stdout, "\t%f", gsl_sf_angle_restrict_symm(x->data[i]) * HKL_RADTODEG); */
 292         fflush(stdout);
 293 #endif
 294
 295         function->f(x, function->params, f);
 296
 297         for(i=0; i<f->size; ++i)
 298                 if (fabs(f_data[i]) > HKL_EPSILON){
 299                         res = FALSE;
 300                         break;
 301                 }
 302
 303 #ifdef DEBUG
 304         for(i=0; i<f->size; ++i)
 305                 if(fabs(f_data[i]) < HKL_EPSILON)
 306                         fprintf(stdout, "\t%f *", f_data[i]);
 307                 else
 308                         fprintf(stdout, "\t%f", f_data[i]);
 309
 310         if(res == FALSE)
 311                 fprintf(stdout, "\t FAIL");
 312         else
 313                 fprintf(stdout, "\t SUCCESS");
 314 #endif
 315
 316         return res;
 317 }
 318
 319 /**
 320  * @brief compute the permutation and test its validity.
 321  *
 322  * @param axes_len number of axes
 323  * @param op_len number of operation per axes. (4 for now)
 324  * @param p The vector containing the current permutation.
 325  * @param axes_idx The index of the axes we are permution.
 326  * @param op the current operation to set.
 327  * @param f The function for the validity test.
 328  * @param x0 The starting point of all geometry permutations.
 329  * @param _x a gsl_vector use to compute the sectors (optimization)
 330  * @param _f a gsl_vector use during the sector test (optimization)
 331  */
 332 static void perm_r(size_t axes_len, size_t op_len[], int p[], size_t axes_idx,
 333                    int op, gsl_multiroot_function *f, double x0[],
 334                    gsl_vector *_x, gsl_vector *_f)
 335 {
 336         size_t i;
 337
 338         p[axes_idx++] = op;
 339         if (axes_idx == axes_len) {
 340                 double *x_data = _x->data;
 341                 change_sector(x_data, x0, p, axes_len);
 342                 if (test_sector(_x, f, _f))
 343                         hkl_engine_add_geometry(f->params, x_data);
 344         } else
 345                 for (i=0; i<op_len[axes_idx]; ++i)
 346                         perm_r(axes_len, op_len, p, axes_idx, i, f, x0, _x, _f);
 347 }
 348
 349 /**
 350  * @brief Find all numerical solutions of a mode.
 351  *
 352  * @param self the current HklEngine
 353  * @param function The mode function
 354  *
 355  * @return TRUE or FALSE
 356  *
 357  * This method find a first solution with a numerical method from the
 358  * GSL library (the multi root solver hybrid). Then it multiplicates the
 359  * solutions from this starting point using cosinus/sinus properties.
 360  * It addes all valid solutions to the self->geometries.
 361  */
 362 static int solve_function(HklEngine *self,
 363                           const HklFunction *function)
 364 {
 365
 366         size_t i;
 367         int p[function->size];
 368         double x0[function->size];
 369         int degenerated[function->size];
 370         size_t op_len[function->size];
 371         int res;
 372         gsl_vector *_x; /* use to compute sectors in perm_r (avoid copy) */
 373         gsl_vector *_f; /* use to test sectors in perm_r (avoid copy) */
 374         gsl_multiroot_function f;
 375         HklParameter **axis;
 376
 377         _x = gsl_vector_alloc(function->size);
 378         _f = gsl_vector_alloc(function->size);
 379
 380         f.f = function->function;
 381         f.n = function->size;
 382         f.params = self;
 383
 384         res = find_first_geometry(self, &f, degenerated);
 385         if (res) {
 386                 memset(p, 0, sizeof(p));
 387                 /* use first solution as starting point for permutations */
 388                 i = 0;
 389                 darray_foreach(axis, self->axes){
 390                         x0[i] = (*axis)->_value;
 391                         op_len[i] = degenerated[i] ? 1 : 4;
 392                         ++i;
 393                 }
 394                 for (i=0; i<op_len[0]; ++i)
 395                         perm_r(function->size, op_len, p, 0, i, &f, x0, _x, _f);
 396         }
 397
 398         gsl_vector_free(_f);
 399         gsl_vector_free(_x);
 400         return res;
 401 }
 402
 403 /* check that the number of axis of the mode is the right number of variables expected by mode functions */
 404 static inline void check_validity(const HklModeAutoInfo *auto_info)
 405 {
 406         const HklFunction **function;
 407         darray_foreach(function, auto_info->functions)
 408                 hkl_assert((*function)->size == darray_size(auto_info->info.axes_w));
 409 }
 410
 411 HklMode *hkl_mode_auto_new(const HklModeAutoInfo *auto_info,
 412                            const HklModeOperations *ops,
 413                            int initialized)
 414 {
 415         check_validity(auto_info);
 416
 417         return hkl_mode_new(&auto_info->info, ops, initialized);
 418
 419 }
 420
 421 void hkl_mode_auto_init(HklMode *self,
 422                         const HklModeAutoInfo *auto_info,
 423                         const HklModeOperations *ops,
 424                         int initialized)
 425 {
 426         check_validity(auto_info);
 427
 428         hkl_mode_init(self, &auto_info->info, ops, initialized);
 429
 430 }
 431
 432 int hkl_mode_auto_set_real(HklMode *self,
 433                            HklEngine *engine,
 434                            HklGeometry *geometry,
 435                            HklDetector *detector,
 436                            HklSample *sample,
 437                            GError **error)
 438 {
 439         int ok = FALSE;
 440         HklModeAutoInfo *auto_info = container_of(self->info, HklModeAutoInfo, info);
 441         const HklFunction **function;
 442
 443         hkl_error (error == NULL || *error == NULL);
 444
 445         if(!self || !engine || !geometry || !detector || !sample){
 446                 g_set_error(error,
 447                             HKL_MODE_AUTO_ERROR,
 448                             HKL_MODE_AUTO_ERROR_SET,
 449                             "Internal error");
 450                 return FALSE;
 451         }
 452
 453         darray_foreach(function, auto_info->functions)
 454                 ok |= solve_function(engine, *function);
 455
 456         if(!ok){
 457                 g_set_error(error,
 458                             HKL_MODE_AUTO_ERROR,
 459                             HKL_MODE_AUTO_ERROR_SET,
 460                             "none of the functions were solved !!!");
 461                 return FALSE;
 462         }
 463
 464 #ifdef DEBUG
 465         hkl_engine_fprintf(stdout, engine);
 466 #endif
 467
 468         return TRUE;
 469 }
 470
 471 HklMode *hkl_mode_auto_with_init_new(const HklModeAutoInfo *auto_info,
 472                                      const HklModeOperations *ops,
 473                                      int initialized)
 474 {
 475         HklModeAutoWithInit *self = g_new(HklModeAutoWithInit, 1);
 476
 477         hkl_mode_auto_init(&self->mode, auto_info, ops, initialized);
 478
 479         self->geometry = NULL;
 480         self->detector = NULL;
 481         self->sample = NULL;
 482
 483         return &self->mode;
 484 }