static int
constraintSatIso(pullTask *task, pullPoint *point,
double stepMax, unsigned int iterMax,
/* output */
int *constrFailP) {
static const char me[]="constraintSatIso";
double
step, /* current step size */
val, aval, /* last and current function values */
hack, /* how to control re-tries in the context of a single
for-loop, instead of a nested do-while loop */
grad[4], dir[3], len, state[1 + 1 + 3 + 3];
unsigned int iter = 0; /* 0: initial probe, 1..iterMax: probes in loop */
PROBE(val, aval, grad);
SAVE(state, aval, val, grad, point->pos);
hack = 1;
for (iter=1; iter<=iterMax; iter++) {
/* consider? http://en.wikipedia.org/wiki/Halley%27s_method */
NORMALIZE(dir, grad, len);
if (!len) {
/* no gradient; back off */
hack *= task->pctx->sysParm.backStepScale;
RESTORE(aval, val, grad, point->pos, state);
continue;
}
step = -val/len; /* the newton-raphson step */
step = step > 0 ? AIR_MIN(stepMax, step) : AIR_MAX(-stepMax, step);
ELL_3V_SCALE_INCR(point->pos, hack*step, dir);
_pullPointHistAdd(point, pullCondConstraintSatA);
PROBE(val, aval, grad);
if (aval <= state[0]) { /* we're no further from the root */
if (AIR_ABS(step) < stepMax*task->pctx->sysParm.constraintStepMin) {
/* we have converged! */
break;
}
SAVE(state, aval, val, grad, point->pos);
hack = 1;
} else { /* oops, try again, don't update dir or len, reset val */
hack *= task->pctx->sysParm.backStepScale;
RESTORE(aval, val, grad, point->pos, state);
}
}
if (iter > iterMax) {
*constrFailP = pullConstraintFailIterMaxed;
} else {
*constrFailP = AIR_FALSE;
}
return 0;
}
static int
constraintSatLapl(pullTask *task, pullPoint *point,
double stepMax, unsigned int iterMax,
/* output */
int *constrFailP) {
static const char me[]="constraintSatLapl";
double
step, /* current step size */
valLast, val, /* last and current function values */
grad[4], dir[3], len,
posOld[3], posNew[3], tmpv[3];
double a=0, b=1, s, fa, fb, fs, tmp, diff;
int side = 0;
unsigned int iter = 0; /* 0: initial probe, 1..iterMax: probes in loop */
step = stepMax/2;
PROBEG(val, grad);
if (0 == val) {
/* already exactly at the zero, we're done. This actually happens! */
/* printf("!%s: a lapl == 0!\n", me); */
return 0;
}
valLast = val;
NORMALIZE(dir, grad, len);
/* first phase: follow normalized gradient until laplacian sign change */
for (iter=1; iter<=iterMax; iter++) {
double sgn;
ELL_3V_COPY(posOld, point->pos);
sgn = airSgn(val); /* lapl < 0 => downhill; lapl > 0 => uphill */
ELL_3V_SCALE_INCR(point->pos, sgn*step, dir);
_pullPointHistAdd(point, pullCondConstraintSatA);
PROBEG(val, grad);
if (val*valLast < 0) {
/* laplacian has changed sign; stop looking */
break;
}
valLast = val;
NORMALIZE(dir, grad, len);
}
if (iter > iterMax) {
*constrFailP = pullConstraintFailIterMaxed;
return 0;
}
/* second phase: find the zero-crossing, looking between
f(posOld)=valLast and f(posNew)=val */
ELL_3V_COPY(posNew, point->pos);
ELL_3V_SUB(tmpv, posNew, posOld);
len = ELL_3V_LEN(tmpv);
fa = valLast;
fb = val;
if (AIR_ABS(fa) < AIR_ABS(fb)) {
ELL_SWAP2(a, b, tmp); ELL_SWAP2(fa, fb, tmp);
}
for (iter=1; iter<=iterMax; iter++) {
s = AIR_AFFINE(fa, 0, fb, a, b);
ELL_3V_LERP(point->pos, s, posOld, posNew);
_pullPointHistAdd(point, pullCondConstraintSatB);
PROBE(fs);
if (0 == fs) {
/* exactly nailed the zero, we're done. This actually happens! */
printf("!%s: b lapl == 0!\n", me);
break;
}
/* "Illinois" false-position. Dumb, but it works. */
if (fs*fb > 0) { /* not between s and b */
b = s;
fb = fs;
if (+1 == side) {
fa /= 2;
}
side = +1;
} else { /* not between a and s */
a = s;
fa = fs;
if (-1 == side) {
fb /= 2;
}
side = -1;
}
diff = (b - a)*len;
if (AIR_ABS(diff) < stepMax*task->pctx->sysParm.constraintStepMin) {
/* converged! */
break;
}
}
if (iter > iterMax) {
*constrFailP = pullConstraintFailIterMaxed;
} else {
*constrFailP = AIR_FALSE;
}
return 0;
}
static int
constraintSatHght(pullTask *task, pullPoint *point,
int tang1Use, int tang2Use,
int negtang1Use, int negtang2Use,
double stepMax, unsigned int iterMax,
int *constrFailP) {
static const char me[]="constraintSatHght";
double val, grad[3], hess[9], posproj[9], negproj[9],
state[1+3+9+9+9+3], hack, step,
d1, d2, pdir[3], plen, pgrad[3];
#if PRAYING
double _tmpv[3]={0,0,0};
#endif
int havePos, haveNeg, haveNada;
unsigned int iter = 0; /* 0: initial probe, 1..iterMax: probes in loop */
/* http://en.wikipedia.org/wiki/Newton%27s_method_in_optimization */
havePos = tang1Use || tang2Use;
haveNeg = negtang1Use || negtang2Use;
haveNada = !havePos && !haveNeg;
#if PRAYING
{
double stpmin;
/* HEY: shouldn't stpmin also be used later in this function? */
stpmin = task->pctx->voxelSizeSpace*task->pctx->sysParm.constraintStepMin;
fprintf(stderr, "!%s(%u): starting at %g %g %g %g\n", me, point->idtag,
point->pos[0], point->pos[1], point->pos[2], point->pos[3]);
fprintf(stderr, "!%s: pt %d %d nt %d %d (nada %d) "
"stepMax %g, iterMax %u\n", me,
tang1Use, tang2Use, negtang1Use, negtang2Use, haveNada,
stepMax, iterMax);
fprintf(stderr, "!%s: stpmin = %g = voxsize %g * parm.stepmin %g\n", me,
stpmin, task->pctx->voxelSizeSpace,
task->pctx->sysParm.constraintStepMin);
}
#endif
_pullPointHistAdd(point, pullCondOld);
PROBE(val, grad, hess, posproj, negproj);
#if PRAYING
PRINT("initial probe");
#endif
SAVE(state, val, grad, hess, posproj, negproj, point->pos);
hack = 1;
for (iter=1; iter<=iterMax; iter++) {
#if PRAYING
fprintf(stderr, "!%s: =============== begin iter %u\n", me, iter);
#endif
/* HEY: no opportunistic increase of hack? */
if (havePos || haveNada) {
POSNORM(d1, d2, pdir, plen, pgrad, grad, hess, posproj);
if (!plen) {
/* this use to be a biff error, which got to be annoying */
*constrFailP = pullConstraintFailProjGradZeroA;
return 0;
}
step = (d2 <= 0 ? -plen : -d1/d2);
#if PRAYING
fprintf(stderr, "!%s: (+) iter %u step = (%g <= 0 ? %g : %g) --> %g\n",
me, iter, d2, -plen, -d1/d2, step);
#endif
step = step > 0 ? AIR_MIN(stepMax, step) : AIR_MAX(-stepMax, step);
if (AIR_ABS(step) < stepMax*task->pctx->sysParm.constraintStepMin) {
/* no further iteration needed; we're converged */
#if PRAYING
fprintf(stderr, " |step| %g < %g*%g = %g ==> converged!\n",
AIR_ABS(step),
stepMax, task->pctx->sysParm.constraintStepMin,
stepMax*task->pctx->sysParm.constraintStepMin);
#endif
if (!haveNeg) {
break;
} else {
goto nextstep;
}
}
/* else we have to take a significant step */
#if PRAYING
fprintf(stderr, " -> step %g, |pdir| = %g\n",
step, ELL_3V_LEN(pdir));
ELL_3V_COPY(_tmpv, point->pos);
fprintf(stderr, " -> pos (%g,%g,%g,%g) += %g * %g * (%g,%g,%g)\n",
point->pos[0], point->pos[1], point->pos[2], point->pos[3],
hack, step, pdir[0], pdir[1], pdir[2]);
#endif
ELL_3V_SCALE_INCR(point->pos, hack*step, pdir);
#if PRAYING
ELL_3V_SUB(_tmpv, _tmpv, point->pos);
fprintf(stderr, " -> moved to %g %g %g %g\n",
point->pos[0], point->pos[1], point->pos[2], point->pos[3]);
fprintf(stderr, " (moved %g)\n", ELL_3V_LEN(_tmpv));
#endif
_pullPointHistAdd(point, pullCondConstraintSatA);
PROBE(val, grad, hess, posproj, negproj);
#if PRAYING
fprintf(stderr, " (+) probed at (%g,%g,%g,%g)\n",
point->pos[0], point->pos[1], point->pos[2], point->pos[3]);
PRINT("after move");
fprintf(stderr, " val(%g,%g,%g,%g)=%g %s state[0]=%g\n",
point->pos[0], point->pos[1], point->pos[2], point->pos[3],
val, val <= state[0] ? "<=" : ">", state[0]);
#endif
if (val <= state[0]) {
/* we made progress */
#if PRAYING
fprintf(stderr, " (+) progress!\n");
#endif
SAVE(state, val, grad, hess, posproj, negproj, point->pos);
hack = 1;
} else {
/* oops, we went uphill instead of down; try again */
#if PRAYING
fprintf(stderr, " val *increased*; backing hack from %g to %g\n",
hack, hack*task->pctx->sysParm.backStepScale);
#endif
hack *= task->pctx->sysParm.backStepScale;
RESTORE(val, grad, hess, posproj, negproj, point->pos, state);
#if PRAYING
fprintf(stderr, " restored to pos (%g,%g,%g,%g)\n",
point->pos[0], point->pos[1], point->pos[2], point->pos[3]);
#endif
}
}
nextstep:
if (haveNeg) {
/* HEY: copy and paste from above, minus fluff */
NEGNORM(d1, d2, pdir, plen, pgrad, grad, hess, negproj);
if (!plen && !haveNeg) {
/* this use to be a biff error, which got to be annoying */
*constrFailP = pullConstraintFailProjGradZeroA;
return 0;
}
step = (d2 <= 0 ? -plen : -d1/d2);
#if PRAYING
fprintf(stderr, "!%s: -+) iter %u step = (%g <= 0 ? %g : %g) --> %g\n",
me, iter, d2, -plen, -d1/d2, step);
#endif
step = step > 0 ? AIR_MIN(stepMax, step) : AIR_MAX(-stepMax, step);
if (AIR_ABS(step) < stepMax*task->pctx->sysParm.constraintStepMin) {
#if PRAYING
fprintf(stderr, " |step| %g < %g*%g = %g ==> converged!\n",
AIR_ABS(step),
stepMax, task->pctx->sysParm.constraintStepMin,
stepMax*task->pctx->sysParm.constraintStepMin);
#endif
/* no further iteration needed; we're converged */
break;
}
/* else we have to take a significant step */
#if PRAYING
fprintf(stderr, " -> step %g, |pdir| = %g\n",
step, ELL_3V_LEN(pdir));
ELL_3V_COPY(_tmpv, point->pos);
fprintf(stderr, " -> pos (%g,%g,%g,%g) += %g * %g * (%g,%g,%g)\n",
point->pos[0], point->pos[1], point->pos[2], point->pos[3],
hack, step, pdir[0], pdir[1], pdir[2]);
#endif
ELL_3V_SCALE_INCR(point->pos, hack*step, pdir);
#if PRAYING
ELL_3V_SUB(_tmpv, _tmpv, point->pos);
fprintf(stderr, " -> moved to %g %g %g %g\n",
point->pos[0], point->pos[1], point->pos[2], point->pos[3]);
fprintf(stderr, " (moved %g)\n", ELL_3V_LEN(_tmpv));
#endif
_pullPointHistAdd(point, pullCondConstraintSatA);
PROBE(val, grad, hess, posproj, negproj);
#if PRAYING
fprintf(stderr, " (-) probed at (%g,%g,%g,%g)\n",
point->pos[0], point->pos[1], point->pos[2], point->pos[3]);
PRINT("after move");
fprintf(stderr, " val(%g,%g,%g,%g)=%g %s state[0]=%g\n",
point->pos[0], point->pos[1], point->pos[2], point->pos[3],
val, val >= state[0] ? ">=" : "<", state[0]);
#endif
if (val >= state[0]) {
/* we made progress */
#if PRAYING
fprintf(stderr, " (-) progress!\n");
#endif
SAVE(state, val, grad, hess, posproj, negproj, point->pos);
hack = 1;
} else {
/* oops, we went uphill instead of down; try again */
#if PRAYING
fprintf(stderr, " val *increased*; backing hack from %g to %g\n",
hack, hack*task->pctx->sysParm.backStepScale);
#endif
hack *= task->pctx->sysParm.backStepScale;
RESTORE(val, grad, hess, posproj, negproj, point->pos, state);
#if PRAYING
fprintf(stderr, " restored to pos (%g,%g,%g,%g)\n",
point->pos[0], point->pos[1], point->pos[2], point->pos[3]);
#endif
}
}
}
if (iter > iterMax) {
*constrFailP = pullConstraintFailIterMaxed;
} else {
*constrFailP = AIR_FALSE;
}
/*
printf("!%s: %d %s\n", me, *constrFailP,
*constrFailP ? "FAILED!" : "ok");
*/
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);
}
