src/hkl-pseudoaxis.c

   1 #include <string.h>
   2 #include <gsl/gsl_sf_trig.h>
   3 #include <hkl/hkl-pseudoaxis.h>
   4
   5 /***********/
   6 /* private */
   7 /***********/
   8
   9 /**
  10  * @brief This private method find the degenerated axes.
  11  *
  12  * @param func the gsl_multiroopt_function to test
  13  * @param x the starting point
  14  * @param f the result of the function evaluation.
  15  *
  16  * with this method we can see if an axis is degenerated or not.
  17  * A degenerated axis is an axis with no effect on the function evaluation.
  18  * In the Jacobian matrix all elements of a columnn is null.
  19  * Once we know this the axis is mark as degenerated and we do not need to
  20  * change is sector.
  21  */
  22 static void find_degenerated(HklPseudoAxisEngine *self,
  23                 gsl_multiroot_function *func,
  24                 gsl_vector const *x, gsl_vector const *f)
  25 {
  26         gsl_matrix *J;
  27         size_t i, j;
  28         int degenerated[x->size];
  29
  30         memset(degenerated, 0, sizeof(degenerated));
  31         J = gsl_matrix_alloc(x->size, f->size);
  32
  33         gsl_multiroot_fdjacobian(func, x, f, GSL_SQRT_DBL_EPSILON, J);
  34         for(j=0; j<x->size && !degenerated[j]; ++j) {
  35                 for(i=0; i<f->size; ++i)
  36                         if (fabs(gsl_matrix_get(J, i, j)) > HKL_EPSILON)
  37                                 break;
  38                 if (i == f->size)
  39                         degenerated[j] = 1;
  40         }
  41
  42         /*
  43         hkl_pseudoAxisEngine_fprintf(func->params, stdout);
  44         fprintf(stdout, "\n");
  45         for(i=0; i<x->size; ++i)
  46                 fprintf(stdout, " %d", degenerated[i]);
  47         for(i=0;i<x->size;++i) {
  48                 fprintf(stdout, "\n   ");
  49                 for(j=0;j<f->size;++j)
  50                         fprintf(stdout, " %f", gsl_matrix_get(J, i, j));
  51         }
  52         fprintf(stdout, "\n");
  53         */
  54         gsl_matrix_free(J);
  55 }
  56
  57 /**
  58  * @brief this private method Add a geometry to the geometries
  59  *
  60  * @param self The current PseudoAxeEngine
  61  * @param x A vector of double with the axes values to put in the geometry.
  62  *
  63  * This method try to be clever by allocating memory only if the current
  64  * length of the geometries is not large enought. Then it just set the
  65  * geometry axes and copy it to the right geometries.
  66  */
  67 static void add_geometry(HklPseudoAxisEngine *self, double const x[])
  68 {
  69         size_t i;
  70         HklGeometry *geometry;
  71
  72         // first check if we can get an old geometry.
  73         if (self->geometries_len == self->geometries_alloc) {
  74                 self->geometries_alloc = alloc_nr(self->geometries_alloc);
  75                 self->geometries = realloc(self->geometries,
  76                                 self->geometries_alloc * sizeof(HklGeometry*));
  77                 for(i=self->geometries_len; i<self->geometries_alloc; i++)
  78                         self->geometries[i] = hkl_geometry_new_copy(self->geometry);
  79         }
  80
  81         /* copy the axes configuration into the engine->geometry*/
  82         for(i=0; i<self->axes_len; ++i)
  83                 self->axes[i]->config.value = x[i];
  84
  85         /* put the axes configuration from engine->geometry -> geometry */
  86         geometry = self->geometries[self->geometries_len++];
  87         hkl_geometry_init_geometry(geometry, self->geometry);
  88 }
  89
  90 /**
  91  * @brief this private method try to find the first solution
  92  *
  93  * @param self the current HklPseudoAxeEngine.
  94  * @param f The function to use for the computation.
  95  *
  96  * If a solution was found it also check for degenerated axes.
  97  * A degenerated axes is an Axes with no effect on the function.
  98  * @see find_degenerated
  99  * @return HKL_SUCCESS or HKL_FAIL.
 100  */
 101 static int find_first_geometry(HklPseudoAxisEngine *self,
 102                 gsl_multiroot_function *f)
 103 {
 104         gsl_multiroot_fsolver_type const *T;
 105         gsl_multiroot_fsolver *s;
 106         gsl_vector *x;
 107         double *x_data;
 108         size_t iter = 0;
 109         int status;
 110         int res = HKL_FAIL;
 111         size_t i;
 112
 113         // get the starting point from the geometry
 114         // must be put in the auto_set method
 115         x = gsl_vector_alloc(self->axes_len);
 116         x_data = gsl_vector_ptr(x, 0);
 117         for(i=0; i<self->axes_len; ++i)
 118                 x_data[i] = self->axes[i]->config.value;
 119
 120         // Initialize method
 121         T = gsl_multiroot_fsolver_hybrid;
 122         s = gsl_multiroot_fsolver_alloc (T, self->axes_len);
 123         gsl_multiroot_fsolver_set (s, f, x);
 124
 125         // iterate to find the solution
 126         do {
 127                 ++iter;
 128                 status = gsl_multiroot_fsolver_iterate(s);
 129                 if (status || iter % 1000 == 0) {
 130                         // Restart from another point.
 131                         for(i=0; i<self->axes_len; ++i)
 132                                 x_data[i] = (double)rand() / RAND_MAX * 180. / M_PI;
 133                         gsl_multiroot_fsolver_set(s, f, x);
 134                         status = gsl_multiroot_fsolver_iterate(s);
 135                 }
 136                 status = gsl_multiroot_test_residual (s->f, HKL_EPSILON);
 137         } while (status == GSL_CONTINUE && iter < 1000);
 138
 139         if (status != GSL_CONTINUE) {
 140                 find_degenerated(self, f, s->x, s->f);
 141                 // set the geometry from the gsl_vector
 142                 // in a futur version the geometry must contain a gsl_vector
 143                 // to avoid this.
 144                 for(i=0; i<self->axes_len; ++i) {
 145                         HklAxis *axis = self->axes[i];
 146                         x_data = gsl_vector_ptr(s->x, 0);
 147                         axis->config.value = gsl_sf_angle_restrict_pos(x_data[i]);
 148                         axis->config.dirty = 1;
 149                 }
 150                 hkl_geometry_update(self->geometry);
 151                 res = HKL_SUCCESS;
 152         }
 153
 154         // release memory
 155         gsl_vector_free(x);
 156         gsl_multiroot_fsolver_free(s);
 157
 158         return res;
 159 }
 160
 161 /**
 162  * @brief This private method change the sector of angles.
 163  *
 164  * @param x The vector of changed angles.
 165  * @param x0 The vector of angles to change.
 166  * @param sector the sector vector operation.
 167  * @param n the size of all vectors.
 168  *
 169  * 0 -> no change
 170  * 1 -> pi - angle
 171  * 2 -> pi + angle
 172  * 3 -> -angle
 173  */
 174 static void change_sector(double x[], double const x0[],
 175                 int const sector[], size_t n)
 176 {
 177         size_t i;
 178
 179         for(i=0; i<n; ++i) {
 180                 switch (sector[i]) {
 181                         case 0:
 182                                 x[i] = x0[i];
 183                                 break;
 184                         case 1:
 185                                 x[i] = M_PI - x0[i];
 186                                 break;
 187                         case 2:
 188                                 x[i] = M_PI + x0[i];
 189                                 break;
 190                         case 3:
 191                                 x[i] = -x0[i];
 192                                 break;
 193                 }
 194         }
 195 }
 196
 197 /**
 198  * @brief Test if an angle combination is compatible with q function.
 199  *
 200  * @param x The vector of angles to test.
 201  * @param function The gsl_multiroot_function used for the test.
 202  * @param f a gsl_vector use to compute the result (optimization)
 203  */
 204 static int test_sector(gsl_vector const *x,
 205                 gsl_multiroot_function *function,
 206                 gsl_vector *f)
 207 {
 208         int ko;
 209         size_t i;
 210         double *f_data = gsl_vector_ptr(f, 0);
 211
 212         function->f(x, function->params, f);
 213         ko = 0;
 214         for(i=0; i<f->size; ++i)
 215                 if (fabs(f_data[i]) > HKL_EPSILON) {
 216                         ko = 1;
 217                         return HKL_FAIL;
 218                 }
 219
 220         return HKL_SUCCESS;
 221 }
 222
 223 /**
 224  * @brief compute the permutation and test its validity.
 225  *
 226  * @param axes_len number of axes
 227  * @param op_len number of operation per axes. (4 for now)
 228  * @param p The vector containing the current permutation.
 229  * @param axes_idx The index of the axes we are permution.
 230  * @param op the current operation to set.
 231  * @param f The function for the validity test.
 232  * @param x0 The starting point of all geometry permutations.
 233  * @param _x a gsl_vector use to compute the sectors (optimization)
 234  * @param _f a gsl_vector use during the sector test (optimization)
 235  */
 236 static void perm_r(size_t axes_len, int op_max, int p[], int axes_idx,
 237                 int op, gsl_multiroot_function *f, double x0[],
 238                 gsl_vector *_x, gsl_vector *_f)
 239 {
 240         int i;
 241
 242         p[axes_idx++] = op;
 243         if (axes_idx == axes_len) {
 244                 //fprintf(stdout, "%d %d %d %d\n", p[0], p[1], p[2], p[3]);
 245                 double *x_data = gsl_vector_ptr(_x, 0);
 246                 change_sector(x_data, x0, p, axes_len);
 247                 if (!test_sector(_x, f, _f))
 248                         add_geometry(f->params, x_data);
 249         } else
 250                 for (i=0; i<op_max; ++i)
 251                         perm_r(axes_len, op_max, p, axes_idx, i, f, x0, _x, _f);
 252 }
 253
 254 /**********/
 255 /* public */
 256 /**********/
 257
 258 /* HklPseudoAxisEngineGetSet part */
 259
 260 HklPseudoAxisEngineGetSet *hkl_pseudo_axis_engine_get_set_new(
 261                 char const *name,
 262                 HklPseudoAxisEngineGetterFunc get,
 263                 HklPseudoAxisEngineSetterFunc set,
 264                 size_t n, ...)
 265 {
 266         HklPseudoAxisEngineGetSet *self = NULL;
 267         va_list ap;
 268         size_t i;
 269         size_t len;
 270
 271         self = calloc(1, sizeof(*self));
 272         if (!self)
 273                 die("Can not allocate memory for an HklPseudoAxisEngineGetSet");
 274
 275         self->name = name;
 276         self->get = get;
 277         self->set = set;
 278
 279         va_start(ap, n);
 280         /* parameters */
 281         if (n) {
 282                 self->parameters = calloc(n, sizeof(HklParameter));
 283                 self->parameters_len = n;
 284                 for(i=0; i<n; ++i)
 285                         self->parameters[i] = *va_arg(ap, HklParameter*);
 286         }
 287
 288         /* axes */
 289         len = va_arg(ap, size_t);
 290         self->axes_names = calloc(len, sizeof(char const *));
 291         self->axes_names_len = len;
 292         for(i=0; i<len; ++i)
 293                 self->axes_names[i] = va_arg(ap, char const *);
 294         va_end(ap);
 295
 296         return self;
 297 }
 298
 299
 300 void hkl_pseudo_axis_engine_get_set_free(HklPseudoAxisEngineGetSet *self)
 301 {
 302         if(self->parameters_len) {
 303                 self->parameters_len = 0;
 304                 free(self->parameters);
 305                 self->parameters = NULL;
 306         }
 307
 308         if(self->axes_names_len) {
 309                 self->axes_names_len = 0;
 310                 free(self->axes_names);
 311                 self->axes_names = NULL;
 312         }
 313         free(self);
 314 }
 315
 316 /* HklPseudoAxisEngineFunc */
 317
 318 HklPseudoAxisEngineFunc *hkl_pseudo_axis_engine_func_new(
 319                 char const *name, size_t n, ...)
 320 {
 321         HklPseudoAxisEngineFunc *self = NULL;
 322         va_list ap;
 323         size_t i;
 324         size_t len;
 325
 326         self = calloc(1, sizeof(*self));
 327         if (!self)
 328                 die("Can not allocate memory for an HklPseudoAxisEngineFunc");
 329
 330         self->name = name;
 331
 332         /* functions */
 333         va_start(ap, n);
 334         if (n) {
 335                 self->f = calloc(n, sizeof(HklPseudoAxisEngineFunction));
 336                 self->f_len = n;
 337                 for(i=0; i<n; ++i)
 338                         self->f[i] = va_arg(ap, HklPseudoAxisEngineFunction);
 339         }
 340
 341         /* parameters */
 342         len = va_arg(ap, size_t);
 343         if (len) {
 344                 self->parameters = calloc(len, sizeof(HklParameter));
 345                 self->parameters_len = len;
 346                 for(i=0; i<len; ++i)
 347                         self->parameters[i] = *va_arg(ap, HklParameter*);
 348         }
 349
 350         /* axes */
 351         len = va_arg(ap, size_t);
 352         self->axes_names = calloc(len, sizeof(char const *));
 353         self->axes_names_len = len;
 354         for(i=0; i<len; ++i)
 355                 self->axes_names[i] = va_arg(ap, char const *);
 356         va_end(ap);
 357
 358         return self;
 359 }
 360
 361 void hkl_pseudo_axis_engine_func_free(HklPseudoAxisEngineFunc *self)
 362 {
 363         if(self->f_len) {
 364                 self->f_len = 0;
 365                 free(self->f);
 366                 self->f = NULL;
 367         }
 368
 369         if(self->parameters_len) {
 370                 self->parameters_len = 0;
 371                 free(self->parameters);
 372                 self->parameters = NULL;
 373         }
 374
 375         if(self->axes_names_len) {
 376                 self->axes_names_len = 0;
 377                 free(self->axes_names);
 378                 self->axes_names = NULL;
 379         }
 380         free(self);
 381 };
 382
 383 /* pseudoAxeEngine */
 384
 385 HklPseudoAxisEngine *hkl_pseudoAxisEngine_new(char const *name,
 386                 size_t n, ...)
 387 {
 388         va_list ap;
 389         size_t i;
 390
 391         HklPseudoAxisEngine *self = NULL;
 392
 393         self = calloc(1, sizeof(*self));
 394         if (!self)
 395                 die("Can not allocate memory for an HklPseudoAxisEngine");
 396
 397         self->name = name;
 398
 399         // create the pseudoAxes
 400         self->pseudoAxes = malloc(n * sizeof(HklPseudoAxis));
 401         self->pseudoAxes_len = n;
 402         va_start(ap, n);
 403         for(i=0; i<n; ++i) {
 404                 self->pseudoAxes[i].name = va_arg(ap, const char*);
 405                 self->pseudoAxes[i].engine = self;
 406         }
 407         va_end(ap);
 408
 409         return self;
 410 }
 411
 412 void hkl_pseudoAxisEngine_free(HklPseudoAxisEngine *self)
 413 {
 414         size_t i;
 415
 416         if (self->geometry)
 417                 hkl_geometry_free(self->geometry);
 418         /* release the axes memory */
 419         if (self->axes_len) {
 420                 free(self->axes);
 421                 self->axes = NULL;
 422                 self->axes_len = 0;
 423         }
 424         /* release the getset added */
 425         if (self->getsets_len) {
 426                 for(i=0; i<self->getsets_len; ++i)
 427                         hkl_pseudo_axis_engine_get_set_free(self->getsets[i]);
 428                 self->getsets_len = 0;
 429                 free(self->getsets);
 430                 self->getsets = NULL;
 431         }
 432         /* release the functions added */
 433         if (self->functions_len) {
 434                 for(i=0; i<self->functions_len; ++i)
 435                         hkl_pseudo_axis_engine_func_free(self->functions[i]);
 436                 self->functions_len = 0;
 437                 free(self->functions);
 438                 self->functions = NULL;
 439         }
 440         /* release the HklPseudoAxe memory */
 441         if (self->pseudoAxes_len) {
 442                 free(self->pseudoAxes);
 443                 self->pseudoAxes = NULL;
 444                 self->pseudoAxes_len = 0;
 445         }
 446         /* release the geometries allocated during calculations */
 447         if (self->geometries_alloc) {
 448                 for(i=0; i<self->geometries_alloc; ++i)
 449                         hkl_geometry_free(self->geometries[i]);
 450                 free(self->geometries);
 451                 self->geometries = NULL;
 452                 self->geometries_alloc = self->geometries_len = 0;
 453         }
 454         free(self);
 455 }
 456
 457 void hkl_pseudoAxisEngine_add_get_set(HklPseudoAxisEngine *self,
 458                 HklPseudoAxisEngineGetSet *getset)
 459 {
 460         size_t n = self->getsets_len++;
 461         self->getsets = realloc(self->getsets,
 462                         self->getsets_len*sizeof(HklPseudoAxisEngineGetSet*));
 463         self->getsets[n] = getset;
 464 }
 465
 466 void hkl_pseudoAxisEngine_add_function(HklPseudoAxisEngine *self,
 467                 HklPseudoAxisEngineFunc *func)
 468 {
 469         size_t n = self->functions_len++;
 470         self->functions = realloc(self->functions,
 471                         self->functions_len*sizeof(HklPseudoAxisEngineFunc *));
 472         self->functions[n] = func;
 473 }
 474
 475 void hkl_pseudoAxisEngine_select_get_set(HklPseudoAxisEngine *self,
 476                 size_t idx)
 477 {
 478         self->getset = self->getsets[idx];
 479 }
 480
 481 void hkl_pseudoAxisEngine_select_function(HklPseudoAxisEngine *self,
 482                 size_t idx)
 483 {
 484         self->function = self->functions[idx];
 485 }
 486
 487 void hkl_pseudoAxisEngine_setter(HklPseudoAxisEngine *self,
 488                 HklGeometry *geom, HklDetector *det, HklSample *sample)
 489 {
 490         self->getset->set(self, geom, det, sample);
 491 }
 492
 493 void hkl_pseudoAxisEngine_getter(HklPseudoAxisEngine *self,
 494                 HklGeometry *geom, HklDetector *det, HklSample *sample)
 495 {
 496         self->getset->get(self, geom, det, sample);
 497 }
 498
 499 void hkl_pseudoAxisEngine_set(HklPseudoAxisEngine *self, size_t idx_f,
 500                 HklGeometry *geometry, HklDetector *detector,
 501                 HklSample *sample)
 502
 503 {
 504         size_t i;
 505
 506         if (self->geometry)
 507                 hkl_geometry_free(self->geometry);
 508         self->geometry = hkl_geometry_new_copy(geometry);
 509         if(!self->geometry)
 510                 die("Can not allocate memory for an HklGeometry");
 511         hkl_geometry_update(self->geometry);
 512
 513         self->detector = detector;
 514         self->sample = sample;
 515
 516         self->function = self->functions[idx_f];
 517
 518         // fill the axes member from the function
 519         if (self->axes_len)
 520                 free(self->axes), self->axes = NULL, self->axes_len = 0;
 521         self->axes_len = self->function->axes_names_len;
 522         self->axes = malloc(self->axes_len * sizeof(HklAxis *));
 523         for(i=0; i<self->axes_len; ++i)
 524                 self->axes[i] = hkl_geometry_get_axis_by_name(self->geometry,
 525                                 self->function->axes_names[i]);
 526 }
 527
 528 int hkl_pseudoAxisEngine_to_pseudoAxes(HklPseudoAxisEngine *self)
 529 {
 530         HklHolder *holder;
 531         HklMatrix RUB;
 532         HklVector hkl, ki, Q;
 533
 534         // update the geometry internals
 535         hkl_geometry_update(self->geometry);
 536
 537         // R * UB
 538         // for now the 0 holder is the sample holder.
 539         holder = &self->geometry->holders[0];
 540         hkl_quaternion_to_smatrix(&holder->q, &RUB);
 541         hkl_matrix_times_smatrix(&RUB, &self->sample->UB);
 542
 543         // kf - ki = Q
 544         hkl_source_compute_ki(&self->geometry->source, &ki);
 545         hkl_detector_compute_kf(self->detector, self->geometry, &Q);
 546         hkl_vector_minus_vector(&Q, &ki);
 547
 548         hkl_matrix_solve(&RUB, &hkl, &Q);
 549
 550         // update the pseudoAxes current and consign parts
 551         self->pseudoAxes[0].config.value = hkl.data[0];
 552         self->pseudoAxes[1].config.value = hkl.data[1];
 553         self->pseudoAxes[2].config.value = hkl.data[2];
 554
 555         return HKL_SUCCESS;
 556 }
 557
 558 int hkl_pseudoAxisEngine_to_geometry(HklPseudoAxisEngine *self)
 559 {
 560         size_t idx, i;
 561         size_t n = self->axes_len;
 562         int p[n];
 563         double x0[n];
 564         int res = 0;
 565         gsl_vector *_x; /* use to compute sectors in perm_r (avoid copy) */
 566         gsl_vector *_f; /* use to test sectors in perm_r (avoid copy) */
 567
 568         self->geometries_len = 0;
 569         _x = gsl_vector_alloc(n);
 570         _f = gsl_vector_alloc(n);
 571         for(idx=0; idx<self->function->f_len; ++idx) {
 572                 gsl_multiroot_function f = {self->function->f[idx], self->axes_len, self};
 573                 int tmp = !find_first_geometry(self, &f);
 574                 res |= tmp;
 575                 if (tmp) {
 576                         memset(p, 0, sizeof(p));
 577                         /* keep the seed solution */
 578                         for(i=0; i<n; ++i)
 579                                 x0[i] = self->axes[i]->config.value;
 580
 581                         for (i=0; i<n; ++i)
 582                                 perm_r(n, 4, p, 0, i, &f, x0, _x, _f);
 583                 }
 584         }
 585         gsl_vector_free(_f);
 586         gsl_vector_free(_x);
 587         return res;
 588 }
 589
 590 void hkl_pseudoAxisEngine_fprintf(HklPseudoAxisEngine *self, FILE *f)
 591 {
 592         size_t i, j;
 593         double value;
 594
 595         fprintf(f, "\nPseudoAxesEngine : \"%s\" %s",
 596                         self->name, self->function->name);
 597
 598         /* function */
 599         for(i=0; i<self->function->parameters_len; ++i)
 600                 fprintf(f, " \"%s\" = %g",
 601                                 self->function->parameters[0].name,
 602                                 self->function->parameters[0].value);
 603
 604         /* the pseudoAxes part */
 605         fprintf(f, "\n   ");
 606         for(i=0; i<self->pseudoAxes_len; ++i)
 607                 fprintf(f, " \"%s\" : %f", self->pseudoAxes[i].name, self->pseudoAxes[i].config.value);
 608
 609         /* axes names */
 610         fprintf(f, "\n   ");
 611         for(i=0; i<self->geometry->axes_len; ++i)
 612                 fprintf(f, "%9s", self->geometry->axes[i]->name);
 613
 614         /* geometries */
 615         fprintf(f, "\n");
 616         for(i=0; i<self->geometries_len; ++i) {
 617                 fprintf(f, "%d :", i);
 618                 for(j=0; j<self->geometry->axes_len; ++j) {
 619                         value = gsl_sf_angle_restrict_symm(self->geometries[i]->axes[j]->config.value);
 620                         fprintf(f, " % 9.6g", value * HKL_RADTODEG);
 621                 }
 622                 fprintf(f, "\n");
 623         }
 624 }