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>
26 #include <hkl/hkl-macros.h>
27 #include <hkl/hkl-vector.h>
28 #include <hkl/hkl-matrix.h>
29 #include <hkl/hkl-quaternion.h>
34 * hkl_quaternion_init:
35 * @self: the #HklQuaternion to initialize
36 * @a: the 1st element value
37 * @b: the 2nd element value
38 * @c: the 3rd element value
39 * @d: the 4th element value
41 * initialize the four elements of an #HklQuaternion
43 void hkl_quaternion_init(HklQuaternion
*self
,
44 double a
, double b
, double c
, double d
)
53 * hkl_quaternion_fprintf:
54 * @file: the file to send the #HklQuaternion into
55 * @self: the #HklQuaternion to write into the file stream.
57 * print an #HklQuaternion into a FILE stream
59 void hkl_quaternion_fprintf(FILE *file
, HklQuaternion
const *self
)
64 fprintf(file
, "<%f, %f, %f, %f>", Q
[0], Q
[1], Q
[2], Q
[3]);
68 * hkl_quaternion_init_from_vector:
69 * @self: the #HklQuaternion to set
70 * @v: the #HklVector used to set the self #HklQuaternion
72 * initialize an #HklQuaternion from an #HklVector
74 void hkl_quaternion_init_from_vector(HklQuaternion
*self
, HklVector
const *v
)
77 memcpy(&self
->data
[1], &v
->data
[0], sizeof(v
->data
));
81 * hkl_quaternion_init_from_angle_and_axe:
82 * @self: the #HklQuaternion to set
83 * @angle: the angles of the rotation
84 * @v: the axe of rotation
86 * initialize an #HklQuaternion from a vector and a angle.
88 void hkl_quaternion_init_from_angle_and_axe(HklQuaternion
*self
,
89 double angle
, HklVector
const *v
)
95 /* check that parameters are ok. */
96 norm
= hkl_vector_norm2(v
);
99 s
= sin(angle
/ 2.) / norm
;
102 self
->data
[1] = s
* v
->data
[0];
103 self
->data
[2] = s
* v
->data
[1];
104 self
->data
[3] = s
* v
->data
[2];
108 * hkl_quaternion_cmp:
109 * @self: the first #HklQuaternion
110 * @q: the second #HklQuaternion
112 * compare two #HklQuaternion.
114 * Returns: #HKL_TRUE if both are equal, #HKL_FAIL otherwise.
116 int hkl_quaternion_cmp(HklQuaternion
const *self
, HklQuaternion
const *q
)
121 if ( fabs(self
->data
[i
] - q
->data
[i
]) > HKL_EPSILON
)
127 * hkl_quaternion_minus_quaternion:
128 * @self: the #HklQuaternion to modify.
129 * @q: the #HklQuaternion to substract
131 * substract two #HklQuaternions
134 void hkl_quaternion_minus_quaternion(HklQuaternion
*self
, const HklQuaternion
*q
)
139 self
->data
[i
] -= q
->data
[i
];
143 * hkl_quaternion_times_quaternion:
144 * @self: the #HklQuaternion to modify
145 * @q: the #HklQuaternion to multiply by
147 * multiply two quaternions
149 void hkl_quaternion_times_quaternion(HklQuaternion
*self
, const HklQuaternion
*q
)
157 self
->data
[0] = Q
[0]*Q
[0] - Q
[1]*Q
[1] - Q
[2]*Q
[2] - Q
[3]*Q
[3];
158 self
->data
[1] = Q
[0]*Q
[1] + Q
[1]*Q
[0] + Q
[2]*Q
[3] - Q
[3]*Q
[2];
159 self
->data
[2] = Q
[0]*Q
[2] - Q
[1]*Q
[3] + Q
[2]*Q
[0] + Q
[3]*Q
[1];
160 self
->data
[3] = Q
[0]*Q
[3] + Q
[1]*Q
[2] - Q
[2]*Q
[1] + Q
[3]*Q
[0];
162 double const *Q1
= q
->data
;
163 self
->data
[0] = Q
[0]*Q1
[0] - Q
[1]*Q1
[1] - Q
[2]*Q1
[2] - Q
[3]*Q1
[3];
164 self
->data
[1] = Q
[0]*Q1
[1] + Q
[1]*Q1
[0] + Q
[2]*Q1
[3] - Q
[3]*Q1
[2];
165 self
->data
[2] = Q
[0]*Q1
[2] - Q
[1]*Q1
[3] + Q
[2]*Q1
[0] + Q
[3]*Q1
[1];
166 self
->data
[3] = Q
[0]*Q1
[3] + Q
[1]*Q1
[2] - Q
[2]*Q1
[1] + Q
[3]*Q1
[0];
171 * hkl_quaternion_norm2:
172 * @self: the quaternion use to compute the norm
174 * compute the norm2 of an #HklQuaternion
176 * Returns: the self #hklquaternion norm
178 double hkl_quaternion_norm2(const HklQuaternion
*self
)
183 sum2
+= self
->data
[i
] *self
->data
[i
];
188 * hkl_quaternion_conjugate:
189 * @self: the #HklQuaternion to conjugate
191 * compute the conjugate of a quaternion
193 void hkl_quaternion_conjugate(HklQuaternion
*self
)
197 self
->data
[i
] = -self
->data
[i
];
201 * hkl_quaternion_to_matrix:
202 * @self: the #HklQuaternion use to compute the #HklMatrix
203 * @m: the #HklMatrix return.
205 * Compute the rotation matrix of a Quaternion.
207 * compute the rotation matrix corresponding to the unitary quaternion.
208 * \f$ q = a + b \cdot i + c \cdot j + d \cdot k \f$
213 * a^2+b^2-c^2-d^2 & 2bc-2ad & 2ac+2bd\\
214 * 2ad+2bc & a^2-b^2+c^2-d^2 & 2cd-2ab\\
215 * 2bd-2ac & 2ab+2cd & a^2-b^2-c^2+d^2
221 void hkl_quaternion_to_matrix(const HklQuaternion
*self
, HklMatrix
*m
)
225 /* check that parameters are ok. */
226 hkl_assert(fabs(hkl_quaternion_norm2(self
) - 1) < HKL_EPSILON
);
230 m
->data
[0][0] = Q
[0]*Q
[0] + Q
[1]*Q
[1] - Q
[2]*Q
[2] - Q
[3]*Q
[3];
231 m
->data
[0][1] = 2 * (Q
[1]*Q
[2] - Q
[0]*Q
[3]);
232 m
->data
[0][2] = 2 * (Q
[0]*Q
[2] + Q
[1]*Q
[3]);
234 m
->data
[1][0] = 2 * (Q
[0]*Q
[3] + Q
[1]*Q
[2]);
235 m
->data
[1][1] = Q
[0]*Q
[0] - Q
[1]*Q
[1] + Q
[2]*Q
[2] - Q
[3]*Q
[3];
236 m
->data
[1][2] = 2 * (Q
[2]*Q
[3] - Q
[0]*Q
[1]);
238 m
->data
[2][0] = 2 * (Q
[1]*Q
[3] - Q
[0]*Q
[2]);
239 m
->data
[2][1] = 2 * (Q
[0]*Q
[1] + Q
[2]*Q
[3]);
240 m
->data
[2][2] = Q
[0]*Q
[0] - Q
[1]*Q
[1] - Q
[2]*Q
[2] + Q
[3]*Q
[3];
244 * hkl_quaternion_to_angle_and_axe:
245 * @self: The #HklQuaternion use to compute the angle and the roation axis.
246 * @angle: the returned angle of the rotation.
247 * @v: the returned axis of the rotation.
249 * compute the axe and angle of the unitary quaternion angle [-pi, pi]
250 * if q is the (1, 0, 0, 0) quaternion return the (0,0,0) axe and a 0 angle
252 void hkl_quaternion_to_angle_and_axe(HklQuaternion
const *self
,
253 double *angle
, HklVector
*v
)
259 /* check that parameters are ok. (norm must be equal to 1) */
260 hkl_assert(fabs(hkl_quaternion_norm2(self
) - 1) < HKL_EPSILON
);
262 /* compute the angle */
263 cos_angle_2
= self
->data
[0];
264 angle_2
= acos(cos_angle_2
);
266 /* we want an angle between -pi, pi */
267 if (*angle
> M_PI
) *angle
-= 2 *M_PI
;
269 /* compute the axe */
270 sin_angle_2
= sin(angle_2
);
271 if (fabs(sin_angle_2
) > HKL_EPSILON
) {
272 /* compute the axe using the vector part of the unitary quaterninon */
273 memcpy(v
->data
, &self
->data
[1], sizeof(v
->data
));
274 hkl_vector_div_double(v
, sin_angle_2
);
277 memset(v
->data
, 0, sizeof(v
->data
));