hkl/hkl-axis.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, 2023 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 <gsl/gsl_nan.h>                // for GSL_NAN
  23 #include <gsl/gsl_sf_trig.h>            // for gsl_sf_angle_restrict_symm
  24 #include <gsl/gsl_sys.h>                // for gsl_isnan
  25 #include <math.h>                       // for M_PI, ceil, fabs, floor
  26 #include <stdio.h>                      // for FILE
  27 #include <stdlib.h>                     // for NULL, free
  28 #include "hkl-axis-private.h"           // for HklAxis
  29 #include "hkl-interval-private.h"       // for HklInterval, etc
  30 #include "hkl-macros-private.h"         // for HKL_MALLOC
  31 #include "hkl-parameter-private.h"      // for _HklParameter, etc
  32 #include "hkl-quaternion-private.h"     // for hkl_quaternion_fprintf, etc
  33 #include "hkl-vector-private.h"         // for hkl_vector_fprintf, etc
  34 #include "hkl.h"                        // for HklParameter, TRUE, etc
  35 #include "hkl/ccan/container_of/container_of.h"  // for container_of
  36
  37 /***********/
  38 /* HklAxis */
  39 /***********/
  40
  41 static inline HklParameter *hkl_axis_copy_real(const HklParameter *base)
  42 {
  43         HklAxis *self = container_of(base, HklAxis, parameter);
  44         HklAxis *dup;
  45
  46         dup = g_new(HklAxis, 1);
  47
  48         *dup = *self;
  49
  50         return &dup->parameter;
  51 }
  52
  53
  54 static inline void hkl_axis_free_real(HklParameter *self)
  55 {
  56         free(container_of(self, HklAxis, parameter));
  57 }
  58
  59 static inline void hkl_axis_update(HklAxis *self)
  60 {
  61         hkl_quaternion_init_from_angle_and_axe(&self->q,
  62                                                self->parameter._value,
  63                                                &self->axis_v);
  64 }
  65
  66 static inline int hkl_axis_init_copy_real(HklParameter *self, const HklParameter *src,
  67                                           GError **error)
  68 {
  69         HklAxis *axis_self = container_of(self, HklAxis, parameter);
  70         HklAxis *axis_src = container_of(src, HklAxis, parameter);
  71
  72         hkl_error (error == NULL || *error == NULL);
  73
  74         *axis_self = *axis_src;
  75         self->changed = TRUE;
  76
  77         return TRUE;
  78 }
  79
  80 static inline int hkl_axis_set_value_real(HklParameter *self, double value,
  81                                           HklUnitEnum unit_type, GError **error)
  82 {
  83         HklAxis *axis = container_of(self, HklAxis, parameter);
  84
  85         hkl_error (error == NULL || *error == NULL);
  86
  87         if(!hkl_parameter_value_set_real(self, value, unit_type, error)){
  88                 hkl_assert (error == NULL || *error != NULL);
  89                 return FALSE;
  90         }
  91         hkl_assert (error == NULL || *error == NULL);
  92
  93         hkl_axis_update(axis);
  94
  95         return TRUE;
  96 }
  97
  98 static inline void hkl_axis_set_value_smallest_in_range_real(HklParameter *self)
  99 {
 100         double value, min;
 101
 102         value = self->_value;
 103         min = self->range.min;
 104
 105         if(value < min)
 106                 hkl_axis_set_value_real(self,
 107                                         value + 2*M_PI*ceil((min - value)/(2*M_PI)),
 108                                         HKL_UNIT_DEFAULT,
 109                                         NULL);
 110         else
 111                 hkl_axis_set_value_real(self,
 112                                         value - 2*M_PI*floor((value - min)/(2*M_PI)),
 113                                         HKL_UNIT_DEFAULT,
 114                                         NULL);
 115 }
 116
 117 static inline void hkl_axis_randomize_real(HklParameter *self)
 118 {
 119         hkl_parameter_randomize_real(self);
 120         hkl_axis_update(container_of(self, HklAxis, parameter));
 121 }
 122
 123 static inline int hkl_axis_is_permutable_real(UNUSED const HklParameter *self)
 124 {
 125         return TRUE;
 126 }
 127
 128 /*
 129  * given a current position of angle a min and max interval find the closest
 130  * equivalent angle + n delta_angle in a given direction.
 131  * CAUSION angle MUST be in [min, max] otherwise...
 132  */
 133 static void find_angle(double current, double *angle, double *distance,
 134                        double min, double max, double delta_angle)
 135 {
 136         double new_angle = *angle;
 137         double new_distance;
 138
 139         while(new_angle >= min && new_angle <= max) {
 140                 new_distance = fabs(new_angle - current);
 141                 if (new_distance <= *distance) {
 142                         *angle = new_angle;
 143                         *distance = new_distance;
 144                 }
 145                 new_angle += delta_angle;
 146         }
 147 }
 148
 149 static inline double hkl_axis_get_value_closest_real(const HklParameter *self,
 150                                                      const HklParameter *ref)
 151 {
 152         double angle = self->_value;
 153
 154         if(hkl_parameter_is_valid(self)){
 155                 if(hkl_interval_length(&self->range) >= 2*M_PI){
 156                         int k;
 157                         double current = ref->_value;
 158                         double distance = fabs(current - angle);
 159                         double delta = 2. * M_PI;
 160                         double min = self->range.min;
 161                         double max = self->range.max;
 162
 163                         /* three cases */
 164                         if (angle > max) {
 165                                 k = (int)(floor((max - angle) / delta));
 166                                 angle += k * delta;
 167                                 find_angle(current, &angle, &distance, min, max, -delta);
 168                         } else if (angle < min) {
 169                                 k = (int) (ceil((min - angle) / delta));
 170                                 angle += k * delta;
 171                                 find_angle(current, &angle, &distance, min, max, delta);
 172                         } else {
 173                                 find_angle(current, &angle, &distance, min, max, -delta);
 174                                 find_angle(current, &angle, &distance, min, max, delta);
 175                         }
 176                 }
 177
 178         }else
 179                 angle = GSL_NAN;
 180         return angle;
 181 }
 182
 183 /*
 184  * check if the angle or its equivalent is in between [min, max]
 185  */
 186 static inline int hkl_axis_is_valid_real(const HklParameter *self)
 187 {
 188         double value = self->_value;
 189         int res = FALSE;
 190         HklInterval range = self->range;
 191
 192         if(hkl_interval_length(&range) > 2*M_PI)
 193                 res = TRUE;
 194         else{
 195                 hkl_interval_angle_restrict_symm(&range);
 196                 value = gsl_sf_angle_restrict_symm(value);
 197
 198                 if(range.min <= range.max){
 199                         if(range.min <= value && range.max >= value)
 200                                 res = TRUE;
 201                 }else{
 202                         if(value <= range.max || value >= range.min)
 203                                 res = TRUE;
 204                 }
 205         }
 206         return res;
 207 }
 208
 209 static inline void hkl_axis_fprintf_real(FILE *f, const HklParameter *self)
 210 {
 211         HklAxis *axis = container_of(self, HklAxis, parameter);
 212
 213         hkl_parameter_fprintf_real(f, self);
 214         hkl_vector_fprintf(f, &axis->axis_v);
 215         hkl_quaternion_fprintf(f, &axis->q);
 216 }
 217
 218 static inline const HklVector *hkl_axis_axis_v_get_real(const HklParameter *self)
 219 {
 220         return &container_of(self, HklAxis, parameter)->axis_v;
 221 }
 222
 223 static inline const HklQuaternion *hkl_axis_quaternion_get_real(const HklParameter *self)
 224 {
 225         return &container_of(self, HklAxis, parameter)->q;
 226 }
 227
 228 static inline int hkl_axis_transformation_cmp_real(const HklParameter *base, const HklParameter *p2)
 229 {
 230         const HklAxis *self = container_of(base, HklAxis, parameter);
 231         const HklAxis *axis2 = container_of(p2, HklAxis, parameter);
 232
 233         return hkl_parameter_transformation_cmp_real(base, p2)
 234                 || hkl_vector_cmp(&self->axis_v, &axis2->axis_v);
 235 }
 236
 237 static inline HklVector hkl_axis_transformation_apply_real(const HklParameter *base,
 238                                                            const HklVector *v)
 239 {
 240         const HklAxis *self = container_of(base, HklAxis, parameter);
 241         HklVector res = *v;
 242
 243         hkl_vector_rotated_quaternion(&res, &self->q);
 244
 245         return res;
 246 }
 247
 248 static inline double hkl_axis_orthodromic_distance_get(const HklParameter *self,
 249                                                        double value)
 250 {
 251         double ref = self->_value;
 252         double d = 0;
 253
 254         d = fabs(gsl_sf_angle_restrict_symm(value) - gsl_sf_angle_restrict_symm(ref));
 255         /* as M_PI and -M_PI are included in the GSL restriction */
 256         if (d > M_PI)
 257                 d = 2*M_PI - d;
 258
 259         return d;
 260 }
 261
 262 #define HKL_PARAMETER_OPERATIONS_AXIS_DEFAULTS                          \
 263         HKL_PARAMETER_OPERATIONS_DEFAULTS,                              \
 264                 .copy = hkl_axis_copy_real,                             \
 265                 .free = hkl_axis_free_real,                             \
 266                 .init_copy = hkl_axis_init_copy_real,                   \
 267                 .get_value_closest = hkl_axis_get_value_closest_real,   \
 268                 .set_value = hkl_axis_set_value_real,                   \
 269                 .set_value_smallest_in_range = hkl_axis_set_value_smallest_in_range_real, \
 270                 .randomize = hkl_axis_randomize_real,                   \
 271                 .is_permutable = hkl_axis_is_permutable_real,           \
 272                 .is_valid = hkl_axis_is_valid_real,                     \
 273                 .fprintf = hkl_axis_fprintf_real,                       \
 274                 .axis_v_get = hkl_axis_axis_v_get_real,                 \
 275                 .quaternion_get = hkl_axis_quaternion_get_real,         \
 276                 .transformation_cmp = hkl_axis_transformation_cmp_real, \
 277                 .transformation_apply = hkl_axis_transformation_apply_real, \
 278                 .orthodromic_distance_get = hkl_axis_orthodromic_distance_get
 279
 280 static HklParameterOperations hkl_parameter_operations_axis = {
 281         HKL_PARAMETER_OPERATIONS_AXIS_DEFAULTS,
 282 };
 283
 284 HklParameter *hkl_parameter_new_rotation(const char *name, HklVector const *axis_v, const HklUnit *punit)
 285 {
 286         HklAxis axis0 = {
 287                 .parameter = { HKL_PARAMETER_DEFAULTS_ANGLE,
 288                         .name = name,
 289                         .punit = punit,
 290                         .ops = &hkl_parameter_operations_axis,
 291                         .type = Rotation(*axis_v),
 292                 },
 293                 .axis_v = *axis_v,
 294                 .q = {{1., 0., 0., 0.}},
 295         };
 296
 297         HklAxis *self =  g_new(HklAxis, 1);
 298
 299         *self = axis0;
 300
 301         return &self->parameter;
 302 }
 303
 304 /*************************/
 305 /* HklRotationWithOrigin */
 306 /*************************/
 307
 308 static inline HklParameter *hkl_rotation_with_origin_copy_real(const HklParameter *base)
 309 {
 310         HklRotationWithOrigin *self = container_of(container_of(base, HklAxis, parameter),
 311                                                    HklRotationWithOrigin, axis);
 312         HklRotationWithOrigin *dup;
 313
 314         dup = g_new(HklRotationWithOrigin, 1);
 315
 316         *dup = *self;
 317
 318         return &dup->axis.parameter;
 319 }
 320
 321 static inline void hkl_rotation_with_origin_free_real(HklParameter *base)
 322 {
 323         free(container_of(container_of(base, HklAxis, parameter),
 324                           HklRotationWithOrigin, axis));
 325 }
 326
 327 static inline int hkl_rotation_with_origin_init_copy_real(HklParameter *base,
 328                                                           const HklParameter *base_src,
 329                                                           GError **error)
 330 {
 331         HklRotationWithOrigin *self = container_of(container_of(base, HklAxis, parameter),
 332                                                    HklRotationWithOrigin, axis);
 333         HklRotationWithOrigin *src = container_of(container_of(base_src, HklAxis, parameter),
 334                                                   HklRotationWithOrigin, axis);
 335
 336         hkl_error (error == NULL || *error == NULL);
 337
 338         *self = *src;
 339         base->changed = TRUE;
 340
 341         return TRUE;
 342 }
 343
 344 static inline int hkl_rotation_with_origin_transformation_cmp_real(const HklParameter *base,
 345                                                                    const HklParameter *base_p2)
 346 {
 347         HklRotationWithOrigin *self = container_of(container_of(base, HklAxis, parameter),
 348                                                    HklRotationWithOrigin, axis);
 349         HklRotationWithOrigin *p2 = container_of(container_of(base_p2, HklAxis, parameter),
 350                                                  HklRotationWithOrigin, axis);
 351
 352         return hkl_axis_transformation_cmp_real(base, base_p2)
 353                 || hkl_vector_cmp(&self->origin, &p2->origin);
 354 }
 355
 356 static inline HklVector hkl_rotation_with_origin_transformation_apply_real(const HklParameter *base,
 357                                                                            const HklVector *v)
 358 {
 359         const HklAxis *axis = container_of(base, HklAxis, parameter);
 360         const HklRotationWithOrigin *self = container_of(axis, HklRotationWithOrigin, axis);
 361         HklVector res = *v;
 362
 363         hkl_vector_minus_vector(&res, &self->origin);
 364         hkl_vector_rotated_quaternion(&res, &axis->q);
 365         hkl_vector_add_vector(&res, &self->origin);
 366
 367         return res;
 368 }
 369
 370 #define HKL_PARAMETER_OPERATIONS_ROTATION_WITH_ORIGIN_DEFAULTS          \
 371         HKL_PARAMETER_OPERATIONS_AXIS_DEFAULTS,                         \
 372                 .copy = hkl_rotation_with_origin_copy_real,             \
 373                 .free = hkl_rotation_with_origin_free_real,             \
 374                 .init_copy = hkl_rotation_with_origin_init_copy_real,   \
 375                 .transformation_cmp = hkl_rotation_with_origin_transformation_cmp_real, \
 376                 .transformation_apply = hkl_rotation_with_origin_transformation_apply_real
 377
 378 static HklParameterOperations hkl_parameter_operations_rotation_with_origin = {
 379         HKL_PARAMETER_OPERATIONS_ROTATION_WITH_ORIGIN_DEFAULTS,
 380 };
 381
 382 HklParameter *hkl_parameter_new_rotation_with_origin(const char *name,
 383                                                      HklVector const *axis_v,
 384                                                      HklVector const *origin,
 385                                                      const HklUnit *punit)
 386 {
 387         HklRotationWithOrigin rotation0 = {
 388                 .axis = {
 389                         .parameter = { HKL_PARAMETER_DEFAULTS_ANGLE,
 390                                 .name = name,
 391                                 .punit = punit,
 392                                 .ops = &hkl_parameter_operations_rotation_with_origin,
 393                                 .type = RotationWithOrigin(*axis_v, *origin),
 394                         },
 395                         .axis_v = *axis_v,
 396                         .q = {{1., 0., 0., 0.}},
 397                 },
 398                 .origin = *origin,
 399         };
 400
 401         HklRotationWithOrigin *self =  g_new(HklRotationWithOrigin, 1);
 402
 403         *self = rotation0;
 404
 405         return &self->axis.parameter;
 406 }
 407
 408 /**********************/
 409 /* HklTranslation */
 410 /**********************/
 411
 412 static inline HklParameter *hkl_translation_copy_real(const HklParameter *base)
 413 {
 414         HklTranslation *self = container_of(base, HklTranslation, parameter);
 415         HklTranslation *dup;
 416
 417         dup = g_new(HklTranslation, 1);
 418
 419         *dup = *self;
 420
 421         return &dup->parameter;
 422 }
 423
 424 static inline void hkl_translation_free_real(HklParameter *base)
 425 {
 426         free(container_of(base, HklTranslation, parameter));
 427 }
 428
 429 static inline int hkl_translation_init_copy_real(HklParameter *base,
 430                                                  const HklParameter *base_src,
 431                                                  GError **error)
 432 {
 433         HklTranslation *self = container_of(base, HklTranslation, parameter);
 434         HklTranslation *src = container_of(base_src, HklTranslation, parameter);
 435
 436         /* need to check that parameters are compatibles */
 437         hkl_error (error == NULL || *error == NULL);
 438
 439         *self = *src;
 440         base->changed = TRUE;
 441
 442         return TRUE;
 443 }
 444
 445 static inline void hkl_translation_fprintf_real(FILE *f, const HklParameter *base)
 446 {
 447         HklTranslation *self = container_of(base, HklTranslation, parameter);
 448
 449         hkl_parameter_fprintf_real(f, base);
 450         hkl_vector_fprintf(f, &self->axis_v);
 451 }
 452
 453 static inline const HklVector *hkl_translation_axis_v_get_real(const HklParameter *base)
 454 {
 455         return &container_of(base, HklTranslation, parameter)->axis_v;
 456 }
 457
 458 static inline int hkl_translation_transformation_cmp_real(const HklParameter *base,
 459                                                           const HklParameter *p2)
 460 {
 461         const HklTranslation *self = container_of(base, HklTranslation, parameter);
 462         const HklTranslation *translation2 = container_of(p2, HklTranslation, parameter);
 463
 464         return hkl_parameter_transformation_cmp_real(base, p2)
 465                 || hkl_vector_cmp(&self->axis_v, &translation2->axis_v);
 466 }
 467
 468 static inline HklVector hkl_translation_transformation_apply_real(const HklParameter *base,
 469                                                                   const HklVector *v)
 470 {
 471         const HklTranslation *self = container_of(base, HklTranslation, parameter);
 472         HklVector res = self->axis_v;
 473
 474         hkl_vector_times_double(&res, base->_value);
 475         hkl_vector_add_vector(&res, v);
 476
 477         return res;
 478 }
 479
 480 #define HKL_PARAMETER_OPERATIONS_TRANSLATION_DEFAULTS                   \
 481         HKL_PARAMETER_OPERATIONS_DEFAULTS,                              \
 482                 .copy = hkl_translation_copy_real,                      \
 483                 .free = hkl_translation_free_real,                      \
 484                 .init_copy = hkl_translation_init_copy_real,            \
 485                 .fprintf = hkl_translation_fprintf_real ,               \
 486                 .axis_v_get = hkl_translation_axis_v_get_real,          \
 487                 .transformation_cmp = hkl_translation_transformation_cmp_real, \
 488                 .transformation_apply = hkl_translation_transformation_apply_real
 489
 490 static HklParameterOperations hkl_parameter_operations_translation =
 491 {
 492         HKL_PARAMETER_OPERATIONS_TRANSLATION_DEFAULTS,
 493 };
 494
 495 HklParameter *hkl_parameter_new_translation(const char *name, HklVector const *axis_v, const HklUnit *punit)
 496 {
 497         HklTranslation translation0 = {
 498                 .parameter = { HKL_PARAMETER_DEFAULTS_LENGTH,
 499                         .name = name,
 500                         .punit = punit,
 501                         .ops = &hkl_parameter_operations_translation,
 502                         .type = Translation(*axis_v),
 503                 },
 504                 .axis_v = *axis_v,
 505         };
 506
 507         HklTranslation *self =  g_new(HklTranslation, 1);
 508
 509         *self = translation0;
 510
 511         return &self->parameter;
 512 }