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));
}
/* returns the index into unitq[] of the quaternion that led to the
right alignment. If it was already aligned, this will be 0,
because unitq[0] is the identity quaternion */
int
_tenQGL_q_align(double qOut[4], const double qRef[4], const double qIn[4]) {
unsigned int ii, maxDotIdx;
double unitq[8][4] = {{+1, 0, 0, 0},
{-1, 0, 0, 0},
{0, +1, 0, 0},
{0, -1, 0, 0},
{0, 0, +1, 0},
{0, 0, -1, 0},
{0, 0, 0, +1},
{0, 0, 0, -1}};
double dot[8], qInMul[8][4], maxDot;
for (ii=0; ii<8; ii++) {
ell_q_mul_d(qInMul[ii], qIn, unitq[ii]);
dot[ii] = ELL_4V_DOT(qRef, qInMul[ii]);
}
maxDotIdx = 0;
maxDot = dot[maxDotIdx];
for (ii=1; ii<8; ii++) {
if (dot[ii] > maxDot) {
maxDotIdx = ii;
maxDot = dot[maxDotIdx];
}
}
ELL_4V_COPY(qOut, qInMul[maxDotIdx]);
return maxDotIdx;
}
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
main(int argc, char *argv[]) {
char *me, *outS;
hestOpt *hopt;
hestParm *hparm;
airArray *mop;
double tval[6], ABC[3], geom[3], rnth[3], RA, norm, mu2, tmpr=0, tmpa=0,
xroot[3], yroot[3], bbox[4], htick, htth, psc;
wheelPS wps;
int correct, labels, drawRA;
me = argv[0];
mop = airMopNew();
hparm = hestParmNew();
hparm->elideMultipleNonExistFloatDefault = AIR_TRUE;
hopt = NULL;
airMopAdd(mop, hparm, (airMopper)hestParmFree, airMopAlways);
hestOptAdd(&hopt, "t", "a b c d e f", airTypeDouble, 6, 6, tval,
"nan nan nan nan nan nan nan",
"six values of symmetric tensors");
hestOptAdd(&hopt, "ABC", "A B C", airTypeDouble, 3, 3, ABC, "nan nan nan",
"directly give coefficients of cubic polynomial "
"(and override info from \"-t\")");
hestOptAdd(&hopt, "g", "c rad th", airTypeDouble, 3, 3, geom, "nan nan nan",
"directly give center, radius, and angle (in degrees) of wheel "
"(and override info from \"-t\" and \"-ABC\"");
hestOptAdd(&hopt, "p", "RA norm th", airTypeDouble, 3, 3, rnth,
"nan nan nan",
"directly give RA, norm, and angle (in degrees) of tensor "
"(and override info from \"-t\", \"-ABC\", and \"-geom\"");
hestOptAdd(&hopt, "correct", NULL, airTypeInt, 0, 0, &correct, NULL,
"when using \"-g\", be honest about what the estimated "
"acos(sqrt(2)*skew)/3 is going to be");
hestOptAdd(&hopt, "labels", NULL, airTypeInt, 0, 0, &labels, NULL,
"put little labels on things; fix with psfrag in LaTeX");
hestOptAdd(&hopt, "RA", NULL, airTypeInt, 0, 0, &drawRA, NULL,
"draw extra geometry associated with RA");
hestOptAdd(&hopt, "htick", "pos", airTypeDouble, 1, 1, &htick, "nan",
"location of single tick mark on horizontal axis");
hestOptAdd(&hopt, "htth", "thick", airTypeDouble, 1, 1, &htth, "3",
"thickness of horizontal tick");
hestOptAdd(&hopt, "bb", "bbox", airTypeDouble, 4, 4, bbox,
"nan nan nan nan", "bounding box, in world space around the "
"region of the graph that should be drawn to EPS");
hestOptAdd(&hopt, "ysc", "scale", airTypeDouble, 1, 1, &(wps.yscale), "0.5",
"scaling on Y axis for drawing graph of characteristic "
"polynomial, or \"0\" to turn this off.");
hestOptAdd(&hopt, "psc", "scale", airTypeDouble, 1, 1, &psc, "100",
"scaling from world space to PostScript points");
hestOptAdd(&hopt, "o", "filename", airTypeString, 1, 1, &outS, "-",
"file to write EPS output to");
hestParseOrDie(hopt, argc-1, argv+1, hparm,
me, wheelInfo, AIR_TRUE, AIR_TRUE, AIR_TRUE);
airMopAdd(mop, hopt, (airMopper)hestOptFree, airMopAlways);
airMopAdd(mop, hopt, (airMopper)hestParseFree, airMopAlways);
if (!(wps.file = airFopen(outS, stdout, "wb"))) {
fprintf(stderr, "%s: couldn't open output file\n", me);
airMopError(mop); return 1;
}
airMopAdd(mop, wps.file, (airMopper)airFclose, airMopAlways);
if (AIR_EXISTS(rnth[0])) {
RA = rnth[0];
norm = rnth[1];
mu2 = (norm*norm/3)*(2*RA*RA/(1 + 2*RA*RA));
geom[0] = sqrt(mu2)/(sqrt(2)*RA);
geom[1] = sqrt(2*mu2);
geom[2] = rnth[2];
wheelGeomToRoot(xroot, yroot, geom);
wheelGeomToABC(ABC, geom[0], geom[1], geom[2]);
} else if (AIR_EXISTS(geom[0])) {
wheelGeomToRoot(xroot, yroot, geom);
if (correct) {
tval[0] = xroot[0];
tval[1] = tval[2] = 0;
tval[3] = xroot[1];
tval[4] = 0;
tval[5] = xroot[2];
wheelTenToGeom(geom,
tval[0], tval[1], tval[2], tval[3], tval[4], tval[5]);
wheelGeomToRoot(xroot, yroot, geom);
}
wheelGeomToABC(ABC, geom[0], geom[1], geom[2]);
wheelGeomToRNTH(rnth, geom[0], geom[1], geom[2]);
} else if (AIR_EXISTS(ABC[0])) {
wheelABCToGeom(geom, ABC[0], ABC[1], ABC[2]);
wheelGeomToRNTH(rnth, geom[0], geom[1], geom[2]);
wheelGeomToRoot(xroot, yroot, geom);
} else {
wheelTenToGeom(geom, tval[0], tval[1], tval[2], tval[3], tval[4], tval[5]);
wheelGeomToRoot(xroot, yroot, geom);
wheelGeomToRNTH(rnth, geom[0], geom[1], geom[2]);
wheelGeomToABC(ABC, geom[0], geom[1], geom[2]);
}
fprintf(stderr, "%s: RNTH: %g %g %g\n", me, rnth[0], rnth[1], rnth[2]);
fprintf(stderr, "%s: ABC: %g %g %g\n", me, ABC[0], ABC[1], ABC[2]);
fprintf(stderr, "%s: xroot: %g %g %g\n",
me, xroot[0], xroot[1], xroot[2]);
fprintf(stderr, "%s: geom: %g %g %g\n", me, geom[0], geom[1], geom[2]);
if (!AIR_EXISTS(bbox[0])) {
bbox[0] = geom[0] - 1.2*geom[1];
bbox[1] = - 1.2*geom[1];
bbox[2] = geom[0] + 1.2*geom[1];
bbox[3] = + 1.2*geom[1];
fprintf(stderr, "%s: bbox %g %g %g %g\n", me,
bbox[0], bbox[1], bbox[2], bbox[3]);
}
wps.psc = psc;
ELL_4V_COPY(wps.bbox, bbox);
wheelPreamble(&wps);
/* graph */
if (wps.yscale) {
wheelWidth(&wps, 4);
wheelGray(&wps, 0.5);
wheelGraph(&wps, xroot[0], xroot[1], xroot[2]);
}
/* axis */
wheelWidth(&wps, 2);
wheelGray(&wps, 0.0);
wheelLine(&wps, bbox[0], 0, bbox[2], 0);
/* circle */
wheelWidth(&wps, 3);
wheelCircle(&wps, geom[0], 0, geom[1]);
/* spokes */
wheelWidth(&wps, 4);
wheelLine(&wps, geom[0], 0, xroot[0], yroot[0]);
wheelLine(&wps, geom[0], 0, xroot[1], yroot[1]);
wheelLine(&wps, geom[0], 0, xroot[2], yroot[2]);
/* dots at spoke ends */
wheelDot(&wps, xroot[0], yroot[0], 0.025*geom[1]);
wheelDot(&wps, xroot[1], yroot[1], 0.025*geom[1]);
wheelDot(&wps, xroot[2], yroot[2], 0.025*geom[1]);
/* lines from dots to roots */
wheelWidth(&wps, 2);
fprintf(wps.file, "gsave\n");
fprintf(wps.file, "[2 4] 0 setdash\n");
wheelLine(&wps, xroot[0], 0, xroot[0], yroot[0]);
wheelLine(&wps, xroot[1], 0, xroot[1], yroot[1]);
wheelLine(&wps, xroot[2], 0, xroot[2], yroot[2]);
fprintf(wps.file, "grestore\n");
/* tickmarks */
wheelWidth(&wps, 6);
wheelLine(&wps, xroot[0], -0.02*geom[1], xroot[0], 0.02*geom[1]);
wheelLine(&wps, xroot[1], -0.02*geom[1], xroot[1], 0.02*geom[1]);
wheelLine(&wps, xroot[2], -0.02*geom[1], xroot[2], 0.02*geom[1]);
if (AIR_EXISTS(htick)) {
wheelWidth(&wps, htth);
wheelLine(&wps, htick, -0.04, htick, 0.04);
}
/* RA angle */
if (drawRA) {
wheelWidth(&wps, 3);
wheelLine(&wps, 0.0, 0.0, geom[0], geom[1]);
wheelWidth(&wps, 2);
fprintf(wps.file, "gsave\n");
fprintf(wps.file, "[2 4] 0 setdash\n");
wheelLine(&wps, geom[0], geom[1], geom[0], 0);
fprintf(wps.file, "grestore\n");
}
/* labels, if wanted */
if (labels) {
fprintf(wps.file, "/Helvetica findfont 20 scalefont setfont\n");
wheelLabel(&wps, geom[0], 0, "center");
wheelLine(&wps, geom[0], -0.02*geom[1], geom[0], 0.02*geom[1]);
wheelLabel(&wps, (geom[0] + xroot[0])/1.8, yroot[0]/1.8, "radius");
wheelWidth(&wps, 2);
wheelArc(&wps, geom[0], 0, geom[1]/2, 0, geom[2]);
wheelLabel(&wps, geom[0] + geom[1]*cos(AIR_PI*geom[2]/180/2)/2.5,
geom[1]*sin(AIR_PI*geom[2]/180/2)/2.5, "theta");
if (drawRA) {
tmpr = sqrt(geom[0]*geom[0] + geom[1]*geom[1]);
tmpa = atan(2.0*rnth[0]);
wheelWidth(&wps, 2);
wheelArc(&wps, 0, 0, 0.2*tmpr, 0, 180*tmpa/AIR_PI);
wheelLabel(&wps, 0.2*tmpr*cos(tmpa/2), 0.2*tmpr*sin(tmpa/2), "phi");
}
wheelLabel(&wps, xroot[0], yroot[0], "spoke0");
wheelLabel(&wps, xroot[1], yroot[1], "spoke-");
wheelLabel(&wps, xroot[2], yroot[2], "spoke+");
wheelLabel(&wps, xroot[0], 0, "root0");
wheelLabel(&wps, xroot[1], 0, "root-");
wheelLabel(&wps, xroot[2], 0, "root+");
}
wheelEpilog(&wps);
airMopOkay(mop);
exit(0);
}
/*
******** limnCameraUpdate()
**
** sets in cam: W2V, V2W, U, V, N, vspNeer, vspFaar, vspDist
** and, if fov and aspect are set, this also sets uRange and vRange
**
** This does use biff to describe problems with camera settings
*/
int
limnCameraUpdate(limnCamera *cam) {
static const char me[] = "limnCameraUpdate";
double len, bb[4], uu[4], vv[4], nn[4], TT[16], RR[16];
if (!cam) {
biffAddf(LIMN, "%s: got NULL pointer", me);
return 1;
}
ELL_4V_SET(uu, 0, 0, 0, 0);
ELL_4V_SET(vv, 0, 0, 0, 0);
ELL_4V_SET(nn, 0, 0, 0, 0);
ELL_4V_SET(bb, 0, 0, 0, 1);
ELL_3V_SUB(nn, cam->at, cam->from);
len = ELL_3V_LEN(nn);
if (!len) {
biffAddf(LIMN, "%s: cam->at (%g,%g,%g) == cam->from", me,
cam->at[0], cam->at[1], cam->at[2]);
return 1;
}
if (cam->atRelative) {
/* ctx->cam->{neer,dist} are "at" relative */
cam->vspNeer = cam->neer + len;
cam->vspFaar = cam->faar + len;
cam->vspDist = cam->dist + len;
}
else {
/* ctx->cam->{neer,dist} are eye relative */
cam->vspNeer = cam->neer;
cam->vspFaar = cam->faar;
cam->vspDist = cam->dist;
}
if (!(cam->vspNeer > 0 && cam->vspDist > 0 && cam->vspFaar > 0)) {
biffAddf(LIMN, "%s: eye-relative near (%g), dist (%g), or far (%g) <= 0",
me, cam->vspNeer, cam->vspDist, cam->vspFaar);
return 1;
}
if (!(cam->vspNeer <= cam->vspFaar)) {
biffAddf(LIMN, "%s: eye-relative near (%g) further than far (%g)",
me, cam->vspNeer, cam->vspFaar);
return 1 ;
}
if (AIR_EXISTS(cam->fov)) {
if (!( AIR_IN_OP(0.0, cam->fov, 180.0) )) {
biffAddf(LIMN, "%s: cam->fov (%g) not in valid range between 0 and 180",
me, cam->fov);
return 1 ;
}
if (!AIR_EXISTS(cam->aspect)) {
biffAddf(LIMN, "%s: cam->fov set, but cam->aspect isn't", me);
return 1;
}
/* "fov" is half vertical angle */
cam->vRange[0] = -tan(cam->fov*AIR_PI/360)*(cam->vspDist);
cam->vRange[1] = -cam->vRange[0];
cam->uRange[0] = cam->vRange[0]*(cam->aspect);
cam->uRange[1] = -cam->uRange[0];
}
/* else cam->fov isn't set, but we're not going to complain if
uRange and vRange aren't both set ... */
ELL_3V_SCALE(nn, 1.0/len, nn);
ELL_3V_CROSS(uu, nn, cam->up);
len = ELL_3V_LEN(uu);
if (!len) {
biffAddf(LIMN, "%s: cam->up is co-linear with view direction", me);
return 1 ;
}
ELL_3V_SCALE(uu, 1.0/len, uu);
if (cam->rightHanded) {
ELL_3V_CROSS(vv, nn, uu);
}
else {
ELL_3V_CROSS(vv, uu, nn);
}
ELL_4V_COPY(cam->U, uu);
ELL_4V_COPY(cam->V, vv);
ELL_4V_COPY(cam->N, nn);
ELL_4M_TRANSLATE_SET(TT, -cam->from[0], -cam->from[1], -cam->from[2]);
ELL_4M_ROWS_SET(RR, uu, vv, nn, bb);
ELL_4M_MUL(cam->W2V, RR, TT);
ell_4m_inv_d(cam->V2W, cam->W2V);
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);
}
