/**
* Intersect a ray with an superellipsoid, where all constant terms
* have been precomputed by rt_superell_prep(). If an intersection
* occurs, a struct seg will be acquired and filled in.
*
* Returns -
* 0 MISS
* >0 HIT
*/
int
rt_superell_shot(struct soltab *stp, struct xray *rp, struct application *ap, struct seg *seghead)
{
static int counter=10;
struct superell_specific *superell = (struct superell_specific *)stp->st_specific;
bn_poly_t equation; /* equation of superell to be solved */
vect_t translated; /* translated shot vector */
vect_t newShotPoint; /* P' */
vect_t newShotDir; /* D' */
vect_t normalizedShotPoint; /* P' with normalized dist from superell */
bn_complex_t complexRoot[4]; /* roots returned from poly solver */
double realRoot[4]; /* real ray distance values */
int i, j;
struct seg *segp;
/* translate ray point */
/* VSUB2(translated, rp->r_pt, superell->superell_V); */
(translated)[X] = (rp->r_pt)[X] - (superell->superell_V)[X];
(translated)[Y] = (rp->r_pt)[Y] - (superell->superell_V)[Y];
(translated)[Z] = (rp->r_pt)[Z] - (superell->superell_V)[Z];
/* scale and rotate point to get P' */
/* MAT4X3VEC(newShotPoint, superell->superell_SoR, translated); */
newShotPoint[X] = (superell->superell_SoR[0]*translated[X] + superell->superell_SoR[1]*translated[Y] + superell->superell_SoR[ 2]*translated[Z]) * 1.0/(superell->superell_SoR[15]);
newShotPoint[Y] = (superell->superell_SoR[4]*translated[X] + superell->superell_SoR[5]*translated[Y] + superell->superell_SoR[ 6]*translated[Z]) * 1.0/(superell->superell_SoR[15]);
newShotPoint[Z] = (superell->superell_SoR[8]*translated[X] + superell->superell_SoR[9]*translated[Y] + superell->superell_SoR[10]*translated[Z]) * 1.0/(superell->superell_SoR[15]);
/* translate ray direction vector */
MAT4X3VEC(newShotDir, superell->superell_SoR, rp->r_dir);
VUNITIZE(newShotDir);
/* normalize distance from the superell. substitutes a corrected ray
* point, which contains a translation along the ray direction to the
* closest approach to vertex of the superell. Translating the ray
* along the direction of the ray to the closest point near the
* primitive's center vertex. New ray origin is hence, normalized.
*/
VSCALE(normalizedShotPoint, newShotDir,
VDOT(newShotPoint, newShotDir));
VSUB2(normalizedShotPoint, newShotPoint, normalizedShotPoint);
/* Now generate the polynomial equation for passing to the root finder */
equation.dgr = 2;
/* (x^2 / A) + (y^2 / B) + (z^2 / C) - 1 */
equation.cf[0] = newShotPoint[X] * newShotPoint[X] * superell->superell_invmsAu + newShotPoint[Y] * newShotPoint[Y] * superell->superell_invmsBu + newShotPoint[Z] * newShotPoint[Z] * superell->superell_invmsCu - 1;
/* (2xX / A) + (2yY / B) + (2zZ / C) */
equation.cf[1] = 2 * newShotDir[X] * newShotPoint[X] * superell->superell_invmsAu + 2 * newShotDir[Y] * newShotPoint[Y] * superell->superell_invmsBu + 2 * newShotDir[Z] * newShotPoint[Z] * superell->superell_invmsCu;
/* (X^2 / A) + (Y^2 / B) + (Z^2 / C) */
equation.cf[2] = newShotDir[X] * newShotDir[X] * superell->superell_invmsAu + newShotDir[Y] * newShotDir[Y] * superell->superell_invmsBu + newShotDir[Z] * newShotDir[Z] * superell->superell_invmsCu;
if ((i = rt_poly_roots(&equation, complexRoot, stp->st_dp->d_namep)) != 2) {
if (i > 0) {
bu_log("rt_superell_shot(): poly roots %d != 2\n", i);
bn_pr_roots(stp->st_name, complexRoot, i);
} else if (i < 0) {
static int reported=0;
bu_log("rt_superell_shot(): The root solver failed to converge on a solution for %s\n", stp->st_dp->d_namep);
if (!reported) {
VPRINT("while shooting from:\t", rp->r_pt);
VPRINT("while shooting at:\t", rp->r_dir);
bu_log("rt_superell_shot(): Additional superellipsoid convergence failure details will be suppressed.\n");
reported=1;
}
}
return 0; /* MISS */
}
/* XXX BEGIN CUT */
/* Only real roots indicate an intersection in real space.
*
* Look at each root returned; if the imaginary part is zero
* or sufficiently close, then use the real part as one value
* of 't' for the intersections
*/
for (j=0, i=0; j < 2; j++) {
if (NEAR_ZERO(complexRoot[j].im, 0.001))
realRoot[i++] = complexRoot[j].re;
}
/* reverse above translation by adding distance to all 'k' values. */
/* for (j = 0; j < i; ++j)
realRoot[j] -= VDOT(newShotPoint, newShotDir);
*/
/* Here, 'i' is number of points found */
switch (i) {
case 0:
return 0; /* No hit */
default:
bu_log("rt_superell_shot(): reduced 4 to %d roots\n", i);
bn_pr_roots(stp->st_name, complexRoot, 4);
return 0; /* No hit */
case 2:
{
/* Sort most distant to least distant. */
fastf_t u;
if ((u=realRoot[0]) < realRoot[1]) {
/* bubble larger towards [0] */
realRoot[0] = realRoot[1];
realRoot[1] = u;
}
}
break;
case 4:
{
short n;
short lim;
/* Inline rt_pt_sort(). Sorts realRoot[] into descending order. */
for (lim = i-1; lim > 0; lim--) {
for (n = 0; n < lim; n++) {
fastf_t u;
if ((u=realRoot[n]) < realRoot[n+1]) {
/* bubble larger towards [0] */
realRoot[n] = realRoot[n+1];
realRoot[n+1] = u;
}
}
}
}
break;
}
if (counter > 0) {
bu_log("rt_superell_shot(): realroot in %f out %f\n", realRoot[1], realRoot[0]);
counter--;
}
/* Now, t[0] > t[npts-1] */
/* realRoot[1] is entry point, and realRoot[0] is farthest exit point */
RT_GET_SEG(segp, ap->a_resource);
segp->seg_stp = stp;
segp->seg_in.hit_dist = realRoot[1];
segp->seg_out.hit_dist = realRoot[0];
/* segp->seg_in.hit_surfno = segp->seg_out.hit_surfno = 0; */
/* Set aside vector for rt_superell_norm() later */
/* VJOIN1(segp->seg_in.hit_vpriv, newShotPoint, realRoot[1], newShotDir); */
/* VJOIN1(segp->seg_out.hit_vpriv, newShotPoint, realRoot[0], newShotDir); */
BU_LIST_INSERT(&(seghead->l), &(segp->l));
if (i == 2) {
return 2; /* HIT */
}
/* 4 points */
/* realRoot[3] is entry point, and realRoot[2] is exit point */
RT_GET_SEG(segp, ap->a_resource);
segp->seg_stp = stp;
segp->seg_in.hit_dist = realRoot[3]*superell->superell_e;
segp->seg_out.hit_dist = realRoot[2]*superell->superell_e;
segp->seg_in.hit_surfno = segp->seg_out.hit_surfno = 1;
VJOIN1(segp->seg_in.hit_vpriv, newShotPoint, realRoot[3], newShotDir);
VJOIN1(segp->seg_out.hit_vpriv, newShotPoint, realRoot[2], newShotDir);
BU_LIST_INSERT(&(seghead->l), &(segp->l));
return 4; /* HIT */
/* XXX END CUT */
}
/**
* Intersect a ray with a revolve. If an intersection occurs, a struct
* seg will be acquired and filled in.
*
* Returns -
* 0 MISS
* >0 HIT
*/
int
rt_revolve_shot(struct soltab *stp, struct xray *rp, struct application *ap, struct seg *seghead)
{
struct revolve_specific *rev =
(struct revolve_specific *)stp->st_specific;
struct seg *segp;
struct hit *hitp;
struct hit *hits[MAX_HITS], hit[MAX_HITS];
size_t i, j, nseg, nhits;
int in, out;
fastf_t k, m, h, aa, bb;
point_t dp, pr, xlated;
vect_t vr, ur, norm, normS, normE;
fastf_t start, end, angle;
vect_t dir;
point_t hit1, hit2;
point2d_t hit2d, pt1, pt2;
fastf_t a, b, c, disc, k1, k2, t1, t2;
uint32_t *lng;
struct line_seg *lsg;
struct carc_seg *csg;
nhits = 0;
for (i=0; i<MAX_HITS; i++) hits[i] = &hit[i];
vr[X] = VDOT(rev->xUnit, rp->r_dir);
vr[Y] = VDOT(rev->yUnit, rp->r_dir);
vr[Z] = VDOT(rev->zUnit, rp->r_dir);
VSUB2(xlated, rp->r_pt, rev->v3d);
pr[X] = VDOT(rev->xUnit, xlated);
pr[Y] = VDOT(rev->yUnit, xlated);
pr[Z] = VDOT(rev->zUnit, xlated);
VMOVE(ur, vr);
VUNITIZE(ur);
if (rev->ang < M_2PI) {
VREVERSE(normS, rev->yUnit); /* start normal */
start = (VDOT(normS, rev->v3d) - VDOT(normS, rp->r_pt)) / VDOT(normS, rp->r_dir);
VCROSS(normE, rev->zUnit, rev->rEnd); /* end normal */
end = (VDOT(normE, rev->v3d) - VDOT(normE, rp->r_pt)) / VDOT(normE, rp->r_dir);
VJOIN1(hit1, pr, start, vr);
hit2d[Y] = hit1[Z];
hit2d[X] = sqrt(hit1[X]*hit1[X] + hit1[Y]*hit1[Y]);
VJOIN1(hit2, xlated, start, rp->r_dir);
if (VDOT(rev->xUnit, hit2) < 0) {
/* set the sign of the 2D point's x coord */
hit2d[X] = -hit2d[X];
}
if (rt_sketch_contains(rev->skt, hit2d)) {
hit2d[X] = -hit2d[X];
if (rev->ang > M_PI && rt_sketch_contains(rev->skt, hit2d)) {
/* skip it */
} else {
hitp = hits[nhits++];
hitp->hit_magic = RT_HIT_MAGIC;
hitp->hit_dist = start;
hitp->hit_surfno = (hit2d[X]>0) ? START_FACE_NEG : START_FACE_POS;
VSET(hitp->hit_vpriv, -hit2d[X], hit2d[Y], 0);
}
}
VJOIN1(hit1, pr, end, vr);
hit2d[Y] = hit1[Z];
hit2d[X] = sqrt(hit1[X]*hit1[X] + hit1[Y]*hit1[Y]);
VJOIN1(hit2, xlated, end, rp->r_dir);
if (VDOT(rev->rEnd, hit2) < 0) {
/* set the sign of the 2D point's x coord */
hit2d[X] = -hit2d[X];
}
if (rt_sketch_contains(rev->skt, hit2d)) {
hit2d[X] = -hit2d[X];
if (rev->ang > M_PI && rt_sketch_contains(rev->skt, hit2d)) {
/* skip it */
} else {
if (nhits >= MAX_HITS) return -1; /* too many hits */
hitp = hits[nhits++];
hitp->hit_magic = RT_HIT_MAGIC;
hitp->hit_dist = end;
hitp->hit_surfno = (hit2d[X]>0) ? END_FACE_NEG : END_FACE_POS;
VSET(hitp->hit_vpriv, -hit2d[X], hit2d[Y], 0);
}
}
}
/**
* calculate hyperbola parameters
*
* [ (x*x) / aa^2 ] - [ (y-h)^2 / bb^2 ] = 1
*
* x = aa cosh(t - k);
* y = h + bb sinh(t - k);
*/
VREVERSE(dp, pr);
VSET(norm, ur[X], ur[Y], 0);
k = VDOT(dp, norm) / VDOT(ur, norm);
h = pr[Z] + k*vr[Z];
if (NEAR_EQUAL(fabs(ur[Z]), 1.0, RT_DOT_TOL)) {
aa = sqrt(pr[X]*pr[X] + pr[Y]*pr[Y]);
bb = MAX_FASTF;
} else {
aa = sqrt((pr[X] + k*vr[X])*(pr[X] + k*vr[X]) + (pr[Y] + k*vr[Y])*(pr[Y] + k*vr[Y]));
bb = sqrt(aa*aa * (1.0/(1 - ur[Z]*ur[Z]) - 1.0));
}
/**
* if (ur[Z] == 1) {
* bb = inf;
* // ray becomes a line parallel to sketch's y-axis instead of a hyberbola
* }
* if (ur[Z] == 0) {
* bb = 0;
* // ray becomes a line parallel to sketch's x-axis instead of a hyperbola
* // all hits must have x > aa
* }
*/
/* handle open sketches */
if (!NEAR_ZERO(ur[Z], RT_DOT_TOL)) {
for (i=0; i<rev->skt->vert_count && rev->ends[i] != -1; i++) {
V2MOVE(pt1, rev->skt->verts[rev->ends[i]]);
hit2d[Y] = pt1[Y];
if (NEAR_EQUAL(fabs(ur[Z]), 1.0, RT_DOT_TOL)) {
/* ur[Z] == 1 */
hit2d[X] = aa;
} else {
hit2d[X] = aa*sqrt((hit2d[Y]-h)*(hit2d[Y]-h)/(bb*bb) + 1);
}
if (pt1[X] < 0) hit2d[X] = -fabs(hit2d[X]);
if (fabs(hit2d[X]) < fabs(pt1[X])) {
/* valid hit */
if (nhits >= MAX_HITS) return -1; /* too many hits */
hitp = hits[nhits++];
hitp->hit_magic = RT_HIT_MAGIC;
hitp->hit_dist = (hit2d[Y] - pr[Z]) / vr[Z];
hitp->hit_surfno = HORIZ_SURF;
VJOIN1(hitp->hit_vpriv, pr, hitp->hit_dist, vr);
hitp->hit_point[X] = hit2d[X];
hitp->hit_point[Y] = hit2d[Y];
hitp->hit_point[Z] = 0;
angle = atan2(hitp->hit_vpriv[Y], hitp->hit_vpriv[X]);
if (pt1[X] < 0) {
angle += M_PI;
} else if (angle < 0) {
angle += M_2PI;
}
hit2d[X] = -hit2d[X];
if (angle > rev->ang) {
nhits--;
continue;
} else if ((angle + M_PI < rev->ang || angle - M_PI > 0)
&& rt_sketch_contains(rev->skt, hit2d)
&& hit2d[X] > 0) {
nhits--;
continue;
}
/* X and Y are used for uv(), Z is used for norm() */
hitp->hit_vpriv[X] = pt1[X];
hitp->hit_vpriv[Y] = angle;
if (i+1 < rev->skt->vert_count && rev->ends[i+1] != -1 &&
NEAR_EQUAL(rev->skt->verts[rev->ends[i]][Y],
rev->skt->verts[rev->ends[i+1]][Y], SMALL)) {
hitp->hit_vpriv[Z] = rev->skt->verts[rev->ends[i+1]][X];
i++;
if (fabs(hit2d[X]) < fabs(hitp->hit_vpriv[Z])) {
nhits--;
}
} else {
hitp->hit_vpriv[Z] = 0;
}
}
}
}
/* find hyperbola intersection with each sketch segment */
nseg = rev->skt->curve.count;
for (i=0; i<nseg; i++) {
lng = (uint32_t *)rev->skt->curve.segment[i];
switch (*lng) {
case CURVE_LSEG_MAGIC:
lsg = (struct line_seg *)lng;
V2MOVE(pt1, rev->skt->verts[lsg->start]);
V2MOVE(pt2, rev->skt->verts[lsg->end]);
V2SUB2(dir, pt2, pt1);
if (ZERO(dir[X])) {
m = 1.0;
} else {
m = dir[Y] / dir[X];
}
if (NEAR_EQUAL(fabs(ur[Z]), 1.0, RT_DOT_TOL)) {
/* ray is vertical line at x=aa */
if (FMIN(pt1[X], pt2[X]) < aa && aa < FMAX(pt1[X], pt2[X])) {
/* check the positive side of the sketch (x > 0) */
k1 = (m * (aa - pt1[X]) + pt1[Y] - pr[Z]) / vr[Z];
VJOIN1(hit1, pr, k1, vr);
angle = atan2(hit1[Y], hit1[X]);
hit2d[X] = -aa; /* use neg to check for overlap in contains() */
hit2d[Y] = hit1[Z];
if (angle < 0) {
angle += M_2PI;
}
if (angle < rev->ang &&
!((angle + M_PI < rev->ang || angle - M_PI > 0)
&& rt_sketch_contains(rev->skt, hit2d))) {
if (nhits >= MAX_HITS) return -1; /* too many hits */
hitp = hits[nhits++];
hitp->hit_point[X] = -hit2d[X];
hitp->hit_point[Y] = hit2d[Y];
hitp->hit_point[Z] = 0;
VMOVE(hitp->hit_vpriv, hit1);
if (ZERO(m)) {
hitp->hit_vpriv[Z] = 0.0;
} else {
hitp->hit_vpriv[Z] = -1.0/m;
}
hitp->hit_magic = RT_HIT_MAGIC;
hitp->hit_dist = k1;
hitp->hit_surfno = i;
}
}
if (FMIN(pt1[X], pt2[X]) < -aa && -aa < FMAX(pt1[X], pt2[X])) {
/* check negative side of the sketch (x < 0) */
k1 = (m * (-aa - pt1[X]) + pt1[Y] - pr[Z]) / vr[Z];
VJOIN1(hit1, pr, k1, vr);
angle = atan2(hit1[Y], hit1[X]);
hit2d[X] = aa; /* use neg to check for overlap in contains() */
hit2d[Y] = hit1[Z];
if (angle < 0) {
angle += M_PI;
}
if (angle < rev->ang &&
!((angle + M_PI < rev->ang || angle - M_PI > 0)
&& rt_sketch_contains(rev->skt, hit2d))) {
if (nhits >= MAX_HITS) return -1; /* too many hits */
hitp = hits[nhits++];
hitp->hit_point[X] = -hit2d[X];
hitp->hit_point[Y] = hit2d[Y];
hitp->hit_point[Z] = 0;
VMOVE(hitp->hit_vpriv, hit1);
if (ZERO(m)) {
hitp->hit_vpriv[Z] = 0.0;
} else {
hitp->hit_vpriv[Z] = 1.0/m;
}
hitp->hit_magic = RT_HIT_MAGIC;
hitp->hit_dist = k1;
hitp->hit_surfno = i;
}
}
} else if (NEAR_ZERO(ur[Z], RT_DOT_TOL)) {
/* ray is horizontal line at y = h; hit2d[X] > aa */
if (FMIN(pt1[Y], pt2[Y]) < h && h < FMAX(pt1[Y], pt2[Y])) {
if (ZERO(m)) {
hit2d[X] = pt1[X];
} else {
hit2d[X] = pt1[X] + (h-pt1[Y])/m;
}
hit2d[Y] = h;
if (fabs(hit2d[X]) > aa) {
k1 = k + sqrt(hit2d[X]*hit2d[X] - aa*aa);
k2 = k - sqrt(hit2d[X]*hit2d[X] - aa*aa);
VJOIN1(hit1, pr, k1, vr);
angle = atan2(hit1[Y], hit1[X]);
if (hit2d[X] < 0) {
angle += M_PI;
} else if (angle < 0) {
angle += M_2PI;
}
hit2d[X] = -hit2d[X];
if (angle < rev->ang &&
!((angle + M_PI < rev->ang || angle - M_PI > 0)
&& rt_sketch_contains(rev->skt, hit2d))) {
if (nhits >= MAX_HITS) return -1; /* too many hits */
hitp = hits[nhits++];
hitp->hit_point[X] = -hit2d[X];
hitp->hit_point[Y] = hit2d[Y];
hitp->hit_point[Z] = 0;
VMOVE(hitp->hit_vpriv, hit1);
if (ZERO(m)) {
hitp->hit_vpriv[Z] = 0.0;
} else {
hitp->hit_vpriv[Z] = (hit2d[X]>0) ? 1.0/m : -1.0/m;
}
hitp->hit_magic = RT_HIT_MAGIC;
hitp->hit_dist = k1;
hitp->hit_surfno = i;
}
VJOIN1(hit2, pr, k2, vr);
angle = atan2(hit2[Y], hit2[X]);
if (-hit2d[X] < 0) {
angle += M_PI;
} else if (angle < 0) {
angle += M_2PI;
}
if (angle < rev->ang &&
!((angle + M_PI < rev->ang || angle - M_PI > 0)
&& rt_sketch_contains(rev->skt, hit2d))) {
if (nhits >= MAX_HITS) return -1; /* too many hits */
hitp = hits[nhits++];
hitp->hit_point[X] = -hit2d[X];
hitp->hit_point[Y] = hit2d[Y];
hitp->hit_point[Z] = 0;
VMOVE(hitp->hit_vpriv, hit2);
if (ZERO(m)) {
hitp->hit_vpriv[Z] = 0.0;
} else {
hitp->hit_vpriv[Z] = (hit2d[X]>0) ? 1.0/m : -1.0/m;
}
hitp->hit_magic = RT_HIT_MAGIC;
hitp->hit_dist = k2;
hitp->hit_surfno = i;
}
}
}
} else {
a = dir[X]*dir[X]/(aa*aa) - dir[Y]*dir[Y]/(bb*bb);
b = 2*(dir[X]*pt1[X]/(aa*aa) - dir[Y]*(pt1[Y]-h)/(bb*bb));
c = pt1[X]*pt1[X]/(aa*aa) - (pt1[Y]-h)*(pt1[Y]-h)/(bb*bb) - 1;
disc = b*b - (4.0 * a * c);
if (!NEAR_ZERO(a, RT_PCOEF_TOL)) {
if (disc > 0) {
disc = sqrt(disc);
t1 = (-b + disc) / (2.0 * a);
t2 = (-b - disc) / (2.0 * a);
k1 = (pt1[Y]-pr[Z] + t1*dir[Y])/vr[Z];
k2 = (pt1[Y]-pr[Z] + t2*dir[Y])/vr[Z];
if (t1 > 0 && t1 < 1) {
if (nhits >= MAX_HITS) return -1; /* too many hits */
VJOIN1(hit1, pr, k1, vr);
angle = atan2(hit1[Y], hit1[X]);
V2JOIN1(hit2d, pt1, t1, dir);
if (hit2d[X] < 0) {
angle += M_PI;
} else if (angle < 0) {
angle += M_2PI;
}
hit2d[X] = -hit2d[X];
if (angle < rev->ang) {
if ((angle + M_PI < rev->ang || angle - M_PI > 0)
&& rt_sketch_contains(rev->skt, hit2d)) {
/* overlap, so ignore it */
} else {
hitp = hits[nhits++];
hitp->hit_point[X] = -hit2d[X];
hitp->hit_point[Y] = hit2d[Y];
hitp->hit_point[Z] = 0;
VMOVE(hitp->hit_vpriv, hit1);
if (ZERO(m)) {
hitp->hit_vpriv[Z] = 0.0;
} else {
hitp->hit_vpriv[Z] = (hit2d[X]>0) ? 1.0/m : -1.0/m;
}
hitp->hit_magic = RT_HIT_MAGIC;
hitp->hit_dist = k1;
hitp->hit_surfno = i;
}
}
}
if (t2 > 0 && t2 < 1) {
if (nhits >= MAX_HITS) return -1; /* too many hits */
VJOIN1(hit2, pr, k2, vr);
angle = atan2(hit2[Y], hit2[X]);
V2JOIN1(hit2d, pt1, t2, dir);
if (hit2d[X] < 0) {
angle += M_PI;
} else if (angle < 0) {
angle += M_2PI;
}
hit2d[X] = -hit2d[X];
if (angle < rev->ang) {
if ((angle + M_PI < rev->ang || angle - M_PI > 0)
&& rt_sketch_contains(rev->skt, hit2d)) {
/* overlap, so ignore it */
} else {
hitp = hits[nhits++];
hitp->hit_point[X] = -hit2d[X];
hitp->hit_point[Y] = hit2d[Y];
hitp->hit_point[Z] = 0;
VMOVE(hitp->hit_vpriv, hit2);
if (ZERO(m)) {
hitp->hit_vpriv[Z] = 0.0;
} else {
hitp->hit_vpriv[Z] = (hit2d[X]>0) ? 1.0/m : -1.0/m;
}
hitp->hit_magic = RT_HIT_MAGIC;
hitp->hit_dist = k2;
hitp->hit_surfno = i;
}
}
}
}
} else if (!NEAR_ZERO(b, RT_PCOEF_TOL)) {
t1 = -c / b;
k1 = (pt1[Y]-pr[Z] + t1*dir[Y])/vr[Z];
if (t1 > 0 && t1 < 1) {
if (nhits >= MAX_HITS) return -1; /* too many hits */
VJOIN1(hit1, pr, k1, vr);
angle = atan2(hit1[Y], hit1[X]);
V2JOIN1(hit2d, pt1, t1, dir);
if (hit2d[X] < 0) {
angle += M_PI;
} else if (angle < 0) {
angle += M_2PI;
}
hit2d[X] = -hit2d[X];
if (angle < rev->ang) {
if ((angle + M_PI < rev->ang || angle - M_PI > 0)
&& rt_sketch_contains(rev->skt, hit2d)) {
/* overlap, so ignore it */
} else {
hitp = hits[nhits++];
hitp->hit_point[X] = -hit2d[X];
hitp->hit_point[Y] = hit2d[Y];
hitp->hit_point[Z] = 0;
VMOVE(hitp->hit_vpriv, hit1);
if (ZERO(m)) {
hitp->hit_vpriv[Z] = 0.0;
} else {
hitp->hit_vpriv[Z] = (hit2d[X]>0) ? 1.0/m : -1.0/m;
}
hitp->hit_magic = RT_HIT_MAGIC;
hitp->hit_dist = k1;
hitp->hit_surfno = i;
}
}
}
}
}
break;
case CURVE_CARC_MAGIC:
/*
circle: (x-cx)^2 + (y-cy)^2 = cr^2
x = (1/2cx)y^2 + (-cy/cx)y + (1/2cx)(cy^2 + cx^2 - cr^2) + (1/2cx)(x^2)
x = f(y) + (1/2cx)x^2
hyperbola:
[(x-hx)/a]^2 - [(y-hy)/b]^2 = 1
x^2 = (a^2/b^2)y^2 + (-2*hy*a^2/b^2)y + (hy^2 * a^2/b^2) + a^2
x^2 = g(y)
plug the second equation into the first to get:
x = f(y) + (1/2cx)g(y)
then square that to get:
x^2 = {f(y) + (1/2cx)g(y)}^2 = g(y)
move all to one side to get:
0 = {f(y) + (1/2cx)g(y)}^2 - g(y)
this is a fourth order polynomial in y.
*/
{
bn_poly_t circleX; /* f(y) */
bn_poly_t hypXsq; /* g(y) */
bn_poly_t hypXsq_scaled; /* g(y) / (2*cx) */
bn_poly_t sum; /* f(y) + g(y)/(2cx) */
bn_poly_t sum_sq; /* {f(y) + g(y)/(2cx)}^2 */
bn_poly_t answer; /* {f(y) + g(y)/(2cx)}^2 - g(y) */
bn_complex_t roots[4];
int rootcnt;
fastf_t cx, cy, crsq = 0; /* carc's (x, y) coords and radius^2 */
point2d_t center, radius;
/* calculate circle parameters */
csg = (struct carc_seg *)lng;
if (csg->radius <= 0.0) {
/* full circle, "end" is center and "start" is on the circle */
V2MOVE(center, rev->skt->verts[csg->end]);
V2SUB2(radius, rev->skt->verts[csg->start], center);
crsq = MAG2SQ(radius);
} else {
point_t startpt, endpt, midpt;
vect_t s_to_m;
vect_t bisector;
vect_t vertical;
fastf_t distance;
fastf_t magsq_s2m;
VSET(vertical, 0, 0, 1);
V2MOVE(startpt, rev->skt->verts[csg->start]);
startpt[Z] = 0.0;
V2MOVE(endpt, rev->skt->verts[csg->end]);
endpt[Z] = 0.0;
VBLEND2(midpt, 0.5, startpt, 0.5, endpt);
VSUB2(s_to_m, midpt, startpt);
VCROSS(bisector, vertical, s_to_m);
VUNITIZE(bisector);
magsq_s2m = MAGSQ(s_to_m);
if (magsq_s2m > csg->radius*csg->radius) {
fastf_t max_radius;
max_radius = sqrt(magsq_s2m);
if (NEAR_EQUAL(max_radius, csg->radius, RT_LEN_TOL)) {
csg->radius = max_radius;
} else {
bu_log("Impossible radius for circular arc in extrusion (%s), is %g, cannot be more than %g!\n",
stp->st_dp->d_namep, csg->radius, sqrt(magsq_s2m));
bu_log("Difference is %g\n", max_radius - csg->radius);
return -1;
}
}
distance = sqrt(csg->radius*csg->radius - magsq_s2m);
/* save arc center */
if (csg->center_is_left) {
V2JOIN1(center, midpt, distance, bisector);
} else {
V2JOIN1(center, midpt, -distance, bisector);
}
}
cx = center[X];
cy = center[Y];
circleX.dgr = 2;
hypXsq.dgr = 2;
hypXsq_scaled.dgr = 2;
sum.dgr = 2;
sum_sq.dgr = 4;
answer.dgr = 4;
circleX.cf[0] = (cy*cy + cx*cx - crsq)/(2.0*cx);
circleX.cf[1] = -cy/cx;
circleX.cf[2] = 1/(2.0*cx);
hypXsq_scaled.cf[0] = hypXsq.cf[0] = aa*aa + h*h*aa*aa/(bb*bb);
hypXsq_scaled.cf[1] = hypXsq.cf[1] = -2.0*h*aa*aa/(bb*bb);
hypXsq_scaled.cf[2] = hypXsq.cf[2] = (aa*aa)/(bb*bb);
bn_poly_scale(&hypXsq_scaled, 1.0 / (2.0 * cx));
bn_poly_add(&sum, &hypXsq_scaled, &circleX);
bn_poly_mul(&sum_sq, &sum, &sum);
bn_poly_sub(&answer, &sum_sq, &hypXsq);
/* It is known that the equation is 4th order. Therefore, if the
* root finder returns other than 4 roots, error.
*/
rootcnt = rt_poly_roots(&answer, roots, stp->st_dp->d_namep);
if (rootcnt != 4) {
if (rootcnt > 0) {
bu_log("tor: rt_poly_roots() 4!=%d\n", rootcnt);
bn_pr_roots(stp->st_name, roots, rootcnt);
} else if (rootcnt < 0) {
static int reported=0;
bu_log("The root solver failed to converge on a solution for %s\n", stp->st_dp->d_namep);
if (!reported) {
VPRINT("while shooting from:\t", rp->r_pt);
VPRINT("while shooting at:\t", rp->r_dir);
bu_log("Additional torus convergence failure details will be suppressed.\n");
reported=1;
}
}
}
break;
}
case CURVE_BEZIER_MAGIC:
break;
case CURVE_NURB_MAGIC:
break;
default:
bu_log("rt_revolve_prep: ERROR: unrecognized segment type!\n");
break;
}
}
if (nhits%2 != 0) {
bu_log("odd number of hits: %zu\n", nhits);
for (i=0; i<nhits; i++) {
bu_log("\t(%6.2f, %6.2f)\t%6.2f\t%2d\n",
hits[i]->hit_point[X], hits[i]->hit_point[Y], hits[i]->hit_dist, hits[i]->hit_surfno);
}
return -1;
}
/* sort hitpoints (an arbitrary number of hits depending on sketch) */
for (i=0; i<nhits; i+=2) {
in = out = -1;
for (j=0; j<nhits; j++) {
if (hits[j] == NULL) continue;
if (in == -1) {
in = j;
continue;
}
/* store shortest dist as 'in', second shortest as 'out' */
if (hits[j]->hit_dist <= hits[in]->hit_dist) {
out = in;
in = j;
} else if (out == -1 || hits[j]->hit_dist <= hits[out]->hit_dist) {
out = j;
}
}
if (in == -1 || out == -1) {
bu_log("failed to find valid segment. nhits: %zu\n", nhits);
break;
}
if (ZERO(hits[in]->hit_dist - hits[out]->hit_dist)) {
hits[in] = NULL;
hits[out] = NULL;
continue;
}
RT_GET_SEG(segp, ap->a_resource);
segp->seg_stp = stp;
segp->seg_in = *hits[in];
hits[in] = NULL;
segp->seg_out = *hits[out];
hits[out] = NULL;
BU_LIST_INSERT(&(seghead->l), &(segp->l));
}
return nhits;
}
int
main(int argc, char *argv[])
{
bn_poly_t equation; /* holds our polynomial equation */
bn_complex_t roots[BN_MAX_POLY_DEGREE]; /* stash up to four roots */
int num_roots;
if (argc > 1)
bu_exit(1, "%s: unexpected argument(s)\n", argv[0]);
/*********************************************
* Linear polynomial (1st degree equation):
* A*X + B = 0
* [0] [1] <=coefficients
*/
equation.dgr = 1;
equation.cf[0] = 1; /* A */
equation.cf[1] = -2; /* B */
/* print the equation */
bu_log("\n*** LINEAR ***\n");
bn_pr_poly("Solving for Linear", &equation);
/* solve for the roots */
num_roots = rt_poly_roots(&equation, roots, "My Linear Polynomial");
if (num_roots == 0) {
bu_log("No roots found!\n");
return 0;
} else if (num_roots < 0) {
bu_log("The root solver failed to converge on a solution\n");
return 1;
}
/* A*X + B = 0
* 1*X + -2 = 0
* X - 2 = 0
* X = 2
*/
/* print the roots */
bu_log("The root should be 2\n");
bn_pr_roots("My Linear Polynomial", roots, num_roots);
/*********************************************
* Quadratic polynomial (2nd degree equation):
* A*X^2 + B*X + C = 0
* [0] [1] [2] <=coefficients
*/
equation.dgr = 2;
equation.cf[0] = 1; /* A */
equation.cf[1] = 0; /* B */
equation.cf[2] = -4; /* C */
/* print the equation */
bu_log("\n*** QUADRATIC ***\n");
bn_pr_poly("Solving for Quadratic", &equation);
/* solve for the roots */
num_roots = rt_poly_roots(&equation, roots, "My Quadratic Polynomial");
if (num_roots == 0) {
bu_log("No roots found!\n");
return 0;
} else if (num_roots < 0) {
bu_log("The root solver failed to converge on a solution\n");
return 1;
}
/* A*X^2 + B*X + C = 0
* 1*X^2 + 0*X + -4 = 0
* X^2 - 4 = 0
* (X - 2) * (X + 2) = 0
* X - 2 = 0, X + 2 = 0
* X = 2, X = -2
*/
/* print the roots */
bu_log("The roots should be 2 and -2\n");
bn_pr_roots("My Quadratic Polynomial", roots, num_roots);
/*****************************************
* Cubic polynomial (3rd degree equation):
* A*X^3 + B*X^2 + C*X + D = 0
* [0] [1] [2] [3] <=coefficients
*/
equation.dgr = 3;
equation.cf[0] = 45;
equation.cf[1] = 24;
equation.cf[2] = -7;
equation.cf[3] = -2;
/* print the equation */
bu_log("\n*** CUBIC ***\n");
bn_pr_poly("Solving for Cubic", &equation);
/* solve for the roots */
num_roots = rt_poly_roots(&equation, roots, "My Cubic Polynomial");
if (num_roots == 0) {
bu_log("No roots found!\n");
return 0;
} else if (num_roots < 0) {
bu_log("The root solver failed to converge on a solution\n");
return 1;
}
/* print the roots */
bu_log("The roots should be 1/3, -1/5, and -2/3\n");
bn_pr_roots("My Cubic Polynomial", roots, num_roots);
/*******************************************
* Quartic polynomial (4th degree equation):
* A*X^4 + B*X^3 + C*X^2 + D*X + E = 0
* [0] [1] [2] [3] [4] <=coefficients
*/
equation.dgr = 4;
equation.cf[0] = 2;
equation.cf[1] = 4;
equation.cf[2] = -26;
equation.cf[3] = -28;
equation.cf[4] = 48;
/* print the equation */
bu_log("\n*** QUARTIC ***\n");
bn_pr_poly("Solving for Quartic", &equation);
/* solve for the roots */
num_roots = rt_poly_roots(&equation, roots, "My Quartic Polynomial");
if (num_roots == 0) {
bu_log("No roots found!\n");
return 0;
} else if (num_roots < 0) {
bu_log("The root solver failed to converge on a solution\n");
return 1;
}
/* print the roots */
bu_log("The roots should be 3, 1, -2, -4\n");
bn_pr_roots("My Quartic Polynomial", roots, num_roots);
/*******************************************
* Sextic polynomial (6th degree equation):
* A*X^6 + B*X^5 + C*X^4 + D*X^3 + E*X^2 + F*X + G = 0
* [0] [1] [2] [3] [4] [5] [6] <=coefficients
*/
equation.dgr = 6;
equation.cf[0] = 1;
equation.cf[1] = -8;
equation.cf[2] = 32;
equation.cf[3] = -78;
equation.cf[4] = 121;
equation.cf[5] = -110;
equation.cf[6] = 50;
/* print the equation */
bu_log("\n*** SEXTIC ***\n");
bn_pr_poly("Solving for Sextic", &equation);
/* solve for the roots */
num_roots = rt_poly_roots(&equation, roots, "My Sextic Polynomial");
if (num_roots == 0) {
bu_log("No roots found!\n");
return 0;
} else if (num_roots < 0) {
bu_log("The root solver failed to converge on a solution\n");
return 1;
}
/* print the roots */
bu_log("The roots should be 1 - i, 1 + i, 2 - i,2 + i, 1 - 2*i, 1 + 2*i \n");
bn_pr_roots("My Sextic Polynomial", roots, num_roots);
return 0;
}