1 |
|
|
/* |
2 |
|
|
Teem: Tools to process and visualize scientific data and images . |
3 |
|
|
Copyright (C) 2013, 2012, 2011, 2010, 2009 University of Chicago |
4 |
|
|
Copyright (C) 2008, 2007, 2006, 2005 Gordon Kindlmann |
5 |
|
|
Copyright (C) 2004, 2003, 2002, 2001, 2000, 1999, 1998 University of Utah |
6 |
|
|
|
7 |
|
|
This library is free software; you can redistribute it and/or |
8 |
|
|
modify it under the terms of the GNU Lesser General Public License |
9 |
|
|
(LGPL) as published by the Free Software Foundation; either |
10 |
|
|
version 2.1 of the License, or (at your option) any later version. |
11 |
|
|
The terms of redistributing and/or modifying this software also |
12 |
|
|
include exceptions to the LGPL that facilitate static linking. |
13 |
|
|
|
14 |
|
|
This library is distributed in the hope that it will be useful, |
15 |
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of |
16 |
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
17 |
|
|
Lesser General Public License for more details. |
18 |
|
|
|
19 |
|
|
You should have received a copy of the GNU Lesser General Public License |
20 |
|
|
along with this library; if not, write to Free Software Foundation, Inc., |
21 |
|
|
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA |
22 |
|
|
*/ |
23 |
|
|
|
24 |
|
|
#include "ell.h" |
25 |
|
|
|
26 |
|
|
#define W 0 |
27 |
|
|
#define X 1 |
28 |
|
|
#define Y 2 |
29 |
|
|
#define Z 3 |
30 |
|
|
|
31 |
|
|
/* |
32 |
|
|
0 1 2 |
33 |
|
|
3 4 5 |
34 |
|
|
6 7 8 |
35 |
|
|
|
36 |
|
|
0 1 2 3 |
37 |
|
|
4 5 6 7 |
38 |
|
|
8 9 10 11 |
39 |
|
|
12 13 14 15 |
40 |
|
|
*/ |
41 |
|
|
|
42 |
|
|
/* |
43 |
|
|
** note: this will always produce a unit length quaternion, by the |
44 |
|
|
** ELL_4V_NORM(q, q, len) at the end (NOTE: actually that's been |
45 |
|
|
** expanded out to deal with warnings about precision loss with |
46 |
|
|
** double->float conversion). However, for proper rotation matrices, |
47 |
|
|
** that normalization should be far from a divide by zero, so it |
48 |
|
|
** should be stable. It *IS* necessary, since it accomplishes |
49 |
|
|
** division by w, x, y, or z, whichever's squared magnitude is biggest |
50 |
|
|
*/ |
51 |
|
|
#define _ELL_M_TO_Q(type, i0, i1, i2, i3, i4, i5, i6, i7, i8) \ |
52 |
|
|
type s[4], wx, wy, wz, xy, xz, yz, len; \ |
53 |
|
|
int mi; \ |
54 |
|
|
\ |
55 |
|
|
s[W] = 1 + m[i0] + m[i4] + m[i8]; \ |
56 |
|
|
s[X] = 1 + m[i0] - m[i4] - m[i8]; \ |
57 |
|
|
s[Y] = 1 - m[i0] + m[i4] - m[i8]; \ |
58 |
|
|
s[Z] = 1 - m[i0] - m[i4] + m[i8]; \ |
59 |
|
|
wx = m[i7] - m[i5]; \ |
60 |
|
|
wy = m[i2] - m[i6]; \ |
61 |
|
|
wz = m[i3] - m[i1]; \ |
62 |
|
|
xy = m[i3] + m[i1]; \ |
63 |
|
|
xz = m[i6] + m[i2]; \ |
64 |
|
|
yz = m[i7] + m[i5]; \ |
65 |
|
|
mi = s[W] > s[X] ? W : X; \ |
66 |
|
|
mi = s[mi] > s[Y] ? mi : Y; \ |
67 |
|
|
mi = s[mi] > s[Z] ? mi : Z; \ |
68 |
|
|
switch (mi) { \ |
69 |
|
|
case W: \ |
70 |
|
|
ELL_4V_SET(q, s[W], wx, wy, wz); \ |
71 |
|
|
break; \ |
72 |
|
|
case X: \ |
73 |
|
|
ELL_4V_SET(q, wx, s[X], xy, xz); \ |
74 |
|
|
break; \ |
75 |
|
|
case Y: \ |
76 |
|
|
ELL_4V_SET(q, wy, xy, s[Y], yz); \ |
77 |
|
|
break; \ |
78 |
|
|
case Z: \ |
79 |
|
|
ELL_4V_SET(q, wz, xz, yz, s[Z]); \ |
80 |
|
|
break; \ |
81 |
|
|
} |
82 |
|
|
|
83 |
|
|
void |
84 |
|
|
ell_3m_to_q_f(float q[4], const float m[9]) { |
85 |
|
|
_ELL_M_TO_Q( float, 0, 1, 2, 3, 4, 5, 6, 7, 8); |
86 |
|
|
len = AIR_CAST(float, ELL_4V_LEN(q)); |
87 |
|
|
ELL_4V_SCALE(q, 1.0f/len, q); |
88 |
|
|
} |
89 |
|
|
|
90 |
|
|
void |
91 |
|
|
ell_3m_to_q_d(double q[4], const double m[9]) { |
92 |
|
|
_ELL_M_TO_Q(double, 0, 1, 2, 3, 4, 5, 6, 7, 8); |
93 |
|
|
ELL_4V_NORM(q, q, len); |
94 |
|
|
} |
95 |
|
|
|
96 |
|
|
void |
97 |
|
|
ell_4m_to_q_f(float q[4], const float m[16]) { |
98 |
|
|
_ELL_M_TO_Q( float, 0, 1, 2, 4, 5, 6, 8, 9, 10); |
99 |
|
|
len = AIR_CAST(float, ELL_4V_LEN(q)); |
100 |
|
|
ELL_4V_SCALE(q, 1.0f/len, q); |
101 |
|
|
} |
102 |
|
|
|
103 |
|
|
void |
104 |
|
|
ell_4m_to_q_d(double q[4], const double m[16]) { |
105 |
|
|
_ELL_M_TO_Q(double, 0, 1, 2, 4, 5, 6, 8, 9, 10); |
106 |
|
|
ELL_4V_NORM(q, q, len); |
107 |
|
|
} |
108 |
|
|
|
109 |
|
|
/* |
110 |
|
|
** note: normalizes the quaternion on the way in, to insure |
111 |
|
|
** creation of a proper rotation matrix. Without the normalization |
112 |
|
|
** the coefficients in the matrix would be off by a factor of |
113 |
|
|
** w*w + x*x + y*y + z*z |
114 |
|
|
** |
115 |
|
|
** See NOTE below about the non-use of ELL_4V_NORM(u, q, w) |
116 |
|
|
*/ |
117 |
|
|
#define _ELL_Q_TO_3M(type) \ |
118 |
|
|
ELL_4V_GET(w, x, y, z, u); \ |
119 |
|
|
ELL_3V_SET(m+0, \ |
120 |
|
|
w*w + x*x - y*y - z*z, \ |
121 |
|
|
2*(x*y - w*z), \ |
122 |
|
|
2*(x*z + w*y)); \ |
123 |
|
|
ELL_3V_SET(m+3, \ |
124 |
|
|
2*(x*y + w*z), \ |
125 |
|
|
w*w - x*x + y*y - z*z, \ |
126 |
|
|
2*(y*z - w*x)); \ |
127 |
|
|
ELL_3V_SET(m+6, \ |
128 |
|
|
2*(x*z - w*y), \ |
129 |
|
|
2*(y*z + w*x), \ |
130 |
|
|
w*w - x*x - y*y + z*z) |
131 |
|
|
|
132 |
|
|
void |
133 |
|
|
ell_q_to_3m_f(float m[9], const float q[4]) { |
134 |
|
|
float u[4], w=0.0, x=0.0, y=0.0, z=0.0; |
135 |
|
|
w = AIR_CAST(float, ELL_4V_LEN(q)); |
136 |
|
|
ELL_4V_SCALE(u, 1.0f/w, q); |
137 |
|
|
_ELL_Q_TO_3M(float); |
138 |
|
|
} |
139 |
|
|
|
140 |
|
|
void |
141 |
|
|
ell_q_to_3m_d(double m[9], const double q[4]) { |
142 |
|
|
double u[4], w=0.0, x=0.0, y=0.0, z=0.0; |
143 |
|
32 |
ELL_4V_NORM(u, q, w); |
144 |
|
16 |
_ELL_Q_TO_3M(double); |
145 |
|
16 |
} |
146 |
|
|
|
147 |
|
|
/* |
148 |
|
|
** HEY: the first two lines of this replace ELL_4V_NORM(u, q, w). The |
149 |
|
|
** replacement was needed to avoid warnings about precision loss with |
150 |
|
|
** double->float converstion. Macros are indeed problematic . . . |
151 |
|
|
*/ |
152 |
|
|
#define _ELL_Q_TO_4M(type) \ |
153 |
|
|
ELL_4V_GET(w, x, y, z, u); \ |
154 |
|
|
ELL_4V_SET(m+0, \ |
155 |
|
|
w*w + x*x - y*y - z*z, \ |
156 |
|
|
2*(x*y - w*z), \ |
157 |
|
|
2*(x*z + w*y), \ |
158 |
|
|
0); \ |
159 |
|
|
ELL_4V_SET(m+4, \ |
160 |
|
|
2*(x*y + w*z), \ |
161 |
|
|
w*w - x*x + y*y - z*z, \ |
162 |
|
|
2*(y*z - w*x), \ |
163 |
|
|
0); \ |
164 |
|
|
ELL_4V_SET(m+8, \ |
165 |
|
|
2*(x*z - w*y), \ |
166 |
|
|
2*(y*z + w*x), \ |
167 |
|
|
w*w - x*x - y*y + z*z, \ |
168 |
|
|
0); \ |
169 |
|
|
ELL_4V_SET(m+12, 0, 0, 0, 1) |
170 |
|
|
|
171 |
|
|
void |
172 |
|
|
ell_q_to_4m_f(float m[16], const float q[4]) { |
173 |
|
|
float u[4], w=0.0, x=0.0, y=0.0, z=0.0; |
174 |
|
|
w = AIR_CAST(float, ELL_3V_LEN(q)); |
175 |
|
|
ELL_4V_SCALE(u, 1.0f/w, q); |
176 |
|
|
_ELL_Q_TO_4M(float); |
177 |
|
|
} |
178 |
|
|
|
179 |
|
|
void |
180 |
|
|
ell_q_to_4m_d(double m[16], const double q[4]) { |
181 |
|
|
double u[4], w=0.0, x=0.0, y=0.0, z=0.0; |
182 |
|
|
ELL_4V_NORM(u, q, w); |
183 |
|
|
_ELL_Q_TO_4M(double); |
184 |
|
|
} |
185 |
|
|
|
186 |
|
|
/* |
187 |
|
|
** note: by the use of atan2, this does NOT assume a |
188 |
|
|
** a unit-length quaternion. The axis output, however, |
189 |
|
|
** will always be unit length, even if the quaternion was |
190 |
|
|
** purely real (rotation angle is zero) |
191 |
|
|
** |
192 |
|
|
** HEY: there are two instances here of non-use of ELL_3V_NORM |
193 |
|
|
** to avoid warnings about precision loss with type conversion |
194 |
|
|
*/ |
195 |
|
|
#define _ELL_Q_TO_AA(type) \ |
196 |
|
|
type len, angle; \ |
197 |
|
|
\ |
198 |
|
|
len = AIR_CAST(type, ELL_3V_LEN(q+1)); \ |
199 |
|
|
angle = AIR_CAST(type, atan2(len, q[0])); \ |
200 |
|
|
if (len) { \ |
201 |
|
|
ELL_3V_SCALE(axis, 1.0f/len, q+1); \ |
202 |
|
|
len = AIR_CAST(type, ELL_3V_LEN(axis)); \ |
203 |
|
|
ELL_3V_SCALE(axis, 1.0f/len, axis); \ |
204 |
|
|
} else { \ |
205 |
|
|
ELL_3V_SET(axis, 1, 0, 0); \ |
206 |
|
|
} \ |
207 |
|
|
return 2*angle |
208 |
|
|
|
209 |
|
|
float |
210 |
|
|
ell_q_to_aa_f(float axis[3], const float q[4]) { |
211 |
|
|
_ELL_Q_TO_AA(float); |
212 |
|
|
} |
213 |
|
|
|
214 |
|
|
double |
215 |
|
|
ell_q_to_aa_d(double axis[3], const double q[4]) { |
216 |
|
|
_ELL_Q_TO_AA(double); |
217 |
|
|
} |
218 |
|
|
|
219 |
|
|
/* |
220 |
|
|
** note: assuming that axis is unit length, this produces a |
221 |
|
|
** a unit length quaternion |
222 |
|
|
*/ |
223 |
|
|
#define _ELL_AA_TO_Q(type) \ |
224 |
|
|
type sa; \ |
225 |
|
|
\ |
226 |
|
|
sa = AIR_CAST(type, sin(angle/2)); \ |
227 |
|
|
ELL_4V_SET(q, \ |
228 |
|
|
AIR_CAST(type, cos(angle/2)), AIR_CAST(type, sa*axis[0]), \ |
229 |
|
|
AIR_CAST(type, sa*axis[1]), AIR_CAST(type, sa*axis[2])) |
230 |
|
|
|
231 |
|
|
void |
232 |
|
|
ell_aa_to_q_f(float q[4], const float angle, const float axis[3]) { |
233 |
|
|
_ELL_AA_TO_Q(float); |
234 |
|
|
} |
235 |
|
|
|
236 |
|
|
void |
237 |
|
|
ell_aa_to_q_d(double q[4], const double angle, const double axis[3]) { |
238 |
|
|
_ELL_AA_TO_Q(double); |
239 |
|
|
} |
240 |
|
|
|
241 |
|
|
float |
242 |
|
|
ell_3m_to_aa_f( float axis[3], const float m[9]) { |
243 |
|
|
float q[4]; |
244 |
|
|
|
245 |
|
|
ell_3m_to_q_f(q, m); |
246 |
|
|
return ell_q_to_aa_f(axis, q); |
247 |
|
|
} |
248 |
|
|
|
249 |
|
|
double |
250 |
|
|
ell_3m_to_aa_d(double axis[3], const double m[9]) { |
251 |
|
|
double q[4]; |
252 |
|
|
|
253 |
|
|
ell_3m_to_q_d(q, m); |
254 |
|
|
return ell_q_to_aa_d(axis, q); |
255 |
|
|
} |
256 |
|
|
|
257 |
|
|
float |
258 |
|
|
ell_4m_to_aa_f( float axis[3], const float m[16]) { |
259 |
|
|
float q[4]; |
260 |
|
|
|
261 |
|
|
ell_4m_to_q_f(q, m); |
262 |
|
|
return ell_q_to_aa_f(axis, q); |
263 |
|
|
} |
264 |
|
|
|
265 |
|
|
double |
266 |
|
|
ell_4m_to_aa_d(double axis[3], const double m[16]) { |
267 |
|
|
double q[4]; |
268 |
|
|
|
269 |
|
|
ell_4m_to_q_d(q, m); |
270 |
|
|
return ell_q_to_aa_d(axis, q); |
271 |
|
|
} |
272 |
|
|
|
273 |
|
|
void |
274 |
|
|
ell_aa_to_3m_f( float m[9], const float angle, const float axis[3]) { |
275 |
|
|
float q[4]; |
276 |
|
|
|
277 |
|
|
ell_aa_to_q_f(q, angle, axis); |
278 |
|
|
ell_q_to_3m_f(m, q); |
279 |
|
|
} |
280 |
|
|
|
281 |
|
|
void |
282 |
|
|
ell_aa_to_3m_d(double m[9], const double angle, const double axis[3]) { |
283 |
|
|
double q[4]; |
284 |
|
|
|
285 |
|
|
ell_aa_to_q_d(q, angle, axis); |
286 |
|
|
ell_q_to_3m_d(m, q); |
287 |
|
|
} |
288 |
|
|
|
289 |
|
|
void |
290 |
|
|
ell_aa_to_4m_f( float m[16], const float angle, const float axis[3]) { |
291 |
|
|
float q[4]; |
292 |
|
|
|
293 |
|
|
ell_aa_to_q_f(q, angle, axis); |
294 |
|
|
ell_q_to_4m_f(m, q); |
295 |
|
|
} |
296 |
|
|
|
297 |
|
|
void |
298 |
|
|
ell_aa_to_4m_d(double m[16], const double angle, const double axis[3]) { |
299 |
|
|
double q[4]; |
300 |
|
|
|
301 |
|
|
ell_aa_to_q_d(q, angle, axis); |
302 |
|
|
ell_q_to_4m_d(m, q); |
303 |
|
|
} |
304 |
|
|
|
305 |
|
|
void |
306 |
|
|
ell_q_mul_f(float q3[4], const float q1[4], const float q2[4]) { |
307 |
|
|
ELL_Q_MUL(q3, q1, q2); |
308 |
|
|
} |
309 |
|
|
|
310 |
|
|
void |
311 |
|
|
ell_q_mul_d(double q3[4], const double q1[4], const double q2[4]) { |
312 |
|
|
ELL_Q_MUL(q3, q1, q2); |
313 |
|
|
} |
314 |
|
|
|
315 |
|
|
void |
316 |
|
|
ell_q_inv_f(float qi[4], const float q[4]) { |
317 |
|
|
float N; |
318 |
|
|
ELL_Q_INV(qi, q, N); |
319 |
|
|
} |
320 |
|
|
|
321 |
|
|
void |
322 |
|
|
ell_q_inv_d(double qi[4], const double q[4]) { |
323 |
|
|
double N; |
324 |
|
|
ELL_Q_INV(qi, q, N); |
325 |
|
|
} |
326 |
|
|
|
327 |
|
|
/* |
328 |
|
|
** div(a, b) = a^-1 * b |
329 |
|
|
*/ |
330 |
|
|
void |
331 |
|
|
ell_q_div_f(float q3[4], const float q1[4], const float q2[4]) { |
332 |
|
|
float N, q1i[4]; |
333 |
|
|
|
334 |
|
|
ELL_Q_INV(q1i, q1, N); |
335 |
|
|
ELL_Q_MUL(q3, q1i, q2); |
336 |
|
|
} |
337 |
|
|
|
338 |
|
|
void |
339 |
|
|
ell_q_div_d(double q3[4], const double q1[4], const double q2[4]) { |
340 |
|
|
double N, q1i[4]; |
341 |
|
|
|
342 |
|
|
ELL_Q_INV(q1i, q1, N); |
343 |
|
|
ELL_Q_MUL(q3, q1i, q2); |
344 |
|
|
} |
345 |
|
|
|
346 |
|
|
/* |
347 |
|
|
** this is good for *ALL* quaternions, any length, including zero. |
348 |
|
|
** the behavior on the zero quaternion is governed by the behavior |
349 |
|
|
** of the log() and atan2() functions in the math library |
350 |
|
|
** |
351 |
|
|
** the basic insight is that doing conversion to angle/axis, |
352 |
|
|
** and doing the atan2(l2(x,y,z),w), |
353 |
|
|
** and that doing a logarithm, are all basically the same thing |
354 |
|
|
*/ |
355 |
|
|
|
356 |
|
|
void |
357 |
|
|
ell_q_log_f(float q2[4], const float q1[4]) { |
358 |
|
|
float a, b, axis[3]; |
359 |
|
|
|
360 |
|
|
a = AIR_CAST(float, log(ELL_4V_LEN(q1))); |
361 |
|
|
b = ell_q_to_aa_f(axis, q1)/2.0f; |
362 |
|
|
ELL_4V_SET(q2, a, b*axis[0], b*axis[1], b*axis[2]); |
363 |
|
|
} |
364 |
|
|
|
365 |
|
|
void |
366 |
|
|
ell_q_log_d(double q2[4], const double q1[4]) { |
367 |
|
|
double a, b, axis[3]; |
368 |
|
|
|
369 |
|
|
a = log(ELL_4V_LEN(q1)); |
370 |
|
|
b = ell_q_to_aa_d(axis, q1)/2.0; |
371 |
|
|
ELL_4V_SET(q2, a, b*axis[0], b*axis[1], b*axis[2]); |
372 |
|
|
} |
373 |
|
|
|
374 |
|
|
/* |
375 |
|
|
** this is good for *ALL* quaternions, any length, including zero |
376 |
|
|
** NOTE: one non-use of ELL_3V_NORM to avoid warnings about |
377 |
|
|
** precision loss from type conversion |
378 |
|
|
*/ |
379 |
|
|
#define _ELL_Q_EXP(type) \ |
380 |
|
|
type ea, b, sb, axis[3], tmp; \ |
381 |
|
|
\ |
382 |
|
|
ea = AIR_CAST(type, exp(q1[0])); \ |
383 |
|
|
b = AIR_CAST(type, ELL_3V_LEN(q1+1)); \ |
384 |
|
|
if (b) { \ |
385 |
|
|
ELL_3V_SCALE(axis, 1.0f/b, q1+1); \ |
386 |
|
|
tmp = AIR_CAST(type, ELL_3V_LEN(axis)); \ |
387 |
|
|
ELL_3V_SCALE(axis, 1.0f/tmp, axis); \ |
388 |
|
|
} else { \ |
389 |
|
|
ELL_3V_SET(axis, 1.0f, 0.0f, 0.0f); \ |
390 |
|
|
} \ |
391 |
|
|
sb = AIR_CAST(type, sin(b)); \ |
392 |
|
|
ELL_4V_SET(q2, AIR_CAST(type, ea*cos(b)), ea*sb*axis[0], \ |
393 |
|
|
ea*sb*axis[1], ea*sb*axis[2]) |
394 |
|
|
|
395 |
|
|
void |
396 |
|
|
ell_q_exp_f(float q2[4], const float q1[4]) { |
397 |
|
|
_ELL_Q_EXP(float); |
398 |
|
|
} |
399 |
|
|
|
400 |
|
|
void |
401 |
|
|
ell_q_exp_d(double q2[4], const double q1[4]) { |
402 |
|
|
_ELL_Q_EXP(double); |
403 |
|
|
} |
404 |
|
|
|
405 |
|
|
void |
406 |
|
|
ell_q_pow_f(float q2[4], const float q1[4], const float p) { |
407 |
|
|
float len, angle, axis[3]; |
408 |
|
|
|
409 |
|
|
len = AIR_CAST(float, pow(ELL_4V_LEN(q1), p)); |
410 |
|
|
angle = ell_q_to_aa_f(axis, q1); |
411 |
|
|
ell_aa_to_q_f(q2, p*angle, axis); |
412 |
|
|
ELL_4V_SCALE(q2, len, q2); |
413 |
|
|
} |
414 |
|
|
|
415 |
|
|
void |
416 |
|
|
ell_q_pow_d(double q2[4], const double q1[4], const double p) { |
417 |
|
|
double len, angle, axis[3]; |
418 |
|
|
|
419 |
|
|
len = pow(ELL_4V_LEN(q1), p); |
420 |
|
|
angle = ell_q_to_aa_d(axis, q1); |
421 |
|
|
ell_aa_to_q_d(q2, p*angle, axis); |
422 |
|
|
ELL_4V_SCALE(q2, len, q2); |
423 |
|
|
} |
424 |
|
|
|
425 |
|
|
/* |
426 |
|
|
** by the wonders of quaternions, this rotation will be the |
427 |
|
|
** same regardless of the quaternion length. This is in |
428 |
|
|
** contrast to doing rotation by first converting to matrix, |
429 |
|
|
** in which an explicit normalization is required. There is |
430 |
|
|
** a divide here (in ELL_Q_INV), but there's no sqrt(). |
431 |
|
|
*/ |
432 |
|
|
#define _ELL_Q3V_ROT(type) \ |
433 |
|
|
type n, a[4], b[4], c[4]; \ |
434 |
|
|
\ |
435 |
|
|
ELL_4V_SET(a, 0, v1[0], v1[1], v1[2]); \ |
436 |
|
|
ELL_Q_INV(b, q, n); \ |
437 |
|
|
ELL_Q_MUL(c, a, b); \ |
438 |
|
|
ELL_Q_MUL(a, q, c); \ |
439 |
|
|
ELL_3V_COPY(v2, a+1) |
440 |
|
|
|
441 |
|
|
void |
442 |
|
|
ell_q_3v_rotate_f( float v2[3], const float q[4], const float v1[3]) { |
443 |
|
|
_ELL_Q3V_ROT(float); |
444 |
|
|
} |
445 |
|
|
|
446 |
|
|
void |
447 |
|
|
ell_q_3v_rotate_d(double v2[3], const double q[4], const double v1[3]) { |
448 |
|
|
_ELL_Q3V_ROT(double); |
449 |
|
|
} |
450 |
|
|
|
451 |
|
|
/* |
452 |
|
|
** we start by ignoring the last (homogenous) coordinate of |
453 |
|
|
** the vector, but then we copy it to the output |
454 |
|
|
*/ |
455 |
|
|
void |
456 |
|
|
ell_q_4v_rotate_f( float v2[4], const float q[4], const float v1[4]) { |
457 |
|
|
_ELL_Q3V_ROT(float); |
458 |
|
|
v2[3] = v1[3]; |
459 |
|
|
} |
460 |
|
|
|
461 |
|
|
void |
462 |
|
|
ell_q_4v_rotate_d(double v2[4], const double q[4], const double v1[4]) { |
463 |
|
|
_ELL_Q3V_ROT(double); |
464 |
|
|
v2[3] = v1[3]; |
465 |
|
|
} |
466 |
|
|
|
467 |
|
|
/* #define _ELL_Q_AVG_ITER_MAX 30 */ |
468 |
|
|
|
469 |
|
|
int |
470 |
|
|
ell_q_avg4_d(double m[4], unsigned int *iterP, |
471 |
|
|
const double _q1[4], const double _q2[4], |
472 |
|
|
const double _q3[4], const double _q4[4], |
473 |
|
|
const double _wght[4], |
474 |
|
|
const double eps, const unsigned int maxIter) { |
475 |
|
|
static const char me[]="ell_q_avg4_d"; |
476 |
|
|
double N, elen, a[4], b[4], c[4], d[4], |
477 |
|
|
tmp[4], la[4], lb[4], lc[4], ld[4], u[4], wght[4]; |
478 |
|
|
unsigned int iter; |
479 |
|
|
|
480 |
|
|
/* *iterP optional */ |
481 |
|
|
if (!( m && _q1 && _q2 && _q3 && _q4 && _wght )) { |
482 |
|
|
biffAddf(ELL, "%s: got NULL pointer", me); |
483 |
|
|
return 1; |
484 |
|
|
} |
485 |
|
|
if (!( eps >= 0 )) { |
486 |
|
|
biffAddf(ELL, "%s: need eps >= 0 (not %g)", me, eps); |
487 |
|
|
return 1; |
488 |
|
|
} |
489 |
|
|
|
490 |
|
|
/* normalize (wrt L2) all given quaternions */ |
491 |
|
|
ELL_4V_NORM(a, _q1, N); |
492 |
|
|
ELL_4V_NORM(b, _q2, N); |
493 |
|
|
ELL_4V_NORM(c, _q3, N); |
494 |
|
|
ELL_4V_NORM(d, _q4, N); |
495 |
|
|
|
496 |
|
|
/* normalize (wrt L1) the given weights */ |
497 |
|
|
ELL_4V_COPY(wght, _wght); |
498 |
|
|
N = wght[0] + wght[1] + wght[2] + wght[3]; |
499 |
|
|
ELL_4V_SCALE(wght, 1.0/N, wght); |
500 |
|
|
|
501 |
|
|
/* initialize mean to normalized euclidean mean */ |
502 |
|
|
ELL_4V_SCALE_ADD4(m, wght[0], a, wght[1], b, wght[2], c, wght[3], d); |
503 |
|
|
ELL_4V_NORM(m, m, N); |
504 |
|
|
|
505 |
|
|
iter = 0; |
506 |
|
|
do { |
507 |
|
|
/* take log of everyone */ |
508 |
|
|
ell_q_div_d(tmp, m, a); ell_q_log_d(la, tmp); |
509 |
|
|
ell_q_div_d(tmp, m, b); ell_q_log_d(lb, tmp); |
510 |
|
|
ell_q_div_d(tmp, m, c); ell_q_log_d(lc, tmp); |
511 |
|
|
ell_q_div_d(tmp, m, d); ell_q_log_d(ld, tmp); |
512 |
|
|
/* average, and find length */ |
513 |
|
|
ELL_4V_SCALE_ADD4(u, wght[0], la, wght[1], lb, wght[2], lc, wght[3], ld); |
514 |
|
|
elen = ELL_4V_LEN(u); |
515 |
|
|
/* use exp to put it back on S^3 */ |
516 |
|
|
ell_q_exp_d(tmp, u); ell_q_mul_d(m, m, tmp); |
517 |
|
|
iter++; |
518 |
|
|
} while ((!maxIter || iter < maxIter) && elen > eps); |
519 |
|
|
if (elen > eps) { |
520 |
|
|
biffAddf(ELL, "%s: still have error %g after max %d iters", me, |
521 |
|
|
elen, maxIter); |
522 |
|
|
return 1; |
523 |
|
|
} |
524 |
|
|
|
525 |
|
|
if (iterP) { |
526 |
|
|
*iterP = iter; |
527 |
|
|
} |
528 |
|
|
return 0; |
529 |
|
|
} |
530 |
|
|
|
531 |
|
|
/* |
532 |
|
|
** unlike the thing above, this one assumes that the quaternions in qq |
533 |
|
|
** have already been normalized, and, that the sum of wght[] is 1.0 |
534 |
|
|
*/ |
535 |
|
|
int |
536 |
|
|
ell_q_avgN_d(double mm[4], unsigned int *iterP, |
537 |
|
|
const double *qq, double *qlog, |
538 |
|
|
const double *wght, const unsigned int NN, |
539 |
|
|
const double eps, const unsigned int maxIter) { |
540 |
|
|
static const char me[]="ell_q_avgN_d"; |
541 |
|
|
double tmp, qdiv[4], elen; |
542 |
|
|
unsigned int ii, iter; |
543 |
|
|
|
544 |
|
|
/* iterP optional */ |
545 |
|
|
/* wght optional, to signify equal 1/NN weighting for all */ |
546 |
|
|
if (!( mm && qq )) { |
547 |
|
|
biffAddf(ELL, "%s: got NULL pointer", me); |
548 |
|
|
return 1; |
549 |
|
|
} |
550 |
|
|
if (!( eps >= 0 )) { |
551 |
|
|
biffAddf(ELL, "%s: need eps >= 0 (not %g)", me, eps); |
552 |
|
|
return 1; |
553 |
|
|
} |
554 |
|
|
|
555 |
|
|
/* initialize with euclidean mean */ |
556 |
|
|
ELL_4V_SET(mm, 0, 0, 0, 0); |
557 |
|
|
for (ii=0; ii<NN; ii++) { |
558 |
|
|
double ww; |
559 |
|
|
ww = wght ? wght[ii] : 1.0/NN; |
560 |
|
|
ELL_4V_SCALE_INCR(mm, ww, qq + 4*ii); |
561 |
|
|
} |
562 |
|
|
ELL_4V_NORM(mm, mm, tmp); |
563 |
|
|
|
564 |
|
|
iter = 0; |
565 |
|
|
do { |
566 |
|
|
double logavg[4], qexp[4]; |
567 |
|
|
/* take log of everyone */ |
568 |
|
|
for (ii=0; ii<NN; ii++) { |
569 |
|
|
ell_q_div_d(qdiv, mm, qq + 4*ii); /* div = mm^1 * qq[ii] */ |
570 |
|
|
ell_q_log_d(qlog + 4*ii, qdiv); |
571 |
|
|
} |
572 |
|
|
/* average, and find length */ |
573 |
|
|
ELL_4V_SET(logavg, 0, 0, 0, 0); |
574 |
|
|
for (ii=0; ii<NN; ii++) { |
575 |
|
|
double ww; |
576 |
|
|
ww = wght ? wght[ii] : 1.0/NN; |
577 |
|
|
ELL_4V_SCALE_INCR(logavg, ww, qlog + 4*ii); |
578 |
|
|
} |
579 |
|
|
elen = ELL_4V_LEN(logavg); |
580 |
|
|
/* use exp to put it back on S^3 */ |
581 |
|
|
ell_q_exp_d(qexp, logavg); |
582 |
|
|
ell_q_mul_d(mm, mm, qexp); |
583 |
|
|
iter++; |
584 |
|
|
} while ((!maxIter || iter < maxIter) && elen > eps); |
585 |
|
|
if (elen > eps) { |
586 |
|
|
biffAddf(ELL, "%s: still have error %g (> eps %g) after max %d iters", me, |
587 |
|
|
elen, eps, maxIter); |
588 |
|
|
return 1; |
589 |
|
|
} |
590 |
|
|
|
591 |
|
|
if (iterP) { |
592 |
|
|
*iterP = iter; |
593 |
|
|
} |
594 |
|
|
return 0; |
595 |
|
|
} |