src/hkl-pseudoaxis.c

   1 #include <string.h>
   2 #include <gsl/gsl_sf_trig.h>
   3 #include <hkl/hkl-pseudoaxis.h>
   4
   5 /* private */
   6
   7 /**
   8  * @brief this method compute the jacobian of a function at a given point.
   9  *
  10  * @param func the gsl_multiroopt_function to test
  11  * @param x the starting point
  12  * @param f the result of the function evaluation.
  13  *
  14  * with this method we can see if an axis is degenerated or not.
  15  * A degenerated axis is an axis with no effect on the function evaluation.
  16  * In the Jacobian matrix all elements of a columnn is null.
  17  * Once we know this the axis is mark as degenerated and we do not need to
  18  * change is sector.
  19  */
  20 static void find_degenerated(HklPseudoAxisEngine *self,
  21                 gsl_multiroot_function *func,
  22                 gsl_vector const *x, gsl_vector const *f)
  23 {
  24         gsl_matrix *J;
  25         size_t i, j;
  26         int degenerated[x->size];
  27
  28         memset(degenerated, 0, sizeof(degenerated));
  29         J = gsl_matrix_alloc(x->size, f->size);
  30
  31         gsl_multiroot_fdjacobian(func, x, f, GSL_SQRT_DBL_EPSILON, J);
  32         for(j=0; j<x->size && !degenerated[j]; ++j) {
  33                 for(i=0; i<f->size; ++i)
  34                         if (fabs(gsl_matrix_get(J, i, j)) > HKL_EPSILON)
  35                                 break;
  36                 if (i == f->size)
  37                         degenerated[j] = 1;
  38         }
  39
  40         /*
  41         hkl_pseudoAxisEngine_fprintf(func->params, stdout);
  42         fprintf(stdout, "\n");
  43         for(i=0; i<x->size; ++i)
  44                 fprintf(stdout, " %d", degenerated[i]);
  45         for(i=0;i<x->size;++i) {
  46                 fprintf(stdout, "\n   ");
  47                 for(j=0;j<f->size;++j)
  48                         fprintf(stdout, " %f", gsl_matrix_get(J, i, j));
  49         }
  50         fprintf(stdout, "\n");
  51         */
  52         gsl_matrix_free(J);
  53 }
  54
  55 /**
  56  * @brief Add a geometry to the PseudoAxeEngine->geometries
  57  *
  58  * @param self The current PseudoAxeEngine
  59  * @param x A vector of double with the axes values to put in the geometry.
  60  *
  61  * This method try to be clever by allocating memory only if the current
  62  * length of the geometries is not large enought. Then it just set the
  63  * geometry axes and copy it to the right geometries.
  64  */
  65 static void add_geometry(HklPseudoAxisEngine *self, double const *x)
  66 {
  67         size_t i;
  68         HklGeometry *geometry;
  69
  70         // first check if we can get an old geometry.
  71         if (self->geometries_len == self->geometries_alloc) {
  72                 self->geometries_alloc = alloc_nr(self->geometries_alloc);
  73                 self->geometries = realloc(self->geometries,
  74                                 self->geometries_alloc * sizeof(HklGeometry*));
  75                 for(i=self->geometries_len; i<self->geometries_alloc; i++)
  76                         self->geometries[i] = hkl_geometry_new_copy(self->geometry);
  77         }
  78
  79         /* copy the axes configuration into the engine->geometry*/
  80         for(i=0; i<self->axes_len; ++i)
  81                 self->axes[i]->config.value = x[i];
  82
  83         /* put the axes configuration from engine->geometry -> geometry */
  84         geometry = self->geometries[self->geometries_len++];
  85         hkl_geometry_init_geometry(geometry, self->geometry);
  86 }
  87
  88 static int find_first_geometry(HklPseudoAxisEngine *self,
  89                 gsl_multiroot_function *f)
  90 {
  91         gsl_multiroot_fsolver_type const *T;
  92         gsl_multiroot_fsolver *s;
  93         gsl_vector *x;
  94         double *x_data;
  95         size_t iter = 0;
  96         int status;
  97         int res = HKL_FAIL;
  98         size_t i;
  99
 100         // get the starting point from the geometry
 101         // must be put in the auto_set method
 102         x = gsl_vector_alloc(self->axes_len);
 103         x_data = gsl_vector_ptr(x, 0);
 104         for(i=0; i<self->axes_len; ++i)
 105                 x_data[i] = self->axes[i]->config.value;
 106
 107         // Initialize method
 108         T = gsl_multiroot_fsolver_hybrid;
 109         s = gsl_multiroot_fsolver_alloc (T, self->axes_len);
 110         gsl_multiroot_fsolver_set (s, f, x);
 111
 112         // iterate to find the solution
 113         do {
 114                 ++iter;
 115                 status = gsl_multiroot_fsolver_iterate(s);
 116                 if (status || iter % 1000 == 0) {
 117                         // Restart from another point.
 118                         for(i=0; i<self->axes_len; ++i)
 119                                 x_data[i] = (double)rand() / RAND_MAX * 180. / M_PI;
 120                         gsl_multiroot_fsolver_set(s, f, x);
 121                         status = gsl_multiroot_fsolver_iterate(s);
 122                 }
 123                 status = gsl_multiroot_test_residual (s->f, HKL_EPSILON);
 124         } while (status == GSL_CONTINUE && iter < 1000);
 125
 126         if (status != GSL_CONTINUE) {
 127                 find_degenerated(self, f, s->x, s->f);
 128                 // set the geometry from the gsl_vector
 129                 // in a futur version the geometry must contain a gsl_vector
 130                 // to avoid this.
 131                 for(i=0; i<self->axes_len; ++i) {
 132                         HklAxis *axis = self->axes[i];
 133                         x_data = gsl_vector_ptr(s->x, 0);
 134                         axis->config.value = gsl_sf_angle_restrict_pos(x_data[i]);
 135                         axis->config.dirty = 1;
 136                 }
 137                 hkl_geometry_update(self->geometry);
 138                 res = HKL_SUCCESS;
 139         }
 140
 141         // release memory
 142         gsl_vector_free(x);
 143         gsl_multiroot_fsolver_free(s);
 144
 145         return res;
 146 }
 147
 148 /**
 149  * @brief given a vector of angles change the sector of thoses angles
 150  *
 151  * @param x The vector of angles to change.
 152  * @param sector the sector vector operation.
 153  *
 154  * 0 -> no change
 155  * 1 -> pi - angle
 156  * 2 -> pi + angle
 157  * 3 -> -angle
 158  */
 159 static void change_sector(double *x, double const *x0,
 160                 int const *sector, size_t n)
 161 {
 162         size_t i;
 163
 164         for(i=0; i<n; ++i) {
 165                 switch (sector[i]) {
 166                         case 0:
 167                                 x[i] = x0[i];
 168                                 break;
 169                         case 1:
 170                                 x[i] = M_PI - x0[i];
 171                                 break;
 172                         case 2:
 173                                 x[i] = M_PI + x0[i];
 174                                 break;
 175                         case 3:
 176                                 x[i] = -x0[i];
 177                                 break;
 178                 }
 179         }
 180 }
 181
 182 /**
 183  * @brief Test if an angle combination is compatible with a pseudoAxisEngine
 184  *
 185  * @param x The vector of angles to test.
 186  * @param function The gsl_multiroot_function used for the test.
 187  * @param f a gsl_vector use to compute the result (optimization)
 188  */
 189 static int test_sector(gsl_vector const *x,
 190                 gsl_multiroot_function *function,
 191                 gsl_vector *f)
 192 {
 193         int ko;
 194         size_t i;
 195         double *f_data = gsl_vector_ptr(f, 0);
 196
 197         function->f(x, function->params, f);
 198         ko = 0;
 199         for(i=0; i<f->size; ++i)
 200                 if (fabs(f_data[i]) > HKL_EPSILON) {
 201                         ko = 1;
 202                         return HKL_FAIL;
 203                 }
 204
 205         return HKL_SUCCESS;
 206 }
 207
 208 /*
 209  * @brief compute the permutation and test its validity.
 210  *
 211  * @param axes_len number of axes
 212  * @param op_len number of operation per axes. (4 for now)
 213  * @param p The vector containing the current permutation.
 214  * @param axes_idx The index of the axes we are permution.
 215  * @param op the current operation to set.
 216  * @param f The function for the validity test.
 217  * @param x0 The starting point of all geometry permutations.
 218  * @param _x a gsl_vector use to compute the sectors (optimization)
 219  * @param _f a gsl_vector use during the sector test (optimization)
 220  */
 221 static void perm_r(size_t axes_len, int op_max, int *p, int axes_idx,
 222                 int op, gsl_multiroot_function *f, double *x0,
 223                 gsl_vector *_x, gsl_vector *_f)
 224 {
 225         int i;
 226
 227         p[axes_idx++] = op;
 228         if (axes_idx == axes_len) {
 229                 //fprintf(stdout, "%d %d %d %d\n", p[0], p[1], p[2], p[3]);
 230                 double *x_data = gsl_vector_ptr(_x, 0);
 231                 change_sector(x_data, x0, p, axes_len);
 232                 if (!test_sector(_x, f, _f))
 233                         add_geometry(f->params, x_data);
 234         } else
 235                 for (i=0; i<op_max; ++i)
 236                         perm_r(axes_len, op_max, p, axes_idx, i, f, x0, _x, _f);
 237 }
 238
 239 /* public */
 240
 241 HklPseudoAxisEngine *hkl_pseudoAxisEngine_new(HklPseudoAxisEngineConfig *config)
 242 {
 243         size_t i;
 244         HklPseudoAxisEngine *self = NULL;
 245
 246         self = malloc(sizeof(*self));
 247         if (!self)
 248                 die("Can not allocate memory for an HklPseudoAxisEngine");
 249
 250         self->config = *config;
 251         self->geometry = NULL;
 252         self->detector = NULL;
 253         self->sample = NULL;
 254         self->axes = NULL;
 255         self->axes_len = 0;
 256
 257         // create the pseudoAxes from the config
 258         self->pseudoAxes_len = config->pseudo_names_len;
 259         self->pseudoAxes = malloc(self->pseudoAxes_len * sizeof(HklPseudoAxis));
 260         for(i=0; i<self->pseudoAxes_len; ++i) {
 261                 self->pseudoAxes[i].name = config->pseudo_names[i];
 262                 self->pseudoAxes[i].engine = self;
 263         }
 264
 265         self->geometries = NULL;
 266         self->geometries_len = 0;
 267         self->geometries_alloc = 0;
 268
 269         return self;
 270 }
 271
 272 void hkl_pseudoAxisEngine_set(HklPseudoAxisEngine *self, size_t idx_f,
 273                 HklGeometry *geometry, HklDetector *detector,
 274                 HklSample *sample)
 275
 276 {
 277         size_t i;
 278
 279         if (self->geometry)
 280                 hkl_geometry_free(self->geometry);
 281         self->geometry = hkl_geometry_new_copy(geometry);
 282         if(!self->geometry)
 283                 die("Can not allocate memory for an HklGeometry");
 284         hkl_geometry_update(self->geometry);
 285
 286         self->detector = detector;
 287         self->sample = sample;
 288
 289         self->function = &self->config.functions[idx_f];
 290
 291         // fill the axes member from the config
 292         if (self->axes_len)
 293                 free(self->axes), self->axes = NULL, self->axes_len = 0;
 294         self->axes_len = self->config.axes_names_len;
 295         self->axes = malloc(self->axes_len * sizeof(HklAxis *));
 296         for(i=0; i<self->axes_len; ++i)
 297                 self->axes[i] = hkl_geometry_get_axis_by_name(self->geometry,
 298                                 self->config.axes_names[i]);
 299 }
 300
 301 void hkl_pseudoAxisEngine_free(HklPseudoAxisEngine *self)
 302 {
 303         size_t i;
 304
 305         if (self->geometry)
 306                 hkl_geometry_free(self->geometry);
 307         if(self->axes_len)
 308                 free(self->axes), self->axes = NULL, self->axes_len = 0;
 309         if (self->pseudoAxes_len)
 310                 free(self->pseudoAxes), self->pseudoAxes = NULL, self->pseudoAxes_len = 0;
 311         if (self->geometries_alloc) {
 312                 for(i=0; i<self->geometries_alloc; ++i)
 313                         hkl_geometry_free(self->geometries[i]);
 314                 free(self->geometries);
 315                 self->geometries = NULL;
 316                 self->geometries_alloc = self->geometries_len = 0;
 317         }
 318         free(self);
 319 }
 320
 321 int RUBh_minus_Q(double const *x, void *params, double *f)
 322 {
 323         HklVector Hkl;
 324         HklVector ki, dQ;
 325         HklPseudoAxisEngine *engine;
 326         HklPseudoAxis *H, *K, *L;
 327         HklHolder *holder;
 328         size_t i;
 329
 330         engine = params;
 331         H = &engine->pseudoAxes[0];
 332         K = &engine->pseudoAxes[1];
 333         L = &engine->pseudoAxes[2];
 334
 335         // update the workspace from x;
 336         for(i=0; i<engine->axes_len; ++i) {
 337                 HklAxis *axis = engine->axes[i];
 338                 axis->config.value = x[i];
 339                 axis->config.dirty = 1;
 340         }
 341         hkl_geometry_update(engine->geometry);
 342
 343         hkl_vector_init(&Hkl, H->config.value, K->config.value,
 344                         L->config.value);
 345
 346         // R * UB * h = Q
 347         // for now the 0 holder is the sample holder.
 348         holder = &engine->geometry->holders[0];
 349         hkl_matrix_times_vector(&engine->sample->UB, &Hkl);
 350         hkl_vector_rotated_quaternion(&Hkl, &holder->q);
 351
 352         // kf - ki = Q
 353         hkl_source_compute_ki(&engine->geometry->source, &ki);
 354         hkl_detector_compute_kf(engine->detector, engine->geometry, &dQ);
 355         hkl_vector_minus_vector(&dQ, &ki);
 356
 357         hkl_vector_minus_vector(&dQ, &Hkl);
 358
 359         f[0] = dQ.data[0];
 360         f[1] = dQ.data[1];
 361         f[2] = dQ.data[2];
 362
 363         return GSL_SUCCESS;
 364 }
 365
 366 int hkl_pseudoAxisEngine_to_pseudoAxes(HklPseudoAxisEngine *self)
 367 {
 368         HklHolder *holder;
 369         HklMatrix RUB;
 370         HklVector hkl, ki, Q;
 371
 372         // update the geometry internals
 373         hkl_geometry_update(self->geometry);
 374
 375         // R * UB
 376         // for now the 0 holder is the sample holder.
 377         holder = &self->geometry->holders[0];
 378         hkl_quaternion_to_smatrix(&holder->q, &RUB);
 379         hkl_matrix_times_smatrix(&RUB, &self->sample->UB);
 380
 381         // kf - ki = Q
 382         hkl_source_compute_ki(&self->geometry->source, &ki);
 383         hkl_detector_compute_kf(self->detector, self->geometry, &Q);
 384         hkl_vector_minus_vector(&Q, &ki);
 385
 386         hkl_matrix_solve(&RUB, &hkl, &Q);
 387
 388         // update the pseudoAxes current and consign parts
 389         self->pseudoAxes[0].config.value = hkl.data[0];
 390         self->pseudoAxes[1].config.value = hkl.data[1];
 391         self->pseudoAxes[2].config.value = hkl.data[2];
 392
 393         return HKL_SUCCESS;
 394 }
 395
 396 int hkl_pseudoAxisEngine_to_geometry(HklPseudoAxisEngine *self)
 397 {
 398         size_t idx, i;
 399         size_t n = self->axes_len;
 400         int p[n];
 401         double x0[n];
 402         int res = 0;
 403         gsl_vector *_x; /* use to compute sectors in perm_r (avoid copy) */
 404         gsl_vector *_f; /* use to test sectors in perm_r (avoid copy) */
 405
 406         self->geometries_len = 0;
 407         _x = gsl_vector_alloc(n);
 408         _f = gsl_vector_alloc(n);
 409         for(idx=0; idx<self->function->f_len; ++idx) {
 410                 gsl_multiroot_function f = {self->function->f[idx], self->axes_len, self};
 411                 int tmp = !find_first_geometry(self, &f);
 412                 res |= tmp;
 413                 if (tmp) {
 414                         memset(p, 0, sizeof(p));
 415                         /* keep the seed solution */
 416                         for(i=0; i<n; ++i)
 417                                 x0[i] = self->axes[i]->config.value;
 418
 419                         for (i=0; i<n; ++i)
 420                                 perm_r(n, 4, p, 0, i, &f, x0, _x, _f);
 421                 }
 422         }
 423         gsl_vector_free(_f);
 424         gsl_vector_free(_x);
 425         return res;
 426 }
 427
 428 void hkl_pseudoAxisEngine_fprintf(HklPseudoAxisEngine *self, FILE *f)
 429 {
 430         size_t i, j;
 431         double value;
 432
 433         fprintf(f, "\nPseudoAxesEngine : %s\n", self->config.name);
 434         /* the pseudoAxes part */
 435         for(i=0; i<self->pseudoAxes_len; ++i)
 436                 fprintf(f, " \"%s\" : %f", self->pseudoAxes[i].name, self->pseudoAxes[i].config.value);
 437         fprintf(f, "\n");
 438         /* the geometry and geometries parts */
 439         for(i=0; i<self->geometry->axes_len; ++i)
 440                 fprintf(f, "\t\"%s\"", self->geometry->axes[i]->name);
 441         fprintf(f, "\n");
 442         for(i=0; i<self->geometry->axes_len; ++i) {
 443                 value = gsl_sf_angle_restrict_symm(self->geometry->axes[i]->config.value);
 444                 fprintf(f, " %f", value * HKL_RADTODEG);
 445         }
 446         fprintf(f, "\n");
 447         for(i=0; i<self->geometries_len; ++i) {
 448                 fprintf(f, "%d :", i);
 449                 for(j=0; j<self->geometry->axes_len; ++j) {
 450                         value = gsl_sf_angle_restrict_symm(self->geometries[i]->axes[j]->config.value);
 451                         fprintf(f, " %f", value * HKL_RADTODEG);
 452                 }
 453                 fprintf(f, "\n");
 454         }
 455 }