src/hkl-pseudoaxis-common-psi.c

   1 #include <gsl/gsl_math.h>
   2 #include <gsl/gsl_vector.h>
   3 #include <gsl/gsl_sf_trig.h>
   4
   5 #include <hkl/hkl-pseudoaxis.h>
   6 #include <hkl/hkl-pseudoaxis-common-psi.h>
   7
   8 static int psi(const gsl_vector *x, void *params, gsl_vector *f)
   9 {
  10
  11         HklVector dhkl0, hkl1;
  12         HklVector ki, kf, Q, n;
  13         HklMatrix RUB;
  14         HklPseudoAxisEngine *engine;
  15         HklPseudoAxisEngineGetSetPsi *getsetpsi;
  16         HklPseudoAxis *psi;
  17         HklHolder *holder;
  18         size_t i;
  19         double const *x_data = gsl_vector_const_ptr(x, 0);
  20         double *f_data = gsl_vector_ptr(f, 0);
  21
  22         engine = params;
  23         getsetpsi = (HklPseudoAxisEngineGetSetPsi *)engine->getset;
  24         psi = &engine->pseudoAxes[0];
  25
  26         // update the workspace from x;
  27         for(i=0; i<engine->axes_len; ++i) {
  28                 HklAxis *axis = engine->axes[i];
  29                 axis->config.value = x_data[i];
  30                 axis->config.dirty = 1;
  31         }
  32         hkl_geometry_update(engine->geometry);
  33
  34         // kf - ki = Q
  35         hkl_source_compute_ki(&engine->geometry->source, &ki);
  36         hkl_detector_compute_kf(engine->detector, engine->geometry, &kf);
  37         Q = kf;
  38         hkl_vector_minus_vector(&Q, &ki);
  39         if (hkl_vector_is_null(&Q)){
  40                 f_data[0] = dhkl0.data[0];
  41                 f_data[1] = dhkl0.data[1];
  42                 f_data[2] = dhkl0.data[2];
  43                 f_data[3] = 1;
  44         }else{
  45                 // R * UB
  46                 // for now the 0 holder is the sample holder.
  47                 holder = &engine->geometry->holders[0];
  48                 hkl_quaternion_to_smatrix(&holder->q, &RUB);
  49                 hkl_matrix_times_smatrix(&RUB, &engine->sample->UB);
  50
  51                 // compute dhkl0
  52                 hkl_matrix_solve(&RUB, &dhkl0, &Q);
  53                 hkl_vector_minus_vector(&dhkl0, &getsetpsi->hkl0);
  54
  55                 // compute the intersection of the plan P(kf, ki) and PQ (normal Q)
  56                 /*
  57                  * now that dhkl0 have been computed we can use a
  58                  * normalized Q to compute n and psi
  59                  */
  60                 hkl_vector_normalize(&Q);
  61                 n = kf;
  62                 hkl_vector_vectorial_product(&n, &ki);
  63                 hkl_vector_vectorial_product(&n, &Q);
  64
  65                 // compute hkl1 in the laboratory referentiel
  66                 // for now the 0 holder is the sample holder.
  67                 hkl1.data[0] = engine->getset->parameters[0].value;
  68                 hkl1.data[1] = engine->getset->parameters[1].value;
  69                 hkl1.data[2] = engine->getset->parameters[2].value;
  70                 hkl_vector_times_smatrix(&hkl1, &engine->sample->UB);
  71                 hkl_vector_rotated_quaternion(&hkl1, &engine->geometry->holders[0].q);
  72
  73                 // project hkl1 on the plan of normal Q
  74                 hkl_vector_project_on_plan(&hkl1, &Q);
  75                 if (hkl_vector_is_null(&hkl1)){ // hkl1 colinear with Q
  76                         f_data[0] = dhkl0.data[0];
  77                         f_data[1] = dhkl0.data[1];
  78                         f_data[2] = dhkl0.data[2];
  79                         f_data[3] = 1;
  80                 }else{
  81                         f_data[0] = dhkl0.data[0];
  82                         f_data[1] = dhkl0.data[1];
  83                         f_data[2] = dhkl0.data[2];
  84                         f_data[3] = psi->config.value - hkl_vector_oriented_angle(&n, &hkl1, &Q);
  85                 }
  86         }
  87         return GSL_SUCCESS;
  88 }
  89
  90 int hkl_pseudo_axis_engine_get_set_init_psi_real(HklPseudoAxisEngine *engine,
  91                                                  HklGeometry *geometry,
  92                                                  HklDetector const *detector,
  93                                                  HklSample const *sample)
  94 {
  95         int status = HKL_SUCCESS;
  96         HklVector ki;
  97         HklVector kf;
  98         HklMatrix RUB;
  99         HklPseudoAxisEngineGetSetPsi *self;
 100         HklPseudoAxisEngineGetSet *base;
 101         HklHolder *holder;
 102
 103         status = hkl_pseudo_axis_engine_init_func(engine, geometry, detector, sample);
 104         if (status == HKL_FAIL)
 105                 return status;
 106
 107         self = (HklPseudoAxisEngineGetSetPsi *)engine->getset;
 108
 109         // update the geometry internals
 110         hkl_geometry_update(geometry);
 111
 112         // R * UB
 113         // for now the 0 holder is the sample holder.
 114         holder = &geometry->holders[0];
 115         hkl_quaternion_to_smatrix(&holder->q, &RUB);
 116         hkl_matrix_times_smatrix(&RUB, &sample->UB);
 117
 118         // kf - ki = Q0
 119         hkl_source_compute_ki(&geometry->source, &ki);
 120         hkl_detector_compute_kf(detector, geometry, &self->Q0);
 121         hkl_vector_minus_vector(&self->Q0, &ki);
 122         if (hkl_vector_is_null(&self->Q0))
 123                 status = HKL_FAIL;
 124         else
 125                 // compute hkl0
 126                 hkl_matrix_solve(&RUB, &self->hkl0, &self->Q0);
 127
 128         return status;
 129 }
 130
 131 int hkl_pseudo_axis_engine_get_set_get_psi_real(HklPseudoAxisEngine *engine,
 132                                                 HklGeometry *geometry,
 133                                                 HklDetector const *detector,
 134                                                 HklSample const *sample)
 135 {
 136         int status = HKL_SUCCESS;
 137
 138         if (!engine || !engine->getset || !geometry || !detector || !sample){
 139                 status = HKL_FAIL;
 140                 return status;
 141         }
 142
 143         double psi;
 144         HklVector ki;
 145         HklVector kf;
 146         HklVector Q;
 147         HklVector hkl1;
 148         HklVector n;
 149         HklPseudoAxisEngineGetSetPsi *self;
 150         HklPseudoAxisEngineGetSet *base;
 151
 152         self = (HklPseudoAxisEngineGetSetPsi *)engine->getset;
 153         base = engine->getset;
 154
 155         // get kf, ki and Q
 156         hkl_source_compute_ki(&geometry->source, &ki);
 157         hkl_detector_compute_kf(detector, geometry, &kf);
 158         Q = kf;
 159         hkl_vector_minus_vector(&Q, &ki);
 160         if (hkl_vector_is_null(&Q))
 161                 status = HKL_FAIL;
 162         else{
 163                 hkl_vector_normalize(&Q); // needed for a problem of precision
 164
 165                 // compute the intersection of the plan P(kf, ki) and PQ (normal Q)
 166                 n = kf;
 167                 hkl_vector_vectorial_product(&n, &ki);
 168                 hkl_vector_vectorial_product(&n, &Q);
 169
 170                 // compute hkl1 in the laboratory referentiel
 171                 // the geometry was already updated in the detector compute kf
 172                 // for now the 0 holder is the sample holder.
 173                 hkl1.data[0] = base->parameters[0].value;
 174                 hkl1.data[1] = base->parameters[1].value;
 175                 hkl1.data[2] = base->parameters[2].value;
 176                 hkl_vector_times_smatrix(&hkl1, &sample->UB);
 177                 hkl_vector_rotated_quaternion(&hkl1, &geometry->holders[0].q);
 178
 179                 // project hkl1 on the plan of normal Q
 180                 hkl_vector_project_on_plan(&hkl1, &Q);
 181
 182                 if (hkl_vector_is_null(&hkl1))
 183                         status = HKL_FAIL;
 184                 else
 185                         // compute the angle beetween hkl1 and n
 186                         engine->pseudoAxes[0].config.value = hkl_vector_oriented_angle(&n, &hkl1, &Q);
 187         }
 188
 189         return status;
 190 }
 191
 192 int hkl_pseudo_axis_engine_get_set_set_psi_real(HklPseudoAxisEngine *engine,
 193                                                 HklGeometry *geometry,
 194                                                 HklDetector *detector,
 195                                                 HklSample *sample)
 196 {
 197         hkl_pseudoAxeEngine_prepare_internal(engine, geometry, detector,
 198                                              sample);
 199
 200         return hkl_pseudoAxeEngine_solve_function(engine, psi);
 201 }