1 /* This file is part of the hkl library.
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.
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.
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/>.
16 * Copyright (C) 2003-2020, 2022, 2023 Synchrotron SOLEIL
17 * L'Orme des Merisiers Saint-Aubin
18 * BP 48 91192 GIF-sur-YVETTE CEDEX
20 * Authors: Picca Frédéric-Emmanuel <picca@synchrotron-soleil.fr>
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
41 static inline HklParameter
*hkl_axis_copy_real(const HklParameter
*base
)
43 HklAxis
*self
= container_of(base
, HklAxis
, parameter
);
46 dup
= g_new(HklAxis
, 1);
50 return &dup
->parameter
;
54 static inline void hkl_axis_free_real(HklParameter
*self
)
56 free(container_of(self
, HklAxis
, parameter
));
59 static inline void hkl_axis_update(HklAxis
*self
)
61 hkl_quaternion_init_from_angle_and_axe(&self
->q
,
62 self
->parameter
._value
,
66 static inline int hkl_axis_init_copy_real(HklParameter
*self
, const HklParameter
*src
,
69 HklAxis
*axis_self
= container_of(self
, HklAxis
, parameter
);
70 HklAxis
*axis_src
= container_of(src
, HklAxis
, parameter
);
72 hkl_error (error
== NULL
|| *error
== NULL
);
74 *axis_self
= *axis_src
;
80 static inline int hkl_axis_set_value_real(HklParameter
*self
, double value
,
81 HklUnitEnum unit_type
, GError
**error
)
83 HklAxis
*axis
= container_of(self
, HklAxis
, parameter
);
85 hkl_error (error
== NULL
|| *error
== NULL
);
87 if(!hkl_parameter_value_set_real(self
, value
, unit_type
, error
)){
88 hkl_assert (error
== NULL
|| *error
!= NULL
);
91 hkl_assert (error
== NULL
|| *error
== NULL
);
93 hkl_axis_update(axis
);
98 static inline void hkl_axis_set_value_smallest_in_range_real(HklParameter
*self
)
102 value
= self
->_value
;
103 min
= self
->range
.min
;
106 hkl_axis_set_value_real(self
,
107 value
+ 2*M_PI
*ceil((min
- value
)/(2*M_PI
)),
111 hkl_axis_set_value_real(self
,
112 value
- 2*M_PI
*floor((value
- min
)/(2*M_PI
)),
117 static inline void hkl_axis_randomize_real(HklParameter
*self
)
119 hkl_parameter_randomize_real(self
);
120 hkl_axis_update(container_of(self
, HklAxis
, parameter
));
123 static inline int hkl_axis_is_permutable_real(UNUSED
const HklParameter
*self
)
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...
133 static void find_angle(double current
, double *angle
, double *distance
,
134 double min
, double max
, double delta_angle
)
136 double new_angle
= *angle
;
139 while(new_angle
>= min
&& new_angle
<= max
) {
140 new_distance
= fabs(new_angle
- current
);
141 if (new_distance
<= *distance
) {
143 *distance
= new_distance
;
145 new_angle
+= delta_angle
;
149 static inline double hkl_axis_get_value_closest_real(const HklParameter
*self
,
150 const HklParameter
*ref
)
152 double angle
= self
->_value
;
154 if(hkl_parameter_is_valid(self
)){
155 if(hkl_interval_length(&self
->range
) >= 2*M_PI
){
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
;
165 k
= (int)(floor((max
- angle
) / delta
));
167 find_angle(current
, &angle
, &distance
, min
, max
, -delta
);
168 } else if (angle
< min
) {
169 k
= (int) (ceil((min
- angle
) / delta
));
171 find_angle(current
, &angle
, &distance
, min
, max
, delta
);
173 find_angle(current
, &angle
, &distance
, min
, max
, -delta
);
174 find_angle(current
, &angle
, &distance
, min
, max
, delta
);
184 * check if the angle or its equivalent is in between [min, max]
186 static inline int hkl_axis_is_valid_real(const HklParameter
*self
)
188 double value
= self
->_value
;
190 HklInterval range
= self
->range
;
192 if(hkl_interval_length(&range
) > 2*M_PI
)
195 hkl_interval_angle_restrict_symm(&range
);
196 value
= gsl_sf_angle_restrict_symm(value
);
198 if(range
.min
<= range
.max
){
199 if(range
.min
<= value
&& range
.max
>= value
)
202 if(value
<= range
.max
|| value
>= range
.min
)
209 static inline void hkl_axis_fprintf_real(FILE *f
, const HklParameter
*self
)
211 HklAxis
*axis
= container_of(self
, HklAxis
, parameter
);
213 hkl_parameter_fprintf_real(f
, self
);
214 hkl_vector_fprintf(f
, &axis
->axis_v
);
215 hkl_quaternion_fprintf(f
, &axis
->q
);
218 static inline const HklVector
*hkl_axis_axis_v_get_real(const HklParameter
*self
)
220 return &container_of(self
, HklAxis
, parameter
)->axis_v
;
223 static inline const HklQuaternion
*hkl_axis_quaternion_get_real(const HklParameter
*self
)
225 return &container_of(self
, HklAxis
, parameter
)->q
;
228 static inline int hkl_axis_transformation_cmp_real(const HklParameter
*base
, const HklParameter
*p2
)
230 const HklAxis
*self
= container_of(base
, HklAxis
, parameter
);
231 const HklAxis
*axis2
= container_of(p2
, HklAxis
, parameter
);
233 return hkl_parameter_transformation_cmp_real(base
, p2
)
234 || hkl_vector_cmp(&self
->axis_v
, &axis2
->axis_v
);
237 static inline HklVector
hkl_axis_transformation_apply_real(const HklParameter
*base
,
240 const HklAxis
*self
= container_of(base
, HklAxis
, parameter
);
243 hkl_vector_rotated_quaternion(&res
, &self
->q
);
248 static inline double hkl_axis_orthodromic_distance_get(const HklParameter
*self
,
251 double ref
= self
->_value
;
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 */
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
280 static HklParameterOperations hkl_parameter_operations_axis
= {
281 HKL_PARAMETER_OPERATIONS_AXIS_DEFAULTS
,
284 HklParameter
*hkl_parameter_new_rotation(const char *name
, HklVector
const *axis_v
, const HklUnit
*punit
)
287 .parameter
= { HKL_PARAMETER_DEFAULTS_ANGLE
,
290 .ops
= &hkl_parameter_operations_axis
,
291 .type
= Rotation(*axis_v
),
294 .q
= {{1., 0., 0., 0.}},
297 HklAxis
*self
= g_new(HklAxis
, 1);
301 return &self
->parameter
;
304 /*************************/
305 /* HklRotationWithOrigin */
306 /*************************/
308 static inline HklParameter
*hkl_rotation_with_origin_copy_real(const HklParameter
*base
)
310 HklRotationWithOrigin
*self
= container_of(container_of(base
, HklAxis
, parameter
),
311 HklRotationWithOrigin
, axis
);
312 HklRotationWithOrigin
*dup
;
314 dup
= g_new(HklRotationWithOrigin
, 1);
318 return &dup
->axis
.parameter
;
321 static inline void hkl_rotation_with_origin_free_real(HklParameter
*base
)
323 free(container_of(container_of(base
, HklAxis
, parameter
),
324 HklRotationWithOrigin
, axis
));
327 static inline int hkl_rotation_with_origin_init_copy_real(HklParameter
*base
,
328 const HklParameter
*base_src
,
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
);
336 hkl_error (error
== NULL
|| *error
== NULL
);
339 base
->changed
= TRUE
;
344 static inline int hkl_rotation_with_origin_transformation_cmp_real(const HklParameter
*base
,
345 const HklParameter
*base_p2
)
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
);
352 return hkl_axis_transformation_cmp_real(base
, base_p2
)
353 || hkl_vector_cmp(&self
->origin
, &p2
->origin
);
356 static inline HklVector
hkl_rotation_with_origin_transformation_apply_real(const HklParameter
*base
,
359 const HklAxis
*axis
= container_of(base
, HklAxis
, parameter
);
360 const HklRotationWithOrigin
*self
= container_of(axis
, HklRotationWithOrigin
, axis
);
363 hkl_vector_minus_vector(&res
, &self
->origin
);
364 hkl_vector_rotated_quaternion(&res
, &axis
->q
);
365 hkl_vector_add_vector(&res
, &self
->origin
);
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
378 static HklParameterOperations hkl_parameter_operations_rotation_with_origin
= {
379 HKL_PARAMETER_OPERATIONS_ROTATION_WITH_ORIGIN_DEFAULTS
,
382 HklParameter
*hkl_parameter_new_rotation_with_origin(const char *name
,
383 HklVector
const *axis_v
,
384 HklVector
const *origin
,
385 const HklUnit
*punit
)
387 HklRotationWithOrigin rotation0
= {
389 .parameter
= { HKL_PARAMETER_DEFAULTS_ANGLE
,
392 .ops
= &hkl_parameter_operations_rotation_with_origin
,
393 .type
= RotationWithOrigin(*axis_v
, *origin
),
396 .q
= {{1., 0., 0., 0.}},
401 HklRotationWithOrigin
*self
= g_new(HklRotationWithOrigin
, 1);
405 return &self
->axis
.parameter
;
408 /**********************/
410 /**********************/
412 static inline HklParameter
*hkl_translation_copy_real(const HklParameter
*base
)
414 HklTranslation
*self
= container_of(base
, HklTranslation
, parameter
);
417 dup
= g_new(HklTranslation
, 1);
421 return &dup
->parameter
;
424 static inline void hkl_translation_free_real(HklParameter
*base
)
426 free(container_of(base
, HklTranslation
, parameter
));
429 static inline int hkl_translation_init_copy_real(HklParameter
*base
,
430 const HklParameter
*base_src
,
433 HklTranslation
*self
= container_of(base
, HklTranslation
, parameter
);
434 HklTranslation
*src
= container_of(base_src
, HklTranslation
, parameter
);
436 /* need to check that parameters are compatibles */
437 hkl_error (error
== NULL
|| *error
== NULL
);
440 base
->changed
= TRUE
;
445 static inline void hkl_translation_fprintf_real(FILE *f
, const HklParameter
*base
)
447 HklTranslation
*self
= container_of(base
, HklTranslation
, parameter
);
449 hkl_parameter_fprintf_real(f
, base
);
450 hkl_vector_fprintf(f
, &self
->axis_v
);
453 static inline const HklVector
*hkl_translation_axis_v_get_real(const HklParameter
*base
)
455 return &container_of(base
, HklTranslation
, parameter
)->axis_v
;
458 static inline int hkl_translation_transformation_cmp_real(const HklParameter
*base
,
459 const HklParameter
*p2
)
461 const HklTranslation
*self
= container_of(base
, HklTranslation
, parameter
);
462 const HklTranslation
*translation2
= container_of(p2
, HklTranslation
, parameter
);
464 return hkl_parameter_transformation_cmp_real(base
, p2
)
465 || hkl_vector_cmp(&self
->axis_v
, &translation2
->axis_v
);
468 static inline HklVector
hkl_translation_transformation_apply_real(const HklParameter
*base
,
471 const HklTranslation
*self
= container_of(base
, HklTranslation
, parameter
);
472 HklVector res
= self
->axis_v
;
474 hkl_vector_times_double(&res
, base
->_value
);
475 hkl_vector_add_vector(&res
, v
);
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
490 static HklParameterOperations hkl_parameter_operations_translation
=
492 HKL_PARAMETER_OPERATIONS_TRANSLATION_DEFAULTS
,
495 HklParameter
*hkl_parameter_new_translation(const char *name
, HklVector
const *axis_v
, const HklUnit
*punit
)
497 HklTranslation translation0
= {
498 .parameter
= { HKL_PARAMETER_DEFAULTS_LENGTH
,
501 .ops
= &hkl_parameter_operations_translation
,
502 .type
= Translation(*axis_v
),
507 HklTranslation
*self
= g_new(HklTranslation
, 1);
509 *self
= translation0
;
511 return &self
->parameter
;