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-2016 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 = HKL_MALLOC(HklAxis);
  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                 g_assert (error == NULL || *error != NULL);
  89                 return FALSE;
  90         }
  91         g_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 /*
 124  * given a current position of angle a min and max interval find the closest
 125  * equivalent angle + n delta_angle in a given direction.
 126  * CAUSION angle MUST be in [min, max] otherwise...
 127  */
 128 static void find_angle(double current, double *angle, double *distance,
 129                        double min, double max, double delta_angle)
 130 {
 131         double new_angle = *angle;
 132         double new_distance;
 133
 134         while(new_angle >= min && new_angle <= max) {
 135                 new_distance = fabs(new_angle - current);
 136                 if (new_distance <= *distance) {
 137                         *angle = new_angle;
 138                         *distance = new_distance;
 139                 }
 140                 new_angle += delta_angle;
 141         }
 142 }
 143
 144 static inline double hkl_axis_get_value_closest_real(const HklParameter *self,
 145                                                      const HklParameter *ref)
 146 {
 147         double angle = self->_value;
 148
 149         if(hkl_parameter_is_valid(self)){
 150                 if(hkl_interval_length(&self->range) >= 2*M_PI){
 151                         int k;
 152                         double current = ref->_value;
 153                         double distance = fabs(current - angle);
 154                         double delta = 2. * M_PI;
 155                         double min = self->range.min;
 156                         double max = self->range.max;
 157
 158                         /* three cases */
 159                         if (angle > max) {
 160                                 k = (int)(floor((max - angle) / delta));
 161                                 angle += k * delta;
 162                                 find_angle(current, &angle, &distance, min, max, -delta);
 163                         } else if (angle < min) {
 164                                 k = (int) (ceil((min - angle) / delta));
 165                                 angle += k * delta;
 166                                 find_angle(current, &angle, &distance, min, max, delta);
 167                         } else {
 168                                 find_angle(current, &angle, &distance, min, max, -delta);
 169                                 find_angle(current, &angle, &distance, min, max, delta);
 170                         }
 171                 }
 172
 173         }else
 174                 angle = GSL_NAN;
 175         return angle;
 176 }
 177
 178 /*
 179  * check if the angle or its equivalent is in between [min, max]
 180  */
 181 static inline int hkl_axis_is_valid_real(const HklParameter *self)
 182 {
 183         double value = self->_value;
 184         int res = FALSE;
 185         HklInterval range = self->range;
 186
 187         if(hkl_interval_length(&range) > 2*M_PI)
 188                 res = TRUE;
 189         else{
 190                 hkl_interval_angle_restrict_symm(&range);
 191                 value = gsl_sf_angle_restrict_symm(value);
 192
 193                 if(range.min <= range.max){
 194                         if(range.min <= value && range.max >= value)
 195                                 res = TRUE;
 196                 }else{
 197                         if(value <= range.max || value >= range.min)
 198                                 res = TRUE;
 199                 }
 200         }
 201         return res;
 202 }
 203
 204 static inline void hkl_axis_fprintf_real(FILE *f, const HklParameter *self)
 205 {
 206         HklAxis *axis = container_of(self, HklAxis, parameter);
 207
 208         hkl_parameter_fprintf_real(f, self);
 209         hkl_vector_fprintf(f, &axis->axis_v);
 210         hkl_quaternion_fprintf(f, &axis->q);
 211 }
 212
 213 static inline const HklVector *hkl_axis_axis_v_get_real(const HklParameter *self)
 214 {
 215         return &container_of(self, HklAxis, parameter)->axis_v;
 216 }
 217
 218 static inline const HklQuaternion *hkl_axis_quaternion_get_real(const HklParameter *self)
 219 {
 220         return &container_of(self, HklAxis, parameter)->q;
 221 }
 222
 223 static HklParameterOperations hkl_parameter_operations_axis = {
 224         HKL_PARAMETER_OPERATIONS_DEFAULTS,
 225         .copy = hkl_axis_copy_real,
 226         .free = hkl_axis_free_real,
 227         .init_copy = hkl_axis_init_copy_real,
 228         .get_value_closest = hkl_axis_get_value_closest_real,
 229         .set_value = hkl_axis_set_value_real,
 230         .set_value_smallest_in_range = hkl_axis_set_value_smallest_in_range_real,
 231         .randomize = hkl_axis_randomize_real,
 232         .is_valid = hkl_axis_is_valid_real,
 233         .fprintf = hkl_axis_fprintf_real,
 234         .axis_v_get = hkl_axis_axis_v_get_real,
 235         .quaternion_get = hkl_axis_quaternion_get_real
 236 };
 237
 238 HklParameter *hkl_parameter_new_axis(const char *name, HklVector const *axis_v, const HklUnit *punit)
 239 {
 240         HklAxis axis0 = {
 241                 .parameter = { HKL_PARAMETER_DEFAULTS_ANGLE,
 242                                .name = name,
 243                                .punit = punit,
 244                                .ops = &hkl_parameter_operations_axis,
 245                 },
 246                 .axis_v = *axis_v,
 247                 .q = {{1., 0., 0., 0.}},
 248         };
 249
 250         HklAxis *self =  HKL_MALLOC(HklAxis);
 251
 252         *self = axis0;
 253
 254         return &self->parameter;
 255 }