hkl/hkl-geometry.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-2010 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 <math.h>
  23 #include <string.h>
  24 #include <stdarg.h>
  25 #ifndef _MSC_VER
  26 # include <alloca.h>
  27 #endif
  28
  29 #include <gsl/gsl_math.h>
  30 #include <gsl/gsl_sf_trig.h>
  31
  32 #include <hkl/hkl-geometry.h>
  33
  34 /*
  35  * Try to add a axis to the axes list,
  36  * if a identical axis is present in the list return it
  37  * else create a new on and add it to the list.
  38  * die if try to add an axis with the same name but a different axis_v
  39  */
  40 static size_t hkl_geometry_add_rotation(HklGeometry *geometry,
  41                                         char const *name, HklVector const *axis_v)
  42 {
  43         size_t i, len;
  44         HklAxis axis;
  45
  46         /* check if an axis with the same name is on the axis list */
  47         for(i=0; i<HKL_LIST_LEN(geometry->axes); ++i){
  48                 HklAxis *axis;
  49
  50                 axis = &geometry->axes[i];
  51                 if(!strcmp(hkl_axis_get_name(axis), name)){
  52                         if (hkl_vector_cmp(&axis->axis_v, axis_v)){
  53                                 fprintf(stderr, "can not add two axis with the same name \"%s\" but different axes <%f, %f, %f> != <%f, %f, %f> into an HklAxes.",
  54                                         name,
  55                                         axis->axis_v.data[0], axis->axis_v.data[1], axis->axis_v.data[2],
  56                                         axis_v->data[0], axis_v->data[1], axis_v->data[2]);
  57                                 exit(128);
  58                         }else{
  59                                 return i;
  60                         }
  61                 }
  62         }
  63
  64         /* no so create and add it to the list */
  65         len = HKL_LIST_LEN(geometry->axes);
  66         hkl_axis_init(&axis, name, axis_v);
  67         HKL_LIST_ADD_VALUE(geometry->axes, axis);
  68
  69         return len;
  70 }
  71
  72 /*************/
  73 /* HklHolder */
  74 /*************/
  75
  76 static void hkl_holder_init(HklHolder *self, HklGeometry *geometry)
  77 {
  78         static HklQuaternion q0 = {{0, 0, 0, 0}};
  79         self->geometry = geometry;
  80         HKL_LIST_INIT(self->idx);
  81         self->q = q0;
  82 }
  83
  84 static int hkl_holder_init_copy(HklHolder *self, HklGeometry *geometry,
  85                                 HklHolder const *holder)
  86 {
  87         /* check axes compatibility */
  88         if (HKL_LIST_LEN(geometry->axes) != HKL_LIST_LEN(holder->geometry->axes))
  89                 return HKL_FAIL;
  90
  91         self->geometry = geometry;
  92
  93         HKL_LIST_ALLOC(self->idx, HKL_LIST_LEN(holder->idx));
  94         HKL_LIST_COPY(self->idx, holder->idx);
  95
  96         self->q = holder->q;
  97
  98         return HKL_SUCCESS;
  99 }
 100
 101 static void hkl_holder_release_memory(HklHolder *self)
 102 {
 103         HKL_LIST_FREE(self->idx);
 104 }
 105
 106 static void hkl_holder_update(HklHolder *self)
 107 {
 108         static HklQuaternion q0 = {{1, 0, 0, 0}};
 109         size_t i;
 110         HklAxis *axes;
 111         size_t *idx;
 112
 113         self->q = q0;
 114         axes = self->geometry->axes;
 115         idx = self->idx;
 116         for(i=0; i<HKL_LIST_LEN(self->idx); ++i)
 117                 hkl_quaternion_times_quaternion(&self->q, &axes[idx[i]].q);
 118 }
 119
 120 HklAxis *hkl_holder_add_rotation_axis(HklHolder * self,
 121                                       char const * name, double x, double y, double z)
 122 {
 123         HklAxis *axis = NULL;
 124         size_t i, idx;
 125         HklVector axis_v;
 126
 127         axis_v.data[0] = x;
 128         axis_v.data[1] = y;
 129         axis_v.data[2] = z;
 130
 131         idx = hkl_geometry_add_rotation(self->geometry, name, &axis_v);
 132
 133         /* check that the axis is not already in the holder */
 134         for(i=0; i<HKL_LIST_LEN(self->idx); i++)
 135                 if (idx == self->idx[i])
 136                         return NULL;
 137
 138         axis = &self->geometry->axes[idx];
 139         HKL_LIST_ADD_VALUE(self->idx, idx);
 140
 141         return axis;
 142 }
 143
 144
 145 /***************/
 146 /* HklGeometry */
 147 /***************/
 148
 149 HklGeometry *hkl_geometry_new(void)
 150 {
 151         HklGeometry *g = NULL;
 152
 153         g = HKL_MALLOC(HklGeometry);
 154
 155         g->config = NULL;
 156         hkl_source_init(&g->source, 1.54, 1, 0, 0);
 157         HKL_LIST_INIT(g->axes);
 158         HKL_LIST_INIT(g->holders);
 159
 160         return g;
 161 }
 162
 163 HklGeometry *hkl_geometry_new_copy(HklGeometry const *src)
 164 {
 165         HklGeometry *self = NULL;
 166         unsigned int i;
 167         size_t len;
 168
 169         self = HKL_MALLOC(HklGeometry);
 170
 171         self->config = src->config;
 172         self->source = src->source;
 173
 174         /* copy the axes */
 175         HKL_LIST_ALLOC(self->axes, HKL_LIST_LEN(src->axes));
 176         HKL_LIST_COPY(self->axes, src->axes);
 177
 178         /* copy the holders */
 179         len = HKL_LIST_LEN(src->holders);
 180         HKL_LIST_ALLOC(self->holders, len);
 181         for(i=0; i<len; ++i)
 182                 hkl_holder_init_copy(&self->holders[i], self,
 183                                      &src->holders[i]);
 184
 185         return self;
 186 }
 187
 188 void hkl_geometry_free(HklGeometry *self)
 189 {
 190         size_t i;
 191         size_t len;
 192
 193         HKL_LIST_FREE(self->axes);
 194
 195         len = HKL_LIST_LEN(self->holders);
 196         if(len) {
 197                 for(i=0; i<len; ++i)
 198                         hkl_holder_release_memory(&self->holders[i]);
 199                 HKL_LIST_FREE(self->holders);
 200         }
 201
 202         free(self);
 203 }
 204
 205 void hkl_geometry_init_geometry(HklGeometry *self, HklGeometry const *src)
 206 {
 207         size_t i;
 208
 209         if(!self || !src
 210            || self->config->type != src->config->type)
 211                 return;
 212
 213         self->source = src->source;
 214
 215         /* copy the axes configuration and mark it as dirty */
 216         HKL_LIST_COPY(self->axes, src->axes);
 217         for(i=0; i<HKL_LIST_LEN(src->holders); ++i)
 218                 self->holders[i].q = src->holders[i].q;
 219 }
 220
 221 HklHolder *hkl_geometry_add_holder(HklGeometry *self)
 222 {
 223         HklHolder *holder;
 224         size_t len;
 225
 226         len = HKL_LIST_LEN(self->holders);
 227         HKL_LIST_RESIZE(self->holders, len + 1);
 228         holder = &self->holders[len];
 229         hkl_holder_init(holder, self);
 230
 231         return holder;
 232 }
 233
 234 void hkl_geometry_update(HklGeometry *self)
 235 {
 236         size_t i, len;
 237         int ko = 0;
 238
 239         len = HKL_LIST_LEN(self->axes);
 240         for(i=0; i<len; ++i)
 241                 if (hkl_axis_get_changed(&self->axes[i]) == HKL_TRUE) {
 242                         ko = 1;
 243                         break;
 244                 }
 245
 246         if (ko) {
 247                 for(i=0; i<HKL_LIST_LEN(self->holders); i++)
 248                         hkl_holder_update(&self->holders[i]);
 249
 250                 for(i=0; i<len; i++)
 251                         hkl_axis_set_changed(&self->axes[i], HKL_FALSE);
 252         }
 253 }
 254
 255 /**
 256  * hkl_geometry_get_axis_idx_by_name:
 257  * @self:
 258  * @name:
 259  *
 260  * get the index of the axes named @name in the geometry
 261  *
 262  * Returns: -1 if the axis was not found
 263  **/
 264 int hkl_geometry_get_axis_idx_by_name(HklGeometry *self, const char *name)
 265 {
 266         size_t i;
 267         HklAxis *axis;
 268
 269         if (!self || !name)
 270                 return -1;
 271
 272         for(i=0; i<HKL_LIST_LEN(self->axes); ++i){
 273                 axis = &self->axes[i];
 274                 if (!strcmp(hkl_axis_get_name(axis), name))
 275                         return i;
 276         }
 277
 278         return -1;
 279 }
 280
 281 HklAxis *hkl_geometry_get_axis_by_name(HklGeometry *self, char const *name)
 282 {
 283         size_t i;
 284         HklAxis *axis;
 285         for(i=0; i<HKL_LIST_LEN(self->axes); ++i) {
 286                 axis = &self->axes[i];
 287                 if (!strcmp(hkl_axis_get_name(axis), name))
 288                         return axis;
 289         }
 290         return NULL;
 291 }
 292
 293 void hkl_geometry_randomize(HklGeometry *self)
 294 {
 295         size_t i;
 296
 297         for(i=0; i<HKL_LIST_LEN(self->axes); ++i)
 298                 hkl_axis_randomize(&self->axes[i]);
 299         hkl_geometry_update(self);
 300 }
 301
 302 int hkl_geometry_set_values_v(HklGeometry *self, size_t len, ...)
 303 {
 304         va_list ap;
 305         size_t i;
 306
 307         if (!self || len != HKL_LIST_LEN(self->axes))
 308                 return HKL_FAIL;
 309
 310         va_start(ap, len);
 311         for(i=0; i<len; ++i)
 312                 hkl_axis_set_value(&self->axes[i], va_arg(ap, double));
 313
 314         va_end(ap);
 315         hkl_geometry_update(self);
 316
 317         return HKL_SUCCESS;
 318 }
 319
 320 double hkl_geometry_distance(HklGeometry *self, HklGeometry *geom)
 321 {
 322         size_t i;
 323         double value1, value2;
 324         double distance = 0.;
 325
 326         if (!self || !geom)
 327                 return 0.;
 328
 329         for(i=0; i<HKL_LIST_LEN(self->axes); ++i){
 330                 value1 = hkl_axis_get_value(&self->axes[i]);
 331                 value2 = hkl_axis_get_value(&geom->axes[i]);
 332                 distance += fabs(value2 - value1);
 333         }
 334
 335         return distance;
 336 }
 337
 338 double hkl_geometry_distance_orthodromic(HklGeometry *self, HklGeometry *geom)
 339 {
 340         size_t i;
 341         double value1, value2;
 342         double distance = 0.;
 343
 344         if (!self || !geom)
 345                 return 0.;
 346
 347         for(i=0; i<HKL_LIST_LEN(self->axes); ++i){
 348                 double d;
 349
 350                 value1 = hkl_axis_get_value(&self->axes[i]);
 351                 value2 = hkl_axis_get_value(&geom->axes[i]);
 352                 d = fabs(gsl_sf_angle_restrict_symm(value2) - gsl_sf_angle_restrict_symm(value1));
 353                 /* as M_PI and -M_PI are included in the GSL restriction */
 354                 if (d > M_PI)
 355                         d = 2*M_PI - d;
 356                 distance += d;
 357         }
 358
 359         return distance;
 360 }
 361
 362 int hkl_geometry_is_valid(const HklGeometry *self)
 363 {
 364         size_t i;
 365         size_t len;
 366
 367         len = HKL_LIST_LEN(self->axes);
 368         for(i=0; i<len; ++i)
 369                 if(hkl_axis_is_valid(&self->axes[i]) == HKL_FALSE)
 370                         return HKL_FALSE;
 371
 372         return HKL_TRUE;
 373 }
 374
 375 int hkl_geometry_closest_from_geometry_with_range(HklGeometry *self, HklGeometry *ref)
 376 {
 377         size_t i;
 378         size_t len = HKL_LIST_LEN(self->axes);
 379         double *values = alloca(len * sizeof(*values));
 380         int ko = HKL_FALSE;
 381
 382         for(i=0;i<len;++i){
 383                 values[i] = hkl_axis_get_value_closest(&self->axes[i], &ref->axes[i]);
 384                 if(gsl_isnan(values[i])){
 385                         ko = HKL_TRUE;
 386                         break;
 387                 }
 388         }
 389         if(!ko){
 390                 for(i=0;i<len;++i)
 391                         hkl_axis_set_value(&self->axes[i], values[i]);
 392                 hkl_geometry_update(self);
 393         }
 394         return ko;
 395 }
 396
 397 void hkl_geometry_fprintf(FILE *file, HklGeometry const *self)
 398 {
 399         size_t i;
 400
 401         for(i=0; i<HKL_LIST_LEN(self->axes); ++i){
 402                 if(i)
 403                         fprintf(file, "\n");
 404                 hkl_parameter_fprintf(file, (HklParameter *)(&self->axes[i]));
 405         }
 406 }
 407
 408 /*******************/
 409 /* HklGeometryList */
 410 /*******************/
 411
 412 HklGeometryList *hkl_geometry_list_new(void)
 413 {
 414         HklGeometryList *self;
 415
 416         self = HKL_MALLOC(HklGeometryList);
 417
 418         HKL_LIST_INIT(self->items);
 419         self->multiply = NULL;
 420
 421         return self;
 422 }
 423
 424 void hkl_geometry_list_free(HklGeometryList *self)
 425 {
 426         HKL_LIST_FREE_DESTRUCTOR(self->items, hkl_geometry_list_item_free);
 427         free(self);
 428 }
 429
 430 /**
 431  * @brief this method Add a geometry to the geometries
 432  *
 433  * @param self The current PseudoAxeEngine
 434  * @param x A vector of double with the axes values to put in the geometry.
 435  *
 436  * This method try to be clever by allocating memory only if the current
 437  * length of the geometries is not large enought. Then it just set the
 438  * geometry axes and copy it to the right geometries. We do not gives the
 439  * x len as it is equal to the self->axes_len.
 440  */
 441 void hkl_geometry_list_add(HklGeometryList *self, HklGeometry *geometry)
 442 {
 443         size_t i;
 444         int ko;
 445
 446         /* now check if the geometry is already in the geometry list */
 447         ko = HKL_FALSE;
 448         for(i=0; i<HKL_LIST_LEN(self->items); ++i)
 449                 if (hkl_geometry_distance_orthodromic(geometry,
 450                                                       self->items[i]->geometry) < HKL_EPSILON)
 451                         ko = HKL_TRUE;
 452
 453         if(ko == HKL_FALSE)
 454                 HKL_LIST_ADD_VALUE(self->items, hkl_geometry_list_item_new(geometry));
 455 }
 456
 457 void hkl_geometry_list_reset(HklGeometryList *self)
 458 {
 459         HKL_LIST_FREE_DESTRUCTOR(self->items, hkl_geometry_list_item_free);
 460 }
 461
 462 void hkl_geometry_list_sort(HklGeometryList *self, HklGeometry *ref)
 463 {
 464         size_t len = HKL_LIST_LEN(self->items);
 465         double *distances = alloca(len * sizeof(*distances));
 466         size_t *idx = alloca(len * sizeof(*idx));
 467         size_t i, x;
 468         int j, p;
 469         HklGeometryListItem **items;
 470
 471         /* compute the distances once for all */
 472         for(i=0; i<len; ++i){
 473                 distances[i] = hkl_geometry_distance(ref, self->items[i]->geometry);
 474                 idx[i] = i;
 475         }
 476
 477         /* insertion sorting */
 478         for(i=1; i<len; ++i){
 479                 x = idx[i];
 480                 /* find the smallest idx p lower than i with distance[idx[p]] >= distance[x] */
 481                 for(p = 0; distances[idx[p]] < distances[x] && fabs(distances[idx[p]] - distances[x]) > HKL_EPSILON; p++);
 482
 483                 /* move evythings in between p and i */
 484                 for(j=i-1; j>=p; j--)
 485                         idx[j+1] = idx[j];
 486
 487                 idx[p] = x; /* insert the saved idx */
 488         }
 489
 490         /* reorder the geometries. */
 491         items = malloc(len * sizeof(HklGeometryListItem *));
 492         for(i=0; i<len; ++i)
 493                 items[i] = self->items[idx[i]];
 494         free(self->items);
 495         self->items = items;
 496 }
 497
 498 void hkl_geometry_list_fprintf(FILE *f, HklGeometryList const *self)
 499 {
 500         HklParameter *parameter;
 501         HklGeometry *g;
 502         double value;
 503         size_t len, axes_len;
 504         size_t i, j;
 505
 506         fprintf(f, "multiply method: %p \n", self->multiply);
 507         len = HKL_LIST_LEN(self->items);
 508         if(len){
 509                 axes_len = HKL_LIST_LEN(self->items[0]->geometry->axes);
 510                 g = self->items[0]->geometry;
 511                 fprintf(f, "    ");
 512                 for(i=0; i<axes_len; ++i)
 513                         fprintf(f, "%19s", hkl_axis_get_name(&g->axes[i]));
 514
 515                 /* geometries */
 516                 for(i=0; i<len; ++i) {
 517                         fprintf(f, "\n%d :", i);
 518                         for(j=0; j<axes_len; ++j) {
 519                                 parameter = (HklParameter *)(&self->items[i]->geometry->axes[j]);
 520                                 value = hkl_parameter_get_value_unit(parameter);
 521                                 if (parameter->punit)
 522                                         fprintf(f, " % 18.15f %s", value, parameter->punit->repr);
 523                                 else
 524                                         fprintf(f, " % 18.15f", value);
 525
 526                         }
 527                         fprintf(f, "\n   ");
 528                         for(j=0; j<axes_len; ++j) {
 529                                 parameter = (HklParameter *)(&self->items[i]->geometry->axes[j]);
 530                                 value = gsl_sf_angle_restrict_symm(parameter->value) * hkl_unit_factor(parameter->unit, parameter->punit);
 531                                 if (parameter->punit)
 532                                         fprintf(f, " % 18.15f %s", value, parameter->punit->repr);
 533                                 else
 534                                         fprintf(f, " % 18.15f", value);
 535                         }
 536                         fprintf(f, "\n");
 537                 }
 538         }
 539 }
 540
 541 void hkl_geometry_list_multiply(HklGeometryList *self)
 542 {
 543         size_t i;
 544         size_t len;
 545
 546         if(!self || !self->multiply)
 547                 return;
 548
 549         len = HKL_LIST_LEN(self->items);
 550         for(i=0; i<len; ++i)
 551                 self->multiply(self, i);
 552 }
 553
 554 static void perm_r(HklGeometryList *self, HklGeometry *ref, HklGeometry *geometry,
 555                    int perm[], size_t axis_idx)
 556 {
 557         size_t len;
 558
 559         len = HKL_LIST_LEN(geometry->axes);
 560
 561         if (axis_idx == len){
 562                 if(hkl_geometry_distance(ref, geometry) > HKL_EPSILON)
 563                         HKL_LIST_ADD_VALUE(self->items, hkl_geometry_list_item_new(geometry));
 564         }else{
 565                 if(perm[axis_idx] == HKL_TRUE){
 566                         HklAxis *axis;
 567                         double max;
 568                         double value;
 569                         double value0;
 570
 571                         axis = &geometry->axes[axis_idx];
 572                         max = hkl_axis_get_max(axis);
 573                         value = hkl_axis_get_value(axis);
 574                         value0 = value;
 575                         do{
 576                                 /* fprintf(stdout, "\n%d %s, %f", axis_idx, hkl_axis_get_name(axis), value * HKL_RADTODEG); */
 577                                 perm_r(self, ref, geometry, perm, axis_idx + 1);
 578                                 value +=  2*M_PI;
 579                                 if(value <= (max + HKL_EPSILON))
 580                                         hkl_axis_set_value(axis, value);
 581                         }while(value <= (max + HKL_EPSILON));
 582                         hkl_axis_set_value(axis, value0);
 583                 } else
 584                         perm_r(self, ref, geometry, perm, axis_idx + 1);
 585         }
 586 }
 587
 588 void hkl_geometry_list_multiply_from_range(HklGeometryList *self)
 589 {
 590         size_t i, j;
 591         size_t len;
 592         if(!self)
 593                 return;
 594
 595         len = HKL_LIST_LEN(self->items);
 596         for(i=0; i<len; ++i){
 597                 HklGeometry *geometry;
 598                 HklGeometry *ref;
 599                 int *perm;
 600
 601                 ref = self->items[i]->geometry;
 602                 geometry = hkl_geometry_new_copy(ref);
 603                 perm = alloca(HKL_LIST_LEN(geometry->axes) * sizeof(*perm));
 604
 605                 /* find axes to permute and the first solution of thoses axes */
 606                 for(j=0; j<HKL_LIST_LEN(geometry->axes); ++j){
 607                         HklAxis *axis = &geometry->axes[j];
 608                         perm[j] = hkl_axis_is_value_compatible_with_range(axis);
 609                         /* fprintf(stdout, "%d %d\n", j, perm[j]); */
 610                         if (perm[j] == HKL_TRUE)
 611                                 hkl_axis_set_value_smallest_in_range(axis);
 612
 613                 }
 614                 /*
 615                  * fprintf(stdout, "FIRST SOLUTION\n");
 616                  * hkl_geometry_fprintf(stdout, geometry);
 617                  */
 618
 619                 perm_r(self, ref, geometry, perm, 0);
 620                 hkl_geometry_free(geometry);
 621         }
 622 }
 623
 624 void hkl_geometry_list_remove_invalid(HklGeometryList *self)
 625 {
 626         size_t i;
 627
 628         if(!self)
 629                 return;
 630
 631         for(i=0; i<HKL_LIST_LEN(self->items); ++i)
 632                 if(!hkl_geometry_is_valid(self->items[i]->geometry)){
 633                         HKL_LIST_DEL_DESTRUCTOR(self->items, i, hkl_geometry_list_item_free);
 634                         --i;
 635                 }
 636 }
 637
 638 int hkl_geometry_list_len(HklGeometryList *self)
 639 {
 640         return HKL_LIST_LEN(self->items);
 641 }
 642
 643 int hkl_geometry_list_is_empty(HklGeometryList *self)
 644 {
 645         return HKL_LIST_LEN(self->items) == 0;
 646 }
 647
 648 /***********************/
 649 /* HklGeometryListItem */
 650 /***********************/
 651
 652 HklGeometryListItem *hkl_geometry_list_item_new(HklGeometry *geometry)
 653 {
 654         HklGeometryListItem *self;
 655
 656         if(!geometry)
 657                 return NULL;
 658
 659         self = HKL_MALLOC(HklGeometryListItem);
 660
 661         self->geometry = hkl_geometry_new_copy(geometry);
 662
 663         return self;
 664 }
 665
 666
 667 void hkl_geometry_list_item_free(HklGeometryListItem *self)
 668 {
 669         if(!self)
 670                 return;
 671
 672         hkl_geometry_free(self->geometry);
 673         free(self);
 674 }