C++ VUNITIZE函数代码示例

/*
 * Replica of STEP function:
 * FUNCTION build_axes() :
 LIST [3:3] OF direction;
 LOCAL
 d1, d2 : direction;
 END_LOCAL;
 d1 := NVL(normalise(axis), dummy_gri || direction([0.0,0.0,1.0]));
 d2 := first_proj_axis(d1, ref_direction);
 RETURN [d2, normalise(cross_product(d1,d2)).orientation, d1];

 END_FUNCTION;
/////////
*/
void
Axis2Placement3D::BuildAxis() {
    double d1[3] = VINIT_ZERO;
    double d2[3] = VINIT_ZERO;
    double d1Xd2[3] = VINIT_ZERO;

    if (axis == NULL) {
	VSET(d1,0.0,0.0,1.0);
    } else {
	VMOVE(d1,axis->DirectionRatios());
	VUNITIZE(d1);
    }
    if (ref_direction == NULL) {
	FirstProjAxis(d2,d1,NULL);
    } else {
	FirstProjAxis(d2,d1,ref_direction->DirectionRatios());
    }
    VCROSS(d1Xd2,d1,d2);
    VUNITIZE(d1Xd2);
    VMOVE(p[0],d2);
    VMOVE(p[1],d1Xd2);
    VMOVE(p[2],d1);

    return;
}

struct hyp_specific *
hyp_internal_to_specific(struct rt_hyp_internal *hyp_in) {
    struct hyp_specific *hyp;
    BU_GET(hyp, struct hyp_specific);

    hyp->hyp_r1 = hyp_in->hyp_bnr * MAGNITUDE(hyp_in->hyp_A);
    hyp->hyp_r2 = hyp_in->hyp_bnr * hyp_in->hyp_b;
    hyp->hyp_c = sqrt(4 * MAGSQ(hyp_in->hyp_A) / MAGSQ(hyp_in->hyp_Hi) * (1 - hyp_in->hyp_bnr * hyp_in->hyp_bnr));

    VSCALE(hyp->hyp_H, hyp_in->hyp_Hi, 0.5);
    VADD2(hyp->hyp_V, hyp_in->hyp_Vi, hyp->hyp_H);
    VMOVE(hyp->hyp_Au, hyp_in->hyp_A);
    VUNITIZE(hyp->hyp_Au);

    hyp->hyp_rx = 1.0 / (hyp->hyp_r1 * hyp->hyp_r1);
    hyp->hyp_ry = 1.0 / (hyp->hyp_r2 * hyp->hyp_r2);
    hyp->hyp_rz = (hyp->hyp_c * hyp->hyp_c) / (hyp->hyp_r1 * hyp->hyp_r1);

    /* calculate height to use for top/bottom intersection planes */
    hyp->hyp_Hmag = MAGNITUDE(hyp->hyp_H);
    hyp->hyp_bounds = hyp->hyp_rz*hyp->hyp_Hmag*hyp->hyp_Hmag + 1.0;

    /* setup unit vectors for hyp_specific */
    VMOVE(hyp->hyp_Hunit, hyp->hyp_H);
    VMOVE(hyp->hyp_Aunit, hyp->hyp_Au);
    VCROSS(hyp->hyp_Bunit, hyp->hyp_Hunit, hyp->hyp_Aunit);

    VUNITIZE(hyp->hyp_Aunit);
    VUNITIZE(hyp->hyp_Bunit);
    VUNITIZE(hyp->hyp_Hunit);

    return hyp;
}

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

int
rt_gen_conic(struct xrays *rays, const struct xray *center_ray,
	     fastf_t theta, vect_t up_vector, int rays_per_radius)
{
    int count = 0;

    point_t start;
    vect_t orig_dir;

    fastf_t x, y;

    /* Setting radius to tan(theta) works because, as shown in the
     * following diagram, the ray that starts at the given point and
     * passes through orig_dir + (radius in any orthogonal direction)
     * has an angle of theta with the original ray; when the
     * resulting vector is normalized, the angle is preserved.
     */
    fastf_t radius = tan(theta);
    fastf_t rsq = radius * radius; /* radius-squared, for use in the loop */

    fastf_t gridsize = 2 * radius / (rays_per_radius - 1);

    vect_t a_dir, b_dir;

    register struct xrays *xrayp;

    VMOVE(start, center_ray->r_pt);
    VMOVE(orig_dir, center_ray->r_dir);

    /* Create vectors a_dir, b_dir that are orthogonal to orig_dir. */
    VMOVE(b_dir, up_vector);
    VUNITIZE(b_dir);

    VCROSS(a_dir, orig_dir, up_vector);
    VUNITIZE(a_dir);

    for (y = -radius; y <= radius; y += gridsize) {
	vect_t tmp;
	printf("y:%f\n", y);
	VSCALE(tmp, b_dir, y);
	printf("y_partofit: %f,%f,%f\n", V3ARGS(tmp));
	for (x = -radius; x <= radius; x += gridsize) {
	    if (((x*x)/rsq + (y*y)/rsq) <= 1) {
		BU_ALLOC(xrayp, struct xrays);
		VMOVE(xrayp->ray.r_pt, start);
		VJOIN2(xrayp->ray.r_dir, orig_dir, x, a_dir, y, b_dir);
		VUNITIZE(xrayp->ray.r_dir);
		xrayp->ray.index = count++;
		xrayp->ray.magic = RT_RAY_MAGIC;
		BU_LIST_APPEND(&rays->l, &xrayp->l);
	    }
	}
    }
    return count;
}

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

/*
 * Replica of STEP function:
 *   FUNCTION vector_difference()
 */
void
Axis2Placement3D::VectorDifference(double *result, double *v1, double *v2) {
    double vec1[3];
    double vec2[3];

    VMOVE(vec1,v1);
    VMOVE(vec2,v2);
    VUNITIZE(vec1);
    VUNITIZE(vec2);

    VADD2(result,vec1,vec2);
}

HIDDEN int
rt_pattern_rect_perspgrid(fastf_t **rays, size_t *ray_cnt, const point_t center_pt, const vect_t dir,
	       const vect_t a_vec, const vect_t b_vec,
	       const fastf_t a_theta, const fastf_t b_theta,
	       const fastf_t a_num, const fastf_t b_num)
{
    int count = 0;
    vect_t rdir;
    vect_t a_dir, b_dir;
    fastf_t x, y;
    fastf_t a_length = tan(a_theta);
    fastf_t b_length = tan(b_theta);
    fastf_t a_inc = 2 * a_length / (a_num - 1);
    fastf_t b_inc = 2 * b_length / (b_num - 1);

    VMOVE(a_dir, a_vec);
    VUNITIZE(a_dir);
    VMOVE(b_dir, b_vec);
    VUNITIZE(b_dir);

    /* Find out how much memory we'll need and get it */
    for (y = -b_length; y <= b_length + BN_TOL_DIST; y += b_inc) {
	for (x = -a_length; x <= a_length + BN_TOL_DIST; x += a_inc) {
	    count++;
	}
    }
    *(rays) = (fastf_t *)bu_calloc(sizeof(fastf_t) * 6, count + 1, "rays");

    /* Now that we have memory, reset count so it can
     * be used to index into the array */
    count = 0;

    /* This adds BN_TOL_DIST to the *_length variables in the
     * condition because in some cases, floating-point problems can
     * make extremely close numbers compare incorrectly. */
    for (y = -b_length; y <= b_length + BN_TOL_DIST; y += b_inc) {
	for (x = -a_length; x <= a_length + BN_TOL_DIST; x += a_inc) {
	    VJOIN2(rdir, dir, x, a_dir, y, b_dir);
	    VUNITIZE(rdir);
	    (*rays)[6*count] = center_pt[0];
	    (*rays)[6*count+1] = center_pt[1];
	    (*rays)[6*count+2] = center_pt[2];
	    (*rays)[6*count+3] = rdir[0];
	    (*rays)[6*count+4] = rdir[1];
	    (*rays)[6*count+5] = rdir[2];
	    count++;

	}
    }
    *(ray_cnt) = count;
    return count;
}

HIDDEN int
rt_pattern_rect_orthogrid(fastf_t **rays, size_t *ray_cnt, const point_t center_pt, const vect_t dir,
		const vect_t a_vec, const vect_t b_vec, const fastf_t da, const fastf_t db)
{
    int count = 0;
    point_t pt;
    vect_t a_dir;
    vect_t b_dir;
    fastf_t x, y;
    fastf_t a_length = MAGNITUDE(a_vec);
    fastf_t b_length = MAGNITUDE(b_vec);

    if (!rays || !ray_cnt) return -1;

    VMOVE(a_dir, a_vec);
    VUNITIZE(a_dir);

    VMOVE(b_dir, b_vec);
    VUNITIZE(b_dir);

    /* Find out how much memory we'll need and get it */
    for (y = -b_length; y <= b_length; y += db) {
	for (x = -a_length; x <= a_length; x += da) {
	    count++;
	}
    }
    *(rays) = (fastf_t *)bu_calloc(sizeof(fastf_t) * 6, count + 1, "rays");

    /* Now that we have memory, reset count so it can
     * be used to index into the array */
    count = 0;

    /* Build the rays */
    for (y = -b_length; y <= b_length; y += db) {
	for (x = -a_length; x <= a_length; x += da) {
	    VJOIN2(pt, center_pt, x, a_dir, y, b_dir);
	    (*rays)[6*count] = pt[0];
	    (*rays)[6*count+1] = pt[1];
	    (*rays)[6*count+2] = pt[2];
	    (*rays)[6*count+3] = dir[0];
	    (*rays)[6*count+4] = dir[1];
	    (*rays)[6*count+5] = dir[2];
	    count++;
	}
    }
    *(ray_cnt) = count;
    return count;
}

/**
 * Create a bounding RPP for an hyp
 */
int
rt_hyp_bbox(struct rt_db_internal *ip, point_t *min, point_t *max, const struct bn_tol *UNUSED(tol)) {
    struct rt_hyp_internal *xip;
    vect_t hyp_Au, hyp_B, hyp_An, hyp_Bn, hyp_H;
    vect_t pt1, pt2, pt3, pt4, pt5, pt6, pt7, pt8;
    RT_CK_DB_INTERNAL(ip);
    xip = (struct rt_hyp_internal *)ip->idb_ptr;
    RT_HYP_CK_MAGIC(xip);

    VMOVE(hyp_H, xip->hyp_Hi);
    VUNITIZE(hyp_H);
    VMOVE(hyp_Au, xip->hyp_A);
    VUNITIZE(hyp_Au);
    VCROSS(hyp_B, hyp_Au, hyp_H);

    VSETALL((*min), INFINITY);
    VSETALL((*max), -INFINITY);

    VSCALE(hyp_B, hyp_B, xip->hyp_b);
    VREVERSE(hyp_An, xip->hyp_A);
    VREVERSE(hyp_Bn, hyp_B);

    VADD3(pt1, xip->hyp_Vi, xip->hyp_A, hyp_B);
    VADD3(pt2, xip->hyp_Vi, xip->hyp_A, hyp_Bn);
    VADD3(pt3, xip->hyp_Vi, hyp_An, hyp_B);
    VADD3(pt4, xip->hyp_Vi, hyp_An, hyp_Bn);
    VADD4(pt5, xip->hyp_Vi, xip->hyp_A, hyp_B, xip->hyp_Hi);
    VADD4(pt6, xip->hyp_Vi, xip->hyp_A, hyp_Bn, xip->hyp_Hi);
    VADD4(pt7, xip->hyp_Vi, hyp_An, hyp_B, xip->hyp_Hi);
    VADD4(pt8, xip->hyp_Vi, hyp_An, hyp_Bn, xip->hyp_Hi);

    /* Find the RPP of the rotated axis-aligned hyp bbox - that is,
     * the bounding box the given hyp would have if its height
     * vector were in the positive Z direction. This does not give
     * us an optimal bbox except in the case where the hyp is
     * actually axis aligned to start with, but it's usually
     * at least a bit better than the bounding sphere RPP. */
    VMINMAX((*min), (*max), pt1);
    VMINMAX((*min), (*max), pt2);
    VMINMAX((*min), (*max), pt3);
    VMINMAX((*min), (*max), pt4);
    VMINMAX((*min), (*max), pt5);
    VMINMAX((*min), (*max), pt6);
    VMINMAX((*min), (*max), pt7);
    VMINMAX((*min), (*max), pt8);

    return 0;
}

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

void
texture_perlin_init(struct texture_perlin_s *P)
{
    int i, j, k;

    P->PV = (int *)bu_malloc(sizeof(int)*(2*B+2), "PV");
    P->RV = (vect_t *)bu_malloc(sizeof(vect_t)*(2*B+2), "RV");

    /* Generate Random Vectors */
    for (i = 0; i < B; i++) {
	P->RV[i][0] = (fastf_t)((PRAND % (2*B)) - B) / B;
	P->RV[i][1] = (fastf_t)((PRAND % (2*B)) - B) / B;
	P->RV[i][2] = (fastf_t)((PRAND % (2*B)) - B) / B;
	VUNITIZE(P->RV[i]);
	P->PV[i] = i;
    }

    /* Permute Indices into Vector List G */
    for (i = 0; i < B; i++) {
	k = P->PV[i];
	P->PV[i] = P->PV[j = PRAND % B];
	P->PV[j] = k;
    }

    for (i = 0; i < B + 2; i++) {
	P->PV[B+i] = P->PV[i];
	VMOVE(P->RV[B+i], P->RV[i]);
    }
}

/**
 * R E C _ N O R M
 *
 * Given ONE ray distance, return the normal and entry/exit point.
 * hit_surfno is a flag indicating if normal needs to be computed or not.
 */
void
rt_rec_norm(struct hit *hitp, struct soltab *stp, struct xray *rp)
{
    struct rec_specific *rec =
	(struct rec_specific *)stp->st_specific;

    VJOIN1(hitp->hit_point, rp->r_pt, hitp->hit_dist, rp->r_dir);
    switch (hitp->hit_surfno) {
	case REC_NORM_BODY:
	    /* compute it */
	    hitp->hit_vpriv[Z] = 0.0;
	    MAT4X3VEC(hitp->hit_normal, rec->rec_invRoS,
		      hitp->hit_vpriv);
	    VUNITIZE(hitp->hit_normal);
	    break;
	case REC_NORM_TOP:
	    VMOVE(hitp->hit_normal, rec->rec_Hunit);
	    break;
	case REC_NORM_BOT:
	    VREVERSE(hitp->hit_normal, rec->rec_Hunit);
	    break;
	default:
	    bu_log("rt_rec_norm: surfno=%d bad\n", hitp->hit_surfno);
	    break;
    }
}

/*
 *	F B M _ R E N D E R
 */
int
fbm_render(struct application *ap, struct partition *pp, struct shadework *swp, char *dp)
{
    register struct fbm_specific *fbm_sp =
	(struct fbm_specific *)dp;
    vect_t v_noise;
    point_t pt;

    if (rdebug&RDEBUG_SHADE)
	bu_struct_print( "foo", fbm_parse, (char *)fbm_sp );

    pt[0] = swp->sw_hit.hit_point[0] * fbm_sp->scale[0];
    pt[1] = swp->sw_hit.hit_point[1] * fbm_sp->scale[1];
    pt[2] = swp->sw_hit.hit_point[2] * fbm_sp->scale[2];

    bn_noise_vec(pt, v_noise);

    VSCALE(v_noise, v_noise, fbm_sp->distortion);

    if (rdebug&RDEBUG_SHADE)
	bu_log("fbm_render: point (%g %g %g) becomes (%g %g %g)\n\tv_noise (%g %g %g)\n",
	       V3ARGS(swp->sw_hit.hit_point),
	       V3ARGS(pt),
	       V3ARGS(v_noise));

    VADD2(swp->sw_hit.hit_normal, swp->sw_hit.hit_normal, v_noise);
    VUNITIZE(swp->sw_hit.hit_normal);

    return(1);
}

/**
 * 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 */
}

static int
isst_load_g(ClientData UNUSED(clientData), Tcl_Interp *interp, int objc,
	    Tcl_Obj *const *objv)
{
    struct isst_s *isst;
    char **argv;
    int argc;
    double az, el;
    struct bu_vls tclstr = BU_VLS_INIT_ZERO;
    vect_t vec;
    Togl   *togl;

    if (objc < 4) {
	Tcl_WrongNumArgs(interp, 1, objv, "load_g pathname object");
	return TCL_ERROR;
    }

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

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

    argv = (char **)malloc(sizeof(char *) * (strlen(Tcl_GetString(objv[3])) + 1));	/* allocate way too much. */
    argc = bu_argv_from_string(argv, strlen(Tcl_GetString(objv[3])), Tcl_GetString(objv[3]));

    load_g(isst->tie, Tcl_GetString(objv[2]), argc, (const char **)argv, &(isst->meshes));
    free(argv);

    VSETALL(isst->camera.pos, isst->tie->radius);
    VMOVE(isst->camera.focus, isst->tie->mid);
    VMOVE(isst->camera_pos_init, isst->camera.pos);
    VMOVE(isst->camera_focus_init, isst->camera.focus);

    /* Set the initial az and el values in Tcl/Tk */
    VSUB2(vec, isst->camera.pos, isst->camera.focus);
    VUNITIZE(vec);
    AZEL_FROM_V3DIR(az, el, vec);
    az = az * -DEG2RAD;
    el = el * -DEG2RAD;
    bu_vls_sprintf(&tclstr, "%f", az);
    Tcl_SetVar(interp, "az", bu_vls_addr(&tclstr), 0);
    bu_vls_sprintf(&tclstr, "%f", el);
    Tcl_SetVar(interp, "el", bu_vls_addr(&tclstr), 0);
    bu_vls_free(&tclstr);

    render_phong_init(&isst->camera.render, NULL);

    isst->ogl = 1;
    isst->w = Togl_Width(togl);
    isst->h = Togl_Height(togl);

    resize_isst(isst);

    isst->t1 = bu_gettime();
    isst->t2 = bu_gettime();

    return TCL_OK;
}