int
_echoRayIntx_Cylinder(RAYINTX_ARGS(Cylinder)) {
echoPos_t A, B, C, aa, bb, cc, dd, ee, ff, dscr, cylt1, cylt2, t, tmax,
twot[2], cylp1, cylp2, pos[3], tmp;
int tidx, radi0, radi1, twocap[2], cap;
AIR_UNUSED(parm);
AIR_UNUSED(tstate);
if (!_echoRayIntx_CubeSolid(&t, &tmax,
-1-ECHO_EPSILON, 1+ECHO_EPSILON,
-1-ECHO_EPSILON, 1+ECHO_EPSILON,
-1-ECHO_EPSILON, 1+ECHO_EPSILON, ray)) {
return AIR_FALSE;
}
switch(obj->axis) {
case 0:
radi0 = 1; radi1 = 2;
break;
case 1:
radi0 = 0; radi1 = 2;
break;
case 2: default:
radi0 = 0; radi1 = 1;
break;
}
aa = ray->dir[radi0];
bb = ray->dir[radi1];
ee = ray->dir[obj->axis];
cc = ray->from[radi0];
dd = ray->from[radi1];
ff = ray->from[obj->axis];
A = aa*aa + bb*bb;
B = 2*(aa*cc + bb*dd);
C = cc*cc + dd*dd - 1;
dscr = B*B - 4*A*C;
if (dscr <= 0) {
/* infinite ray grazes or misses the infinite cylinder (not
bounded to [-1,1] along cylinder's axis), so therefore the ray
grazes or misses the actual cylinder */
return AIR_FALSE;
}
/* else infinite ray intersects the infinite cylinder */
dscr = sqrt(dscr);
cylt1 = (-B - dscr)/(2*A);
cylp1 = ff + cylt1*ee;
cylt2 = (-B + dscr)/(2*A);
cylp2 = ff + cylt2*ee;
if ( (cylp1 <= -1 && cylp2 <= -1)
|| (cylp1 >= 1 && cylp2 >= 1) ) {
/* both intersections with infinite cylinder lie on ONE side of the
finite extent, so there can't be an intersection */
return AIR_FALSE;
}
/* else infinite ray DOES intersect finite cylinder; we have to find
if any of the intersections are in the [neer, faar] interval */
tidx = 0;
if (AIR_IN_CL(-1, cylp1, 1)) {
twot[tidx] = cylt1;
twocap[tidx] = 0;
tidx++;
}
if (AIR_IN_CL(-1, cylp2, 1)) {
twot[tidx] = cylt2;
twocap[tidx] = 0;
tidx++;
}
if (tidx < 2) {
/* at least one of the two intersections is with the endcaps */
t = (-ff - 1)/ee;
ELL_3V_SCALE_ADD2(pos, 1, ray->from, t, ray->dir);
aa = pos[radi0]; bb = pos[radi1]; cc = aa*aa + bb*bb;
if (cc <= 1) {
twot[tidx] = t;
twocap[tidx] = 1;
tidx++;
}
if (tidx < 2) {
/* try other endcap */
t = (-ff + 1)/ee;
ELL_3V_SCALE_ADD2(pos, 1, ray->from, t, ray->dir);
aa = pos[radi0]; bb = pos[radi1]; cc = aa*aa + bb*bb;
if (cc <= 1) {
twot[tidx] = t;
twocap[tidx] = 1;
tidx++;
}
}
}
if (!tidx) {
return AIR_FALSE;
}
if (2 == tidx && twot[0] > twot[1]) {
ELL_SWAP2(twot[0], twot[1], aa);
ELL_SWAP2(twocap[0], twocap[1], cap);
}
t = twot[0];
cap = twocap[0];
if (!AIR_IN_CL(ray->neer, t, ray->faar)) {
if (1 == tidx) {
return AIR_FALSE;
}
t = twot[1];
cap = twocap[1];
if (!AIR_IN_CL(ray->neer, t, ray->faar)) {
return AIR_FALSE;
}
}
/* else one of the intxs is in [neer,faar] segment */
intx->t = t;
ELL_3V_SCALE_ADD2(pos, 1, ray->from, t, ray->dir);
switch(obj->axis) {
case 0:
ELL_3V_SET(intx->norm, cap*pos[0], (1-cap)*pos[1], (1-cap)*pos[2]);
break;
case 1:
ELL_3V_SET(intx->norm, (1-cap)*pos[0], cap*pos[1], (1-cap)*pos[2]);
break;
case 2: default:
ELL_3V_SET(intx->norm, (1-cap)*pos[0], (1-cap)*pos[1], cap*pos[2]);
break;
}
ELL_3V_NORM(intx->norm, intx->norm, tmp);
intx->obj = OBJECT(obj);
/* does NOT set u, v */
return AIR_TRUE;
}
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;
}
