void
tenTripleCalcSingle_d(double dst[3], int ttype, double ten[7]) {
double eval[3];
/* in time this can become more efficient ... */
switch (ttype) {
case tenTripleTypeEigenvalue:
tenEigensolve_d(dst, NULL, ten);
break;
case tenTripleTypeMoment:
case tenTripleTypeXYZ:
case tenTripleTypeRThetaZ:
case tenTripleTypeRThetaPhi:
case tenTripleTypeK:
case tenTripleTypeJ:
case tenTripleTypeWheelParm:
tenEigensolve_d(eval, NULL, ten);
tenTripleConvertSingle_d(dst, ttype, eval, tenTripleTypeEigenvalue);
break;
case tenTripleTypeR:
dst[0] = sqrt(_tenAnisoTen_d[tenAniso_S](ten));
dst[1] = _tenAnisoTen_d[tenAniso_FA](ten);
dst[2] = _tenAnisoTen_d[tenAniso_Mode](ten);
break;
default:
/* what on earth? */
ELL_3V_SET(dst, AIR_NAN, AIR_NAN, AIR_NAN);
}
return;
}
/* unlike with the K stuff, with the R stuff I seemed to have more luck
implementing pair-wise interpolation in terms of log and exp
*/
void
tenQGLInterpTwoEvalR(double oeval[3],
const double evalA[3], const double evalB[3],
const double tt) {
double RThPhA[3], RThPhB[3], rlog[3], oRThPh[3];
tenTripleConvertSingle_d(RThPhA, tenTripleTypeRThetaPhi,
evalA, tenTripleTypeEigenvalue);
tenTripleConvertSingle_d(RThPhB, tenTripleTypeRThetaPhi,
evalB, tenTripleTypeEigenvalue);
_tenQGL_Rlog(rlog, RThPhA, RThPhB);
ELL_3V_SCALE(rlog, tt, rlog);
_tenQGL_Rexp(oRThPh, RThPhA, rlog);
tenTripleConvertSingle_d(oeval, tenTripleTypeEigenvalue,
oRThPh, tenTripleTypeRThetaPhi);
return;
}
void
tenTripleConvertSingle_f(float _dst[3], int dstType,
const float _src[3], const int srcType) {
double dst[3], src[3];
ELL_3V_COPY(src, _src);
tenTripleConvertSingle_d(dst, dstType, src, srcType);
ELL_3V_COPY_TT(_dst, float, dst);
}
void
tenQGLInterpTwoEvalK(double oeval[3],
const double evalA[3], const double evalB[3],
const double tt) {
double RThZA[3], RThZB[3], oRThZ[3], bb;
tenTripleConvertSingle_d(RThZA, tenTripleTypeRThetaZ,
evalA, tenTripleTypeEigenvalue);
tenTripleConvertSingle_d(RThZB, tenTripleTypeRThetaZ,
evalB, tenTripleTypeEigenvalue);
if (rr1 > rr0) {
/* the bb calculation below could blow up, so we recurse
with flipped order */
tenQGLInterpTwoEvalK(oeval, evalB, evalA, 1-tt);
} else {
rr = AIR_LERP(tt, rr0, rr1);
zz = AIR_LERP(tt, zz0, zz1);
bb = rr0 ? (rr1/rr0 - 1) : 0;
/* bb can't be positive, because rr1 <= rr0 enforced above, so below
is really test for -0.001 < bb <= 0 */
if (bb > -0.0001) {
double dth;
dth = th1 - th0;
/* rr0 and rr1 are similar, use stable approximation */
th = th0 + tt*(dth
+ (0.5 - tt/2)*dth*bb
+ (-1.0/12 - tt/4 + tt*tt/3)*dth*bb*bb
+ (1.0/24 + tt/24 + tt*tt/6 - tt*tt*tt/4)*dth*bb*bb*bb);
} else {
/* use original formula */
/* have to clamp value of b so that log() values don't explode */
bb = AIR_MAX(bb, -1 + 100*FLT_EPSILON);
th = th0 + (th1 - th0)*log(1 + bb*tt)/log(1 + bb);
}
tenTripleConvertSingle_d(oeval, tenTripleTypeEigenvalue,
oRThZ, tenTripleTypeRThetaZ);
/*
fprintf(stderr, "%s: (b = %g) %g %g %g <-- %g %g %g\n", "blah", bb,
oeval[0], oeval[1], oeval[2],
oRThZ[0], oRThZ[1], oRThZ[2]);
*/
}
}
/* normalized gradients of k or r invariants, in XYZ space,
to help determine if path is really a loxodrome */
void
kgrads(double grad[3][3], const double eval[3]) {
double rtz[3];
tenTripleConvertSingle_d(rtz, tenTripleTypeRThetaZ,
eval, tenTripleTypeEigenvalue);
ELL_3V_SET(grad[0], cos(rtz[1]), sin(rtz[1]), 0);
ELL_3V_SET(grad[1], -sin(rtz[1]), cos(rtz[1]), 0);
ELL_3V_SET(grad[2], 0, 0, 1);
}
int
main(int argc, char *argv[]) {
char *me;
hestOpt *hopt=NULL;
airArray *mop;
int *itype, itypeNum, ii;
double src[3], last[3], dst[3];
char space[] = " ";
me = argv[0];
hestOptAdd(&hopt, NULL, "v1 v2 v3", airTypeDouble, 3, 3, src, NULL,
"source triple");
hestOptAdd(&hopt, "t", "type", airTypeEnum, 2, -1, &itype, NULL,
"sequence of triple types to convert through",
&itypeNum, tenTripleType);
hestParseOrDie(hopt, argc-1, argv+1, NULL,
me, info, AIR_TRUE, AIR_TRUE, AIR_TRUE);
mop = airMopNew();
airMopAdd(mop, hopt, (airMopper)hestOptFree, airMopAlways);
airMopAdd(mop, hopt, (airMopper)hestParseFree, airMopAlways);
printf("%s", space + strlen(airEnumStr(tenTripleType, itype[0])));
printf("%s", airEnumStr(tenTripleType, itype[0]));
ell_3v_print_d(stdout, src);
ELL_3V_COPY(last, src);
for (ii=1; ii<itypeNum; ii++) {
tenTripleConvertSingle_d(dst, itype[ii], src, itype[ii-1]);
printf("%s", space + strlen(airEnumStr(tenTripleType, itype[ii])));
printf("%s", airEnumStr(tenTripleType, itype[ii]));
ell_3v_print_d(stdout, dst);
ELL_3V_COPY(src, dst);
}
/*
tenTripleConvert_d(dst, dstType, src, srcType);
tenTripleConvert_d(tst, srcType, dst, dstType);
*/
/*
printf("%s: %s %s --> %s --> %s\n", me,
tenTriple->name,
airEnumStr(tenTriple, srcType),
airEnumStr(tenTriple, dstType),
airEnumStr(tenTriple, srcType));
ell_3v_print_d(stdout, src);
ell_3v_print_d(stdout, dst);
ell_3v_print_d(stdout, tst);
*/
airMopOkay(mop);
return 0;
}
int
tenTripleConvert(Nrrd *nout, int dstType,
const Nrrd *nin, int srcType) {
static const char me[]="tenTripleConvert";
size_t II, NN;
double (*ins)(void *, size_t, double), (*lup)(const void *, size_t);
if (!( nout && nin )) {
biffAddf(TEN, "%s: got NULL pointer", me);
return 1;
}
if ( airEnumValCheck(tenTripleType, dstType) ||
airEnumValCheck(tenTripleType, srcType) ) {
biffAddf(TEN, "%s: got invalid %s dst (%d) or src (%d)", me,
tenTripleType->name, dstType, srcType);
return 1;
}
if (3 != nin->axis[0].size) {
char stmp[AIR_STRLEN_SMALL];
biffAddf(TEN, "%s: need axis[0].size 3, not %s", me,
airSprintSize_t(stmp, nin->axis[0].size));
return 1;
}
if (nrrdTypeBlock == nin->type) {
biffAddf(TEN, "%s: input has non-scalar %s type",
me, airEnumStr(nrrdType, nrrdTypeBlock));
return 1;
}
if (nrrdCopy(nout, nin)) {
biffMovef(TEN, NRRD, "%s: couldn't initialize output", me);
return 1;
}
lup = nrrdDLookup[nin->type];
ins = nrrdDInsert[nout->type];
NN = nrrdElementNumber(nin)/3;
for (II=0; II<NN; II++) {
double src[3], dst[3];
src[0] = lup(nin->data, 0 + 3*II);
src[1] = lup(nin->data, 1 + 3*II);
src[2] = lup(nin->data, 2 + 3*II);
tenTripleConvertSingle_d(dst, dstType, src, srcType);
ins(nout->data, 0 + 3*II, dst[0]);
ins(nout->data, 1 + 3*II, dst[1]);
ins(nout->data, 2 + 3*II, dst[2]);
}
return 0;
}
void
rgrads(double grad[3][3], const double eval[3]) {
double rtp[3];
tenTripleConvertSingle_d(rtp, tenTripleTypeRThetaPhi,
eval, tenTripleTypeEigenvalue);
ELL_3V_SET(grad[0],
cos(rtp[1])*sin(rtp[2]),
sin(rtp[1])*sin(rtp[2]),
cos(rtp[2]));
ELL_3V_SET(grad[1], -sin(rtp[1]), cos(rtp[1]), 0);
ELL_3V_SET(grad[2],
cos(rtp[1])*cos(rtp[2]),
sin(rtp[1])*cos(rtp[2]),
-sin(rtp[2]));
}
/*
** This does (non-optionally) use biff, to report convergence failures
**
** we do in fact require non-NULL tip, because it holds the buffers we need
*/
int
_tenQGLInterpNEval(double evalOut[3],
const double *evalIn, /* size 3 -by- NN */
const double *wght, /* size NN */
unsigned int NN,
int ptype, tenInterpParm *tip) {
static const char me[]="_tenQGLInterpNEval";
double RTh_Out[3], elen;
unsigned int ii, iter;
int rttype;
void (*llog)(double lg[3], const double RTh_A[3], const double RTh_B[3]);
void (*lexp)(double RTh_B[3], const double RTh_A[3], const double lg[3]);
if (!(evalOut && evalIn && tip)) {
biffAddf(TEN, "%s: got NULL pointer", me);
return 1;
}
/* convert to (R,Th,_) and initialize RTh_Out */
if (tenInterpTypeQuatGeoLoxK == ptype) {
rttype = tenTripleTypeRThetaZ;
llog = _tenQGL_Klog;
lexp = _tenQGL_Kexp;
} else {
rttype = tenTripleTypeRThetaPhi;
llog = _tenQGL_Rlog;
lexp = _tenQGL_Rexp;
}
ELL_3V_SET(RTh_Out, 0, 0, 0);
for (ii=0; ii<NN; ii++) {
double ww;
tenTripleConvertSingle_d(tip->rtIn + 3*ii, rttype,
evalIn + 3*ii, tenTripleTypeEigenvalue);
ww = wght ? wght[ii] : 1.0/NN;
ELL_3V_SCALE_INCR(RTh_Out, ww, tip->rtIn + 3*ii);
}
/* compute iterated weighted mean, stored in RTh_Out */
iter = 0;
do {
double logavg[3];
/* take log of everyone */
for (ii=0; ii<NN; ii++) {
llog(tip->rtLog + 3*ii, RTh_Out, tip->rtIn + 3*ii);
}
/* average, and find length */
ELL_3V_SET(logavg, 0, 0, 0);
for (ii=0; ii<NN; ii++) {
double ww;
ww = wght ? wght[ii] : 1.0/NN;
ELL_3V_SCALE_INCR(logavg, ww, tip->rtLog + 3*ii);
}
elen = ELL_3V_LEN(logavg);
lexp(RTh_Out, RTh_Out, logavg);
iter++;
} while ((!tip->maxIter || iter < tip->maxIter) && elen > tip->convEps);
if (elen > tip->convEps) {
ELL_3V_SET(evalOut, AIR_NAN, AIR_NAN, AIR_NAN);
biffAddf(TEN, "%s: still have error %g (> eps %g) after max %d iters", me,
elen, tip->convEps, tip->maxIter);
return 1;
}
/* finish, convert to eval */
tenTripleConvertSingle_d(evalOut, tenTripleTypeEigenvalue,
RTh_Out, rttype);
return 0;
}
int
main(int argc, char *argv[]) {
char *me;
hestOpt *hopt=NULL;
airArray *mop;
double tripA[3], tripB[3], evalA[3], evalB[3],
rt_A[3], rt_B[3], trip[3], eval[3], lasteval[3], lastxyz[3],
logAB[3], ndist;
int ittype, ottype, ptype, rttype;
unsigned int NN, ii;
tenInterpParm *tip;
void (*interp)(double oeval[3], const double evalA[3],
const double evalB[3], const double tt);
double (*qdist)(const double RTh_A[3], const double RTh_B[3]);
void (*qlog)(double klog[3],
const double RThZA[3], const double RThZB[3]);
void (*qexp)(double RThZB[3],
const double RThZA[3], const double klog[3]);
void (*grads)(double grad[3][3], const double eval[3]);
me = argv[0];
mop = airMopNew();
tip = tenInterpParmNew();
airMopAdd(mop, tip, (airMopper)tenInterpParmNix, airMopAlways);
hestOptAdd(&hopt, "a", "start", airTypeDouble, 3, 3, tripA, NULL,
"start triple of values");
hestOptAdd(&hopt, "b", "end", airTypeDouble, 3, 3, tripB, NULL,
"end triple of values");
hestOptAdd(&hopt, "it", "type", airTypeEnum, 1, 1, &ittype, NULL,
"type of given start and end triples", NULL, tenTripleType);
hestOptAdd(&hopt, "ot", "type", airTypeEnum, 1, 1, &ottype, NULL,
"type of triples for output", NULL, tenTripleType);
hestOptAdd(&hopt, "p", "type", airTypeEnum, 1, 1, &ptype, NULL,
"type of path interpolation", NULL, tenInterpType);
hestOptAdd(&hopt, "n", "# steps", airTypeUInt, 1, 1, &NN, "100",
"number of steps along path");
hestOptAdd(&hopt, "v", "verbosity", airTypeInt, 1, 1,
&(tip->verbose), "0", "verbosity");
hestOptAdd(&hopt, "s", "stepsize", airTypeDouble, 1, 1,
&(tip->convStep), "1", "step size in update");
hestOptAdd(&hopt, "r", "recurse", airTypeInt, 0, 0,
&(tip->enableRecurse), NULL,
"enable recursive solution, when useful");
hestOptAdd(&hopt, "mn", "minnorm", airTypeDouble, 1, 1,
&(tip->minNorm), "0.000001",
"minnorm of something");
hestOptAdd(&hopt, "mi", "maxiter", airTypeUInt, 1, 1,
&(tip->maxIter), "0",
"if non-zero, max # iterations for computation");
hestOptAdd(&hopt, "c", "conv", airTypeDouble, 1, 1,
&(tip->convEps), "0.0001",
"convergence threshold of length fraction");
hestParseOrDie(hopt, argc-1, argv+1, NULL,
me, info, AIR_TRUE, AIR_TRUE, AIR_TRUE);
airMopAdd(mop, hopt, (airMopper)hestOptFree, airMopAlways);
airMopAdd(mop, hopt, (airMopper)hestParseFree, airMopAlways);
if (!( tenInterpTypeQuatGeoLoxK == ptype
|| tenInterpTypeQuatGeoLoxR == ptype )) {
fprintf(stderr, "%s: need type %s or %s, not %s\n", me,
airEnumStr(tenInterpType, tenInterpTypeQuatGeoLoxK),
airEnumStr(tenInterpType, tenInterpTypeQuatGeoLoxR),
airEnumStr(tenInterpType, ptype));
airMopError(mop);
return 1;
}
if (tenInterpTypeQuatGeoLoxK == ptype) {
interp = tenQGLInterpTwoEvalK;
qdist = _tenQGL_Kdist;
qlog = _tenQGL_Klog;
qexp = _tenQGL_Kexp;
grads = kgrads;
rttype = tenTripleTypeRThetaZ;
} else {
interp = tenQGLInterpTwoEvalR;
qdist = _tenQGL_Rdist;
qlog = _tenQGL_Rlog;
qexp = _tenQGL_Rexp;
grads = rgrads;
rttype = tenTripleTypeRThetaPhi;
}
fprintf(stderr, "%s: (%s) %f %f %f \n--%s--> %f %f %f\n", me,
airEnumStr(tenTripleType, ittype),
tripA[0], tripA[1], tripA[2],
airEnumStr(tenInterpType, ptype),
tripB[0], tripB[1], tripB[2]);
tenTripleConvertSingle_d(evalA, tenTripleTypeEigenvalue, tripA, ittype);
tenTripleConvertSingle_d(evalB, tenTripleTypeEigenvalue, tripB, ittype);
tenTripleConvertSingle_d(rt_A, rttype, tripA, ittype);
tenTripleConvertSingle_d(rt_B, rttype, tripB, ittype);
ndist = 0;
ELL_3V_SET(lasteval, AIR_NAN, AIR_NAN, AIR_NAN);
ELL_3V_SET(lastxyz, AIR_NAN, AIR_NAN, AIR_NAN);
qlog(logAB, rt_A, rt_B);
fprintf(stderr, "%s: log = %g %g %g (%g)\n", me,
logAB[0], logAB[1], logAB[2], ELL_3V_LEN(logAB));
for (ii=0; ii<NN; ii++) {
double tt, xyz[3], dot[3], ll[3], prayRT[3], prayO[3];
tt = AIR_AFFINE(0, ii, NN-1, 0.0, 1.0);
interp(eval, evalA, evalB, tt);
tenTripleConvertSingle_d(trip, ottype,
eval, tenTripleTypeEigenvalue);
tenTripleConvertSingle_d(xyz, tenTripleTypeXYZ,
eval, tenTripleTypeEigenvalue);
ELL_3V_SCALE(ll, tt, logAB);
qexp(prayRT, rt_A, ll);
tenTripleConvertSingle_d(prayO, ottype, prayRT, rttype);
if (ii) {
double diff[3], gr[3][3];
ELL_3V_SUB(diff, lasteval, eval);
ndist += ELL_3V_LEN(diff);
ELL_3V_SUB(diff, lastxyz, xyz);
grads(gr, eval);
dot[0] = ELL_3V_DOT(diff, gr[0]);
dot[1] = ELL_3V_DOT(diff, gr[1]);
dot[2] = ELL_3V_DOT(diff, gr[2]);
} else {
ELL_3V_SET(dot, 0, 0, 0);
}
printf("%03u %g %g %g %g %g %g 00 %g %g %g\n", ii,
trip[0], prayO[0],
trip[1], prayO[1],
trip[2], prayO[2],
dot[0], dot[1], dot[2]);
ELL_3V_COPY(lasteval, eval);
ELL_3V_COPY(lastxyz, xyz);
}
fprintf(stderr, "%s: dist %g =?= %g\n", me,
qdist(rt_A, rt_B), ndist);
airMopOkay(mop);
return 0;
}
