* I forgot this change.
[hkl.git] / hkl / hkl-pseudoaxis-common-hkl.c
blob21f55a1d6aaa1fbe6d06e7f6806d03ecbeef394f
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-2010 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>
21 * Maria-Teresa Nunez-Pardo-de-Verra <tnunez@mail.desy.de>
23 #include <string.h>
24 #include <gsl/gsl_sf_trig.h>
25 #include <hkl/hkl-pseudoaxis.h>
26 #include <hkl/hkl-pseudoaxis-common.h>
27 #include <hkl/hkl-pseudoaxis-common-hkl.h>
28 #include <hkl/hkl-pseudoaxis-auto.h>
30 /*******************************************/
31 /* common methode use by hkl getter/setter */
32 /*******************************************/
34 typedef struct _HklDetectorFit HklDetectorFit;
35 struct _HklDetectorFit
37 HklGeometry *geometry;
38 HklDetector *detector;
39 HklVector *kf0;
40 HklAxis **axes;
41 int len;
44 /* this method is used to fit only the detector position */
45 static int fit_detector_function(const gsl_vector *x, void *params, gsl_vector *f)
47 int i;
48 const double *x_data = gsl_vector_const_ptr(x, 0);
49 double *f_data = gsl_vector_ptr(f, 0);
50 HklDetectorFit *fitp = params;
51 HklVector kf;
53 /* update the workspace from x; */
54 for(i=0; i<fitp->len; ++i)
55 hkl_axis_set_value(fitp->axes[i], x_data[i]);
57 hkl_geometry_update(fitp->geometry);
59 hkl_detector_compute_kf(fitp->detector, fitp->geometry, &kf);
61 f_data[0] = fabs(fitp->kf0->data[0] - kf.data[0])
62 + fabs(fitp->kf0->data[1] - kf.data[1])
63 + fabs(fitp->kf0->data[2] - kf.data[2]);
64 f_data[1] = fabs(fitp->kf0->data[1] - kf.data[1]);
66 #ifdef DEBUG
67 fprintf(stdout, "kf0 [%f, %f, %f], kf [%f, %f, %f]",
68 fitp->kf0->data[0], fitp->kf0->data[1], fitp->kf0->data[2],
69 kf.data[0], kf.data[1], kf.data[2]);
70 fprintf(stdout, " x : [");
71 for(i=0; i<fitp->len; ++i)
72 fprintf(stdout, " %.7f", x_data[i]);
73 fprintf(stdout, "] | f : [");
74 for(i=0; i<fitp->len; ++i)
75 fprintf(stdout, " %.7f", f_data[i]);
76 fprintf(stdout, "]\n");
77 #endif
78 return GSL_SUCCESS;
82 static int fit_detector_position(HklPseudoAxisEngineMode *mode, HklGeometry *geometry, HklDetector *detector, HklVector *kf)
84 int i;
85 HklDetectorFit params;
86 gsl_multiroot_fsolver_type const *T;
87 gsl_multiroot_fsolver *s;
88 gsl_multiroot_function f;
89 gsl_vector *x;
90 double *x_data;
91 int status;
92 int res = HKL_FAIL;
93 int iter;
95 /* fit the sample part to find the position of the detector */
96 /* FIXME for now the detector holder is the second one */
97 /* we need to be able to differenciate by holder the axes uses for the fit */
98 /* for now compare the holder axes with the axes of the mode to generate the right gsl multiroot solver */
99 /* we need to set up also the engine->axes for the fit at least detector holder len */
100 params.geometry = geometry;
101 params.detector = detector;
102 params.kf0 = kf;
103 params.axes = malloc(sizeof(*params.axes) * params.geometry->holders[1].config->len);
104 params.len = 0;
105 for(i=0; i<mode->axes_names_len; ++i){
106 int k;
107 int tmp;
109 tmp = hkl_geometry_get_axis_idx_by_name(params.geometry, mode->axes_names[i]);
110 for(k=0; k<params.geometry->holders[1].config->len; ++k)
111 if(tmp == params.geometry->holders[1].config->idx[k])
112 params.axes[params.len++] = &params.geometry->axes[tmp];
115 /* if no detector axis found ???? abort */
116 /* maybe put this at the begining of the method */
117 if (params.len > 0){
118 /* now solve the system */
119 /* Initialize method */
120 T = gsl_multiroot_fsolver_hybrid;
121 s = gsl_multiroot_fsolver_alloc (T, params.len);
122 x = gsl_vector_alloc(params.len);
123 x_data = gsl_vector_ptr(x, 0);
125 /* initialize x with the right values */
126 for(i=0; i<params.len; ++i)
127 x_data[i] = hkl_axis_get_value(params.axes[i]);
129 f.f = fit_detector_function;
130 f.n = params.len;
131 f.params = &params;
132 gsl_multiroot_fsolver_set (s, &f, x);
134 /* iterate to find the solution */
135 iter = 0;
136 do {
137 ++iter;
138 status = gsl_multiroot_fsolver_iterate(s);
139 if (status || iter % 100 == 0) {
140 /* Restart from another point. */
141 for(i=0; i<params.len; ++i)
142 x_data[i] = (double)rand() / RAND_MAX * 180. / M_PI;
143 gsl_multiroot_fsolver_set(s, &f, x);
144 gsl_multiroot_fsolver_iterate(s);
146 status = gsl_multiroot_test_residual (s->f, HKL_EPSILON);
147 } while (status == GSL_CONTINUE && iter < 1000);
149 if(status != GSL_CONTINUE){
150 res = HKL_SUCCESS;
151 /* put the axes in the -pi, pi range. */
152 for(i=0; i<params.len; ++i)
153 gsl_sf_angle_restrict_pos_e(&((HklParameter *)params.axes[i])->value);
154 #ifdef DEBUG
155 fprintf(stdout, "\nstatus : %d iter : %d", status, iter);
156 for(i=0; i<params.len; ++i)
157 fprintf(stdout, " %.7f", s->f->data[i]);
158 fprintf(stdout, "\n");
159 hkl_geometry_fprintf(stdout, params.geometry);
160 #endif
162 /* release memory */
163 gsl_vector_free(x);
164 gsl_multiroot_fsolver_free(s);
166 free(params.axes);
168 return res;
171 /* get the highest index of the axis in a holder */
172 /* NOT the axis index in the geometry->axes list becarefull */
173 /* which is part of the axes_names of the mode */
174 /* return -1 if their is no axes of the mode in the sample part of the geometry */
175 static int get_last_axis_idx(HklGeometry *geometry, int holder_idx, char const **axes_names, int len)
177 int last = -1;
178 int i;
179 HklHolder *holder;
181 holder = &geometry->holders[holder_idx];
182 for(i=0; i<len; ++i){
183 int j;
184 int idx;
186 /* FIXME for now the sample holder is the first one */
187 idx = hkl_geometry_get_axis_idx_by_name(geometry, axes_names[i]);
188 for(j=0; j<holder->config->len; ++j)
189 if(idx == holder->config->idx[j]){
190 last = last > j ? last : j;
191 break;
194 return last;
197 int RUBh_minus_Q_func(const gsl_vector *x, void *params, gsl_vector *f)
199 double const *x_data = gsl_vector_const_ptr(x, 0);
200 double *f_data = gsl_vector_ptr(f, 0);
202 RUBh_minus_Q(x_data, params, f_data);
204 return GSL_SUCCESS;
207 int RUBh_minus_Q(double const x[], void *params, double f[])
209 HklVector Hkl;
210 HklVector ki, dQ;
211 HklPseudoAxisEngine *engine;
212 HklHolder *holder;
213 size_t i;
215 engine = params;
217 /* update the workspace from x; */
218 for(i=0; i<engine->axes_len; ++i)
219 hkl_axis_set_value(engine->axes[i], x[i]);
220 hkl_geometry_update(engine->geometry);
222 hkl_vector_init(&Hkl,
223 ((HklParameter *)engine->pseudoAxes[0])->value,
224 ((HklParameter *)engine->pseudoAxes[1])->value,
225 ((HklParameter *)engine->pseudoAxes[2])->value);
227 /* R * UB * h = Q */
228 /* for now the 0 holder is the sample holder. */
229 holder = &engine->geometry->holders[0];
230 hkl_matrix_times_vector(&engine->sample->UB, &Hkl);
231 hkl_vector_rotated_quaternion(&Hkl, &holder->q);
233 /* kf - ki = Q */
234 hkl_source_compute_ki(&engine->geometry->source, &ki);
235 hkl_detector_compute_kf(engine->detector, engine->geometry, &dQ);
236 hkl_vector_minus_vector(&dQ, &ki);
238 hkl_vector_minus_vector(&dQ, &Hkl);
240 f[0] = dQ.data[0];
241 f[1] = dQ.data[1];
242 f[2] = dQ.data[2];
244 return GSL_SUCCESS;
247 int hkl_pseudo_axis_engine_mode_get_hkl_real(HklPseudoAxisEngineMode *self,
248 HklPseudoAxisEngine *engine,
249 HklGeometry *geometry,
250 HklDetector *detector,
251 HklSample *sample,
252 HklError **error)
254 HklHolder *holder;
255 HklMatrix RUB;
256 HklVector hkl, ki, Q;
257 double min, max;
258 size_t i;
260 /* update the geometry internals */
261 hkl_geometry_update(geometry);
263 /* R * UB */
264 /* for now the 0 holder is the sample holder. */
265 holder = &geometry->holders[0];
266 hkl_quaternion_to_matrix(&holder->q, &RUB);
267 hkl_matrix_times_matrix(&RUB, &sample->UB);
269 /* kf - ki = Q */
270 hkl_source_compute_ki(&geometry->source, &ki);
271 hkl_detector_compute_kf(detector, geometry, &Q);
272 hkl_vector_minus_vector(&Q, &ki);
274 hkl_matrix_solve(&RUB, &hkl, &Q);
276 /* compute the min and max */
277 min = -1;
278 max = 1;
280 /* update the pseudoAxes config part */
281 for(i=0;i<engine->pseudoAxes_len;++i){
282 HklParameter *parameter = (HklParameter *)(engine->pseudoAxes[i]);
283 parameter->value = hkl.data[i];
284 parameter->range.min = min;
285 parameter->range.max = max;
288 return HKL_SUCCESS;
291 int hkl_pseudo_axis_engine_mode_set_hkl_real(HklPseudoAxisEngineMode *self,
292 HklPseudoAxisEngine *engine,
293 HklGeometry *geometry,
294 HklDetector *detector,
295 HklSample *sample,
296 HklError **error)
298 int res = HKL_SUCCESS;
300 res &= hkl_pseudo_axis_engine_mode_set_real(self, engine,
301 geometry, detector, sample,
302 error);
304 if(res == HKL_SUCCESS){
305 int last_axis;
307 /* check that the mode allow to move a sample axis */
308 /* FIXME for now the sample holder is the first one */
309 last_axis = get_last_axis_idx(geometry, 0, self->axes_names, self->axes_names_len);
310 if(last_axis >= 0){
311 int i;
312 int len;
314 /* For each solution already found we will generate another one */
315 /* using the Ewalds construction by rotating Q around the last sample */
316 /* axis of the mode until it intersect again the Ewald sphere. */
317 /* FIXME do not work if ki is colinear with the axis. */
319 /* for this we needs : */
320 /* - the coordinates of the end of the Q vector (q) */
321 /* - the last sample axis orientation of the mode (axis_v) */
322 /* - the coordinates of the center of the ewalds sphere (c) */
323 /* - the coordinates of the center of rotation of the sample (o = 0, 0, 0) */
325 /* then we can : */
326 /* - project the origin in plane of normal axis_v containing q (o') */
327 /* - project the center of the ewalds sphere into the same plan (c') */
328 /* - rotate q around this (o', c') line of 180° to find the (q2) solution */
329 /* - compute the (kf2) corresponding to this q2 solution */
330 /* at the end we just need to solve numerically the position of the detector */
332 /* we will add solution to the geometries so save its length before */
333 len = engine->engines->geometries->len;
334 for(i=0; i<len; ++i){
335 int j;
336 HklGeometry *geom;
337 HklVector ki;
338 HklVector kf;
339 HklVector kf2;
340 HklVector q;
341 HklVector axis_v;
342 HklQuaternion qr;
343 HklAxis *axis;
344 HklVector cp = {0};
345 HklVector op = {0};
346 double angle;
348 geom = hkl_geometry_new_copy(engine->engines->geometries->items[i].geometry);
350 /* get the Q vector kf - ki */
351 hkl_detector_compute_kf(detector, geom, &q);
352 hkl_source_compute_ki(&geom->source, &ki);
353 hkl_vector_minus_vector(&q, &ki);
355 /* compute the current orientation of the last axis */
356 axis = &geom->axes[geom->holders[0].config->idx[last_axis]];
357 axis_v = axis->axis_v;
358 hkl_quaternion_init(&qr, 1, 0, 0, 0);
359 for(j=0; j<last_axis; ++j)
360 hkl_quaternion_times_quaternion(&qr, &geom->axes[geom->holders[0].config->idx[j]].q);
361 hkl_vector_rotated_quaternion(&axis_v, &qr);
363 /* - project the center of the ewalds sphere into the same plan (c') */
364 hkl_vector_minus_vector(&cp, &ki);
365 hkl_vector_project_on_plan(&cp, &axis_v, &q);
366 hkl_vector_project_on_plan(&op, &axis_v, &q);
369 /* - rotate q around this (o', c') line of 180° to find the (q2) solution */
370 kf2 = q;
371 hkl_vector_rotated_around_line(&kf2, M_PI, &cp, &op);
372 angle = hkl_vector_oriented_angle_points(&q, &op, &kf2, &axis_v);
373 hkl_axis_set_value(axis, ((HklParameter *)axis)->value + angle);
374 hkl_geometry_update(geom);
375 hkl_vector_add_vector(&kf2, &ki);
377 /* at the end we just need to solve numerically the position of the detector */
378 if(fit_detector_position(self, geom, detector, &kf2) == HKL_SUCCESS)
379 hkl_geometry_list_add(engine->engines->geometries, geom);
381 hkl_geometry_free(geom);
385 return res;
388 /***************************************/
389 /* the double diffraction get set part */
390 /***************************************/
392 int double_diffraction_func(gsl_vector const *x, void *params, gsl_vector *f)
394 double const *x_data = gsl_vector_const_ptr(x, 0);
395 double *f_data = gsl_vector_ptr(f, 0);
397 double_diffraction(x_data, params, f_data);
399 return GSL_SUCCESS;
402 int double_diffraction(double const x[], void *params, double f[])
404 HklPseudoAxisEngine *engine = params;
405 HklVector hkl, kf2;
406 HklVector ki;
407 HklVector dQ;
408 size_t i;
409 HklHolder *holder;
411 /* update the workspace from x; */
412 for(i=0; i<engine->axes_len; ++i)
413 hkl_axis_set_value(engine->axes[i], x[i]);
414 hkl_geometry_update(engine->geometry);
416 hkl_vector_init(&hkl,
417 ((HklParameter *)engine->pseudoAxes[0])->value,
418 ((HklParameter *)engine->pseudoAxes[1])->value,
419 ((HklParameter *)engine->pseudoAxes[2])->value);
421 hkl_vector_init(&kf2,
422 engine->mode->parameters[0].value,
423 engine->mode->parameters[1].value,
424 engine->mode->parameters[2].value);
426 /* R * UB * hkl = Q */
427 /* for now the 0 holder is the sample holder. */
428 holder = &engine->geometry->holders[0];
429 hkl_matrix_times_vector(&engine->sample->UB, &hkl);
430 hkl_vector_rotated_quaternion(&hkl, &holder->q);
432 /* kf - ki = Q */
433 hkl_source_compute_ki(&engine->geometry->source, &ki);
434 hkl_detector_compute_kf(engine->detector, engine->geometry, &dQ);
435 hkl_vector_minus_vector(&dQ, &ki);
436 hkl_vector_minus_vector(&dQ, &hkl);
438 /* R * UB * hlk2 = Q2 */
439 hkl_matrix_times_vector(&engine->sample->UB, &kf2);
440 hkl_vector_rotated_quaternion(&kf2, &holder->q);
441 hkl_vector_add_vector(&kf2, &ki);
443 f[0] = dQ.data[0];
444 f[1] = dQ.data[1];
445 f[2] = dQ.data[2];
446 f[3] = hkl_vector_norm2(&kf2) - hkl_vector_norm2(&ki);
448 return GSL_SUCCESS;
451 /******************************************/
452 /* the psi_constant_vertical get set part */
453 /******************************************/
455 int psi_constant_vertical_func(gsl_vector const *x, void *params, gsl_vector *f)
458 double const *x_data = gsl_vector_const_ptr(x, 0);
459 double *f_data = gsl_vector_ptr(f, 0);
461 HklVector hkl;
462 HklVector ki, kf, Q, n;
463 HklPseudoAxisEngine *engine;
464 size_t i;
466 RUBh_minus_Q(x_data, params, f_data);
467 engine = params;
469 /* update the workspace from x; */
470 for(i=0; i<engine->axes_len; ++i)
471 hkl_axis_set_value(engine->axes[i], x_data[i]);
472 hkl_geometry_update(engine->geometry);
474 hkl_vector_init(&hkl, 1, 0, 0);
476 /* kf - ki = Q */
477 hkl_source_compute_ki(&engine->geometry->source, &ki);
478 hkl_detector_compute_kf(engine->detector, engine->geometry, &kf);
479 Q = kf;
480 hkl_vector_minus_vector(&Q, &ki);
482 hkl_vector_normalize(&Q);
483 n = kf;
484 hkl_vector_vectorial_product(&n, &ki);
485 hkl_vector_vectorial_product(&n, &Q);
488 hkl_vector_times_matrix(&hkl, &engine->sample->UB);
489 hkl_vector_rotated_quaternion(&hkl, &engine->geometry->holders[0].q);
491 /* project hkl on the plan of normal Q */
492 hkl_vector_project_on_plan(&hkl, &Q, NULL);
494 f_data[3] = engine->mode->parameters[3].value - hkl_vector_oriented_angle(&n, &hkl, &Q);
496 return GSL_SUCCESS;
499 int hkl_pseudo_axis_engine_mode_init_psi_constant_vertical_real(HklPseudoAxisEngineMode *self,
500 HklPseudoAxisEngine *engine,
501 HklGeometry *geometry,
502 HklDetector *detector,
503 HklSample *sample,
504 HklError **error)
506 HklVector hkl;
507 HklVector ki, kf, Q, n;
508 int status = HKL_SUCCESS;
510 if (!self || !engine || !engine->mode || !geometry || !detector || !sample){
511 status = HKL_FAIL;
512 return status;
515 status = hkl_pseudo_axis_engine_init_func(self, engine, geometry, detector, sample);
516 if(status == HKL_FAIL)
517 return status;
519 /* Compute the constant psi value (to be kept) */
520 hkl_vector_init(&hkl, 1, 0, 0);
522 /* kf - ki = Q */
523 hkl_source_compute_ki(&geometry->source, &ki);
524 hkl_detector_compute_kf(detector, geometry, &kf);
525 Q = kf;
526 hkl_vector_minus_vector(&Q, &ki);
528 if (hkl_vector_is_null(&Q))
529 status = HKL_FAIL;
530 else{
531 /* needed for a problem of precision */
532 hkl_vector_normalize(&Q);
534 /* compute the intersection of the plan P(kf, ki) and PQ (normal Q) */
535 n = kf;
536 hkl_vector_vectorial_product(&n, &ki);
537 hkl_vector_vectorial_product(&n, &Q);
539 /* compute hkl in the laboratory referentiel */
540 /* the geometry was already updated in the detector compute kf */
541 /* for now the 0 holder is the sample holder */
542 hkl.data[0] = self->parameters[0].value;
543 hkl.data[1] = self->parameters[1].value;
544 hkl.data[2] = self->parameters[2].value;
545 hkl_vector_times_matrix(&hkl, &sample->UB);
546 hkl_vector_rotated_quaternion(&hkl, &geometry->holders[0].q);
548 /* project hkl on the plan of normal Q */
549 hkl_vector_project_on_plan(&hkl, &Q, NULL);
551 if (hkl_vector_is_null(&hkl))
552 status = HKL_FAIL;
553 else
554 /* compute the angle beetween hkl and n and
555 * store in in the fourth parameter */
556 hkl_parameter_set_value(&self->parameters[3],
557 hkl_vector_oriented_angle(&n, &hkl, &Q));
560 return status;
563 HklPseudoAxisEngine *hkl_pseudo_axis_engine_hkl_new(void)
565 HklPseudoAxisEngine *self;
567 self = hkl_pseudo_axis_engine_new("hkl", 3, "h", "k", "l");
569 /* h */
570 hkl_parameter_init((HklParameter *)self->pseudoAxes[0],
571 "h",
572 -1, 0., 1,
573 HKL_TRUE, HKL_TRUE,
574 NULL, NULL);
575 /* k */
576 hkl_parameter_init((HklParameter *)self->pseudoAxes[1],
577 "k",
578 -1, 0., 1,
579 HKL_TRUE, HKL_TRUE,
580 NULL, NULL);
581 /* l */
582 hkl_parameter_init((HklParameter *)self->pseudoAxes[2],
583 "l",
584 -1, 0., 1,
585 HKL_TRUE, HKL_TRUE,
586 NULL, NULL);
588 return self;