C++ VSUB2函数代码示例

static int
move_walk(ClientData UNUSED(clientData), Tcl_Interp *interp, int UNUSED(objc), Tcl_Obj *const *objv)
{
    struct isst_s *isst;
    Togl *togl;
    vect_t vec;
    int flag;

    if (Togl_GetToglFromObj(interp, objv[1], &togl) != TCL_OK)
	return TCL_ERROR;

    isst = (struct isst_s *) Togl_GetClientData(togl);

    if (Tcl_GetIntFromObj(interp, objv[2], &flag) != TCL_OK)
	return TCL_ERROR;

    if (flag >= 0) {
	VSUB2(vec, isst->camera.focus, isst->camera.pos);
	VSCALE(vec, vec, 0.1 * isst->tie->radius);
	VADD2(isst->camera.pos, isst->camera.pos, vec);
	VADD2(isst->camera.focus, isst->camera.focus, vec);
    } else {
	VSUB2(vec, isst->camera.pos, isst->camera.focus);
	VSCALE(vec, vec, 0.1 * isst->tie->radius);
	VADD2(isst->camera.pos, isst->camera.pos, vec);
	VADD2(isst->camera.focus, isst->camera.focus, vec);
    }
    isst->dirty = 1;
    return TCL_OK;
}

void
render_flos_work(render_t *render, struct tie_s *tie, struct tie_ray_s *ray, vect_t *pixel) {
    struct tie_id_s id, tid;
    vect_t vec;
    fastf_t angle;
    struct render_flos_s *rd;

    rd = (struct render_flos_s *)render->data;

    if (tie_work(tie, ray, &id, render_hit, NULL) != NULL) {
	VSET(*pixel, 0.0, 0.5, 0.0);
    } else
	return;

    VSUB2(vec, ray->pos, id.pos);
    VUNITIZE(vec);
    angle = VDOT(vec, id.norm);

    /* Determine if direct line of sight to fragment */
    VMOVE(ray->pos, rd->frag_pos);
    VSUB2(ray->dir, id.pos, rd->frag_pos);
    VUNITIZE(ray->dir);

    if (tie_work(tie, ray, &tid, render_hit, NULL)) {
	if (fabs (id.pos[0] - tid.pos[0]) < TIE_PREC
	    && fabs (id.pos[1] - tid.pos[1]) < TIE_PREC
	    && fabs (id.pos[2] - tid.pos[2]) < TIE_PREC)
	{
	    VSET(*pixel, 1.0, 0.0, 0.0);
	}
    }

    VSCALE(*pixel, *pixel, (0.5+angle*0.5));
}

/**
 * R T _ P G _ P L O T _ P O L Y
 *
 * Convert to vlist, draw as polygons.
 */
int
rt_pg_plot_poly(struct bu_list *vhead, struct rt_db_internal *ip, const struct rt_tess_tol *UNUSED(ttol), const struct bn_tol *UNUSED(tol))
{
    size_t i;
    size_t p;	/* current polygon number */
    struct rt_pg_internal *pgp;

    BU_CK_LIST_HEAD(vhead);
    RT_CK_DB_INTERNAL(ip);
    pgp = (struct rt_pg_internal *)ip->idb_ptr;
    RT_PG_CK_MAGIC(pgp);

    for (p = 0; p < pgp->npoly; p++) {
	struct rt_pg_face_internal *pp;
	vect_t aa, bb, norm;

	pp = &pgp->poly[p];
	if (pp->npts < 3)
	    continue;
	VSUB2(aa, &pp->verts[3*(0)], &pp->verts[3*(1)]);
	VSUB2(bb, &pp->verts[3*(0)], &pp->verts[3*(2)]);
	VCROSS(norm, aa, bb);
	VUNITIZE(norm);
	RT_ADD_VLIST(vhead, norm, BN_VLIST_POLY_START);

	RT_ADD_VLIST(vhead, &pp->verts[3*(pp->npts-1)], BN_VLIST_POLY_MOVE);
	for (i=0; i < pp->npts-1; i++) {
	    RT_ADD_VLIST(vhead, &pp->verts[3*i], BN_VLIST_POLY_DRAW);
	}
	RT_ADD_VLIST(vhead, &pp->verts[3*(pp->npts-1)], BN_VLIST_POLY_END);
    }
    return 0;		/* OK */
}

int
_ged_do_tra(struct ged *gedp,
            char coord,
            vect_t tvec,
            int (*func)())
{
    point_t delta;
    point_t work;
    point_t vc, nvc;

    if (func != (int (*)())0)
        return (*func)(gedp, coord, tvec);

    switch (coord) {
    case 'm':
        VSCALE(delta, tvec, -gedp->ged_wdbp->dbip->dbi_base2local);
        MAT_DELTAS_GET_NEG(vc, gedp->ged_gvp->gv_center);
        break;
    case 'v':
    default:
        VSCALE(tvec, tvec, -2.0*gedp->ged_wdbp->dbip->dbi_base2local*gedp->ged_gvp->gv_isize);
        MAT4X3PNT(work, gedp->ged_gvp->gv_view2model, tvec);
        MAT_DELTAS_GET_NEG(vc, gedp->ged_gvp->gv_center);
        VSUB2(delta, work, vc);
        break;
    }

    VSUB2(nvc, vc, delta);
    MAT_DELTAS_VEC_NEG(gedp->ged_gvp->gv_center, nvc);
    ged_view_update(gedp->ged_gvp);

    return GED_OK;
}

inline void
rt_metaball_norm_internal(vect_t *n, point_t *p, struct rt_metaball_internal *mb)
{
    struct wdb_metaballpt *mbpt;
    vect_t v;
    fastf_t a;

    VSETALL(*n, 0.0);

    switch (mb->method) {
	case METABALL_METABALL: bu_log("Sorry, strict metaballs are not yet implemented\n");
	    break;
	case METABALL_ISOPOTENTIAL:
	    for (BU_LIST_FOR(mbpt, wdb_metaballpt, &mb->metaball_ctrl_head)) {
		VSUB2(v, *p, mbpt->coord);
		a = MAGSQ(v);
		VJOIN1(*n, *n, fabs(mbpt->fldstr)*mbpt->fldstr / (SQ(a)), v);	/* f/r^4 */
	    }
	    break;
	case METABALL_BLOB:
	    for (BU_LIST_FOR(mbpt, wdb_metaballpt, &mb->metaball_ctrl_head)) {
		VSUB2(v, *p, mbpt->coord);
		a = MAGSQ(v);
		VJOIN1(*n, *n, 2.0*mbpt->sweat/SQ(mbpt->fldstr)*exp(mbpt->sweat*(1-(a/SQ(mbpt->fldstr)))) , v);
	    }
	    break;
	default: bu_log("unknown metaball method\n"); break;
    }
    VUNITIZE(*n);
}

static void
render_camera_prep_ortho(render_camera_t *camera)
{
    vect_t look, up, side, temp;
    tfloat angle, s, c;

    /* Generate standard up vector */
    up[0] = 0;
    up[1] = 0;
    up[2] = 1;

    /* Generate unitized lookector */
    VSUB2(look, camera->focus, camera->pos);
    VUNITIZE(look);

    /* Make unitized up vector perpendicular to lookector */
    VMOVE(temp, look);
    angle = VDOT(up, temp);
    VSCALE(temp, temp, angle);
    VSUB2(up, up, temp);
    VUNITIZE(up);

    /* Generate a temporary side vector */
    VCROSS(side, up, look);

    /* Apply tilt to up vector - negate angle to make positive angles clockwise */
    s = sin(-camera->tilt * DEG2RAD);
    c = cos(-camera->tilt * DEG2RAD);
    VSCALE(up, up, c);
    VSCALE(side, side, s);
    VADD2(up, up, side);

    /* Create final side vector */
    VCROSS(side, up, look);

    /* look direction */
    VMOVE(camera->view_list[0].top_l, look);

    /* gridsize is millimeters along the horizontal axis to display */
    /* left (side) */
    VSCALE(temp, side, (camera->aspect * camera->gridsize * 0.5));
    VADD2(camera->view_list[0].pos, camera->pos, temp);
    /* and (up) */
    VSCALE(temp, up, (camera->gridsize * 0.5));
    VADD2(camera->view_list[0].pos, camera->view_list[0].pos, temp);

    /* compute step vectors for camera position */

    /* X */
    VSCALE(camera->view_list[0].step_x, side, (-camera->gridsize * camera->aspect / (tfloat)camera->w));

    /* Y */
    VSCALE(camera->view_list[0].step_y, up, (-camera->gridsize / (tfloat)camera->h));
}

void
top(fastf_t *vec1, fastf_t *vec2, fastf_t *t)
{
    fastf_t	tooch, mag;
    vect_t	del, tvec;
    int i, j;

    tooch = t[2] * .25;
    del[0] = vec2[0] - vec1[0];
    del[1] = 0.0;
    del[2] = vec2[2] - vec1[2];
    mag = MAGNITUDE( del );
    VSCALE(tvec, del, tooch/mag);
    VSUB2(&sol.s_values[0], vec1, tvec);
    VADD2(del, del, tvec);
    VADD2(&sol.s_values[3], del, tvec);
    tvec[0] = tvec[2] = 0.0;
    tvec[1] = t[1] - t[0];
    VCROSS(del, tvec, &sol.s_values[3]);
    mag = MAGNITUDE( del );
    if (del[2] < 0)
	mag *= -1.0;
    VSCALE(&sol.s_values[9], del, t[2]/mag);
    VADD2(&sol.s_values[6], &sol.s_values[3], &sol.s_values[9]);
    VMOVE(&sol.s_values[12], tvec);

    for (i=3; i<=9; i+=3) {
	j = i + 12;
	VADD2(&sol.s_values[j], &sol.s_values[i], tvec);
    }
}

/**
 * For a hit on the surface of an METABALL, return the (u, v)
 * coordinates of the hit point, 0 <= u, v <= 1.
 *
 * u = azimuth
 * v = elevation
 */
void
rt_metaball_uv(struct application *ap, struct soltab *stp, struct hit *hitp, struct uvcoord *uvp)
{
    struct rt_metaball_internal *metaball = (struct rt_metaball_internal *)stp->st_specific;
    vect_t work, pprime;
    fastf_t r;

    if (ap) RT_CK_APPLICATION(ap);
    if (stp) RT_CK_SOLTAB(stp);
    if (hitp) RT_CK_HIT(hitp);
    if (!uvp) return;
    if (!metaball) return;

    /* stuff stolen from sph */
    VSUB2(work, hitp->hit_point, stp->st_center);
    VSCALE(pprime, work, 1.0/MAGNITUDE(work));
    /* Assert that pprime has unit length */

    /* U is azimuth, atan() range: -pi to +pi */
    uvp->uv_u = bn_atan2(pprime[Y], pprime[X]) * M_1_2PI;
    if (uvp->uv_u < 0)
	uvp->uv_u += 1.0;
    /*
     * V is elevation, atan() range: -pi/2 to +pi/2, because sqrt()
     * ensures that X parameter is always >0
     */
    uvp->uv_v = bn_atan2(pprime[Z],
			 sqrt(pprime[X] * pprime[X] + pprime[Y] * pprime[Y])) * M_1_2PI;

    /* approximation: r / (circumference, 2 * pi * aradius) */
    r = ap->a_rbeam + ap->a_diverge * hitp->hit_dist;
    uvp->uv_du = uvp->uv_dv =
	M_1_2PI * r / stp->st_aradius;
    return;
}

void writeCylinder
(
    rt_wdb* wdbp,
    Form&   form,
    bool    translate
) {
    char   name[NAMELEN + 1];
    vect_t base, height;

    if (translate) {
      VADD2(base, form.data.pt[0], form.tr_vec);
    } else {
      VMOVE(base, form.data.pt[0]);
    }

    VSUB2(height, form.data.pt[1], form.data.pt[0]);

    VSCALE(base, base, IntavalUnitInMm);
    VSCALE(height, height, IntavalUnitInMm);

    fastf_t radius = form.radius1 * IntavalUnitInMm;

    sprintf(name, "s%lu.rcc", (long unsigned)++rcc_counter);
    mk_rcc(wdbp, name, base, height, radius);
    addToRegion(form.compnr, name);

    if (form.s_compnr >= 1000)
	excludeFromRegion(form.s_compnr, name);
}

/**
 * Clip a ray against a rectangular parallelepiped (RPP) that has faces
 * parallel to the coordinate planes (a clipping RPP).  The RPP is
 * defined by a minimum point and a maximum point.
 *
 * Returns -
 * 0 if ray does not hit RPP,
 * !0 if ray hits RPP.
 *
 * Implicit Return -
 * if !0 was returned, "a" and "b" have been clipped to the RPP.
 */
int
vclip(vect_t a, vect_t b, fastf_t *min, fastf_t *max)
{
    static vect_t diff;
    static double sv;
    static double st;
    static double mindist, maxdist;
    fastf_t *pt = &a[0];
    fastf_t *dir = &diff[0];
    int i;

    mindist = -CLIP_DISTANCE;
    maxdist = CLIP_DISTANCE;
    VSUB2(diff, b, a);

    for (i=0; i < 3; i++, pt++, dir++, max++, min++) {
	if (*dir < -EPSILON) {
	    if ((sv = (*min - *pt) / *dir) < 0.0)
		return 0;	/* MISS */
	    if (maxdist > sv)
		maxdist = sv;
	    if (mindist < (st = (*max - *pt) / *dir))
		mindist = st;
	}  else if (*dir > EPSILON) {
	    if ((st = (*max - *pt) / *dir) < 0.0)
		return 0;	/* MISS */
	    if (maxdist > st)
		maxdist = st;
	    if (mindist < ((sv = (*min - *pt) / *dir)))
		mindist = sv;
	} else {
	    /*
	     * If direction component along this axis is NEAR 0,
	     * (i.e., this ray is aligned with this axis), merely
	     * check against the boundaries.
	     */
	    if ((*min > *pt) || (*max < *pt))
		return 0;	/* MISS */;
	}
    }
    if (mindist >= maxdist)
	return 0;	/* MISS */

    if (mindist > 1 || maxdist < 0)
	return 0;	/* MISS */

    if (mindist <= 0 && maxdist >= 1)
	return 1;	/* HIT, no clipping needed */

    /* Don't grow one end of a contained segment */
    if (mindist < 0)
	mindist = 0;
    if (maxdist > 1)
	maxdist = 1;

    /* Compute actual intercept points */
    VJOIN1(b, a, maxdist, diff);		/* b must go first */
    VJOIN1(a, a, mindist, diff);
    return 1;		/* HIT */
}

/*
 * W R A Y
 */
void
wray(struct partition *pp, struct application *ap, FILE *fp, const vect_t inormal)
{
    struct vldray vldray;
    register struct hit *hitp= pp->pt_inhit;

    VMOVE(&(vldray.ox), hitp->hit_point);
    VSUB2(&(vldray.rx), pp->pt_outhit->hit_point,
	  hitp->hit_point);

    WRAY_NORMAL(vldray, inormal);

    vldray.pa = vldray.pe = vldray.pc = vldray.sc = 0;	/* no curv */

    /* Air is marked by zero or negative region ID codes.
     * When air is encountered, the air code is taken from reg_aircode.
     * The negative of the air code is used for the "ob" field, to
     * distinguish air from other regions.
     */
    if ((vldray.ob = pp->pt_regionp->reg_regionid) <= 0)
	vldray.ob = -(pp->pt_regionp->reg_aircode);

    WRAY_TAG(vldray, ap);

    if (fwrite(&vldray, sizeof(struct vldray), 1, fp) != 1)
	bu_bomb("rway:  write error");
}

/**
 * R E C _ C U R V E
 *
 * Return the "curvature" of the cylinder.  If an endplate,
 * pick a principle direction orthogonal to the normal, and
 * indicate no curvature.  Otherwise, compute curvature.
 * Normal must have been computed before calling this routine.
 */
void
rt_rec_curve(struct curvature *cvp, struct hit *hitp, struct soltab *stp)
{
    struct rec_specific *rec =
	(struct rec_specific *)stp->st_specific;
    vect_t uu;
    fastf_t ax, bx, q;

    switch (hitp->hit_surfno) {
	case REC_NORM_BODY:
	    /* This could almost certainly be simpler if we used
	     * inverse A rather than inverse A squared, right Ed?
	     */
	    VMOVE(cvp->crv_pdir, rec->rec_Hunit);
	    VSUB2(uu, hitp->hit_point, rec->rec_V);
	    cvp->crv_c1 = 0;
	    ax = VDOT(uu, rec->rec_A) * rec->rec_iAsq;
	    bx = VDOT(uu, rec->rec_B) * rec->rec_iBsq;
	    q = sqrt(ax * ax * rec->rec_iAsq + bx * bx * rec->rec_iBsq);
	    cvp->crv_c2 = - rec->rec_iAsq * rec->rec_iBsq / (q*q*q);
	    break;
	case REC_NORM_TOP:
	case REC_NORM_BOT:
	    bn_vec_ortho(cvp->crv_pdir, hitp->hit_normal);
	    cvp->crv_c1 = cvp->crv_c2 = 0;
	    break;
	default:
	    bu_log("rt_rec_curve: bad surfno %d\n", hitp->hit_surfno);
	    break;
    }
}

/*
 * Map "display plate coordinates" (which can just be the screen viewing cube),
 * into [-1, +1] coordinates, with perspective.
 * Per "High Resolution Virtual Reality" by Michael Deering,
 * Computer Graphics 26, 2, July 1992, pp 195-201.
 *
 * L is lower left corner of screen, H is upper right corner.
 * L[Z] is the front (near) clipping plane location.
 * H[Z] is the back (far) clipping plane location.
 *
 * This corresponds to the SGI "window()" routine, but taking into account
 * skew due to the eyepoint being offset parallel to the image plane.
 *
 * The gist of the algorithm is to translate the display plate to the
 * view center, shear the eye point to (0, 0, 1), translate back,
 * then apply an off-axis perspective projection.
 *
 * Another (partial) reference is "A comparison of stereoscopic cursors
 * for the interactive manipulation of B-splines" by Barham & McAllister,
 * SPIE Vol 1457 Stereoscopic Display & Applications, 1991, pg 19.
 */
void
ged_deering_persp_mat(fastf_t *m, const fastf_t *l, const fastf_t *h, const fastf_t *eye)
/* lower left corner of screen */
/* upper right (high) corner of screen */
/* eye location.  Traditionally at (0, 0, 1) */
{
    vect_t diff;	/* H - L */
    vect_t sum;	/* H + L */

    VSUB2(diff, h, l);
    VADD2(sum, h, l);

    m[0] = 2 * eye[Z] / diff[X];
    m[1] = 0;
    m[2] = (sum[X] - 2 * eye[X]) / diff[X];
    m[3] = -eye[Z] * sum[X] / diff[X];

    m[4] = 0;
    m[5] = 2 * eye[Z] / diff[Y];
    m[6] = (sum[Y] - 2 * eye[Y]) / diff[Y];
    m[7] = -eye[Z] * sum[Y] / diff[Y];

    /* Multiplied by -1, to do right-handed Z coords */
    m[8] = 0;
    m[9] = 0;
    m[10] = -(sum[Z] - 2 * eye[Z]) / diff[Z];
    m[11] = -(-eye[Z] + 2 * h[Z] * eye[Z]) / diff[Z];

    m[12] = 0;
    m[13] = 0;
    m[14] = -1;
    m[15] = eye[Z];

    /* XXX May need to flip Z ? (lefthand to righthand?) */
}

static int
find_closest_color(float color[3])
{
    int icolor[3];
    int i;
    int dist_sq;
    int color_num;

    VSCALE(icolor, color, 255);

    color_num = 0;
    dist_sq = MAGSQ(icolor);

    for (i = 1; i < 256; i++) {
	int tmp_dist;
	int diff[3];

	VSUB2(diff, icolor, &rgb[i*3]);
	tmp_dist = MAGSQ(diff);
	if (tmp_dist < dist_sq) {
	    dist_sq = tmp_dist;
	    color_num = i;
	}
    }

    return color_num;
}

static int
hit_headon(struct application *ap, struct partition *PartHeadp)
{
    register char diff_solid;
    vect_t	diff;
    register fastf_t len;

    if (PartHeadp->pt_forw->pt_forw != PartHeadp)
	Tcl_AppendResult(interp, "hit_headon: multiple partitions\n", (char *)NULL);

    VJOIN1(PartHeadp->pt_forw->pt_inhit->hit_point, ap->a_ray.r_pt,
	   PartHeadp->pt_forw->pt_inhit->hit_dist, ap->a_ray.r_dir);
    VSUB2(diff, PartHeadp->pt_forw->pt_inhit->hit_point, aim_point);

    diff_solid = (FIRST_SOLID(sp) !=
		  PartHeadp->pt_forw->pt_inseg->seg_stp->st_dp);
    len = MAGNITUDE(diff);

    if (	NEAR_ZERO(len, epsilon)
		||
		( diff_solid &&
		  VDOT(diff, ap->a_ray.r_dir) > 0 )
	)
	return(1);
    else
	return(0);
}