void
ell_q_to_4m_f(float m[16], const float q[4]) {
float u[4], w=0.0, x=0.0, y=0.0, z=0.0;
w = AIR_CAST(float, ELL_3V_LEN(q));
ELL_4V_SCALE(u, 1.0f/w, q);
_ELL_Q_TO_4M(float);
}
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_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);
}
void
ell_q_pow_f(float q2[4], const float q1[4], const float p) {
float len, angle, axis[3];
len = AIR_CAST(float, pow(ELL_4V_LEN(q1), p));
angle = ell_q_to_aa_f(axis, q1);
ell_aa_to_q_f(q2, p*angle, axis);
ELL_4V_SCALE(q2, len, q2);
}
/*
** its in here that we scale from "energy gradient" to "force"
*/
double
_pullPointEnergyTotal(pullTask *task, pullBin *bin, pullPoint *point,
/* output */
double force[4]) {
char me[]="_pullPointEnergyTotal";
double enrIm, enrPt, egradIm[4], egradPt[4], energy;
ELL_4V_SET(egradIm, 0, 0, 0, 0); /* sssh */
ELL_4V_SET(egradPt, 0, 0, 0, 0); /* sssh */
enrIm = _energyFromImage(task, point,
force ? egradIm : NULL);
enrPt = _energyFromPoints(task, bin, point,
force ? egradPt : NULL);
energy = AIR_LERP(task->pctx->alpha, enrIm, enrPt);
/*
fprintf(stderr, "!%s(%u): energy = lerp(%g, im %g, pt %g) = %g\n", me,
point->idtag, task->pctx->alpha, enrIm, enrPt, energy);
*/
if (force) {
ELL_4V_LERP(force, task->pctx->alpha, egradIm, egradPt);
ELL_4V_SCALE(force, -1, force);
/*
fprintf(stderr, "!%s(%u): egradIm = %g %g %g %g\n", me, point->idtag,
egradIm[0], egradIm[1], egradIm[2], egradIm[3]);
fprintf(stderr, "!%s(%u): egradPt = %g %g %g %g\n", me, point->idtag,
egradPt[0], egradPt[1], egradPt[2], egradPt[3]);
fprintf(stderr, "!%s(%u): ---> force = %g %g %g %g\n", me,
point->idtag, force[0], force[1], force[2], force[3]);
*/
}
if (task->pctx->wall) {
unsigned int axi;
double frc;
for (axi=0; axi<4; axi++) {
frc = task->pctx->bboxMin[axi] - point->pos[axi];
if (frc < 0) {
/* not below min */
frc = task->pctx->bboxMax[axi] - point->pos[axi];
if (frc > 0) {
/* not above max */
frc = 0;
}
}
energy += task->pctx->wall*frc*frc/2;
if (force) {
force[axi] += task->pctx->wall*frc;
}
}
}
if (!AIR_EXISTS(energy)) {
fprintf(stderr, "!%s(%u): HEY! non-exist energy %g\n", me, point->idtag,
energy);
}
return energy;
}
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;
}
void
ell_4m_to_q_f(float q[4], const float m[16]) {
_ELL_M_TO_Q( float, 0, 1, 2, 4, 5, 6, 8, 9, 10);
len = AIR_CAST(float, ELL_4V_LEN(q));
ELL_4V_SCALE(q, 1.0f/len, q);
}
void
ell_3m_to_q_f(float q[4], const float m[9]) {
_ELL_M_TO_Q( float, 0, 1, 2, 3, 4, 5, 6, 7, 8);
len = AIR_CAST(float, ELL_4V_LEN(q));
ELL_4V_SCALE(q, 1.0f/len, q);
}
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;
}
/*
******** limnCameraPathMake
**
** uses limnSplines to do camera paths based on key-frames
**
** output: cameras at all "numFrames" frames are set in the
** PRE-ALLOCATED array of output cameras, "cam".
**
** input:
** keycam: array of keyframe cameras
** time: times associated with the key frames
** ---> both of these arrays are length "numKeys" <---
** trackWhat: takes values from the limnCameraPathTrack* enum
** quatType: spline to control camera orientations. This is needed for
** tracking at or from, but not needed for limnCameraPathTrackBoth.
** This is the only limnSplineTypeSpec* argument that can be NULL.
** posType: spline to control whichever of from, at, and up are needed for
** the given style of tracking.
** distType: spline to control neer, faar, dist: positions of near clipping,
** far clipping, and image plane, as well as the
** distance between from and at (which is used if not doing
** limnCameraPathTrackBoth)
** viewType: spline to control fov (and aspect, if you're crazy)
**
** NOTE: The "atRelative", "orthographic", and "rightHanded" fields
** are copied from keycam[0] into all output cam[i], but you still need
** to correctly set them for all keycam[i] for limnCameraUpdate to work
** as expected. Also, for the sake of simplicity, this function only works
** with fov and aspect, instead of {u,v}Range, and hence both "fov" and
** "aspect" need to set in *all* the keycams, even if neither of them
** ever changes!
*/
int
limnCameraPathMake(limnCamera *cam, int numFrames,
limnCamera *keycam, double *time, int numKeys,
int trackWhat,
limnSplineTypeSpec *quatType,
limnSplineTypeSpec *posType,
limnSplineTypeSpec *distType,
limnSplineTypeSpec *viewType) {
static const char me[]="limnCameraPathMake";
char which[AIR_STRLEN_MED];
airArray *mop;
Nrrd *nquat, *nfrom, *natpt, *nupvc, *ndist, *nfova, *ntime, *nsample;
double fratVec[3], *quat, *from, *atpt, *upvc, *dist, *fova,
W2V[9], N[3], fratDist;
limnSpline *timeSpline, *quatSpline, *fromSpline, *atptSpline, *upvcSpline,
*distSpline, *fovaSpline;
limnSplineTypeSpec *timeType;
int ii, E;
if (!( cam && keycam && time && posType && distType && viewType )) {
biffAddf(LIMN, "%s: got NULL pointer", me);
return 1;
}
if (!( AIR_IN_OP(limnCameraPathTrackUnknown, trackWhat,
limnCameraPathTrackLast) )) {
biffAddf(LIMN, "%s: trackWhat %d not in valid range [%d,%d]", me,
trackWhat, limnCameraPathTrackUnknown+1,
limnCameraPathTrackLast-1);
return 1;
}
if (limnCameraPathTrackBoth != trackWhat && !quatType) {
biffAddf(LIMN, "%s: need the quaternion limnSplineTypeSpec if not "
"doing trackBoth", me);
return 1;
}
/* create and allocate nrrds. For the time being, we're allocating
more different nrrds, and filling their contents, than we need
to-- nquat is not needed if we're doing limnCameraPathTrackBoth,
for example. However, we do make an effort to only do the spline
evaluation on the things we actually need to know. */
mop = airMopNew();
airMopAdd(mop, nquat = nrrdNew(), (airMopper)nrrdNuke, airMopAlways);
airMopAdd(mop, nfrom = nrrdNew(), (airMopper)nrrdNuke, airMopAlways);
airMopAdd(mop, natpt = nrrdNew(), (airMopper)nrrdNuke, airMopAlways);
airMopAdd(mop, nupvc = nrrdNew(), (airMopper)nrrdNuke, airMopAlways);
airMopAdd(mop, ndist = nrrdNew(), (airMopper)nrrdNuke, airMopAlways);
airMopAdd(mop, nfova = nrrdNew(), (airMopper)nrrdNuke, airMopAlways);
airMopAdd(mop, ntime = nrrdNew(), (airMopper)nrrdNix, airMopAlways);
if (nrrdWrap_va(ntime, time, nrrdTypeDouble, 1,
AIR_CAST(size_t, numKeys))) {
biffMovef(LIMN, NRRD, "%s: trouble wrapping time values", me);
airMopError(mop); return 1;
}
airMopAdd(mop, nsample = nrrdNew(), (airMopper)nrrdNuke, airMopAlways);
timeType = limnSplineTypeSpecNew(limnSplineTypeTimeWarp);
airMopAdd(mop, timeType, (airMopper)limnSplineTypeSpecNix, airMopAlways);
if (nrrdMaybeAlloc_va(nquat, nrrdTypeDouble, 2,
AIR_CAST(size_t, 4), AIR_CAST(size_t, numKeys))
|| nrrdMaybeAlloc_va(nfrom, nrrdTypeDouble, 2,
AIR_CAST(size_t, 3), AIR_CAST(size_t, numKeys))
|| nrrdMaybeAlloc_va(natpt, nrrdTypeDouble, 2,
AIR_CAST(size_t, 3), AIR_CAST(size_t, numKeys))
|| nrrdMaybeAlloc_va(nupvc, nrrdTypeDouble, 2,
AIR_CAST(size_t, 3), AIR_CAST(size_t, numKeys))
|| nrrdMaybeAlloc_va(ndist, nrrdTypeDouble, 2,
AIR_CAST(size_t, 4), AIR_CAST(size_t, numKeys))
|| nrrdMaybeAlloc_va(nfova, nrrdTypeDouble, 2,
AIR_CAST(size_t, 2), AIR_CAST(size_t, numKeys))) {
biffMovef(LIMN, NRRD, "%s: couldn't allocate buffer nrrds", me);
airMopError(mop); return 1;
}
quat = (double*)(nquat->data);
from = (double*)(nfrom->data);
atpt = (double*)(natpt->data);
upvc = (double*)(nupvc->data);
dist = (double*)(ndist->data);
fova = (double*)(nfova->data);
/* check cameras, and put camera information into nrrds */
for (ii=0; ii<numKeys; ii++) {
if (limnCameraUpdate(keycam + ii)) {
biffAddf(LIMN, "%s: trouble with camera at keyframe %d\n", me, ii);
airMopError(mop); return 1;
}
if (!( AIR_EXISTS(keycam[ii].fov) && AIR_EXISTS(keycam[ii].aspect) )) {
biffAddf(LIMN, "%s: fov, aspect not both defined on keyframe %d",
me, ii);
airMopError(mop); return 1;
}
ell_4m_to_q_d(quat + 4*ii, keycam[ii].W2V);
if (ii) {
if (0 > ELL_4V_DOT(quat + 4*ii, quat + 4*(ii-1))) {
ELL_4V_SCALE(quat + 4*ii, -1, quat + 4*ii);
}
}
ELL_3V_COPY(from + 3*ii, keycam[ii].from);
ELL_3V_COPY(atpt + 3*ii, keycam[ii].at);
ELL_3V_COPY(upvc + 3*ii, keycam[ii].up);
ELL_3V_SUB(fratVec, keycam[ii].from, keycam[ii].at);
fratDist = ELL_3V_LEN(fratVec);
ELL_4V_SET(dist + 4*ii, fratDist,
keycam[ii].neer, keycam[ii].dist, keycam[ii].faar);
ELL_2V_SET(fova + 2*ii, keycam[ii].fov, keycam[ii].aspect);
}
/* create splines from nrrds */
if (!( (strcpy(which, "quaternion"), quatSpline =
limnSplineCleverNew(nquat, limnSplineInfoQuaternion, quatType))
&& (strcpy(which, "from point"), fromSpline =
limnSplineCleverNew(nfrom, limnSplineInfo3Vector, posType))
&& (strcpy(which, "at point"), atptSpline =
limnSplineCleverNew(natpt, limnSplineInfo3Vector, posType))
&& (strcpy(which, "up vector"), upvcSpline =
limnSplineCleverNew(nupvc, limnSplineInfo3Vector, posType))
&& (strcpy(which, "plane distances"), distSpline =
limnSplineCleverNew(ndist, limnSplineInfo4Vector, distType))
&& (strcpy(which, "field-of-view"), fovaSpline =
limnSplineCleverNew(nfova, limnSplineInfo2Vector, viewType))
&& (strcpy(which, "time warp"), timeSpline =
limnSplineCleverNew(ntime, limnSplineInfoScalar, timeType)) )) {
biffAddf(LIMN, "%s: trouble creating %s spline", me, which);
airMopError(mop); return 1;
}
airMopAdd(mop, quatSpline, (airMopper)limnSplineNix, airMopAlways);
airMopAdd(mop, fromSpline, (airMopper)limnSplineNix, airMopAlways);
airMopAdd(mop, atptSpline, (airMopper)limnSplineNix, airMopAlways);
airMopAdd(mop, upvcSpline, (airMopper)limnSplineNix, airMopAlways);
airMopAdd(mop, distSpline, (airMopper)limnSplineNix, airMopAlways);
airMopAdd(mop, fovaSpline, (airMopper)limnSplineNix, airMopAlways);
airMopAdd(mop, timeSpline, (airMopper)limnSplineNix, airMopAlways);
/* evaluate splines */
E = AIR_FALSE;
if (!E) E |= limnSplineSample(nsample, timeSpline,
limnSplineMinT(timeSpline), numFrames,
limnSplineMaxT(timeSpline));
quat = NULL;
from = NULL;
atpt = NULL;
upvc = NULL;
switch(trackWhat) {
case limnCameraPathTrackAt:
if (!E) E |= limnSplineNrrdEvaluate(natpt, atptSpline, nsample);
if (!E) atpt = (double*)(natpt->data);
if (!E) E |= limnSplineNrrdEvaluate(nquat, quatSpline, nsample);
if (!E) quat = (double*)(nquat->data);
break;
case limnCameraPathTrackFrom:
if (!E) E |= limnSplineNrrdEvaluate(nfrom, fromSpline, nsample);
if (!E) from = (double*)(nfrom->data);
if (!E) E |= limnSplineNrrdEvaluate(nquat, quatSpline, nsample);
if (!E) quat = (double*)(nquat->data);
break;
case limnCameraPathTrackBoth:
if (!E) E |= limnSplineNrrdEvaluate(nfrom, fromSpline, nsample);
if (!E) from = (double*)(nfrom->data);
if (!E) E |= limnSplineNrrdEvaluate(natpt, atptSpline, nsample);
if (!E) atpt = (double*)(natpt->data);
if (!E) E |= limnSplineNrrdEvaluate(nupvc, upvcSpline, nsample);
if (!E) upvc = (double*)(nupvc->data);
break;
}
dist = NULL;
if (!E) E |= limnSplineNrrdEvaluate(ndist, distSpline, nsample);
if (!E) dist = (double*)(ndist->data);
fova = NULL;
if (!E) E |= limnSplineNrrdEvaluate(nfova, fovaSpline, nsample);
if (!E) fova = (double*)(nfova->data);
if (E) {
biffAddf(LIMN, "%s: trouble evaluating splines", me);
airMopError(mop); return 1;
}
/* copy information from nrrds back into cameras */
for (ii=0; ii<numFrames; ii++) {
cam[ii].atRelative = keycam[0].atRelative;
cam[ii].orthographic = keycam[0].orthographic;
cam[ii].rightHanded = keycam[0].rightHanded;
if (limnCameraPathTrackBoth == trackWhat) {
ELL_3V_COPY(cam[ii].from, from + 3*ii);
ELL_3V_COPY(cam[ii].at, atpt + 3*ii);
ELL_3V_COPY(cam[ii].up, upvc + 3*ii);
} else {
fratDist = (dist + 4*ii)[0];
ell_q_to_3m_d(W2V, quat + 4*ii);
ELL_3MV_ROW1_GET(cam[ii].up, W2V);
if (cam[ii].rightHanded) {
ELL_3V_SCALE(cam[ii].up, -1, cam[ii].up);
}
ELL_3MV_ROW2_GET(N, W2V);
if (limnCameraPathTrackFrom == trackWhat) {
ELL_3V_COPY(cam[ii].from, from + 3*ii);
ELL_3V_SCALE_ADD2(cam[ii].at, 1.0, cam[ii].from, fratDist, N);
} else {
ELL_3V_COPY(cam[ii].at, atpt + 3*ii);
ELL_3V_SCALE_ADD2(cam[ii].from, 1.0, cam[ii].at, -fratDist, N);
}
}
cam[ii].neer = (dist + 4*ii)[1];
cam[ii].dist = (dist + 4*ii)[2];
cam[ii].faar = (dist + 4*ii)[3];
cam[ii].fov = (fova + 2*ii)[0];
cam[ii].aspect = (fova + 2*ii)[1];
if (limnCameraUpdate(cam + ii)) {
biffAddf(LIMN, "%s: trouble with output camera %d\n", me, ii);
airMopError(mop); return 1;
}
}
airMopOkay(mop);
return 0;
}
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);
}
