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 |
|
|
|
25 |
|
|
#include "ten.h" |
26 |
|
|
#include "privateTen.h" |
27 |
|
|
|
28 |
|
|
/* |
29 |
|
|
** computes (r1 - r0)/(log(r1) - log(r0)) |
30 |
|
|
*/ |
31 |
|
|
double |
32 |
|
|
_tenQGL_blah(double rr0, double rr1) { |
33 |
|
|
double bb, ret; |
34 |
|
|
|
35 |
|
|
if (rr1 > rr0) { |
36 |
|
|
/* the bb calculation below could blow up, so we recurse |
37 |
|
|
with flipped order */ |
38 |
|
|
ret = _tenQGL_blah(rr1, rr0); |
39 |
|
|
} else { |
40 |
|
|
/* rr1 <= rr0 --> rr1/rr0 <= 1 --> rr1/rr0 - 1 <= 0 --> bb <= 0 */ |
41 |
|
|
/* and rr1 >= 0 --> rr1/rr0 >= 0 --> rr1/rr0 - 1 >= -1 --> bb >= -1 */ |
42 |
|
|
bb = rr0 ? (rr1/rr0 - 1) : 0; |
43 |
|
|
if (bb > -0.0001) { |
44 |
|
|
ret = rr0*(1 + bb*(0.5001249976477329 |
45 |
|
|
- bb*(7.0/6 + bb*(1.0/6 - bb/720.0)))); |
46 |
|
|
} else { |
47 |
|
|
/* had trouble finding a high-quality approximation for b near -1 */ |
48 |
|
|
bb = AIR_MAX(bb, -1 + 100*FLT_EPSILON); |
49 |
|
|
ret = rr0*bb/log(bb + 1); |
50 |
|
|
} |
51 |
|
|
} |
52 |
|
|
return ret; |
53 |
|
|
} |
54 |
|
|
|
55 |
|
|
#define rr0 (RThZA[0]) |
56 |
|
|
#define rr1 (RThZB[0]) |
57 |
|
|
#define rr (oRThZ[0]) |
58 |
|
|
#define th0 (RThZA[1]) |
59 |
|
|
#define th1 (RThZB[1]) |
60 |
|
|
#define th (oRThZ[1]) |
61 |
|
|
#define zz0 (RThZA[2]) |
62 |
|
|
#define zz1 (RThZB[2]) |
63 |
|
|
#define zz (oRThZ[2]) |
64 |
|
|
|
65 |
|
|
void |
66 |
|
|
tenQGLInterpTwoEvalK(double oeval[3], |
67 |
|
|
const double evalA[3], const double evalB[3], |
68 |
|
|
const double tt) { |
69 |
|
|
double RThZA[3], RThZB[3], oRThZ[3], bb; |
70 |
|
|
|
71 |
|
|
tenTripleConvertSingle_d(RThZA, tenTripleTypeRThetaZ, |
72 |
|
|
evalA, tenTripleTypeEigenvalue); |
73 |
|
|
tenTripleConvertSingle_d(RThZB, tenTripleTypeRThetaZ, |
74 |
|
|
evalB, tenTripleTypeEigenvalue); |
75 |
|
|
if (rr1 > rr0) { |
76 |
|
|
/* the bb calculation below could blow up, so we recurse |
77 |
|
|
with flipped order */ |
78 |
|
|
tenQGLInterpTwoEvalK(oeval, evalB, evalA, 1-tt); |
79 |
|
|
} else { |
80 |
|
|
rr = AIR_LERP(tt, rr0, rr1); |
81 |
|
|
zz = AIR_LERP(tt, zz0, zz1); |
82 |
|
|
bb = rr0 ? (rr1/rr0 - 1) : 0; |
83 |
|
|
/* bb can't be positive, because rr1 <= rr0 enforced above, so below |
84 |
|
|
is really test for -0.001 < bb <= 0 */ |
85 |
|
|
if (bb > -0.0001) { |
86 |
|
|
double dth; |
87 |
|
|
dth = th1 - th0; |
88 |
|
|
/* rr0 and rr1 are similar, use stable approximation */ |
89 |
|
|
th = th0 + tt*(dth |
90 |
|
|
+ (0.5 - tt/2)*dth*bb |
91 |
|
|
+ (-1.0/12 - tt/4 + tt*tt/3)*dth*bb*bb |
92 |
|
|
+ (1.0/24 + tt/24 + tt*tt/6 - tt*tt*tt/4)*dth*bb*bb*bb); |
93 |
|
|
} else { |
94 |
|
|
/* use original formula */ |
95 |
|
|
/* have to clamp value of b so that log() values don't explode */ |
96 |
|
|
bb = AIR_MAX(bb, -1 + 100*FLT_EPSILON); |
97 |
|
|
th = th0 + (th1 - th0)*log(1 + bb*tt)/log(1 + bb); |
98 |
|
|
} |
99 |
|
|
tenTripleConvertSingle_d(oeval, tenTripleTypeEigenvalue, |
100 |
|
|
oRThZ, tenTripleTypeRThetaZ); |
101 |
|
|
/* |
102 |
|
|
fprintf(stderr, "%s: (b = %g) %g %g %g <-- %g %g %g\n", "blah", bb, |
103 |
|
|
oeval[0], oeval[1], oeval[2], |
104 |
|
|
oRThZ[0], oRThZ[1], oRThZ[2]); |
105 |
|
|
*/ |
106 |
|
|
} |
107 |
|
|
} |
108 |
|
|
|
109 |
|
|
double |
110 |
|
|
_tenQGL_Kdist(const double RThZA[3], const double RThZB[3]) { |
111 |
|
|
double dr, dth, dz, bl, dist; |
112 |
|
|
|
113 |
|
|
dr = rr1 - rr0; |
114 |
|
|
bl = _tenQGL_blah(rr0, rr1); |
115 |
|
|
dth = th1 - th0; |
116 |
|
|
dz = zz1 - zz0; |
117 |
|
|
dist = sqrt(dr*dr + bl*bl*dth*dth + dz*dz); |
118 |
|
|
return dist; |
119 |
|
|
} |
120 |
|
|
|
121 |
|
|
void |
122 |
|
|
_tenQGL_Klog(double klog[3], |
123 |
|
|
const double RThZA[3], const double RThZB[3]) { |
124 |
|
|
double dr, bl, dth, dz; |
125 |
|
|
|
126 |
|
|
dr = rr1 - rr0; |
127 |
|
|
bl = _tenQGL_blah(rr0, rr1); |
128 |
|
|
dth = th1 - th0; |
129 |
|
|
dz = zz1 - zz0; |
130 |
|
|
ELL_3V_SET(klog, dr, bl*dth, dz); |
131 |
|
|
return; |
132 |
|
|
} |
133 |
|
|
|
134 |
|
|
void |
135 |
|
|
_tenQGL_Kexp(double RThZB[3], |
136 |
|
|
const double RThZA[3], const double klog[3]) { |
137 |
|
|
double bl; |
138 |
|
|
|
139 |
|
|
rr1 = rr0 + klog[0]; |
140 |
|
|
bl = _tenQGL_blah(rr0, rr1); |
141 |
|
|
th1 = th0 + (bl ? klog[1]/bl : 0); |
142 |
|
|
zz1 = zz0 + klog[2]; |
143 |
|
|
return; |
144 |
|
|
} |
145 |
|
|
|
146 |
|
|
#undef rr0 |
147 |
|
|
#undef rr1 |
148 |
|
|
#undef rr |
149 |
|
|
#undef th0 |
150 |
|
|
#undef th1 |
151 |
|
|
#undef th |
152 |
|
|
#undef zz0 |
153 |
|
|
#undef zz1 |
154 |
|
|
#undef zz |
155 |
|
|
|
156 |
|
|
/* |
157 |
|
|
** stable computation of (ph1 - ph0)/(log(tan(ph1/2)) - log(tan(ph0/2))) |
158 |
|
|
*/ |
159 |
|
|
double |
160 |
|
|
_tenQGL_fooo(double ph1, double ph0) { |
161 |
|
|
double ret; |
162 |
|
|
|
163 |
|
|
if (ph0 > ph1) { |
164 |
|
|
ret = _tenQGL_fooo(ph0, ph1); |
165 |
|
|
} else if (0 == ph0/2) { |
166 |
|
|
ret = 0; |
167 |
|
|
} else { |
168 |
|
|
/* ph1 >= ph0 > 0 */ |
169 |
|
|
if (ph1 - ph0 < 0.0001) { |
170 |
|
|
double dph, ss, cc; |
171 |
|
|
dph = ph1 - ph0; |
172 |
|
|
ss = sin(ph1); |
173 |
|
|
cc = cos(ph1); |
174 |
|
|
ret = (ss |
175 |
|
|
+ cc*dph/2 |
176 |
|
|
+ ((cos(2*ph1) - 3)/ss)*dph*dph/24 |
177 |
|
|
+ (cc/(ss*ss))*dph*dph*dph/24); |
178 |
|
|
} else { |
179 |
|
|
ret = (ph1 - ph0)/(log(tan(ph1/2)) - log(tan(ph0/2))); |
180 |
|
|
} |
181 |
|
|
} |
182 |
|
|
return ret; |
183 |
|
|
} |
184 |
|
|
|
185 |
|
|
#define rr0 (RThPhA[0]) |
186 |
|
|
#define rr1 (RThPhB[0]) |
187 |
|
|
#define rr (oRThPh[0]) |
188 |
|
|
#define th0 (RThPhA[1]) |
189 |
|
|
#define th1 (RThPhB[1]) |
190 |
|
|
#define th (oRThPh[1]) |
191 |
|
|
#define ph0 (RThPhA[2]) |
192 |
|
|
#define ph1 (RThPhB[2]) |
193 |
|
|
#define ph (oRThPh[2]) |
194 |
|
|
|
195 |
|
|
void |
196 |
|
|
_tenQGL_Rlog(double rlog[3], |
197 |
|
|
const double RThPhA[3], const double RThPhB[3]) { |
198 |
|
|
double dr, dth, dph, bl, fo; |
199 |
|
|
|
200 |
|
|
dr = rr1 - rr0; |
201 |
|
|
dth = th1 - th0; |
202 |
|
|
dph = ph1 - ph0; |
203 |
|
|
bl = _tenQGL_blah(rr0, rr1); |
204 |
|
|
fo = _tenQGL_fooo(ph0, ph1); |
205 |
|
|
/* rlog[0] rlog[1] rlog[2] */ |
206 |
|
|
ELL_3V_SET(rlog, dr, bl*dth*fo, dph*bl); |
207 |
|
|
} |
208 |
|
|
|
209 |
|
|
void |
210 |
|
|
_tenQGL_Rexp(double RThPhB[3], |
211 |
|
|
const double RThPhA[3], const double rlog[3]) { |
212 |
|
|
double bl, fo; |
213 |
|
|
|
214 |
|
|
rr1 = rr0 + rlog[0]; |
215 |
|
|
bl = _tenQGL_blah(rr0, rr1); |
216 |
|
|
ph1 = ph0 + (bl ? rlog[2]/bl : 0); |
217 |
|
|
fo = _tenQGL_fooo(ph0, ph1); |
218 |
|
|
th1 = th0 + (bl*fo ? rlog[1]/(bl*fo) : 0); |
219 |
|
|
return; |
220 |
|
|
} |
221 |
|
|
|
222 |
|
|
/* unlike with the K stuff, with the R stuff I seemed to have more luck |
223 |
|
|
implementing pair-wise interpolation in terms of log and exp |
224 |
|
|
*/ |
225 |
|
|
void |
226 |
|
|
tenQGLInterpTwoEvalR(double oeval[3], |
227 |
|
|
const double evalA[3], const double evalB[3], |
228 |
|
|
const double tt) { |
229 |
|
|
double RThPhA[3], RThPhB[3], rlog[3], oRThPh[3]; |
230 |
|
|
|
231 |
|
|
tenTripleConvertSingle_d(RThPhA, tenTripleTypeRThetaPhi, |
232 |
|
|
evalA, tenTripleTypeEigenvalue); |
233 |
|
|
tenTripleConvertSingle_d(RThPhB, tenTripleTypeRThetaPhi, |
234 |
|
|
evalB, tenTripleTypeEigenvalue); |
235 |
|
|
_tenQGL_Rlog(rlog, RThPhA, RThPhB); |
236 |
|
|
ELL_3V_SCALE(rlog, tt, rlog); |
237 |
|
|
_tenQGL_Rexp(oRThPh, RThPhA, rlog); |
238 |
|
|
tenTripleConvertSingle_d(oeval, tenTripleTypeEigenvalue, |
239 |
|
|
oRThPh, tenTripleTypeRThetaPhi); |
240 |
|
|
return; |
241 |
|
|
} |
242 |
|
|
|
243 |
|
|
double |
244 |
|
|
_tenQGL_Rdist(const double RThPhA[3], const double RThPhB[3]) { |
245 |
|
|
double dr, dth, dph, bl, fo; |
246 |
|
|
|
247 |
|
|
dr = rr1 - rr0; |
248 |
|
|
dth = th1 - th0; |
249 |
|
|
dph = ph1 - ph0; |
250 |
|
|
bl = _tenQGL_blah(rr0, rr1); |
251 |
|
|
fo = _tenQGL_fooo(ph0, ph1); |
252 |
|
|
return sqrt(dr*dr + bl*bl*(dth*dth*fo*fo + dph*dph)); |
253 |
|
|
} |
254 |
|
|
|
255 |
|
|
#undef rr0 |
256 |
|
|
#undef rr1 |
257 |
|
|
#undef rr |
258 |
|
|
#undef th0 |
259 |
|
|
#undef th1 |
260 |
|
|
#undef th |
261 |
|
|
#undef ph0 |
262 |
|
|
#undef ph1 |
263 |
|
|
#undef ph |
264 |
|
|
|
265 |
|
|
/* returns the index into unitq[] of the quaternion that led to the |
266 |
|
|
right alignment. If it was already aligned, this will be 0, |
267 |
|
|
because unitq[0] is the identity quaternion */ |
268 |
|
|
int |
269 |
|
|
_tenQGL_q_align(double qOut[4], const double qRef[4], const double qIn[4]) { |
270 |
|
|
unsigned int ii, maxDotIdx; |
271 |
|
|
double unitq[8][4] = {{+1, 0, 0, 0}, |
272 |
|
|
{-1, 0, 0, 0}, |
273 |
|
|
{0, +1, 0, 0}, |
274 |
|
|
{0, -1, 0, 0}, |
275 |
|
|
{0, 0, +1, 0}, |
276 |
|
|
{0, 0, -1, 0}, |
277 |
|
|
{0, 0, 0, +1}, |
278 |
|
|
{0, 0, 0, -1}}; |
279 |
|
|
double dot[8], qInMul[8][4], maxDot; |
280 |
|
|
|
281 |
|
|
for (ii=0; ii<8; ii++) { |
282 |
|
|
ell_q_mul_d(qInMul[ii], qIn, unitq[ii]); |
283 |
|
|
dot[ii] = ELL_4V_DOT(qRef, qInMul[ii]); |
284 |
|
|
} |
285 |
|
|
maxDotIdx = 0; |
286 |
|
|
maxDot = dot[maxDotIdx]; |
287 |
|
|
for (ii=1; ii<8; ii++) { |
288 |
|
|
if (dot[ii] > maxDot) { |
289 |
|
|
maxDotIdx = ii; |
290 |
|
|
maxDot = dot[maxDotIdx]; |
291 |
|
|
} |
292 |
|
|
} |
293 |
|
|
ELL_4V_COPY(qOut, qInMul[maxDotIdx]); |
294 |
|
|
return maxDotIdx; |
295 |
|
|
} |
296 |
|
|
|
297 |
|
|
void |
298 |
|
|
tenQGLInterpTwoEvec(double oevec[9], |
299 |
|
|
const double evecA[9], const double evecB[9], |
300 |
|
|
double tt) { |
301 |
|
|
double rotA[9], rotB[9], orot[9], |
302 |
|
|
oq[4], qA[4], qB[4], _qB[4], qdiv[4], angle, axis[3], qq[4]; |
303 |
|
|
|
304 |
|
|
ELL_3M_TRANSPOSE(rotA, evecA); |
305 |
|
|
ELL_3M_TRANSPOSE(rotB, evecB); |
306 |
|
|
ell_3m_to_q_d(qA, rotA); |
307 |
|
|
ell_3m_to_q_d(_qB, rotB); |
308 |
|
|
_tenQGL_q_align(qB, qA, _qB); |
309 |
|
|
/* there's probably a faster way to do this slerp qA --> qB */ |
310 |
|
|
ell_q_div_d(qdiv, qA, qB); /* div = A^-1 * B */ |
311 |
|
|
angle = ell_q_to_aa_d(axis, qdiv); |
312 |
|
|
ell_aa_to_q_d(qq, angle*tt, axis); |
313 |
|
|
ell_q_mul_d(oq, qA, qq); |
314 |
|
|
ell_q_to_3m_d(orot, oq); |
315 |
|
|
ELL_3M_TRANSPOSE(oevec, orot); |
316 |
|
|
} |
317 |
|
|
|
318 |
|
|
void |
319 |
|
|
tenQGLInterpTwo(double oten[7], |
320 |
|
|
const double tenA[7], const double tenB[7], |
321 |
|
|
int ptype, double tt, tenInterpParm *tip) { |
322 |
|
|
double oeval[3], evalA[3], evalB[3], oevec[9], evecA[9], evecB[9], cc; |
323 |
|
|
|
324 |
|
|
AIR_UNUSED(tip); |
325 |
|
|
tenEigensolve_d(evalA, evecA, tenA); |
326 |
|
|
tenEigensolve_d(evalB, evecB, tenB); |
327 |
|
|
cc = AIR_LERP(tt, tenA[0], tenB[0]); |
328 |
|
|
|
329 |
|
|
if (tenInterpTypeQuatGeoLoxK == ptype) { |
330 |
|
|
tenQGLInterpTwoEvalK(oeval, evalA, evalB, tt); |
331 |
|
|
} else { |
332 |
|
|
tenQGLInterpTwoEvalR(oeval, evalA, evalB, tt); |
333 |
|
|
} |
334 |
|
|
tenQGLInterpTwoEvec(oevec, evecA, evecB, tt); |
335 |
|
|
tenMakeSingle_d(oten, cc, oeval, oevec); |
336 |
|
|
|
337 |
|
|
return; |
338 |
|
|
} |
339 |
|
|
|
340 |
|
|
/* |
341 |
|
|
** This does (non-optionally) use biff, to report convergence failures |
342 |
|
|
** |
343 |
|
|
** we do in fact require non-NULL tip, because it holds the buffers we need |
344 |
|
|
*/ |
345 |
|
|
int |
346 |
|
|
_tenQGLInterpNEval(double evalOut[3], |
347 |
|
|
const double *evalIn, /* size 3 -by- NN */ |
348 |
|
|
const double *wght, /* size NN */ |
349 |
|
|
unsigned int NN, |
350 |
|
|
int ptype, tenInterpParm *tip) { |
351 |
|
|
static const char me[]="_tenQGLInterpNEval"; |
352 |
|
|
double RTh_Out[3], elen; |
353 |
|
|
unsigned int ii, iter; |
354 |
|
|
int rttype; |
355 |
|
|
void (*llog)(double lg[3], const double RTh_A[3], const double RTh_B[3]); |
356 |
|
|
void (*lexp)(double RTh_B[3], const double RTh_A[3], const double lg[3]); |
357 |
|
|
|
358 |
|
|
if (!(evalOut && evalIn && tip)) { |
359 |
|
|
biffAddf(TEN, "%s: got NULL pointer", me); |
360 |
|
|
return 1; |
361 |
|
|
} |
362 |
|
|
/* convert to (R,Th,_) and initialize RTh_Out */ |
363 |
|
|
if (tenInterpTypeQuatGeoLoxK == ptype) { |
364 |
|
|
rttype = tenTripleTypeRThetaZ; |
365 |
|
|
llog = _tenQGL_Klog; |
366 |
|
|
lexp = _tenQGL_Kexp; |
367 |
|
|
} else { |
368 |
|
|
rttype = tenTripleTypeRThetaPhi; |
369 |
|
|
llog = _tenQGL_Rlog; |
370 |
|
|
lexp = _tenQGL_Rexp; |
371 |
|
|
} |
372 |
|
|
ELL_3V_SET(RTh_Out, 0, 0, 0); |
373 |
|
|
for (ii=0; ii<NN; ii++) { |
374 |
|
|
double ww; |
375 |
|
|
tenTripleConvertSingle_d(tip->rtIn + 3*ii, rttype, |
376 |
|
|
evalIn + 3*ii, tenTripleTypeEigenvalue); |
377 |
|
|
ww = wght ? wght[ii] : 1.0/NN; |
378 |
|
|
ELL_3V_SCALE_INCR(RTh_Out, ww, tip->rtIn + 3*ii); |
379 |
|
|
} |
380 |
|
|
|
381 |
|
|
/* compute iterated weighted mean, stored in RTh_Out */ |
382 |
|
|
iter = 0; |
383 |
|
|
do { |
384 |
|
|
double logavg[3]; |
385 |
|
|
/* take log of everyone */ |
386 |
|
|
for (ii=0; ii<NN; ii++) { |
387 |
|
|
llog(tip->rtLog + 3*ii, RTh_Out, tip->rtIn + 3*ii); |
388 |
|
|
} |
389 |
|
|
/* average, and find length */ |
390 |
|
|
ELL_3V_SET(logavg, 0, 0, 0); |
391 |
|
|
for (ii=0; ii<NN; ii++) { |
392 |
|
|
double ww; |
393 |
|
|
ww = wght ? wght[ii] : 1.0/NN; |
394 |
|
|
ELL_3V_SCALE_INCR(logavg, ww, tip->rtLog + 3*ii); |
395 |
|
|
} |
396 |
|
|
elen = ELL_3V_LEN(logavg); |
397 |
|
|
lexp(RTh_Out, RTh_Out, logavg); |
398 |
|
|
iter++; |
399 |
|
|
} while ((!tip->maxIter || iter < tip->maxIter) && elen > tip->convEps); |
400 |
|
|
|
401 |
|
|
if (elen > tip->convEps) { |
402 |
|
|
ELL_3V_SET(evalOut, AIR_NAN, AIR_NAN, AIR_NAN); |
403 |
|
|
biffAddf(TEN, "%s: still have error %g (> eps %g) after max %d iters", me, |
404 |
|
|
elen, tip->convEps, tip->maxIter); |
405 |
|
|
return 1; |
406 |
|
|
} |
407 |
|
|
|
408 |
|
|
/* finish, convert to eval */ |
409 |
|
|
tenTripleConvertSingle_d(evalOut, tenTripleTypeEigenvalue, |
410 |
|
|
RTh_Out, rttype); |
411 |
|
|
|
412 |
|
|
return 0; |
413 |
|
|
} |
414 |
|
|
|
415 |
|
|
double |
416 |
|
|
_tenQGL_q_interdot(unsigned int *centerIdxP, |
417 |
|
|
double *qq, double *inter, unsigned int NN) { |
418 |
|
|
unsigned int ii, jj; |
419 |
|
|
double sum, dot, max; |
420 |
|
|
|
421 |
|
|
for (jj=0; jj<NN; jj++) { |
422 |
|
|
for (ii=0; ii<NN; ii++) { |
423 |
|
|
inter[ii + NN*jj] = 0; |
424 |
|
|
} |
425 |
|
|
} |
426 |
|
|
sum = 0; |
427 |
|
|
for (jj=0; jj<NN; jj++) { |
428 |
|
|
inter[jj + NN*jj] = 1.0; |
429 |
|
|
for (ii=jj+1; ii<NN; ii++) { |
430 |
|
|
dot = ELL_4V_DOT(qq + 4*ii, qq + 4*jj); |
431 |
|
|
inter[ii + NN*jj] = dot; |
432 |
|
|
inter[jj + NN*ii] = dot; |
433 |
|
|
sum += dot; |
434 |
|
|
} |
435 |
|
|
} |
436 |
|
|
for (jj=0; jj<NN; jj++) { |
437 |
|
|
for (ii=1; ii<NN; ii++) { |
438 |
|
|
inter[0 + NN*jj] += inter[ii + NN*jj]; |
439 |
|
|
} |
440 |
|
|
} |
441 |
|
|
*centerIdxP = 0; |
442 |
|
|
max = inter[0 + NN*(*centerIdxP)]; |
443 |
|
|
for (jj=1; jj<NN; jj++) { |
444 |
|
|
if (inter[0 + NN*jj] > max) { |
445 |
|
|
*centerIdxP = jj; |
446 |
|
|
max = inter[0 + NN*(*centerIdxP)]; |
447 |
|
|
} |
448 |
|
|
} |
449 |
|
|
return sum; |
450 |
|
|
} |
451 |
|
|
|
452 |
|
|
/* |
453 |
|
|
** This does (non-optionally) use biff, to report convergence failures |
454 |
|
|
** |
455 |
|
|
** we do in fact require non-NULL tip, because it holds the buffers we need |
456 |
|
|
*/ |
457 |
|
|
int |
458 |
|
|
_tenQGLInterpNEvec(double evecOut[9], |
459 |
|
|
const double *evecIn, /* size 9 -by- NN */ |
460 |
|
|
const double *wght, /* size NN */ |
461 |
|
|
unsigned int NN, |
462 |
|
|
tenInterpParm *tip) { |
463 |
|
|
static const char me[]="_tenQGLInterpNEvec"; |
464 |
|
|
double qOut[4], maxWght, len, /* odsum, */ dsum, rot[9]; |
465 |
|
|
unsigned int ii, centerIdx=0, fix, qiter; |
466 |
|
|
|
467 |
|
|
if (!( evecOut && evecIn && tip )) { |
468 |
|
|
biffAddf(TEN, "%s: got NULL pointer", me); |
469 |
|
|
return 1; |
470 |
|
|
} |
471 |
|
|
/* convert to quaternions */ |
472 |
|
|
for (ii=0; ii<NN; ii++) { |
473 |
|
|
ELL_3M_TRANSPOSE(rot, evecIn + 9*ii); |
474 |
|
|
ell_3m_to_q_d(tip->qIn + 4*ii, rot); |
475 |
|
|
} |
476 |
|
|
/* HEY: what should this be used for? variable odsum set but not used */ |
477 |
|
|
/* odsum = _tenQGL_q_interdot(¢erIdx, tip->qIn, tip->qInter, NN); */ |
478 |
|
|
|
479 |
|
|
/* find quaternion with maximal weight, use it as is (decree that |
480 |
|
|
its the right representative), and then align rest with that. |
481 |
|
|
This is actually principled; symmetry allows it */ |
482 |
|
|
centerIdx = 0; |
483 |
|
|
if (wght) { |
484 |
|
|
maxWght = wght[centerIdx]; |
485 |
|
|
for (ii=1; ii<NN; ii++) { |
486 |
|
|
if (wght[ii] > maxWght) { |
487 |
|
|
centerIdx = ii; |
488 |
|
|
maxWght = wght[centerIdx]; |
489 |
|
|
} |
490 |
|
|
} |
491 |
|
|
} |
492 |
|
|
for (ii=0; ii<NN; ii++) { |
493 |
|
|
if (ii == centerIdx) { |
494 |
|
|
continue; |
495 |
|
|
} |
496 |
|
|
_tenQGL_q_align(tip->qIn + 4*ii, tip->qIn + 4*centerIdx, tip->qIn + 4*ii); |
497 |
|
|
} |
498 |
|
|
dsum = _tenQGL_q_interdot(¢erIdx, tip->qIn, tip->qInter, NN); |
499 |
|
|
|
500 |
|
|
/* try to settle on tightest set of representatives */ |
501 |
|
|
qiter = 0; |
502 |
|
|
do { |
503 |
|
|
fix = 0; |
504 |
|
|
for (ii=0; ii<NN; ii++) { |
505 |
|
|
unsigned int ff; |
506 |
|
|
if (ii == centerIdx) { |
507 |
|
|
continue; |
508 |
|
|
} |
509 |
|
|
ff = _tenQGL_q_align(tip->qIn + 4*ii, tip->qIn + 4*centerIdx, |
510 |
|
|
tip->qIn + 4*ii); |
511 |
|
|
fix = AIR_MAX(fix, ff); |
512 |
|
|
} |
513 |
|
|
dsum = _tenQGL_q_interdot(¢erIdx, tip->qIn, tip->qInter, NN); |
514 |
|
|
if (tip->maxIter && qiter > tip->maxIter) { |
515 |
|
|
biffAddf(TEN, "%s: q tightening unconverged after %u iters; " |
516 |
|
|
"interdot = %g -> maxfix = %u; center = %u\n", |
517 |
|
|
me, tip->maxIter, dsum, fix, centerIdx); |
518 |
|
|
return 1; |
519 |
|
|
} |
520 |
|
|
qiter++; |
521 |
|
|
} while (fix); |
522 |
|
|
/* |
523 |
|
|
fprintf(stderr, "!%s: dsum %g --%u--> %g\n", me, odsum, qiter, dsum); |
524 |
|
|
*/ |
525 |
|
|
/* make sure they're normalized */ |
526 |
|
|
for (ii=0; ii<NN; ii++) { |
527 |
|
|
ELL_4V_NORM(tip->qIn + 4*ii, tip->qIn + 4*ii, len); |
528 |
|
|
} |
529 |
|
|
|
530 |
|
|
/* compute iterated weighted mean, stored in qOut */ |
531 |
|
|
if (ell_q_avgN_d(qOut, &qiter, tip->qIn, tip->qBuff, wght, |
532 |
|
|
NN, tip->convEps, tip->maxIter)) { |
533 |
|
|
biffMovef(TEN, ELL, "%s: problem doing quaternion mean", me); |
534 |
|
|
return 1; |
535 |
|
|
} |
536 |
|
|
/* |
537 |
|
|
fprintf(stderr, "!%s: q avg converged in %u\n", me, qiter); |
538 |
|
|
*/ |
539 |
|
|
|
540 |
|
|
/* finish, convert back to evec */ |
541 |
|
|
ell_q_to_3m_d(rot, qOut); |
542 |
|
|
ELL_3M_TRANSPOSE(evecOut, rot); |
543 |
|
|
|
544 |
|
|
return 0; |
545 |
|
|
} |
546 |
|
|
|