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;
}
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);
}
