static T findMin( boost::function<T (double)> f, double ax, double bx, double cx, double eps )
{
// implementation from numerical recipies
const double R = 0.6180339;
const double C = (1.0-R);
T f1,f2;
double x0,x1,x2,x3;
x0 = ax;
x3 = cx;
if( fabs(cx-bx) > fabs(bx-ax) )
{
x1 = bx;
x2 = bx + C*(cx-bx);
}
else
{
x2 = bx;
x1 = bx - C*(bx-ax);
}
f1 = f(x1);
f2 = f(x2);
while (fabs(x3-x0) > eps*(fabs(x1)+fabs(x2)))
{
if (f2 < f1) {
SHIFT3(x0,x1,x2,R*x1+C*x3);
SHIFT2(f1,f2,f(x2));
}
else
{
SHIFT3(x3,x2,x1,R*x2+C*x0);
SHIFT2(f2,f1,f(x1));
}
}
if (f1 < f2)
{
return f1;
}
else
{
return f2;
}
}
void
tend_helixDoit(Nrrd *nout, double bnd,
double orig[3], double i2w[9], double mf[9],
double r, double R, double S, double angle, int incrtwist,
double ev[3], double bgEval) {
int sx, sy, sz, xi, yi, zi;
double th, t0, t1, t2, t3, v1, v2,
wpos[3], vpos[3], mfT[9],
W2H[9], H2W[9], H2C[9], C2H[9], fv[3], rv[3], uv[3], mA[9], mB[9], inside,
tmp[3], len;
float *out;
sx = nout->axis[1].size;
sy = nout->axis[2].size;
sz = nout->axis[3].size;
out = (float*)nout->data;
ELL_3M_TRANSPOSE(mfT, mf);
for (zi=0; zi<sz; zi++) {
fprintf(stderr, "zi = %d/%d\n", zi, sz);
for (yi=0; yi<sy; yi++) {
for (xi=0; xi<sx; xi++) {
ELL_3V_SET(tmp, xi, yi, zi);
ELL_3MV_MUL(vpos, i2w, tmp);
ELL_3V_INCR(vpos, orig);
#define WPOS(pos, th) ELL_3V_SET((pos),R*cos(th), R*sin(th), S*(th)/(2*AIR_PI))
#define VAL(th) (WPOS(wpos, th), ELL_3V_DIST(wpos, vpos))
#define RR 0.61803399
#define CC (1.0-RR)
#define SHIFT3(a,b,c,d) (a)=(b); (b)=(c); (c)=(d)
#define SHIFT2(a,b,c) (a)=(b); (b)=(c)
th = atan2(vpos[1], vpos[0]);
th += 2*AIR_PI*floor(0.5 + vpos[2]/S - th/(2*AIR_PI));
if (S*th/(2*AIR_PI) > vpos[2]) {
t0 = th - AIR_PI; t3 = th;
} else {
t0 = th; t3 = th + AIR_PI;
}
t1 = RR*t0 + CC*t3;
t2 = CC*t0 + RR*t3;
v1 = VAL(t1);
v2 = VAL(t2);
while ( t3-t0 > 0.000001*(AIR_ABS(t1)+AIR_ABS(t2)) ) {
if (v1 < v2) {
SHIFT3(t3, t2, t1, CC*t0 + RR*t2);
SHIFT2(v2, v1, VAL(t1));
} else {
SHIFT3(t0, t1, t2, RR*t1 + CC*t3);
SHIFT2(v1, v2, VAL(t2));
}
}
/* t1 (and t2) are now the th for which the point on the helix
(R*cos(th), R*sin(th), S*(th)/(2*AIR_PI)) is closest to vpos */
WPOS(wpos, t1);
ELL_3V_SUB(wpos, vpos, wpos);
ELL_3V_SET(fv, -R*sin(t1), R*cos(t1), S/AIR_PI); /* helix tangent */
ELL_3V_NORM(fv, fv, len);
ELL_3V_COPY(rv, wpos);
ELL_3V_NORM(rv, rv, len);
len = ELL_3V_DOT(rv, fv);
ELL_3V_SCALE(tmp, -len, fv);
ELL_3V_ADD2(rv, rv, tmp);
ELL_3V_NORM(rv, rv, len); /* rv now normal to helix, closest to
pointing to vpos */
ELL_3V_CROSS(uv, rv, fv);
ELL_3V_NORM(uv, uv, len); /* (rv,fv,uv) now right-handed frame */
ELL_3MV_ROW0_SET(W2H, uv); /* as is (uv,rv,fv) */
ELL_3MV_ROW1_SET(W2H, rv);
ELL_3MV_ROW2_SET(W2H, fv);
ELL_3M_TRANSPOSE(H2W, W2H);
inside = 0.5 - 0.5*airErf((ELL_3V_LEN(wpos)-r)/(bnd + 0.0001));
if (incrtwist) {
th = angle*ELL_3V_LEN(wpos)/r;
} else {
th = angle;
}
ELL_3M_ROTATE_Y_SET(H2C, th);
ELL_3M_TRANSPOSE(C2H, H2C);
ELL_3M_SCALE_SET(mA,
AIR_LERP(inside, bgEval, ev[1]),
AIR_LERP(inside, bgEval, ev[2]),
AIR_LERP(inside, bgEval, ev[0]));
ELL_3M_MUL(mB, mA, H2C);
ELL_3M_MUL(mA, mB, W2H);
ELL_3M_MUL(mB, mA, mf);
ELL_3M_MUL(mA, C2H, mB);
ELL_3M_MUL(mB, H2W, mA);
ELL_3M_MUL(mA, mfT, mB);
TEN_M2T_TT(out, float, mA);
out[0] = 1.0;
out += 7;
}
}
}
return;
}
int
_echoRayIntx_Superquad(RAYINTX_ARGS(Superquad)) {
char me[]="_echoRayIntx_Superquad";
echoPos_t TT=0, Tmin, Tmax, t0, t1, t2, t3, v1, v2, diff, tol,
saveTmin, Vmin, Vmax, VV=0, dV, dVmin, dVmax, tmp,
(*v)(echoPos_t, echoPos_t, echoPos_t,
echoPos_t, echoPos_t),
(*vg)(echoPos_t[3],
echoPos_t, echoPos_t, echoPos_t,
echoPos_t, echoPos_t),
(*lvg)(echoPos_t[3],
echoPos_t, echoPos_t, echoPos_t,
echoPos_t, echoPos_t),
from[3], grad[3], pos[3]; /* these two used only by macros */
int iter;
if (!_echoRayIntx_CubeSolid(&Tmin, &Tmax,
-1-2*ECHO_EPSILON, 1+2*ECHO_EPSILON,
-1-2*ECHO_EPSILON, 1+2*ECHO_EPSILON,
-1-2*ECHO_EPSILON, 1+2*ECHO_EPSILON, ray)) {
return AIR_FALSE;
}
switch(obj->axis) {
case 0:
v = _echo_SuperquadX_v;
vg = _echo_SuperquadX_vg;
lvg = _echo_SuperquadX_lvg;
break;
case 1:
v = _echo_SuperquadY_v;
vg = _echo_SuperquadY_vg;
lvg = _echo_SuperquadY_lvg;
break;
case 2: default:
v = _echo_SuperquadZ_v;
vg = _echo_SuperquadZ_vg;
lvg = _echo_SuperquadZ_lvg;
break;
}
if (tstate->verbose) {
fprintf(stderr, "%s%s: Tmin, Tmax = %g, %g, ax = %d\n",
_echoDot(tstate->depth), me, Tmin, Tmax, obj->axis);
}
#define VAL(TT) \
(ELL_3V_SCALE_ADD2(pos, 1, from, (TT), ray->dir), \
v(pos[0], pos[1], pos[2], obj->A, obj->B))
#define VALGRAD(VV, DV, TT) \
ELL_3V_SCALE_ADD2(pos, 1, from, (TT), ray->dir); \
(VV) = vg(grad, pos[0], pos[1], pos[2], obj->A, obj->B); \
(DV) = ELL_3V_DOT(grad, ray->dir)
#define LVALGRAD(VV, DV, TT) \
ELL_3V_SCALE_ADD2(pos, 1, from, (TT), ray->dir); \
(VV) = lvg(grad, pos[0], pos[1], pos[2], obj->A, obj->B); \
(DV) = ELL_3V_DOT(grad, ray->dir)
#define RR 0.61803399
#define CC (1.0-RR)
#define SHIFT3(a,b,c,d) (a)=(b); (b)=(c); (c)=(d)
#define SHIFT2(a,b,c) (a)=(b); (b)=(c)
/* testing */
ELL_3V_SCALE_ADD2(from, 1, ray->from, Tmin, ray->dir);
saveTmin = Tmin;
Tmin = 0;
Tmax -= saveTmin;
/*
ELL_3V_COPY(from, ray->from);
saveTmin = 0;
*/
/* evaluate value and derivatives at Tmin and Tmax */
VALGRAD(Vmin, dVmin, Tmin);
VALGRAD(Vmax, dVmax, Tmax);
/* if the segment start and end are both positive or both negative,
and if the derivatives also don't change sign, there's no root.
Also, due to convexity, if values at start and end are negative,
then there is no root */
if ( (Vmin*Vmax >= 0 && dVmin*dVmax >= 0)
|| (Vmin <= 0 && Vmax <= 0) ) {
return AIR_FALSE;
}
if (tstate->verbose) {
fprintf(stderr, "%s%s: dVmin = %g, dVmax = %g, Vmin = %g, Vmax = %g\n",
_echoDot(tstate->depth), me, dVmin, dVmax, Vmin, Vmax);
}
/* either the value changed sign, or the derivative changed sign, or
both. If, as is common, the derivatives changed sign, but the
values didn't (which means they are both positive, due to a test
above), we need to limit the interval by minimizing the value
until either we see a negative value, or until the minimization
converged. Based on Golden Section Search, NR pg.401 */
if (dVmin*dVmax < 0 && Vmin*Vmax >= 0) {
t0 = Tmin;
t1 = RR*Tmin + CC*Tmax;
t2 = CC*Tmin + RR*Tmax;
t3 = Tmax;
v1 = VAL(t1);
v2 = VAL(t2);
if (tstate->verbose) {
fprintf(stderr, "%s%s: \n"
" t0 = % 31.15f\n"
" t1 = % 31.15f -> v1 = % 31.15f\n"
" t2 = % 31.15f -> v2 = % 31.15f\n"
" t3 = % 31.15f\n",
_echoDot(tstate->depth), me,
t0, t1, v1, t2, v2, t3);
}
tol = sqrt(ECHO_POS_EPS);
while ( (t3-t0 > tol*(t1+t2)) /* still haven't converged */
&& (v1 > 0 && v2 > 0) ) { /* v1 and v2 both positive */
diff = v2 - v1;
if (v1 < v2) {
SHIFT3(t3, t2, t1, CC*t0 + RR*t2);
SHIFT2(v2, v1, VAL(t1));
} else {
SHIFT3(t0, t1, t2, RR*t1 + CC*t3);
SHIFT2(v1, v2, VAL(t2));
}
if (tstate->verbose) {
fprintf(stderr, "%s%s: %s ---> \n"
" t0 = % 31.15f\n"
" t1 = % 31.15f -> v1 = % 31.15f\n"
" t2 = % 31.15f -> v2 = % 31.15f\n"
" t3 = % 31.15f\n",
_echoDot(tstate->depth), me,
diff > 0 ? "v1 < v2" : "v1 > v2",
t0, t1, v1, t2, v2, t3);
}
}
if (v1 > 0 && v2 > 0) {
/* the minimization stopped, yet both v1 and v2 are still positive,
so there's no way we can have a root */
if (tstate->verbose) {
fprintf(stderr, "%s%s: minimization found no root\n",
_echoDot(tstate->depth), me);
}
return AIR_FALSE;
}
/* else either v1 or v2 <= 0, so there is a root (actually two).
By construction, f(t0) is positive, so we can now bracket the
root between t0 and t1 or t2, whichever one is associated with
a smaller value */
Tmin = t0;
Tmax = v1 < v2 ? t1 : t2; /* HEY: shouldn't I just be using whichever one is closer? */
Vmin = VAL(Tmin);
Vmax = VAL(Tmax);
}
/* the value isn't necessarily monotonic between Tmin and Tmax, but
we know that there is only one root. Find it with newton-raphson,
using the log of function, both for values and for derivatives */
iter = 0;
TT = (Tmin + Tmax)/2;
LVALGRAD(VV, dV, TT);
while (iter < parm->sqNRI && AIR_ABS(VV) > parm->sqTol
&& AIR_EXISTS(VV) && AIR_EXISTS(dV)) {
if (tstate->verbose) {
fprintf(stderr, "%s%s: iter = %d: TT = %g, VV = %g, dV = %g\n",
_echoDot(tstate->depth), me, iter, TT, VV, dV);
}
TT -= VV/dV;
if (!AIR_IN_OP(Tmin, TT, Tmax)) {
/* newton-raphson sent us flying out of bounds; find a tighter
[Tmin,Tmax] bracket with bisection and try again */
TT = (Tmin + Tmax)/2;
VV = VAL(TT);
if (Vmin*VV < 0) {
Tmax = TT;
Vmax = VV;
} else {
Tmin = TT;
Vmin = VV;
}
TT = (Tmin + Tmax)/2;
}
LVALGRAD(VV, dV, TT);
iter++;
}
if (!( AIR_EXISTS(VV) && AIR_EXISTS(dV) )) {
/* we bailed out of the loop above because
we got screwed by numerical errors
--> pretend that there was no intersection,
and HEY this will have to be debugged later */
return AIR_FALSE;
}
/* else we succeedded in finding the intersection */
intx->t = TT + saveTmin;
VALGRAD(VV, dV, TT); /* puts gradient into grad */
ELL_3V_NORM(intx->norm, grad, tmp);
intx->obj = OBJECT(obj);
/* set in intx:
yes: t, norm
no: u, v
*/
return AIR_TRUE;
}
