/**
* hkl_vector_oriented_angle_points: (skip)
* @self: the first point
* @p2: the second point
* @p3: the third point
* @ref: the reference #HklVector
*
* compute the angles beetween three points (p1, p2, p3) 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_points(const HklVector *self,
const HklVector *p2,
const HklVector *p3,
const HklVector *ref)
{
HklVector v1;
HklVector v2;
v1 = *self;
v2 = *p3;
hkl_vector_minus_vector(&v1, p2);
hkl_vector_minus_vector(&v2, p2);
return hkl_vector_oriented_angle(&v1, &v2, ref);
}
/**
* hkl_vector_rotated_around_line: (skip)
* @self: the point to rotate around a line
* @angle: the angle of the rotation
* @c1: the fist point of the line
* @c2: the second point of the line
*
* This method rotate a point around a line defined by two points
* of a certain amount of angle. The rotation is right handed.
* this mean that c2 - c1 gives the direction of the rotation.
**/
void hkl_vector_rotated_around_line(HklVector *self, double angle,
const HklVector *c1, const HklVector *c2)
{
HklVector axis;
if (!self || !c1 || !c2 || fabs(angle) < HKL_EPSILON)
return;
axis = *c2;
hkl_vector_minus_vector(&axis, c1);
/* the c2 - c1 vector must be non null */
hkl_vector_minus_vector(self, c1);
hkl_vector_rotated_around_vector(self, &axis, angle);
hkl_vector_add_vector(self, c1);
}
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 hkl_pseudo_axis_engine_mode_init_psi_real(HklPseudoAxisEngineMode *base,
HklPseudoAxisEngine *engine,
HklGeometry *geometry,
HklDetector *detector,
HklSample *sample,
HklError **error)
{
int status = HKL_SUCCESS;
HklVector ki;
HklMatrix RUB;
HklPseudoAxisEngineModePsi *self = (HklPseudoAxisEngineModePsi *)base;
HklHolder *holder;
status = hkl_pseudo_axis_engine_init_func(base, engine, geometry, detector, sample);
if (status == HKL_FAIL)
return status;
/* update the geometry internals */
hkl_geometry_update(geometry);
/* R * UB */
/* for now the 0 holder is the sample holder. */
holder = &geometry->holders[0];
hkl_quaternion_to_matrix(&holder->q, &RUB);
hkl_matrix_times_matrix(&RUB, &sample->UB);
/* kf - ki = Q0 */
hkl_source_compute_ki(&geometry->source, &ki);
hkl_detector_compute_kf(detector, geometry, &self->Q0);
hkl_vector_minus_vector(&self->Q0, &ki);
if (hkl_vector_is_null(&self->Q0))
status = HKL_FAIL;
else
/* compute hkl0 */
hkl_matrix_solve(&RUB, &self->hkl0, &self->Q0);
return status;
}
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;
}
