/** * hkl_vector_rotated_around_vector: (skip) * @self: the #HklVector to rotate * @axe: the axe of rotation * @angle: the angle of the rotation * * rotate a vector around another one with a given angle. **/ void hkl_vector_rotated_around_vector(HklVector *self, const HklVector *axe, double angle) { double c = cos(angle); double s = sin(angle); HklVector axe_n; HklVector tmp; axe_n = *axe; hkl_vector_normalize(&axe_n); tmp = *self; self->data[0] = (c + (1 - c) * axe_n.data[0] * axe_n.data[0]) * tmp.data[0]; self->data[0] += ((1 - c) * axe_n.data[0] * axe_n.data[1] - axe_n.data[2] * s) * tmp.data[1]; self->data[0] += ((1 - c) * axe_n.data[0] * axe_n.data[2] + axe_n.data[1] * s) * tmp.data[2]; self->data[1] = ((1 - c) * axe_n.data[0] * axe_n.data[1] + axe_n.data[2] * s) * tmp.data[0]; self->data[1] += (c + (1 - c) * axe_n.data[1] * axe_n.data[1]) * tmp.data[1]; self->data[1] += ((1 - c) * axe_n.data[1] * axe_n.data[2] - axe_n.data[0] * s) * tmp.data[2]; self->data[2] = ((1 - c) * axe_n.data[0] * axe_n.data[2] - axe_n.data[1] * s) * tmp.data[0]; self->data[2] += ((1 - c) * axe_n.data[1] * axe_n.data[2] + axe_n.data[0] * s) * tmp.data[1]; self->data[2] += (c + (1 - c) * axe_n.data[2] * axe_n.data[2]) * tmp.data[2]; }
/** * hkl_vector_oriented_angle: (skip) * @self: the first #HklVector * @vector: the second #HklVector * @ref: the reference #HklVector * * compute the angles beetween two #HklVector and use * a reference #HklVector to orientate the space. That's * way the return value can be in beetween [-pi, pi]. * the (self, vector, ref) is a right oriented base. * * Returns: the angles [-pi, pi] **/ double hkl_vector_oriented_angle(const HklVector *self, const HklVector *vector, const HklVector *ref) { double angle; HklVector tmp; HklVector ref_u; angle = hkl_vector_angle(self, vector); tmp = *self; hkl_vector_vectorial_product(&tmp, vector); hkl_vector_normalize(&tmp); ref_u = *ref; hkl_vector_normalize(&ref_u); if (hkl_vector_is_opposite(&tmp, &ref_u)) angle = -angle; return angle; }
static int hkl_pseudo_axis_engine_mode_get_psi_real(HklPseudoAxisEngineMode *base, HklPseudoAxisEngine *engine, HklGeometry *geometry, HklDetector *detector, HklSample *sample, HklError **error) { int status = HKL_SUCCESS; HklVector ki; HklVector kf; HklVector Q; HklVector hkl1; HklVector n; if (!base || !engine || !engine->mode || !geometry || !detector || !sample){ status = HKL_FAIL; return status; } /* get kf, ki and Q */ hkl_source_compute_ki(&geometry->source, &ki); hkl_detector_compute_kf(detector, geometry, &kf); Q = kf; hkl_vector_minus_vector(&Q, &ki); if (hkl_vector_is_null(&Q)) status = HKL_FAIL; else{ /* needed for a problem of precision */ hkl_vector_normalize(&Q); /* compute the intersection of the plan P(kf, ki) and PQ (normal Q) */ n = kf; hkl_vector_vectorial_product(&n, &ki); hkl_vector_vectorial_product(&n, &Q); /* compute hkl1 in the laboratory referentiel */ /* the geometry was already updated in the detector compute kf */ /* for now the 0 holder is the sample holder. */ hkl1.data[0] = base->parameters[0].value; hkl1.data[1] = base->parameters[1].value; hkl1.data[2] = base->parameters[2].value; hkl_vector_times_matrix(&hkl1, &sample->UB); hkl_vector_rotated_quaternion(&hkl1, &geometry->holders[0].q); /* project hkl1 on the plan of normal Q */ hkl_vector_project_on_plan(&hkl1, &Q); if (hkl_vector_is_null(&hkl1)) status = HKL_FAIL; else /* compute the angle beetween hkl1 and n */ ((HklParameter *)engine->pseudoAxes[0])->value = hkl_vector_oriented_angle(&n, &hkl1, &Q); } return status; }
/** * hkl_vector_project_on_plan: (skip) * @self: the vector to project * @normal: the normal of the plane. * * project an #HklVector on a plan of normal which contain * the origin [0, 0, 0] * **/ void hkl_vector_project_on_plan(HklVector *self, const HklVector *normal) { HklVector tmp; if(!self || !normal) return; tmp = *normal; hkl_vector_normalize(&tmp); hkl_vector_times_double(&tmp, hkl_vector_scalar_product(self, &tmp)); hkl_vector_minus_vector(self, &tmp); }
/** * hkl_vector_project_on_plan_with_point: (skip) * @self: the vector to project (modify) * @normal: the normal of the plane. * @point: a point of the plan. * * project an #HklVector on a plan of normal #normal which contain #point. **/ void hkl_vector_project_on_plan_with_point(HklVector *self, const HklVector *normal, const HklVector *point) { HklVector tmp; double d1, d2; if(!self || !normal || !point) return; tmp = *normal; hkl_vector_normalize(&tmp); d1 = hkl_vector_scalar_product(self, &tmp); d2 = hkl_vector_scalar_product(point, &tmp); hkl_vector_times_double(&tmp, d1 - d2); hkl_vector_minus_vector(self, &tmp); }
static int psi_func(const gsl_vector *x, void *params, gsl_vector *f) { HklVector dhkl0, hkl1; HklVector ki, kf, Q, n; HklMatrix RUB; HklPseudoAxisEngine *engine; HklPseudoAxisEngineModePsi *modepsi; HklPseudoAxis *psi; HklHolder *holder; size_t i; size_t len; double const *x_data = gsl_vector_const_ptr(x, 0); double *f_data = gsl_vector_ptr(f, 0); engine = params; modepsi = (HklPseudoAxisEngineModePsi *)engine->mode; psi = engine->pseudoAxes[0]; /* update the workspace from x; */ len = HKL_LIST_LEN(engine->axes); for(i=0; i<len; ++i) hkl_axis_set_value(engine->axes[i], x_data[i]); hkl_geometry_update(engine->geometry); /* kf - ki = Q */ hkl_source_compute_ki(&engine->geometry->source, &ki); hkl_detector_compute_kf(engine->detector, engine->geometry, &kf); Q = kf; hkl_vector_minus_vector(&Q, &ki); if (hkl_vector_is_null(&Q)){ f_data[0] = 1; f_data[1] = 1; f_data[2] = 1; f_data[3] = 1; }else{ /* R * UB */ /* for now the 0 holder is the sample holder. */ holder = &engine->geometry->holders[0]; hkl_quaternion_to_matrix(&holder->q, &RUB); hkl_matrix_times_matrix(&RUB, &engine->sample->UB); /* compute dhkl0 */ hkl_matrix_solve(&RUB, &dhkl0, &Q); hkl_vector_minus_vector(&dhkl0, &modepsi->hkl0); /* compute the intersection of the plan P(kf, ki) and PQ (normal Q) */ /* * now that dhkl0 have been computed we can use a * normalized Q to compute n and psi */ hkl_vector_normalize(&Q); n = kf; hkl_vector_vectorial_product(&n, &ki); hkl_vector_vectorial_product(&n, &Q); /* compute hkl1 in the laboratory referentiel */ /* for now the 0 holder is the sample holder. */ hkl1.data[0] = engine->mode->parameters[0].value; hkl1.data[1] = engine->mode->parameters[1].value; hkl1.data[2] = engine->mode->parameters[2].value; hkl_vector_times_matrix(&hkl1, &engine->sample->UB); hkl_vector_rotated_quaternion(&hkl1, &engine->geometry->holders[0].q); /* project hkl1 on the plan of normal Q */ hkl_vector_project_on_plan(&hkl1, &Q); if (hkl_vector_is_null(&hkl1)){ /* hkl1 colinear with Q */ f_data[0] = dhkl0.data[0]; f_data[1] = dhkl0.data[1]; f_data[2] = dhkl0.data[2]; f_data[3] = 1; }else{ f_data[0] = dhkl0.data[0]; f_data[1] = dhkl0.data[1]; f_data[2] = dhkl0.data[2]; f_data[3] = psi->parent.value - hkl_vector_oriented_angle(&n, &hkl1, &Q); } } return GSL_SUCCESS; }