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