hkl/hkl-pseudoaxis-common-psi.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 <gsl/gsl_math.h>
  23 #include <gsl/gsl_vector.h>
  24 #include <gsl/gsl_sf_trig.h>
  25
  26 #include <hkl/hkl-pseudoaxis.h>
  27 #include <hkl/hkl-pseudoaxis-common.h>
  28 #include <hkl/hkl-pseudoaxis-auto.h>
  29 #include <hkl/hkl-pseudoaxis-common-psi.h>
  30
  31 static int psi_func(const gsl_vector *x, void *params, gsl_vector *f)
  32 {
  33
  34         HklVector dhkl0, hkl1;
  35         HklVector ki, kf, Q, n;
  36         HklMatrix RUB;
  37         HklPseudoAxisEngine *engine;
  38         HklPseudoAxisEngineModePsi *modepsi;
  39         HklPseudoAxis *psi;
  40         HklHolder *holder;
  41         size_t i;
  42         size_t len;
  43         double const *x_data = gsl_vector_const_ptr(x, 0);
  44         double *f_data = gsl_vector_ptr(f, 0);
  45
  46         engine = params;
  47         modepsi = (HklPseudoAxisEngineModePsi *)engine->mode;
  48         psi = engine->pseudoAxes[0];
  49
  50         // update the workspace from x;
  51         len = HKL_LIST_LEN(engine->axes);
  52         for(i=0; i<len; ++i)
  53                 hkl_axis_set_value(engine->axes[i], x_data[i]);
  54         hkl_geometry_update(engine->geometry);
  55
  56         // kf - ki = Q
  57         hkl_source_compute_ki(&engine->geometry->source, &ki);
  58         hkl_detector_compute_kf(engine->detector, engine->geometry, &kf);
  59         Q = kf;
  60         hkl_vector_minus_vector(&Q, &ki);
  61         if (hkl_vector_is_null(&Q)){
  62                 f_data[0] = 1;
  63                 f_data[1] = 1;
  64                 f_data[2] = 1;
  65                 f_data[3] = 1;
  66         }else{
  67                 // R * UB
  68                 // for now the 0 holder is the sample holder.
  69                 holder = &engine->geometry->holders[0];
  70                 hkl_quaternion_to_matrix(&holder->q, &RUB);
  71                 hkl_matrix_times_matrix(&RUB, &engine->sample->UB);
  72
  73                 // compute dhkl0
  74                 hkl_matrix_solve(&RUB, &dhkl0, &Q);
  75                 hkl_vector_minus_vector(&dhkl0, &modepsi->hkl0);
  76
  77                 // compute the intersection of the plan P(kf, ki) and PQ (normal Q)
  78                 /*
  79                  * now that dhkl0 have been computed we can use a
  80                  * normalized Q to compute n and psi
  81                  */
  82                 hkl_vector_normalize(&Q);
  83                 n = kf;
  84                 hkl_vector_vectorial_product(&n, &ki);
  85                 hkl_vector_vectorial_product(&n, &Q);
  86
  87                 // compute hkl1 in the laboratory referentiel
  88                 // for now the 0 holder is the sample holder.
  89                 hkl1.data[0] = engine->mode->parameters[0].value;
  90                 hkl1.data[1] = engine->mode->parameters[1].value;
  91                 hkl1.data[2] = engine->mode->parameters[2].value;
  92                 hkl_vector_times_smatrix(&hkl1, &engine->sample->UB);
  93                 hkl_vector_rotated_quaternion(&hkl1, &engine->geometry->holders[0].q);
  94
  95                 // project hkl1 on the plan of normal Q
  96                 hkl_vector_project_on_plan(&hkl1, &Q);
  97                 if (hkl_vector_is_null(&hkl1)){ // hkl1 colinear with Q
  98                         f_data[0] = dhkl0.data[0];
  99                         f_data[1] = dhkl0.data[1];
 100                         f_data[2] = dhkl0.data[2];
 101                         f_data[3] = 1;
 102                 }else{
 103                         f_data[0] = dhkl0.data[0];
 104                         f_data[1] = dhkl0.data[1];
 105                         f_data[2] = dhkl0.data[2];
 106                         f_data[3] = psi->parent.value - hkl_vector_oriented_angle(&n, &hkl1, &Q);
 107                 }
 108         }
 109         return GSL_SUCCESS;
 110 }
 111
 112 static int hkl_pseudo_axis_engine_mode_init_psi_real(HklPseudoAxisEngineMode *base,
 113                                                      HklPseudoAxisEngine *engine,
 114                                                      HklGeometry *geometry,
 115                                                      HklDetector *detector,
 116                                                      HklSample *sample,
 117                                                      HklError **error)
 118 {
 119         int status = HKL_SUCCESS;
 120         HklVector ki;
 121         HklMatrix RUB;
 122         HklPseudoAxisEngineModePsi *self = (HklPseudoAxisEngineModePsi *)base;
 123         HklHolder *holder;
 124
 125         status = hkl_pseudo_axis_engine_init_func(base, engine, geometry, detector, sample);
 126         if (status == HKL_FAIL)
 127                 return status;
 128
 129         // update the geometry internals
 130         hkl_geometry_update(geometry);
 131
 132         // R * UB
 133         // for now the 0 holder is the sample holder.
 134         holder = &geometry->holders[0];
 135         hkl_quaternion_to_matrix(&holder->q, &RUB);
 136         hkl_matrix_times_matrix(&RUB, &sample->UB);
 137
 138         // kf - ki = Q0
 139         hkl_source_compute_ki(&geometry->source, &ki);
 140         hkl_detector_compute_kf(detector, geometry, &self->Q0);
 141         hkl_vector_minus_vector(&self->Q0, &ki);
 142         if (hkl_vector_is_null(&self->Q0))
 143                 status = HKL_FAIL;
 144         else
 145                 // compute hkl0
 146                 hkl_matrix_solve(&RUB, &self->hkl0, &self->Q0);
 147
 148         return status;
 149 }
 150
 151 static int hkl_pseudo_axis_engine_mode_get_psi_real(HklPseudoAxisEngineMode *base,
 152                                                     HklPseudoAxisEngine *engine,
 153                                                     HklGeometry *geometry,
 154                                                     HklDetector *detector,
 155                                                     HklSample *sample,
 156                                                     HklError **error)
 157 {
 158         int status = HKL_SUCCESS;
 159
 160         if (!base || !engine || !engine->mode || !geometry || !detector || !sample){
 161                 status = HKL_FAIL;
 162                 return status;
 163         }
 164
 165         HklVector ki;
 166         HklVector kf;
 167         HklVector Q;
 168         HklVector hkl1;
 169         HklVector n;
 170
 171         // get kf, ki and Q
 172         hkl_source_compute_ki(&geometry->source, &ki);
 173         hkl_detector_compute_kf(detector, geometry, &kf);
 174         Q = kf;
 175         hkl_vector_minus_vector(&Q, &ki);
 176         if (hkl_vector_is_null(&Q))
 177                 status = HKL_FAIL;
 178         else{
 179                 hkl_vector_normalize(&Q); // needed for a problem of precision
 180
 181                 // compute the intersection of the plan P(kf, ki) and PQ (normal Q)
 182                 n = kf;
 183                 hkl_vector_vectorial_product(&n, &ki);
 184                 hkl_vector_vectorial_product(&n, &Q);
 185
 186                 // compute hkl1 in the laboratory referentiel
 187                 // the geometry was already updated in the detector compute kf
 188                 // for now the 0 holder is the sample holder.
 189                 hkl1.data[0] = base->parameters[0].value;
 190                 hkl1.data[1] = base->parameters[1].value;
 191                 hkl1.data[2] = base->parameters[2].value;
 192                 hkl_vector_times_smatrix(&hkl1, &sample->UB);
 193                 hkl_vector_rotated_quaternion(&hkl1, &geometry->holders[0].q);
 194
 195                 // project hkl1 on the plan of normal Q
 196                 hkl_vector_project_on_plan(&hkl1, &Q);
 197
 198                 if (hkl_vector_is_null(&hkl1))
 199                         status = HKL_FAIL;
 200                 else
 201                         // compute the angle beetween hkl1 and n
 202                         ((HklParameter *)engine->pseudoAxes[0])->value = hkl_vector_oriented_angle(&n, &hkl1, &Q);
 203         }
 204
 205         return status;
 206 }
 207
 208 HklPseudoAxisEngineModePsi *hkl_pseudo_axis_engine_mode_psi_new(char const *name,
 209                                                                 size_t axes_names_len,
 210                                                                 char const *axes_names[])
 211 {
 212         HklPseudoAxisEngineModePsi *self;
 213         HklParameter parameters[3];
 214         HklFunction functions[] = {psi_func};
 215
 216         if (axes_names_len != 4)
 217                 die("This generic HklPseudoAxisEngineModePsi need exactly 4 axes");
 218
 219         self = HKL_MALLOC(HklPseudoAxisEngineModePsi);
 220
 221         // h1
 222         hkl_parameter_init(&parameters[0],
 223                            "h1",
 224                            -1, 1, 1,
 225                            HKL_TRUE, HKL_FALSE,
 226                            NULL, NULL);
 227
 228         // k1
 229         hkl_parameter_init(&parameters[1],
 230                            "k1",
 231                            -1, 0, 1,
 232                            HKL_TRUE, HKL_FALSE,
 233                            NULL, NULL);
 234
 235         // l1
 236         hkl_parameter_init(&parameters[2],
 237                            "l1",
 238                            -1, 0, 1,
 239                            HKL_TRUE, HKL_FALSE,
 240                            NULL, NULL);
 241
 242         // the base constructor;
 243         hkl_pseudo_axis_engine_mode_init(&self->parent,
 244                                          name,
 245                                          hkl_pseudo_axis_engine_mode_init_psi_real,
 246                                          hkl_pseudo_axis_engine_mode_get_psi_real,
 247                                          hkl_pseudo_axis_engine_mode_set_real,
 248                                          1, functions,
 249                                          3, parameters,
 250                                          axes_names_len, axes_names);
 251
 252
 253         return self;
 254 }
 255
 256 HklPseudoAxisEngine *hkl_pseudo_axis_engine_psi_new(void)
 257 {
 258         HklPseudoAxisEngine *self;
 259
 260         self = hkl_pseudo_axis_engine_new("psi", 1, "psi");
 261
 262         // psi
 263         hkl_parameter_init((HklParameter *)self->pseudoAxes[0],
 264                            "psi",
 265                            -M_PI, 0., M_PI,
 266                            HKL_TRUE, HKL_TRUE,
 267                            &hkl_unit_angle_rad, &hkl_unit_angle_deg);
 268
 269         return self;
 270 }