static void fill_colortbl(unsigned char *lo_rgb, unsigned char *hi_rgb) { int i; double a, b; #if BLEND_USING_HSV fastf_t lo_hsv[3], hi_hsv[3]; fastf_t hsv[3]; bu_rgb_to_hsv(lo_rgb, lo_hsv); bu_rgb_to_hsv(hi_rgb, hi_hsv); #endif for (i = 0; i < MAX_COLORTBL; ++i) { b = ((double) i) / (MAX_COLORTBL - 1); a = 1.0 - b; #if BLEND_USING_HSV VBLEND2(hsv, a, lo_hsv, b, hi_hsv); bu_hsv_to_rgb(hsv, colortbl[i]); #else VBLEND2(colortbl[i], a, lo_rgb, b, hi_rgb); #endif } }
static void val_To_RGB(cell_val cv, unsigned char *rgb) { double val; if (color_flag) { COPYRGB(rgb, cv.v_color); return; } val = (cv.v_scalar - dom_min) * dom_cvt; if ((boolean_flag && !ZERO(cv.v_scalar - bool_val)) || (val < SMALL_FASTF) || (val > 10.0)) { COPYRGB(rgb, BACKGROUND); } else if (ZERO(val)) { COPYRGB(rgb, WHITE); } else { int idx; double rem; double res; if (interp_flag) { vect_t prev_hsv; vect_t hsv; vect_t next_hsv; idx = val + 0.01; /* convert to range [0 to 10] */ if ((rem = val - (double) idx) < 0.0) /* remainder */ rem = 0.0; res = 1.0 - rem; #if BLEND_USING_HSV bu_rgb_to_hsv(colortbl[idx], prev_hsv); bu_rgb_to_hsv(colortbl[idx+1], next_hsv); VBLEND2(hsv, res, prev_hsv, rem, next_hsv); bu_hsv_to_rgb(hsv, rgb); #else VBLEND2(rgb, res, colortbl[idx], rem, colortbl[idx+1]); #endif } else { idx = val + 0.51; COPYRGB(rgb, colortbl[idx]); } } return; }
void quat_slerp(fastf_t *qout, const fastf_t *q1, const fastf_t *q2, double f) { double omega; double cos_omega; double invsin; register double s1, s2; cos_omega = QDOT( q1, q2 ); if ( (1.0 + cos_omega) > 1.0e-5 ) { /* cos_omega > -0.99999 */ if ( (1.0 - cos_omega) > 1.0e-5 ) { /* usual case */ omega = acos(cos_omega); /* XXX atan2? */ invsin = 1.0 / sin(omega); s1 = sin( (1.0-f)*omega ) * invsin; s2 = sin( f*omega ) * invsin; } else { /* * cos_omega > 0.99999 * The ends are very close to each other, * use linear interpolation, to avoid divide-by-zero */ s1 = 1.0 - f; s2 = f; } QBLEND2( qout, s1, q1, s2, q2 ); } else { /* * cos_omega == -1, omega = M_PI. * The ends are nearly opposite, 180 degrees (M_PI) apart. */ /* (I have no idea what permuting the elements accomplishes, * perhaps it creates a perpendicular? */ qout[X] = -q1[Y]; qout[Y] = q1[X]; qout[Z] = -q1[W]; s1 = sin( (0.5-f) * M_PI ); s2 = sin( f * M_PI ); VBLEND2( qout, s1, q1, s2, qout ); qout[W] = q1[Z]; } }
/* * M A I N ( ) */ int main (int argc, char **argv) { char *inbuf; /* The input scanline */ char *outbuf; /* " output " */ char *in, *out; /* Pointers into inbuf and outbuf */ fastf_t twice_r1r2; fastf_t squares; fastf_t scale_fac; fastf_t theta; fastf_t x; /* Scale factor for pixel blending */ int i; /* Pixel index in inbuf */ int j; /* " " " outbuf */ long int row; long int row_width; if (!get_args( argc, argv )) { (void) fputs(usage, stderr); bu_exit (1, NULL); } if (solid_type == SPHERE) { (void) fprintf(stderr, "Sphere scaling not yet implemented\n"); bu_exit (1, NULL); } else if (solid_type != TORUS) { (void) fprintf(stderr, "Illegal solid type %d\n", solid_type); bu_exit (0, NULL); } /* * Autosize the input if appropriate */ if (fileinput && autosize) { unsigned long int w, h; if (fb_common_file_size(&w, &h, file_name, 3)) { file_width = (long)w; file_height = (long)h; } else (void) fprintf(stderr, "texturescale: unable to autosize\n"); } /* * Initialize some runtime constants */ twice_r1r2 = 2 * r1 * r2; squares = r1 * r1 + r2 * r2; scale_fac = file_width / (r1 + r2); /* * Allocate 1-scanline buffers for input and output */ outbuf = bu_malloc(3*file_width, "outbuf"); inbuf = bu_malloc(3*file_width, "inbuf"); /* * Do the filtering */ for (row = 0; row < file_height; ++row) { /* * Read an input scanline */ if (! read_row(inbuf, file_width, infp)) { perror(file_name); (void) fprintf(stderr, "texturescale: fread() error\n"); bu_exit (1, NULL); } /* * Determine how much of the input scanline we want */ theta = 2 * bn_pi * row / file_height; row_width = scale_fac * sqrt(squares - twice_r1r2 * cos(theta)); in = inbuf + ((file_width - row_width) / 2) * 3; out = outbuf; /* * Scale the input scanline into the output scanline */ for (i = j = 1; j <= file_width; ++j) { if (i * file_width < j * row_width) { x = j - (i * file_width) / row_width; VBLEND2(out, (1.0 - x), in, x, in + 3); ++i; in += 3; } else VMOVE(out, in); out += 3; } /* * Write the output scanline */ if (fwrite(outbuf, 3, file_width, stdout) != file_width) { perror("stdout"); bu_exit (2, NULL); } } bu_exit (1, NULL); }
/** * 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 bn_math_cmd(ClientData clientData, Tcl_Interp *interp, int argc, char **argv) { void (*math_func)(); struct bu_vls result; math_func = (void (*)())clientData; /* object-to-function cast */ bu_vls_init(&result); if (math_func == bn_mat_mul) { mat_t o, a, b; if (argc < 3 || bn_decode_mat(a, argv[1]) < 16 || bn_decode_mat(b, argv[2]) < 16) { bu_vls_printf(&result, "usage: %s matA matB", argv[0]); goto error; } bn_mat_mul(o, a, b); bn_encode_mat(&result, o); } else if (math_func == bn_mat_inv || math_func == bn_mat_trn) { mat_t o, a; if (argc < 2 || bn_decode_mat(a, argv[1]) < 16) { bu_vls_printf(&result, "usage: %s mat", argv[0]); goto error; } (*math_func)(o, a); bn_encode_mat(&result, o); } else if (math_func == bn_matXvec) { mat_t m; hvect_t i, o; if (argc < 3 || bn_decode_mat(m, argv[1]) < 16 || bn_decode_hvect(i, argv[2]) < 4) { bu_vls_printf(&result, "usage: %s mat hvect", argv[0]); goto error; } bn_matXvec(o, m, i); bn_encode_hvect(&result, o); } else if (math_func == bn_mat4x3pnt) { mat_t m; point_t i, o; if (argc < 3 || bn_decode_mat(m, argv[1]) < 16 || bn_decode_vect(i, argv[2]) < 3) { bu_vls_printf(&result, "usage: %s mat point", argv[0]); goto error; } bn_mat4x3pnt(o, m, i); bn_encode_vect(&result, o); } else if (math_func == bn_mat4x3vec) { mat_t m; vect_t i, o; if (argc < 3 || bn_decode_mat(m, argv[1]) < 16 || bn_decode_vect(i, argv[2]) < 3) { bu_vls_printf(&result, "usage: %s mat vect", argv[0]); goto error; } bn_mat4x3vec(o, m, i); bn_encode_vect(&result, o); } else if (math_func == bn_hdivide) { hvect_t i; vect_t o; if (argc < 2 || bn_decode_hvect(i, argv[1]) < 4) { bu_vls_printf(&result, "usage: %s hvect", argv[0]); goto error; } bn_hdivide(o, i); bn_encode_vect(&result, o); } else if (math_func == bn_vjoin1) { point_t o; point_t b, d; fastf_t c; if (argc < 4) { bu_vls_printf(&result, "usage: %s pnt scale dir", argv[0]); goto error; } if ( bn_decode_vect(b, argv[1]) < 3) goto error; if (Tcl_GetDouble(interp, argv[2], &c) != TCL_OK) goto error; if ( bn_decode_vect(d, argv[3]) < 3) goto error; VJOIN1( o, b, c, d ); /* bn_vjoin1( o, b, c, d ) */ bn_encode_vect(&result, o); } else if ( math_func == bn_vblend) { point_t a, c, e; fastf_t b, d; if ( argc < 5 ) { bu_vls_printf(&result, "usage: %s scale pnt scale pnt", argv[0]); goto error; } if ( Tcl_GetDouble(interp, argv[1], &b) != TCL_OK) goto error; if ( bn_decode_vect( c, argv[2] ) < 3) goto error; if ( Tcl_GetDouble(interp, argv[3], &d) != TCL_OK) goto error; if ( bn_decode_vect( e, argv[4] ) < 3) goto error; VBLEND2( a, b, c, d, e ) bn_encode_vect( &result, a ); } else if (math_func == bn_mat_ae) { mat_t o; double az, el; if (argc < 3) { bu_vls_printf(&result, "usage: %s azimuth elevation", argv[0]); goto error; } if (Tcl_GetDouble(interp, argv[1], &az) != TCL_OK) goto error; if (Tcl_GetDouble(interp, argv[2], &el) != TCL_OK) goto error; bn_mat_ae(o, (fastf_t)az, (fastf_t)el); bn_encode_mat(&result, o); } else if (math_func == bn_ae_vec) { fastf_t az, el; vect_t v; if (argc < 2 || bn_decode_vect(v, argv[1]) < 3) { bu_vls_printf(&result, "usage: %s vect", argv[0]); goto error; } bn_ae_vec(&az, &el, v); bu_vls_printf(&result, "%g %g", az, el); } else if (math_func == bn_aet_vec) { fastf_t az, el, twist, accuracy; vect_t vec_ae, vec_twist; if (argc < 4 || bn_decode_vect(vec_ae, argv[1]) < 3 || bn_decode_vect(vec_twist, argv[2]) < 3 || sscanf(argv[3], "%lf", &accuracy) < 1) { bu_vls_printf(&result, "usage: %s vec_ae vec_twist accuracy", argv[0]); goto error; } bn_aet_vec(&az, &el, &twist, vec_ae, vec_twist, accuracy); bu_vls_printf(&result, "%g %g %g", az, el, twist); } else if (math_func == bn_mat_angles) { mat_t o; double alpha, beta, ggamma; if (argc < 4) { bu_vls_printf(&result, "usage: %s alpha beta gamma", argv[0]); goto error; } if (Tcl_GetDouble(interp, argv[1], &alpha) != TCL_OK) goto error; if (Tcl_GetDouble(interp, argv[2], &beta) != TCL_OK) goto error; if (Tcl_GetDouble(interp, argv[3], &ggamma) != TCL_OK) goto error; bn_mat_angles(o, alpha, beta, ggamma); bn_encode_mat(&result, o); } else if (math_func == bn_eigen2x2) { fastf_t val1, val2; vect_t vec1, vec2; double a, b, c; if (argc < 4) { bu_vls_printf(&result, "usage: %s a b c", argv[0]); goto error; } if (Tcl_GetDouble(interp, argv[1], &a) != TCL_OK) goto error; if (Tcl_GetDouble(interp, argv[2], &c) != TCL_OK) goto error; if (Tcl_GetDouble(interp, argv[3], &b) != TCL_OK) goto error; bn_eigen2x2(&val1, &val2, vec1, vec2, (fastf_t)a, (fastf_t)b, (fastf_t)c); bu_vls_printf(&result, "%g %g {%g %g %g} {%g %g %g}", val1, val2, V3ARGS(vec1), V3ARGS(vec2)); } else if (math_func == bn_mat_fromto) { mat_t o; vect_t from, to; if (argc < 3 || bn_decode_vect(from, argv[1]) < 3 || bn_decode_vect(to, argv[2]) < 3) { bu_vls_printf(&result, "usage: %s vecFrom vecTo", argv[0]); goto error; } bn_mat_fromto(o, from, to); bn_encode_mat(&result, o); } else if (math_func == bn_mat_xrot || math_func == bn_mat_yrot || math_func == bn_mat_zrot) { mat_t o; double s, c; if (argc < 3) { bu_vls_printf(&result, "usage: %s sinAngle cosAngle", argv[0]); goto error; } if (Tcl_GetDouble(interp, argv[1], &s) != TCL_OK) goto error; if (Tcl_GetDouble(interp, argv[2], &c) != TCL_OK) goto error; (*math_func)(o, s, c); bn_encode_mat(&result, o); } else if (math_func == bn_mat_lookat) { mat_t o; vect_t dir; int yflip; if (argc < 3 || bn_decode_vect(dir, argv[1]) < 3) { bu_vls_printf(&result, "usage: %s dir yflip", argv[0]); goto error; } if (Tcl_GetBoolean(interp, argv[2], &yflip) != TCL_OK) goto error; bn_mat_lookat(o, dir, yflip); bn_encode_mat(&result, o); } else if (math_func == bn_vec_ortho || math_func == bn_vec_perp) { vect_t ov, vec; if (argc < 2 || bn_decode_vect(vec, argv[1]) < 3) { bu_vls_printf(&result, "usage: %s vec", argv[0]); goto error; } (*math_func)(ov, vec); bn_encode_vect(&result, ov); } else if (math_func == bn_mat_scale_about_pt_wrapper) { mat_t o; vect_t v; double scale; int status; if (argc < 3 || bn_decode_vect(v, argv[1]) < 3) { bu_vls_printf(&result, "usage: %s pt scale", argv[0]); goto error; } if (Tcl_GetDouble(interp, argv[2], &scale) != TCL_OK) goto error; bn_mat_scale_about_pt_wrapper(&status, o, v, scale); if (status != 0) { bu_vls_printf(&result, "error performing calculation"); goto error; } bn_encode_mat(&result, o); } else if (math_func == bn_mat_xform_about_pt) { mat_t o, xform; vect_t v; if (argc < 3 || bn_decode_mat(xform, argv[1]) < 16 || bn_decode_vect(v, argv[2]) < 3) { bu_vls_printf(&result, "usage: %s xform pt", argv[0]); goto error; } bn_mat_xform_about_pt(o, xform, v); bn_encode_mat(&result, o); } else if (math_func == bn_mat_arb_rot) { mat_t o; point_t pt; vect_t dir; double angle; if (argc < 4 || bn_decode_vect(pt, argv[1]) < 3 || bn_decode_vect(dir, argv[2]) < 3) { bu_vls_printf(&result, "usage: %s pt dir angle", argv[0]); goto error; } if (Tcl_GetDouble(interp, argv[3], &angle) != TCL_OK) return TCL_ERROR; bn_mat_arb_rot(o, pt, dir, (fastf_t)angle); bn_encode_mat(&result, o); } else if (math_func == quat_mat2quat) { mat_t mat; quat_t quat; if (argc < 2 || bn_decode_mat(mat, argv[1]) < 16) { bu_vls_printf(&result, "usage: %s mat", argv[0]); goto error; } quat_mat2quat(quat, mat); bn_encode_quat(&result, quat); } else if (math_func == quat_quat2mat) { mat_t mat; quat_t quat; if (argc < 2 || bn_decode_quat(quat, argv[1]) < 4) { bu_vls_printf(&result, "usage: %s quat", argv[0]); goto error; } quat_quat2mat(mat, quat); bn_encode_mat(&result, mat); } else if (math_func == bn_quat_distance_wrapper) { quat_t q1, q2; double d; if (argc < 3 || bn_decode_quat(q1, argv[1]) < 4 || bn_decode_quat(q2, argv[2]) < 4) { bu_vls_printf(&result, "usage: %s quatA quatB", argv[0]); goto error; } bn_quat_distance_wrapper(&d, q1, q2); bu_vls_printf(&result, "%g", d); } else if (math_func == quat_double || math_func == quat_bisect || math_func == quat_make_nearest) { quat_t oqot, q1, q2; if (argc < 3 || bn_decode_quat(q1, argv[1]) < 4 || bn_decode_quat(q2, argv[2]) < 4) { bu_vls_printf(&result, "usage: %s quatA quatB", argv[0]); goto error; } (*math_func)(oqot, q1, q2); bn_encode_quat(&result, oqot); } else if (math_func == quat_slerp) { quat_t oq, q1, q2; double d; if (argc < 4 || bn_decode_quat(q1, argv[1]) < 4 || bn_decode_quat(q2, argv[2]) < 4) { bu_vls_printf(&result, "usage: %s quat1 quat2 factor", argv[0]); goto error; } if (Tcl_GetDouble(interp, argv[3], &d) != TCL_OK) goto error; quat_slerp(oq, q1, q2, d); bn_encode_quat(&result, oq); } else if (math_func == quat_sberp) { quat_t oq, q1, qa, qb, q2; double d; if (argc < 6 || bn_decode_quat(q1, argv[1]) < 4 || bn_decode_quat(qa, argv[2]) < 4 || bn_decode_quat(qb, argv[3]) < 4 || bn_decode_quat(q2, argv[4]) < 4) { bu_vls_printf(&result, "usage: %s quat1 quatA quatB quat2 factor", argv[0]); goto error; } if (Tcl_GetDouble(interp, argv[5], &d) != TCL_OK) goto error; quat_sberp(oq, q1, qa, qb, q2, d); bn_encode_quat(&result, oq); } else if (math_func == quat_exp || math_func == quat_log) { quat_t qout, qin; if (argc < 2 || bn_decode_quat(qin, argv[1]) < 4) { bu_vls_printf(&result, "usage: %s quat", argv[0]); goto error; } (*math_func)(qout, qin); bn_encode_quat(&result, qout); } else if (math_func == (void (*)())bn_isect_line3_line3) { double t, u; point_t pt, a; vect_t dir, c; int i; static const struct bn_tol tol = { BN_TOL_MAGIC, 0.005, 0.005*0.005, 1e-6, 1-1e-6 }; if (argc != 5) { bu_vls_printf(&result, "Usage: bn_isect_line3_line3 pt dir pt dir (%d args specified)", argc-1); goto error; } if (bn_decode_vect(pt, argv[1]) < 3) { bu_vls_printf(&result, "bn_isect_line3_line3 no pt: %s\n", argv[0]); goto error; } if (bn_decode_vect(dir, argv[2]) < 3) { bu_vls_printf(&result, "bn_isect_line3_line3 no dir: %s\n", argv[0]); goto error; } if (bn_decode_vect(a, argv[3]) < 3) { bu_vls_printf(&result, "bn_isect_line3_line3 no a pt: %s\n", argv[0]); goto error; } if (bn_decode_vect(c, argv[4]) < 3) { bu_vls_printf(&result, "bn_isect_line3_line3 no c dir: %s\n", argv[0]); goto error; } i = bn_isect_line3_line3(&t, &u, pt, dir, a, c, &tol); if (i != 1) { bu_vls_printf(&result, "bn_isect_line3_line3 no intersection: %s\n", argv[0]); goto error; } VJOIN1(a, pt, t, dir); bn_encode_vect(&result, a); } else if (math_func == (void (*)())bn_isect_line2_line2) { double dist[2]; point_t pt, a; vect_t dir, c; int i; static const struct bn_tol tol = { BN_TOL_MAGIC, 0.005, 0.005*0.005, 1e-6, 1-1e-6 }; if (argc != 5) { bu_vls_printf(&result, "Usage: bn_isect_line2_line2 pt dir pt dir (%d args specified)", argc-1); goto error; } /* i = bn_isect_line2_line2 {0 0} {1 0} {1 1} {0 -1} */ VSETALL(pt, 0.0); VSETALL(dir, 0.0); VSETALL(a, 0.0); VSETALL(c, 0.0); if (bn_decode_vect(pt, argv[1]) < 2) { bu_vls_printf(&result, "bn_isect_line2_line2 no pt: %s\n", argv[0]); goto error; } if (bn_decode_vect(dir, argv[2]) < 2) { bu_vls_printf(&result, "bn_isect_line2_line2 no dir: %s\n", argv[0]); goto error; } if (bn_decode_vect(a, argv[3]) < 2) { bu_vls_printf(&result, "bn_isect_line2_line2 no a pt: %s\n", argv[0]); goto error; } if (bn_decode_vect(c, argv[4]) < 2) { bu_vls_printf(&result, "bn_isect_line2_line2 no c dir: %s\n", argv[0]); goto error; } i = bn_isect_line2_line2(dist, pt, dir, a, c, &tol); if (i != 1) { bu_vls_printf(&result, "bn_isect_line2_line2 no intersection: %s\n", argv[0]); goto error; } VJOIN1(a, pt, dist[0], dir); bu_vls_printf(&result, "%g %g", a[0], a[1]); } else { bu_vls_printf(&result, "libbn/bn_tcl.c: math function %s not supported yet\n", argv[0]); goto error; } Tcl_AppendResult(interp, bu_vls_addr(&result), (char *)NULL); bu_vls_free(&result); return TCL_OK; error: Tcl_AppendResult(interp, bu_vls_addr(&result), (char *)NULL); bu_vls_free(&result); return TCL_ERROR; }
static void bn_vblend(mat_t a, fastf_t b, mat_t c, fastf_t d, mat_t e) { VBLEND2( a, b, c, d, e ); }