void
washQtoM3(double m[9], double q[4]) {
double p[4], w, x, y, z, len;
ELL_4V_COPY(p, q);
len = ELL_4V_LEN(p);
ELL_4V_SCALE(p, 1.0/len, p);
w = p[0];
x = p[1];
y = p[2];
z = p[3];
/* mathematica work implies that we should be
setting ROW vectors here */
ELL_3V_SET(m+0,
1 - 2*(y*y + z*z),
2*(x*y - w*z),
2*(x*z + w*y));
ELL_3V_SET(m+3,
2*(x*y + w*z),
1 - 2*(x*x + z*z),
2*(y*z - w*x));
ELL_3V_SET(m+6,
2*(x*z - w*y),
2*(y*z + w*x),
1 - 2*(x*x + y*y));
}
void
ell_q_log_d(double q2[4], const double q1[4]) {
double a, b, axis[3];
a = log(ELL_4V_LEN(q1));
b = ell_q_to_aa_d(axis, q1)/2.0;
ELL_4V_SET(q2, a, b*axis[0], b*axis[1], b*axis[2]);
}
void
ell_q_pow_d(double q2[4], const double q1[4], const double p) {
double len, angle, axis[3];
len = pow(ELL_4V_LEN(q1), p);
angle = ell_q_to_aa_d(axis, q1);
ell_aa_to_q_d(q2, p*angle, axis);
ELL_4V_SCALE(q2, len, q2);
}
int
main(int argc, const char *argv[]) {
const char *me;
char *err, *outS;
hestOpt *hopt=NULL;
airArray *mop;
int xi, yi, zi, samp;
float *tdata;
double clp[2], xyz[3], q[4], len;
double mD[9], mRF[9], mRI[9], mT[9];
Nrrd *nten;
mop = airMopNew();
me = argv[0];
hestOptAdd(&hopt, "n", "# samples", airTypeInt, 1, 1, &samp, "4",
"number of samples along each edge of cube");
hestOptAdd(&hopt, "c", "cl cp", airTypeDouble, 2, 2, clp, NULL,
"shape of tensor to use; \"cl\" and \"cp\" are cl1 "
"and cp1 values, both in [0.0,1.0]");
hestOptAdd(&hopt, "o", "nout", airTypeString, 1, 1, &outS, "-",
"output file to save tensors into");
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);
nten = nrrdNew();
airMopAdd(mop, nten, (airMopper)nrrdNuke, airMopAlways);
_clp2xyz(xyz, clp);
fprintf(stderr, "%s: want evals = %g %g %g\n", me, xyz[0], xyz[1], xyz[2]);
if (nrrdMaybeAlloc_va(nten, nrrdTypeFloat, 4,
AIR_CAST(size_t, 7),
AIR_CAST(size_t, samp),
AIR_CAST(size_t, samp),
AIR_CAST(size_t, samp))) {
airMopAdd(mop, err = biffGetDone(NRRD), airFree, airMopAlways);
fprintf(stderr, "%s: couldn't allocate output:\n%s\n", me, err);
airMopError(mop);
return 1;
}
ELL_3M_IDENTITY_SET(mD);
ELL_3M_DIAG_SET(mD, xyz[0], xyz[1], xyz[2]);
tdata = (float*)nten->data;
for (zi=0; zi<samp; zi++) {
for (yi=0; yi<samp; yi++) {
for (xi=0; xi<samp; xi++) {
q[0] = 1.0;
q[1] = AIR_AFFINE(-0.5, (float)xi, samp-0.5, -1, 1);
q[2] = AIR_AFFINE(-0.5, (float)yi, samp-0.5, -1, 1);
q[3] = AIR_AFFINE(-0.5, (float)zi, samp-0.5, -1, 1);
len = ELL_4V_LEN(q);
ELL_4V_SCALE(q, 1.0/len, q);
washQtoM3(mRF, q);
ELL_3M_TRANSPOSE(mRI, mRF);
ELL_3M_IDENTITY_SET(mT);
ell_3m_post_mul_d(mT, mRI);
ell_3m_post_mul_d(mT, mD);
ell_3m_post_mul_d(mT, mRF);
tdata[0] = 1.0;
TEN_M2T(tdata, mT);
tdata += 7;
}
}
}
if (nrrdSave(outS, nten, NULL)) {
airMopAdd(mop, err = biffGetDone(NRRD), airFree, airMopAlways);
fprintf(stderr, "%s: couldn't save output:\n%s\n", me, err);
airMopError(mop);
return 1;
}
airMopOkay(mop);
return 0;
}
int
main(int argc, char *argv[]) {
gageKind *kind;
char *me, *whatS, *err, *outS, *stackSavePath;
hestParm *hparm;
hestOpt *hopt = NULL;
NrrdKernelSpec *k00, *k11, *k22, *kSS, *kSSblur;
float pos[3], lineInfo[4];
double gmc, rangeSS[2], posSS, *scalePos;
unsigned int ansLen, numSS, ninSSIdx, lineStepNum;
int what, E=0, renorm, SSrenorm, SSuniform, verbose;
const double *answer;
Nrrd *nin, **ninSS=NULL, *nout=NULL;
gageContext *ctx;
gagePerVolume *pvl;
limnPolyData *lpld=NULL;
airArray *mop;
int worldSpace;
Nrrd *ngrad=NULL, *nbmat=NULL;
double bval, eps;
unsigned int *skip, skipNum;
mop = airMopNew();
me = argv[0];
hparm = hestParmNew();
airMopAdd(mop, hparm, (airMopper)hestParmFree, airMopAlways);
hparm->elideSingleOtherType = AIR_TRUE;
hestOptAdd(&hopt, "i", "nin", airTypeOther, 1, 1, &nin, NULL,
"input volume", NULL, NULL, nrrdHestNrrd);
hestOptAdd(&hopt, "k", "kind", airTypeOther, 1, 1, &kind, NULL,
"\"kind\" of volume (\"scalar\", \"vector\", or \"tensor\")",
NULL, NULL, &probeKindHestCB);
hestOptAdd(&hopt, "p", "x y z", airTypeFloat, 3, 3, pos, NULL,
"the position in space at which to probe");
hestOptAdd(&hopt, "wsp", NULL, airTypeInt, 0, 0, &worldSpace, NULL,
"if using this option, position (\"-p\") will be in world "
"space, instead of index space (the default)");
hestOptAdd(&hopt, "pi", "lpld in", airTypeOther, 1, 1, &lpld, "",
"input polydata (overrides \"-p\")",
NULL, NULL, limnHestPolyDataLMPD);
hestOptAdd(&hopt, "pl", "x y z s", airTypeFloat, 4, 4, lineInfo, "0 0 0 0",
"probe along line, instead of at point. "
"The \"-p\" three coords are the line start point. "
"If \"s\" is zero, (x,y,z) is the line end point. "
"If \"s\" is non-zero, (x,y,z) is the line direction, "
"which is scaled to have length \"s\", "
"and then used as the step between line samples. ");
hestOptAdd(&hopt, "pln", "num", airTypeUInt, 1, 1, &lineStepNum, "0",
"if non-zero, number of steps of probing to do along line, "
"which overrides \"-p\" and \"-pi\"");
hestOptAdd(&hopt, "v", "verbosity", airTypeInt, 1, 1, &verbose, "1",
"verbosity level");
hestOptAdd(&hopt, "q", "query", airTypeString, 1, 1, &whatS, NULL,
"the quantity (scalar, vector, or matrix) to learn by probing");
hestOptAdd(&hopt, "eps", "epsilon", airTypeDouble, 1, 1, &eps, "0",
"if non-zero, and if query is a scalar, epsilon around probe "
"location where we will do discrete differences to find the "
"gradient and hessian (for debugging)");
hestOptAdd(&hopt, "k00", "kern00", airTypeOther, 1, 1, &k00,
"tent", "kernel for gageKernel00",
NULL, NULL, nrrdHestKernelSpec);
hestOptAdd(&hopt, "k11", "kern11", airTypeOther, 1, 1, &k11,
"cubicd:1,0", "kernel for gageKernel11",
NULL, NULL, nrrdHestKernelSpec);
hestOptAdd(&hopt, "k22", "kern22", airTypeOther, 1, 1, &k22,
"cubicdd:1,0", "kernel for gageKernel22",
NULL, NULL, nrrdHestKernelSpec);
hestOptAdd(&hopt, "ssn", "SS #", airTypeUInt, 1, 1, &numSS,
"0", "how many scale-space samples to evaluate, or, "
"0 to turn-off all scale-space behavior");
hestOptAdd(&hopt, "ssr", "scale range", airTypeDouble, 2, 2, rangeSS,
"nan nan", "range of scales in scale-space");
hestOptAdd(&hopt, "sss", "scale save path", airTypeString, 1, 1,
&stackSavePath, "",
"give a non-empty path string (like \"./\") to save out "
"the pre-blurred volumes computed for the stack");
hestOptAdd(&hopt, "ssp", "SS pos", airTypeDouble, 1, 1, &posSS, "0",
"position at which to sample in scale-space");
hestOptAdd(&hopt, "kssblur", "kernel", airTypeOther, 1, 1, &kSSblur,
"dgauss:1,5", "blurring kernel, to sample scale space",
NULL, NULL, nrrdHestKernelSpec);
hestOptAdd(&hopt, "kss", "kernel", airTypeOther, 1, 1, &kSS,
"tent", "kernel for reconstructing from scale space samples",
NULL, NULL, nrrdHestKernelSpec);
hestOptAdd(&hopt, "ssrn", "ssrn", airTypeInt, 1, 1, &SSrenorm, "0",
"enable derivative normalization based on scale space");
hestOptAdd(&hopt, "ssu", NULL, airTypeInt, 0, 0, &SSuniform, NULL,
"do uniform samples along sigma, and not (by default) "
"samples according to the logarithm of diffusion time");
hestOptAdd(&hopt, "rn", NULL, airTypeInt, 0, 0, &renorm, NULL,
"renormalize kernel weights at each new sample location. "
"\"Accurate\" kernels don't need this; doing it always "
"makes things go slower");
hestOptAdd(&hopt, "gmc", "min gradmag", airTypeDouble, 1, 1, &gmc,
"0.0", "For curvature-based queries, use zero when gradient "
"magnitude is below this");
hestOptAdd(&hopt, "o", "nout", airTypeString, 1, 1, &outS, "-",
"output array, when probing on polydata vertices");
hestParseOrDie(hopt, argc-1, argv+1, hparm,
me, probeInfo, AIR_TRUE, AIR_TRUE, AIR_TRUE);
airMopAdd(mop, hopt, (airMopper)hestOptFree, airMopAlways);
airMopAdd(mop, hopt, (airMopper)hestParseFree, airMopAlways);
what = airEnumVal(kind->enm, whatS);
if (!what) {
/* 0 indeed always means "unknown" for any gageKind */
fprintf(stderr, "%s: couldn't parse \"%s\" as measure of \"%s\" volume\n",
me, whatS, kind->name);
hestUsage(stderr, hopt, me, hparm);
hestGlossary(stderr, hopt, hparm);
airMopError(mop);
return 1;
}
if (ELL_4V_LEN(lineInfo) && !lineStepNum) {
fprintf(stderr, "%s: gave line info (\"-pl\") but not "
"# samples (\"-pln\")", me);
hestUsage(stderr, hopt, me, hparm);
hestGlossary(stderr, hopt, hparm);
airMopError(mop);
return 1;
}
/* special set-up required for DWI kind */
if (!strcmp(TEN_DWI_GAGE_KIND_NAME, kind->name)) {
if (tenDWMRIKeyValueParse(&ngrad, &nbmat, &bval, &skip, &skipNum, nin)) {
airMopAdd(mop, err = biffGetDone(TEN), airFree, airMopAlways);
fprintf(stderr, "%s: trouble parsing DWI info:\n%s\n", me, err);
airMopError(mop); return 1;
}
if (skipNum) {
fprintf(stderr, "%s: sorry, can't do DWI skipping in tenDwiGage", me);
airMopError(mop); return 1;
}
/* this could stand to use some more command-line arguments */
if (tenDwiGageKindSet(kind, 50, 1, bval, 0.001, ngrad, nbmat,
tenEstimate1MethodLLS,
tenEstimate2MethodQSegLLS,
/* randSeed */ 7919)) {
airMopAdd(mop, err = biffGetDone(TEN), airFree, airMopAlways);
fprintf(stderr, "%s: trouble parsing DWI info:\n%s\n", me, err);
airMopError(mop); return 1;
}
}
ansLen = kind->table[what].answerLength;
/* for setting up pre-blurred scale-space samples */
if (numSS) {
unsigned int vi;
ninSS = AIR_CAST(Nrrd **, calloc(numSS, sizeof(Nrrd *)));
scalePos = AIR_CAST(double *, calloc(numSS, sizeof(double)));
if (!(ninSS && scalePos)) {
fprintf(stderr, "%s: couldn't allocate ninSS", me);
airMopError(mop); return 1;
}
for (ninSSIdx=0; ninSSIdx<numSS; ninSSIdx++) {
ninSS[ninSSIdx] = nrrdNew();
airMopAdd(mop, ninSS[ninSSIdx], (airMopper)nrrdNuke, airMopAlways);
}
if (SSuniform) {
for (vi=0; vi<numSS; vi++) {
scalePos[vi] = AIR_AFFINE(0, vi, numSS-1, rangeSS[0], rangeSS[1]);
}
} else {
double rangeTau[2], tau;
rangeTau[0] = gageTauOfSig(rangeSS[0]);
rangeTau[1] = gageTauOfSig(rangeSS[1]);
for (vi=0; vi<numSS; vi++) {
tau = AIR_AFFINE(0, vi, numSS-1, rangeTau[0], rangeTau[1]);
scalePos[vi] = gageSigOfTau(tau);
}
}
if (verbose > 2) {
fprintf(stderr, "%s: sampling scale range %g--%g %suniformly:\n", me,
rangeSS[0], rangeSS[1], SSuniform ? "" : "non-");
for (vi=0; vi<numSS; vi++) {
fprintf(stderr, " scalePos[%u] = %g\n", vi, scalePos[vi]);
}
}
if (gageStackBlur(ninSS, scalePos, numSS,
nin, kind->baseDim, kSSblur,
nrrdBoundaryBleed, AIR_TRUE,
verbose)) {
airMopAdd(mop, err = biffGetDone(GAGE), airFree, airMopAlways);
fprintf(stderr, "%s: trouble pre-computing blurrings:\n%s\n", me, err);
airMopError(mop); return 1;
}
if (airStrlen(stackSavePath)) {
char fnform[AIR_STRLEN_LARGE];
sprintf(fnform, "%s/blur-%%02u.nrrd", stackSavePath);
fprintf(stderr, "%s: |%s|\n", me, fnform);
if (nrrdSaveMulti(fnform, AIR_CAST(const Nrrd *const *, ninSS),
numSS, 0, NULL)) {
airMopAdd(mop, err = biffGetDone(NRRD), airFree, airMopAlways);
fprintf(stderr, "%s: trouble saving blurrings:\n%s\n", me, err);
airMopError(mop); return 1;
}
}
} else {
/*
** unlike the thing above, this one assumes that the quaternions in qq
** have already been normalized, and, that the sum of wght[] is 1.0
*/
int
ell_q_avgN_d(double mm[4], unsigned int *iterP,
const double *qq, double *qlog,
const double *wght, const unsigned int NN,
const double eps, const unsigned int maxIter) {
static const char me[]="ell_q_avgN_d";
double tmp, qdiv[4], elen;
unsigned int ii, iter;
/* iterP optional */
/* wght optional, to signify equal 1/NN weighting for all */
if (!( mm && qq )) {
biffAddf(ELL, "%s: got NULL pointer", me);
return 1;
}
if (!( eps >= 0 )) {
biffAddf(ELL, "%s: need eps >= 0 (not %g)", me, eps);
return 1;
}
/* initialize with euclidean mean */
ELL_4V_SET(mm, 0, 0, 0, 0);
for (ii=0; ii<NN; ii++) {
double ww;
ww = wght ? wght[ii] : 1.0/NN;
ELL_4V_SCALE_INCR(mm, ww, qq + 4*ii);
}
ELL_4V_NORM(mm, mm, tmp);
iter = 0;
do {
double logavg[4], qexp[4];
/* take log of everyone */
for (ii=0; ii<NN; ii++) {
ell_q_div_d(qdiv, mm, qq + 4*ii); /* div = mm^1 * qq[ii] */
ell_q_log_d(qlog + 4*ii, qdiv);
}
/* average, and find length */
ELL_4V_SET(logavg, 0, 0, 0, 0);
for (ii=0; ii<NN; ii++) {
double ww;
ww = wght ? wght[ii] : 1.0/NN;
ELL_4V_SCALE_INCR(logavg, ww, qlog + 4*ii);
}
elen = ELL_4V_LEN(logavg);
/* use exp to put it back on S^3 */
ell_q_exp_d(qexp, logavg);
ell_q_mul_d(mm, mm, qexp);
iter++;
} while ((!maxIter || iter < maxIter) && elen > eps);
if (elen > eps) {
biffAddf(ELL, "%s: still have error %g (> eps %g) after max %d iters", me,
elen, eps, maxIter);
return 1;
}
if (iterP) {
*iterP = iter;
}
return 0;
}
int
ell_q_avg4_d(double m[4], unsigned int *iterP,
const double _q1[4], const double _q2[4],
const double _q3[4], const double _q4[4],
const double _wght[4],
const double eps, const unsigned int maxIter) {
static const char me[]="ell_q_avg4_d";
double N, elen, a[4], b[4], c[4], d[4],
tmp[4], la[4], lb[4], lc[4], ld[4], u[4], wght[4];
unsigned int iter;
/* *iterP optional */
if (!( m && _q1 && _q2 && _q3 && _q4 && _wght )) {
biffAddf(ELL, "%s: got NULL pointer", me);
return 1;
}
if (!( eps >= 0 )) {
biffAddf(ELL, "%s: need eps >= 0 (not %g)", me, eps);
return 1;
}
/* normalize (wrt L2) all given quaternions */
ELL_4V_NORM(a, _q1, N);
ELL_4V_NORM(b, _q2, N);
ELL_4V_NORM(c, _q3, N);
ELL_4V_NORM(d, _q4, N);
/* normalize (wrt L1) the given weights */
ELL_4V_COPY(wght, _wght);
N = wght[0] + wght[1] + wght[2] + wght[3];
ELL_4V_SCALE(wght, 1.0/N, wght);
/* initialize mean to normalized euclidean mean */
ELL_4V_SCALE_ADD4(m, wght[0], a, wght[1], b, wght[2], c, wght[3], d);
ELL_4V_NORM(m, m, N);
iter = 0;
do {
/* take log of everyone */
ell_q_div_d(tmp, m, a); ell_q_log_d(la, tmp);
ell_q_div_d(tmp, m, b); ell_q_log_d(lb, tmp);
ell_q_div_d(tmp, m, c); ell_q_log_d(lc, tmp);
ell_q_div_d(tmp, m, d); ell_q_log_d(ld, tmp);
/* average, and find length */
ELL_4V_SCALE_ADD4(u, wght[0], la, wght[1], lb, wght[2], lc, wght[3], ld);
elen = ELL_4V_LEN(u);
/* use exp to put it back on S^3 */
ell_q_exp_d(tmp, u); ell_q_mul_d(m, m, tmp);
iter++;
} while ((!maxIter || iter < maxIter) && elen > eps);
if (elen > eps) {
biffAddf(ELL, "%s: still have error %g after max %d iters", me,
elen, maxIter);
return 1;
}
if (iterP) {
*iterP = iter;
}
return 0;
}
int
_pullPointProcess(pullTask *task, pullBin *bin, pullPoint *point) {
char me[]="pullPointProcess", err[BIFF_STRLEN];
double energyOld, energyNew, force[4], distLimit, posOld[4],
capvec[3], caplen, capscl; /* related to capping distance traveled
in a per-iteration way */
int stepBad, giveUp;
if (!point->stepEnergy) {
sprintf(err, "%s: whoa, point %u step is zero!", me, point->idtag);
biffAdd(PULL, err); return 1;
}
if (0 && 162 == point->idtag) {
fprintf(stderr, "!%s: !!!!!!!!!!!! praying ON!\n", me);
_pullPraying = AIR_TRUE;
ELL_3V_COPY(_pullPrayCorner[0][0], point->pos);
_pullPrayCorner[0][0][2] -= 1;
ELL_3V_COPY(_pullPrayCorner[0][1], point->pos);
_pullPrayCorner[0][1][2] += 1;
fprintf(stderr, "!%s: corner[0][0] = %g %g %g\n", me,
_pullPrayCorner[0][0][0],
_pullPrayCorner[0][0][1],
_pullPrayCorner[0][0][2]);
fprintf(stderr, "!%s: corner[0][1] = %g %g %g\n", me,
_pullPrayCorner[0][1][0],
_pullPrayCorner[0][1][1],
_pullPrayCorner[0][1][2]);
} else {
_pullPraying = AIR_FALSE;
}
if (_pullPraying) {
fprintf(stderr, "%s: =============================== (%u) hi @ %g %g %g\n",
me, point->idtag, point->pos[0], point->pos[1], point->pos[2]);
}
energyOld = _pullPointEnergyTotal(task, bin, point, force);
if (_pullPraying) {
fprintf(stderr, "!%s: =================== point %u has:\n "
" energy = %g ; ndist = %g, force %g %g %g %g\n", me,
point->idtag, energyOld, point->neighDist,
force[0], force[1], force[2], force[3]);
}
if (!( AIR_EXISTS(energyOld) && ELL_4V_EXISTS(force) )) {
sprintf(err, "%s: point %u non-exist energy or force", me, point->idtag);
biffAdd(PULL, err); return 1;
}
if (task->pctx->constraint) {
/* we have a constraint, so do something to get the force more
tangential to the constriant surface */
double proj[9], pfrc[3];
_pullConstraintTangent(task, point, proj);
ELL_3MV_MUL(pfrc, proj, force);
ELL_3V_COPY(force, pfrc);
}
point->status = 0; /* reset status bitflag */
ELL_4V_COPY(posOld, point->pos);
_pullPointHistInit(point);
_pullPointHistAdd(point, pullCondOld);
if (!ELL_4V_LEN(force)) {
/* this particle has no reason to go anywhere; we're done with it */
point->energy = energyOld;
return 0;
}
distLimit = _pullDistLimit(task, point);
/* find capscl */
ELL_3V_SCALE(capvec, point->stepEnergy, force);
caplen = ELL_3V_LEN(capvec);
if (caplen > distLimit) {
capscl = distLimit/caplen;
} else {
capscl = 1;
}
if (_pullPraying) {
fprintf(stderr, "%s: ======= (%u) capscl = %g\n", me,
point->idtag, capscl);
}
if (_pullPraying) {
double nfrc[3], len, phold[4], ee;
int cfail;
ELL_4V_COPY(phold, point->pos);
fprintf(stderr, "!%s(%u,%u): energy(%g,%g,%g) = %f (original)\n",
me, task->pctx->iter, point->idtag,
point->pos[0], point->pos[1], point->pos[2], energyOld);
ELL_4V_SCALE_ADD2(point->pos, 1.0, posOld,
capscl*point->stepEnergy, force);
ELL_3V_COPY(_pullPrayCorner[1][0], point->pos);
_pullPrayCorner[1][0][2] -= 1;
ELL_3V_COPY(_pullPrayCorner[1][1], point->pos);
_pullPrayCorner[1][1][2] += 1;
fprintf(stderr, "!%s: corner[1][0] = %g %g %g\n", me,
_pullPrayCorner[1][0][0],
_pullPrayCorner[1][0][1],
_pullPrayCorner[1][0][2]);
fprintf(stderr, "!%s: corner[1][1] = %g %g %g\n", me,
_pullPrayCorner[1][1][0],
_pullPrayCorner[1][1][1],
_pullPrayCorner[1][1][2]);
#define PROBE(l) if (_pullProbe(task, point)) { \
sprintf(err, "%s: while praying", me); \
biffAdd(PULL, err); return 1; \
} \
(l) = _pullPointScalar(task->pctx, point, \
pullInfoHeight, NULL, NULL);
if (1) {
double *enr, *lpl, uu, vv, vpos[2][3], ll, mid[3];
unsigned int ui, vi;
Nrrd *nenr, *nlpl;
nenr = nrrdNew();
nlpl = nrrdNew();
nrrdMaybeAlloc_nva(nenr, nrrdTypeDouble, 2, _pullPrayRes);
enr = AIR_CAST(double *, nenr->data);
nrrdMaybeAlloc_nva(nlpl, nrrdTypeDouble, 2, _pullPrayRes);
lpl = AIR_CAST(double *, nlpl->data);
for (vi=0; vi<_pullPrayRes[1]; vi++) {
vv = AIR_AFFINE(0, vi, _pullPrayRes[1]-1, 0, 1);
ELL_3V_LERP(vpos[0], vv, _pullPrayCorner[0][0], _pullPrayCorner[0][1]);
ELL_3V_LERP(vpos[1], vv, _pullPrayCorner[1][0], _pullPrayCorner[1][1]);
for (ui=0; ui<_pullPrayRes[0]; ui++) {
uu = AIR_AFFINE(0, ui, _pullPrayRes[0]-1, 0, 1);
ELL_3V_LERP(point->pos, uu, vpos[0], vpos[1]);
PROBE(ll);
lpl[ui + _pullPrayRes[0]*vi] = ll;
enr[ui + _pullPrayRes[0]*vi] = _pullPointEnergyTotal(task, bin,
point, NULL);
}
}
nrrdSave("nenr.nrrd", nenr, NULL);
nrrdSave("nlpl.nrrd", nlpl, NULL);
nenr = nrrdNuke(nenr);
nlpl = nrrdNuke(nlpl);
}
#undef PROBE
ELL_4V_COPY(point->pos, phold);
_pullPointEnergyTotal(task, bin, point, NULL);
}
int
main(int argc, char *argv[]) {
char *me, *err, *outS;
hestOpt *hopt=NULL;
airArray *mop;
int sx, sy, xi, yi, samp, version, whole, right;
float *tdata;
double p[3], xyz[3], q[4], len, hackcp=0, maxca;
double ca, cp, mD[9], mRF[9], mRI[9], mT[9], hack;
Nrrd *nten;
mop = airMopNew();
me = argv[0];
hestOptAdd(&hopt, "n", "# samples", airTypeInt, 1, 1, &samp, "4",
"number of glyphs along each edge of triangle");
hestOptAdd(&hopt, "p", "x y z", airTypeDouble, 3, 3, p, NULL,
"location in quaternion quotient space");
hestOptAdd(&hopt, "ca", "max ca", airTypeDouble, 1, 1, &maxca, "0.8",
"maximum ca to use at bottom edge of triangle");
hestOptAdd(&hopt, "r", NULL, airTypeInt, 0, 0, &right, NULL,
"sample a right-triangle-shaped region, instead of "
"a roughly equilateral triangle. ");
hestOptAdd(&hopt, "w", NULL, airTypeInt, 0, 0, &whole, NULL,
"sample the whole triangle of constant trace, "
"instead of just the "
"sixth of it in which the eigenvalues have the "
"traditional sorted order. ");
hestOptAdd(&hopt, "hack", "hack", airTypeDouble, 1, 1, &hack, "0.04",
"this is a hack");
hestOptAdd(&hopt, "v", "version", airTypeInt, 1, 1, &version, "1",
"which version of the Westin metrics to use to parameterize "
"triangle; \"1\" for ISMRM 97, \"2\" for MICCAI 99");
hestOptAdd(&hopt, "o", "nout", airTypeString, 1, 1, &outS, "-",
"output file to save tensors into");
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);
nten = nrrdNew();
airMopAdd(mop, nten, (airMopper)nrrdNuke, airMopAlways);
if (!( 1 == version || 2 == version )) {
fprintf(stderr, "%s: version must be 1 or 2 (not %d)\n", me, version);
airMopError(mop);
return 1;
}
if (right) {
sx = samp;
sy = (int)(1.0*samp/sqrt(3.0));
} else {
sx = 2*samp-1;
sy = samp;
}
if (nrrdMaybeAlloc_va(nten, nrrdTypeFloat, 4,
AIR_CAST(size_t, 7),
AIR_CAST(size_t, sx),
AIR_CAST(size_t, sy),
AIR_CAST(size_t, 3))) {
airMopAdd(mop, err = biffGetDone(NRRD), airFree, airMopAlways);
fprintf(stderr, "%s: couldn't allocate output:\n%s\n", me, err);
airMopError(mop);
return 1;
}
q[0] = 1.0;
q[1] = p[0];
q[2] = p[1];
q[3] = p[2];
len = ELL_4V_LEN(q);
ELL_4V_SCALE(q, 1.0/len, q);
washQtoM3(mRF, q);
ELL_3M_TRANSPOSE(mRI, mRF);
if (right) {
_ra2t(nten, 0.00, 0.0, mRI, mRF, hack);
_ra2t(nten, 0.10, 0.0, mRI, mRF, hack);
_ra2t(nten, 0.10, 60.0, mRI, mRF, hack);
_ra2t(nten, 0.20, 0.0, mRI, mRF, hack);
_ra2t(nten, 0.20, 30.0, mRI, mRF, hack);
_ra2t(nten, 0.20, 60.0, mRI, mRF, hack);
_ra2t(nten, 0.30, 0.0, mRI, mRF, hack);
_ra2t(nten, 0.30, 20.0, mRI, mRF, hack);
_ra2t(nten, 0.30, 40.0, mRI, mRF, hack);
_ra2t(nten, 0.30, 60.0, mRI, mRF, hack);
_ra2t(nten, 0.40, 0.0, mRI, mRF, hack);
_ra2t(nten, 0.40, 15.0, mRI, mRF, hack);
_ra2t(nten, 0.40, 30.0, mRI, mRF, hack);
_ra2t(nten, 0.40, 45.0, mRI, mRF, hack);
_ra2t(nten, 0.40, 60.0, mRI, mRF, hack);
_ra2t(nten, 0.50, 0.0, mRI, mRF, hack);
_ra2t(nten, 0.50, 12.0, mRI, mRF, hack);
_ra2t(nten, 0.50, 24.0, mRI, mRF, hack);
_ra2t(nten, 0.50, 36.0, mRI, mRF, hack);
_ra2t(nten, 0.50, 48.0, mRI, mRF, hack);
_ra2t(nten, 0.50, 60.0, mRI, mRF, hack);
/* _ra2t(nten, 0.60, 30.0, mRI, mRF, hack); */
_ra2t(nten, 0.60, 40.0, mRI, mRF, hack);
_ra2t(nten, 0.60, 50.0, mRI, mRF, hack);
_ra2t(nten, 0.60, 60.0, mRI, mRF, hack);
/* _ra2t(nten, 0.70, 34.3, mRI, mRF, hack); */
/* _ra2t(nten, 0.70, 42.8, mRI, mRF, hack); */
_ra2t(nten, 0.70, 51.4, mRI, mRF, hack);
_ra2t(nten, 0.70, 60.0, mRI, mRF, hack);
/* _ra2t(nten, 0.80, 45.0, mRI, mRF, hack); */
_ra2t(nten, 0.80, 52.5, mRI, mRF, hack);
_ra2t(nten, 0.80, 60.0, mRI, mRF, hack);
_ra2t(nten, 0.90, 60.0, mRI, mRF, hack);
_ra2t(nten, 1.00, 60.0, mRI, mRF, hack);
/*
_ra2t(nten, 0.000, 0.0, mRI, mRF, hack);
_ra2t(nten, 0.125, 0.0, mRI, mRF, hack);
_ra2t(nten, 0.125, 60.0, mRI, mRF, hack);
_ra2t(nten, 0.250, 0.0, mRI, mRF, hack);
_ra2t(nten, 0.250, 30.0, mRI, mRF, hack);
_ra2t(nten, 0.250, 60.0, mRI, mRF, hack);
_ra2t(nten, 0.375, 0.0, mRI, mRF, hack);
_ra2t(nten, 0.375, 20.0, mRI, mRF, hack);
_ra2t(nten, 0.375, 40.0, mRI, mRF, hack);
_ra2t(nten, 0.375, 60.0, mRI, mRF, hack);
_ra2t(nten, 0.500, 0.0, mRI, mRF, hack);
_ra2t(nten, 0.500, 15.0, mRI, mRF, hack);
_ra2t(nten, 0.500, 30.0, mRI, mRF, hack);
_ra2t(nten, 0.500, 45.0, mRI, mRF, hack);
_ra2t(nten, 0.500, 60.0, mRI, mRF, hack);
_ra2t(nten, 0.625, 37.0, mRI, mRF, hack);
_ra2t(nten, 0.625, 47.5, mRI, mRF, hack);
_ra2t(nten, 0.625, 60.0, mRI, mRF, hack);
_ra2t(nten, 0.750, 49.2, mRI, mRF, hack);
_ra2t(nten, 0.750, 60.0, mRI, mRF, hack);
_ra2t(nten, 0.875, 60.0, mRI, mRF, hack);
_ra2t(nten, 1.000, 60.0, mRI, mRF, hack);
*/
nten->axis[1].spacing = 1;
nten->axis[2].spacing = (sx-1)/(sqrt(3.0)*(sy-1));
nten->axis[3].spacing = 1;
} else {
for (yi=0; yi<samp; yi++) {
if (whole) {
ca = AIR_AFFINE(0, yi, samp-1, 0.0, 1.0);
} else {
ca = AIR_AFFINE(0, yi, samp-1, hack, maxca);
hackcp = AIR_AFFINE(0, yi, samp-1, hack, 0);
}
for (xi=0; xi<=yi; xi++) {
if (whole) {
cp = AIR_AFFINE(0, xi, samp-1, 0.0, 1.0);
} else {
cp = AIR_AFFINE(0, xi, samp-1, hackcp, maxca-hack/2.0);
}
_cap2xyz(xyz, ca, cp, version, whole);
/*
fprintf(stderr, "%s: (%d,%d) -> (%g,%g) -> %g %g %g\n", me,
yi, xi, ca, cp, xyz[0], xyz[1], xyz[2]);
*/
ELL_3M_IDENTITY_SET(mD);
ELL_3M_DIAG_SET(mD, xyz[0], xyz[1], xyz[2]);
ELL_3M_IDENTITY_SET(mT);
ell_3m_post_mul_d(mT, mRI);
ell_3m_post_mul_d(mT, mD);
ell_3m_post_mul_d(mT, mRF);
tdata = (float*)nten->data +
7*(2*(samp-1-xi) - (samp-1-yi) + (2*samp-1)*((samp-1-yi) + samp));
tdata[0] = 1.0;
TEN_M2T(tdata, mT);
}
}
nten->axis[1].spacing = 1;
nten->axis[2].spacing = 1.5;
nten->axis[3].spacing = 1;
}
if (nrrdSave(outS, nten, NULL)) {
airMopAdd(mop, err = biffGetDone(NRRD), airFree, airMopAlways);
fprintf(stderr, "%s: couldn't save output:\n%s\n", me, err);
airMopError(mop);
return 1;
}
airMopOkay(mop);
return 0;
}
int
main(int argc, char *argv[]) {
float angleA_f, axisA_f[3], angleB_f, axisB_f[3],
qA_f[4], qB_f[4], qC_f[4],
mat3A_f[9], mat4A_f[16], mat3B_f[9], mat4B_f[16], mat3C_f[9], mat4C_f[16],
pntA_f[4], pntB_f[4], pntC_f[4];
double angleA_d, axisA_d[3], angleB_d, axisB_d[3],
qA_d[4], qB_d[4], qC_d[4],
mat3A_d[9], mat4A_d[16], mat3B_d[9], mat4B_d[16], mat3C_d[9], mat4C_d[16],
pntA_d[4], pntB_d[4], pntC_d[4];
int I, N;
double tmp, det, frob;
me = argv[0];
N = 100000;
AIR_UNUSED(pntA_d);
AIR_UNUSED(pntB_d);
AIR_UNUSED(pntC_d);
AIR_UNUSED(mat4C_d);
AIR_UNUSED(mat3C_d);
AIR_UNUSED(mat4B_d);
AIR_UNUSED(mat3B_d);
AIR_UNUSED(mat4A_d);
AIR_UNUSED(mat3A_d);
AIR_UNUSED(qC_d);
AIR_UNUSED(qB_d);
AIR_UNUSED(qA_d);
AIR_UNUSED(axisB_d);
AIR_UNUSED(angleB_d);
AIR_UNUSED(axisA_d);
AIR_UNUSED(angleA_d);
AIR_UNUSED(argc);
for (I=0; I<N; I++) {
/* make a rotation (as a quaternion) */
ELL_3V_SET(axisA_f, 2*airDrandMT()-1, 2*airDrandMT()-1, 2*airDrandMT()-1);
ELL_3V_NORM(axisA_f, axisA_f, tmp); /* yea, not uniform, so what */
angleA_f = AIR_PI*(2*airDrandMT()-1);
ell_aa_to_q_f(qA_f, angleA_f, axisA_f);
/* convert to AA and back, and back */
angleB_f = ell_q_to_aa_f(axisB_f, qA_f);
if (ELL_3V_DOT(axisB_f, axisA_f) < 0) {
ELL_3V_SCALE(axisB_f, -1, axisB_f);
angleB_f *= -1;
}
ELL_3V_SUB(axisA_f, axisA_f, axisB_f);
printf(" aa -> q -> aa error: %g, %g\n",
CA + AIR_ABS(angleA_f - angleB_f), CA + ELL_3V_LEN(axisA_f));
/* convert to 3m and back, and back */
ell_q_to_3m_f(mat3A_f, qA_f);
ell_3m_to_q_f(qB_f, mat3A_f);
if (ELL_4V_DOT(qA_f, qB_f) < 0) {
ELL_4V_SCALE(qB_f, -1, qB_f);
}
ELL_4V_SUB(qC_f, qA_f, qB_f);
ELL_Q_TO_3M(mat3B_f, qA_f);
ELL_3M_SUB(mat3C_f, mat3B_f, mat3A_f);
printf(" q -> 3m -> q error: %g, %g\n",
CA + ELL_4V_LEN(qC_f), CA + ELL_3M_FROB(mat3C_f));
/* convert to 4m and back, and back */
ell_q_to_4m_f(mat4A_f, qA_f);
ell_4m_to_q_f(qB_f, mat4A_f);
if (ELL_4V_DOT(qA_f, qB_f) < 0) {
ELL_4V_SCALE(qB_f, -1, qB_f);
}
ELL_4V_SUB(qC_f, qA_f, qB_f);
ELL_Q_TO_4M(mat4B_f, qA_f);
ELL_4M_SUB(mat4C_f, mat4B_f, mat4A_f);
printf(" q -> 4m -> q error: %g, %g\n",
CA + ELL_4V_LEN(qC_f), CA + ELL_4M_FROB(mat4C_f));
/* make a point that we'll rotate */
ELL_3V_SET(pntA_f, 2*airDrandMT()-1, 2*airDrandMT()-1, 2*airDrandMT()-1);
/* effect rotation in two different ways, and compare results */
ELL_3MV_MUL(pntB_f, mat3A_f, pntA_f);
ell_q_3v_rotate_f(pntC_f, qA_f, pntA_f);
ELL_3V_SUB(pntA_f, pntB_f, pntC_f);
printf(" rotation error = %g\n", CA + ELL_3V_LEN(pntA_f));
/* mix up inversion with conversion */
ell_3m_inv_f(mat3C_f, mat3A_f);
ell_3m_to_q_f(qB_f, mat3C_f);
ell_q_mul_f(qC_f, qA_f, qB_f);
if (ELL_4V_DOT(qA_f, qC_f) < 0) {
ELL_4V_SCALE(qC_f, -1, qC_f);
}
printf(" inv mul = %g %g %g %g\n", qC_f[0],
CA + qC_f[1], CA + qC_f[2], CA + qC_f[3]);
ell_q_inv_f(qC_f, qB_f);
ELL_4V_SUB(qC_f, qB_f, qB_f);
printf(" inv diff = %g %g %g %g\n", CA + qC_f[0],
CA + qC_f[1], CA + qC_f[2], CA + qC_f[3]);
/* exp and log */
ell_q_log_f(qC_f, qA_f);
ell_q_log_f(qB_f, qC_f);
ell_q_exp_f(qC_f, qB_f);
ell_q_exp_f(qB_f, qC_f);
ELL_4V_SUB(qC_f, qB_f, qA_f);
printf(" exp/log diff = %g %g %g %g\n", CA + qC_f[0],
CA + qC_f[1], CA + qC_f[2], CA + qC_f[3]);
/* pow, not very exhaustive */
ell_q_to_3m_f(mat3A_f, qA_f);
ell_3m_post_mul_f(mat3A_f, mat3A_f);
ell_3m_post_mul_f(mat3A_f, mat3A_f);
ell_q_pow_f(qB_f, qA_f, 4);
ell_q_to_3m_f(mat3B_f, qB_f);
ELL_3M_SUB(mat3B_f, mat3B_f, mat3A_f);
printf(" pow diff = %g\n", CA + ELL_3M_FROB(mat3B_f));
if (ELL_3M_FROB(mat3B_f) > 2) {
printf(" start q = %g %g %g %g\n", qA_f[0], qA_f[1], qA_f[2], qA_f[3]);
angleA_f = ell_q_to_aa_f(axisA_f, qA_f);
printf(" --> aa = %g (%g %g %g)\n", angleA_f,
axisA_f[0], axisA_f[1], axisA_f[2]);
printf(" q^3 = %g %g %g %g\n", qB_f[0], qB_f[1], qB_f[2], qB_f[3]);
angleA_f = ell_q_to_aa_f(axisA_f, qB_f);
printf(" --> aa = %g (%g %g %g)\n", angleA_f,
axisA_f[0], axisA_f[1], axisA_f[2]);
exit(1);
}
/* make sure it looks like a rotation matrix */
ell_q_to_3m_f(mat3A_f, qA_f);
det = ELL_3M_DET(mat3A_f);
frob = ELL_3M_FROB(mat3A_f);
ELL_3M_TRANSPOSE(mat3B_f, mat3A_f);
ell_3m_inv_f(mat3C_f, mat3A_f);
ELL_3M_SUB(mat3C_f, mat3B_f, mat3C_f);
printf(" det = %g; size = %g; err = %g\n", det, frob*frob/3,
CA + ELL_3M_FROB(mat3C_f));
}
exit(0);
}
