void
bn_mat_mul3(mat_t o, const mat_t a, const mat_t b, const mat_t c)
{
mat_t t;
bn_mat_mul(t, b, c);
bn_mat_mul(o, a, t);
}
/**
* B N _ M A T _ M U L 4
*
* o = a * b * c * d
*
* The output matrix may be the same as any input matrix.
*/
void
bn_mat_mul4(mat_t ao, const mat_t a, const mat_t b, const mat_t c, const mat_t d)
{
mat_t t, u;
bn_mat_mul( u, c, d );
bn_mat_mul( t, a, b );
bn_mat_mul( ao, t, u );
}
void
bn_mat_lookat(mat_t rot, const vect_t dir, int yflip)
{
mat_t first;
mat_t second;
mat_t prod12;
mat_t third;
vect_t x;
vect_t z;
vect_t t1;
fastf_t hypot_xy;
vect_t xproj;
vect_t zproj;
/* First, rotate D around Z axis to match +X axis (azimuth) */
hypot_xy = hypot(dir[X], dir[Y]);
bn_mat_zrot(first, -dir[Y] / hypot_xy, dir[X] / hypot_xy);
/* Next, rotate D around Y axis to match -Z axis (elevation) */
bn_mat_yrot(second, -hypot_xy, -dir[Z]);
bn_mat_mul(prod12, second, first);
/* Produce twist correction, by re-orienting projection of X axis */
VSET(x, 1, 0, 0);
MAT4X3VEC(xproj, prod12, x);
hypot_xy = hypot(xproj[X], xproj[Y]);
if (hypot_xy < 1.0e-10) {
bu_log("Warning: bn_mat_lookat: unable to twist correct, hypot=%g\n", hypot_xy);
VPRINT("xproj", xproj);
MAT_COPY(rot, prod12);
return;
}
bn_mat_zrot(third, -xproj[Y] / hypot_xy, xproj[X] / hypot_xy);
bn_mat_mul(rot, third, prod12);
if (yflip) {
VSET(z, 0, 0, 1);
MAT4X3VEC(zproj, rot, z);
/* If original Z inverts sign, flip sign on resulting Y */
if (zproj[Y] < 0.0) {
MAT_COPY(prod12, rot);
MAT_IDN(third);
third[5] = -1;
bn_mat_mul(rot, third, prod12);
}
}
/* Check the final results */
MAT4X3VEC(t1, rot, dir);
if (t1[Z] > -0.98) {
bu_log("Error: bn_mat_lookat final= (%g, %g, %g)\n", V3ARGS(t1));
}
}
/**
* R T _ D S P _ M I R R O R
*
* Given a pointer to an internal GED database object, mirror the
* object's values about the given transformation matrix.
*/
int
rt_dsp_mirror(struct rt_db_internal *ip, register const plane_t plane)
{
struct rt_dsp_internal *dsp;
mat_t mirmat;
mat_t rmat;
mat_t temp;
vect_t nvec;
vect_t xvec;
vect_t mirror_dir;
point_t mirror_pt;
fastf_t ang;
static point_t origin = {0.0, 0.0, 0.0};
RT_CK_DB_INTERNAL(ip);
dsp = (struct rt_dsp_internal *)ip->idb_ptr;
RT_EBM_CK_MAGIC(dsp);
MAT_IDN(mirmat);
VMOVE(mirror_dir, plane);
VSCALE(mirror_pt, plane, plane[W]);
/* Build mirror transform matrix, for those who need it. */
/* First, perform a mirror down the X axis */
mirmat[0] = -1.0;
/* Create the rotation matrix */
VSET(xvec, 1, 0, 0);
VCROSS(nvec, xvec, mirror_dir);
VUNITIZE(nvec);
ang = -acos(VDOT(xvec, mirror_dir));
bn_mat_arb_rot(rmat, origin, nvec, ang*2.0);
/* Add the rotation to mirmat */
MAT_COPY(temp, mirmat);
bn_mat_mul(mirmat, temp, rmat);
/* Add the translation to mirmat */
mirmat[3 + X*4] += mirror_pt[X] * mirror_dir[X];
mirmat[3 + Y*4] += mirror_pt[Y] * mirror_dir[Y];
mirmat[3 + Z*4] += mirror_pt[Z] * mirror_dir[Z];
bn_mat_mul(temp, mirmat, dsp->dsp_mtos);
MAT_COPY(dsp->dsp_mtos, temp);
return 0;
}
void
bn_mat_xform_about_pt(mat_t mat, const mat_t xform, const point_t pt)
{
mat_t xlate;
mat_t tmp;
MAT_IDN(xlate);
MAT_DELTAS_VEC_NEG(xlate, pt);
bn_mat_mul(tmp, xform, xlate);
MAT_DELTAS_VEC(xlate, pt);
bn_mat_mul(mat, xlate, tmp);
}
/**
* Create a perspective matrix that transforms the +/1 viewing cube,
* with the actual eye position (not at Z=+1) specified in viewing
* coords, into a related space where the eye has been sheared onto
* the Z axis and repositioned at Z=(0, 0, 1), with the same
* perspective field of view as before.
*
* The Zbuffer clips off stuff with negative Z values.
*
* pmat = persp * xlate * shear
*/
void
ged_mike_persp_mat(mat_t pmat,
const point_t eye)
{
mat_t shear;
mat_t persp;
mat_t xlate;
mat_t t1, t2;
point_t sheared_eye;
if (eye[Z] <= SMALL) {
VPRINT("mike_persp_mat(): ERROR, z<0, eye", eye);
return;
}
/* Shear "eye" to +Z axis */
MAT_IDN(shear);
shear[2] = -eye[X]/eye[Z];
shear[6] = -eye[Y]/eye[Z];
MAT4X3VEC(sheared_eye, shear, eye);
if (!NEAR_ZERO(sheared_eye[X], .01) || !NEAR_ZERO(sheared_eye[Y], .01)) {
VPRINT("ERROR sheared_eye", sheared_eye);
return;
}
/* Translate along +Z axis to put sheared_eye at (0, 0, 1). */
MAT_IDN(xlate);
/* XXX should I use MAT_DELTAS_VEC_NEG()? X and Y should be 0 now */
MAT_DELTAS(xlate, 0, 0, 1-sheared_eye[Z]);
/* Build perspective matrix inline, substituting fov=2*atan(1, Z) */
MAT_IDN(persp);
/* From page 492 of Graphics Gems */
persp[0] = sheared_eye[Z]; /* scaling: fov aspect term */
persp[5] = sheared_eye[Z]; /* scaling: determines fov */
/* From page 158 of Rogers Mathematical Elements */
/* Z center of projection at Z=+1, r=-1/1 */
persp[14] = -1;
bn_mat_mul(t1, xlate, shear);
bn_mat_mul(t2, persp, t1);
/* Now, move eye from Z=1 to Z=0, for clipping purposes */
MAT_DELTAS(xlate, 0, 0, -1);
bn_mat_mul(pmat, xlate, t2);
}
/**
* Compute a perspective matrix for a right-handed coordinate system.
* Reference: SGI Graphics Reference Appendix C
*
* (Note: SGI is left-handed, but the fix is done in the Display
* Manager).
*/
void
ged_persp_mat(mat_t m,
fastf_t fovy,
fastf_t aspect,
fastf_t near1,
fastf_t far1,
fastf_t backoff)
{
mat_t m2, tran;
fovy *= DEG2RAD;
MAT_IDN(m2);
m2[5] = cos(fovy/2.0) / sin(fovy/2.0);
m2[0] = m2[5]/aspect;
m2[10] = (far1+near1) / (far1-near1);
m2[11] = 2*far1*near1 / (far1-near1); /* This should be negative */
m2[14] = -1; /* XXX This should be positive */
m2[15] = 0;
/* Move eye to origin, then apply perspective */
MAT_IDN(tran);
tran[11] = -backoff;
bn_mat_mul(m, m2, tran);
}
void
bn_mat_mul2(register const mat_t i, register mat_t o)
{
mat_t temp;
bn_mat_mul(temp, i, o);
MAT_COPY(o, temp);
}
void
draw_e_axes()
{
point_t v_ap1; /* axes position in view coordinates */
point_t v_ap2; /* axes position in view coordinates */
mat_t rot_mat;
if (state == ST_S_EDIT) {
MAT4X3PNT(v_ap1, view_state->vs_vop->vo_model2view, e_axes_pos);
MAT4X3PNT(v_ap2, view_state->vs_vop->vo_model2view, curr_e_axes_pos);
} else if (state == ST_O_EDIT) {
point_t m_ap2;
MAT4X3PNT(v_ap1, view_state->vs_vop->vo_model2view, es_keypoint);
MAT4X3PNT(m_ap2, modelchanges, es_keypoint);
MAT4X3PNT(v_ap2, view_state->vs_vop->vo_model2view, m_ap2);
} else
return;
dmo_drawAxes_cmd(dmp,
view_state->vs_vop->vo_size,
view_state->vs_vop->vo_rotation,
v_ap1,
axes_state->ax_edit_size1 * INV_GED,
color_scheme->cs_edit_axes1,
color_scheme->cs_edit_axes_label1,
axes_state->ax_edit_linewidth1,
0, /* positive direction only */
0, /* three colors (i.e. X-red, Y-green, Z-blue) */
0, /* no ticks */
0, /* tick len */
0, /* major tick len */
0, /* tick interval */
0, /* ticks per major */
NULL, /* tick color */
NULL, /* major tick color */
0 /* tick threshold */);
bn_mat_mul(rot_mat, view_state->vs_vop->vo_rotation, acc_rot_sol);
dmo_drawAxes_cmd(dmp,
view_state->vs_vop->vo_size,
rot_mat,
v_ap2,
axes_state->ax_edit_size2 * INV_GED,
color_scheme->cs_edit_axes2,
color_scheme->cs_edit_axes_label2,
axes_state->ax_edit_linewidth2,
0, /* positive direction only */
0, /* three colors (i.e. X-red, Y-green, Z-blue) */
0, /* no ticks */
0, /* tick len */
0, /* major tick len */
0, /* tick interval */
0, /* ticks per major */
NULL, /* tick color */
NULL, /* major tick color */
0 /* tick threshold */);
}
void
bn_mat_arb_rot(
mat_t m,
const point_t pt,
const vect_t dir,
const fastf_t ang)
{
mat_t tran1, tran2, rot;
double cos_ang, sin_ang, one_m_cosang;
double n1_sq, n2_sq, n3_sq;
double n1_n2, n1_n3, n2_n3;
if (ZERO(ang)) {
MAT_IDN(m);
return;
}
MAT_IDN(tran1);
MAT_IDN(tran2);
/* construct translation matrix to pt */
tran1[MDX] = (-pt[X]);
tran1[MDY] = (-pt[Y]);
tran1[MDZ] = (-pt[Z]);
/* construct translation back from pt */
tran2[MDX] = pt[X];
tran2[MDY] = pt[Y];
tran2[MDZ] = pt[Z];
/* construct rotation matrix */
cos_ang = cos(ang);
sin_ang = sin(ang);
one_m_cosang = 1.0 - cos_ang;
n1_sq = dir[X] * dir[X];
n2_sq = dir[Y] * dir[Y];
n3_sq = dir[Z] * dir[Z];
n1_n2 = dir[X] * dir[Y];
n1_n3 = dir[X] * dir[Z];
n2_n3 = dir[Y] * dir[Z];
MAT_IDN(rot);
rot[0] = n1_sq + (1.0 - n1_sq) * cos_ang;
rot[1] = n1_n2 * one_m_cosang - dir[Z] * sin_ang;
rot[2] = n1_n3 * one_m_cosang + dir[Y] * sin_ang;
rot[4] = n1_n2 * one_m_cosang + dir[Z] * sin_ang;
rot[5] = n2_sq + (1.0 - n2_sq) * cos_ang;
rot[6] = n2_n3 * one_m_cosang - dir[X] * sin_ang;
rot[8] = n1_n3 * one_m_cosang - dir[Y] * sin_ang;
rot[9] = n2_n3 * one_m_cosang + dir[X] * sin_ang;
rot[10] = n3_sq + (1.0 - n3_sq) * cos_ang;
bn_mat_mul(m, rot, tran1);
bn_mat_mul2(tran2, m);
}
void
ged_view_update(struct ged_view *gvp)
{
vect_t work, work1;
vect_t temp, temp1;
if (!gvp)
return;
bn_mat_mul(gvp->gv_model2view,
gvp->gv_rotation,
gvp->gv_center);
gvp->gv_model2view[15] = gvp->gv_scale;
bn_mat_inv(gvp->gv_view2model, gvp->gv_model2view);
/* Find current azimuth, elevation, and twist angles */
VSET(work, 0.0, 0.0, 1.0); /* view z-direction */
MAT4X3VEC(temp, gvp->gv_view2model, work);
VSET(work1, 1.0, 0.0, 0.0); /* view x-direction */
MAT4X3VEC(temp1, gvp->gv_view2model, work1);
/* calculate angles using accuracy of 0.005, since display
* shows 2 digits right of decimal point */
bn_aet_vec(&gvp->gv_aet[0],
&gvp->gv_aet[1],
&gvp->gv_aet[2],
temp, temp1, (fastf_t)0.005);
/* Force azimuth range to be [0, 360] */
if ((NEAR_EQUAL(gvp->gv_aet[1], 90.0, (fastf_t)0.005) ||
NEAR_EQUAL(gvp->gv_aet[1], -90.0, (fastf_t)0.005)) &&
gvp->gv_aet[0] < 0 &&
!NEAR_ZERO(gvp->gv_aet[0], (fastf_t)0.005))
gvp->gv_aet[0] += 360.0;
else if (NEAR_ZERO(gvp->gv_aet[0], (fastf_t)0.005))
gvp->gv_aet[0] = 0.0;
/* apply the perspective angle to model2view */
bn_mat_mul(gvp->gv_pmodel2view, gvp->gv_pmat, gvp->gv_model2view);
if (gvp->gv_callback)
(*gvp->gv_callback)(gvp, gvp->gv_clientData);
}
int
bn_mat_scale_about_pt(mat_t mat, const point_t pt, const double scale)
{
mat_t xlate;
mat_t s;
mat_t tmp;
MAT_IDN(xlate);
MAT_DELTAS_VEC_NEG(xlate, pt);
MAT_IDN(s);
if (ZERO(scale)) {
MAT_ZERO(mat);
return -1; /* ERROR */
}
s[15] = 1 / scale;
bn_mat_mul(tmp, s, xlate);
MAT_DELTAS_VEC(xlate, pt);
bn_mat_mul(mat, xlate, tmp);
return 0; /* OK */
}
static int
test_bn_mat_mul(int argc, char *argv[])
{
mat_t m1, m2, expected, actual;
if (argc != 5) {
bu_exit(1, "<args> format: M1 M2 <expected_result> [%s]\n", argv[0]);
}
scan_mat_args(argv, 2, &m1);
scan_mat_args(argv, 3, &m2);
scan_mat_args(argv, 4, &expected);
bn_mat_mul(actual, m1, m2);
return !mat_equal(expected, actual);
}
void
do_light(char *name, fastf_t *pos, fastf_t *dir_at, int da_flag, double r, unsigned char *rgb, struct wmember *headp)
/* direction or aim point */
/* 0 = direction, !0 = aim point */
/* radius of light */
{
char nbuf[64];
vect_t center;
mat_t rot;
mat_t xlate;
mat_t both;
vect_t from;
vect_t dir;
if ( da_flag ) {
VSUB2( dir, dir_at, pos );
VUNITIZE( dir );
} else {
VMOVE( dir, dir_at );
}
snprintf( nbuf, 64, "%s.s", name );
VSETALL( center, 0 );
mk_sph( outfp, nbuf, center, r );
/*
* Need to rotate from 0, 0, -1 to vect "dir",
* then xlate to final position.
*/
VSET( from, 0, 0, -1 );
bn_mat_fromto( rot, from, dir );
MAT_IDN( xlate );
MAT_DELTAS_VEC( xlate, pos);
bn_mat_mul( both, xlate, rot );
mk_region1( outfp, name, nbuf, "light", "shadows=1", rgb );
(void)mk_addmember( name, &(headp->l), NULL, WMOP_UNION );
}
void
draw_e_axes()
{
point_t v_ap1; /* axes position in view coordinates */
point_t v_ap2; /* axes position in view coordinates */
mat_t rot_mat;
struct ged_axes_state gas;
if (STATE == ST_S_EDIT) {
MAT4X3PNT(v_ap1, view_state->vs_gvp->gv_model2view, e_axes_pos);
MAT4X3PNT(v_ap2, view_state->vs_gvp->gv_model2view, curr_e_axes_pos);
} else if (STATE == ST_O_EDIT) {
point_t m_ap2;
MAT4X3PNT(v_ap1, view_state->vs_gvp->gv_model2view, es_keypoint);
MAT4X3PNT(m_ap2, modelchanges, es_keypoint);
MAT4X3PNT(v_ap2, view_state->vs_gvp->gv_model2view, m_ap2);
} else
return;
memset(&gas, 0, sizeof(struct ged_axes_state));
VMOVE(gas.gas_axes_pos, v_ap1);
gas.gas_axes_size = axes_state->ax_edit_size1 * INV_GED;
VMOVE(gas.gas_axes_color, color_scheme->cs_edit_axes1);
VMOVE(gas.gas_label_color, color_scheme->cs_edit_axes_label1);
gas.gas_line_width = axes_state->ax_edit_linewidth1;
dm_draw_axes(dmp, view_state->vs_gvp->gv_size, view_state->vs_gvp->gv_rotation, &gas);
memset(&gas, 0, sizeof(struct ged_axes_state));
VMOVE(gas.gas_axes_pos, v_ap2);
gas.gas_axes_size = axes_state->ax_edit_size2 * INV_GED;
VMOVE(gas.gas_axes_color, color_scheme->cs_edit_axes2);
VMOVE(gas.gas_label_color, color_scheme->cs_edit_axes_label2);
gas.gas_line_width = axes_state->ax_edit_linewidth2;
bn_mat_mul(rot_mat, view_state->vs_gvp->gv_rotation, acc_rot_sol);
dm_draw_axes(dmp, view_state->vs_gvp->gv_size, rot_mat, &gas);
}
/**
* make a copy of a v5 solid by adding it to our book-keeping list,
* adding it to the db directory, and writing it out to disk.
*/
static void
copy_v5_solid(struct db_i *_dbip, struct directory *proto, struct clone_state *state, int idx)
{
size_t i;
mat_t matrix;
MAT_IDN(matrix);
/* mirror */
if (state->miraxis != W) {
matrix[state->miraxis*5] = -1.0;
matrix[3 + state->miraxis*4] -= 2 * (matrix[3 + state->miraxis*4] - state->mirpos);
}
/* translate */
if (state->trans[W] > SMALL_FASTF)
MAT_DELTAS_ADD_VEC(matrix, state->trans);
/* rotation */
if (state->rot[W] > SMALL_FASTF) {
mat_t m2, t;
bn_mat_angles(m2, state->rot[X], state->rot[Y], state->rot[Z]);
if (state->rpnt[W] > SMALL_FASTF) {
mat_t m3;
bn_mat_xform_about_pt(m3, m2, state->rpnt);
bn_mat_mul(t, matrix, m3);
} else
bn_mat_mul(t, matrix, m2);
MAT_COPY(matrix, t);
}
/* make n copies */
for (i = 0; i < state->n_copies; i++) {
char *argv[6] = {"wdb_copy", (char *)NULL, (char *)NULL, (char *)NULL, (char *)NULL, (char *)NULL};
struct bu_vls *name;
int ret;
struct directory *dp = (struct directory *)NULL;
struct rt_db_internal intern;
if (i==0)
dp = proto;
else
dp = db_lookup(_dbip, bu_vls_addr(&obj_list.names[idx].dest[i-1]), LOOKUP_QUIET);
if (!dp) {
continue;
}
name = get_name(_dbip, dp, state, i); /* get new name */
bu_vls_strcpy(&obj_list.names[idx].dest[i], bu_vls_addr(name));
/* actually copy the primitive to the new name */
argv[1] = proto->d_namep;
argv[2] = bu_vls_addr(name);
ret = wdb_copy_cmd(_dbip->dbi_wdbp, 3, (const char **)argv);
if (ret != TCL_OK)
bu_log("WARNING: failure cloning \"%s\" to \"%s\"\n", proto->d_namep, bu_vls_addr(name));
/* get the original objects matrix */
if (rt_db_get_internal(&intern, dp, _dbip, matrix, &rt_uniresource) < 0) {
bu_log("ERROR: clone internal error copying %s\n", proto->d_namep);
bu_vls_free(name);
return;
}
RT_CK_DB_INTERNAL(&intern);
/* pull the new name */
dp = db_lookup(_dbip, bu_vls_addr(name), LOOKUP_QUIET);
bu_vls_free(name);
if (!dp) {
bu_vls_free(name);
continue;
}
/* write the new matrix to the new object */
if (rt_db_put_internal(dp, wdbp->dbip, &intern, &rt_uniresource) < 0)
bu_log("ERROR: clone internal error copying %s\n", proto->d_namep);
rt_db_free_internal(&intern);
} /* end iteration over each copy */
return;
}
/**
* Given a pointer to an internal GED database object, mirror the
* object's values about the given transformation matrix.
*/
int
rt_half_mirror(struct rt_db_internal *ip, register const plane_t plane)
{
struct rt_half_internal *half;
mat_t mirmat;
mat_t rmat;
mat_t temp;
vect_t nvec;
vect_t xvec;
vect_t mirror_dir;
point_t mirror_pt;
fastf_t ang;
vect_t n1;
vect_t n2;
static fastf_t tol_dist_sq = 0.0005 * 0.0005;
static point_t origin = {0.0, 0.0, 0.0};
RT_CK_DB_INTERNAL(ip);
half = (struct rt_half_internal *)ip->idb_ptr;
RT_HALF_CK_MAGIC(half);
MAT_IDN(mirmat);
VMOVE(mirror_dir, plane);
VSCALE(mirror_pt, plane, plane[W]);
/* Build mirror transform matrix, for those who need it. */
/* First, perform a mirror down the X axis */
mirmat[0] = -1.0;
/* Create the rotation matrix */
VSET(xvec, 1, 0, 0);
VCROSS(nvec, xvec, mirror_dir);
VUNITIZE(nvec);
ang = -acos(VDOT(xvec, mirror_dir));
bn_mat_arb_rot(rmat, origin, nvec, ang*2.0);
/* Add the rotation to mirmat */
MAT_COPY(temp, mirmat);
bn_mat_mul(mirmat, temp, rmat);
/* Add the translation to mirmat */
mirmat[3 + X*4] += mirror_pt[X] * mirror_dir[X];
mirmat[3 + Y*4] += mirror_pt[Y] * mirror_dir[Y];
mirmat[3 + Z*4] += mirror_pt[Z] * mirror_dir[Z];
/* FIXME: this is not using the mirmat we just computed, not clear
* it's even right given it's only taking the mirror direction
* into account and not the mirror point.
*/
VMOVE(n1, half->eqn);
VCROSS(n2, mirror_dir, n1);
VUNITIZE(n2);
ang = M_PI_2 - acos(VDOT(n1, mirror_dir));
bn_mat_arb_rot(rmat, origin, n2, ang*2);
MAT4X3VEC(half->eqn, rmat, n1);
if (!NEAR_EQUAL(VDOT(n1, half->eqn), 1.0, tol_dist_sq)) {
point_t ptA;
point_t ptB;
point_t ptC;
vect_t h;
fastf_t mag;
fastf_t cosa;
VSCALE(ptA, n1, half->eqn[H]);
VADD2(ptB, ptA, mirror_dir);
VSUB2(h, ptB, ptA);
mag = MAGNITUDE(h);
VUNITIZE(h);
cosa = VDOT(h, mirror_dir);
VSCALE(ptC, half->eqn, -mag * cosa);
VADD2(ptC, ptC, ptA);
half->eqn[H] = VDOT(half->eqn, ptC);
}
return 0;
}
int
main (int argc, char **argv)
{
int val;
fastf_t time, yaw1, pitch1, roll1, yaw2, pitch2, roll2;
vect_t cen1, cen2, cen_ans, ang_ans, rad_ang_ans, rotated;
mat_t m_rot1, m_rot2, m_ans;
int one_time, read_cen1, read_cen2, read_rot1, read_rot2;
read_cen1 = read_cen2 = read_rot1 = read_rot2 = 1;
if (!get_args(argc, argv))
fprintf(stderr, "anim_cascade: Argument error.");
switch (output_mode) {
case CASCADE_A:
if (cmd_fcen) {
VMOVE(cen1, fcenter);
read_cen1 = 0;
}
if (cmd_rcen) {
VMOVE(cen2, rcenter);
read_cen2 = 0;
}
if (cmd_fypr) {
anim_dy_p_r2mat(m_rot1, fypr[0], fypr[1], fypr[2]);
read_rot1 = 0;
}
if (cmd_rypr) {
anim_dy_p_r2mat(m_rot2, rypr[0], rypr[1], rypr[2]);
read_rot2 = 0;
}
break;
case CASCADE_R:
if (cmd_fcen) {
VMOVE(cen1, fcenter);
read_cen1 = 0;
}
if (cmd_acen) {
VMOVE(cen2, acenter);
read_cen2 = 0;
}
if (cmd_fypr) {
anim_dy_p_r2mat(m_rot1, fypr[0], fypr[1], fypr[2]);
read_rot1 = 0;
}
if (cmd_aypr) {
anim_dy_p_r2mat(m_rot2, aypr[0], aypr[1], aypr[2]);
read_rot2 = 0;
}
break;
case CASCADE_F:
if (cmd_acen) {
VMOVE(cen1, acenter);
read_cen1 = 0;
}
if (cmd_rcen) {
VMOVE(cen2, rcenter);
read_cen2 = 0;
}
if (cmd_aypr) {
anim_dy_p_r2mat(m_rot1, aypr[0], aypr[1], aypr[2]);
read_rot1 = 0;
}
if (cmd_rypr) {
anim_dy_p_r2mat(m_rot2, rypr[0], rypr[1], rypr[2]);
read_rot2 = 0;
}
break;
default:
break;
}
one_time = (!(read_cen1||read_cen2||read_rot1||read_rot2));
read_time = one_time ? 0 : print_time;
time = 0.0;
val = 3;
while (1) {
if (read_time) {
val=scanf("%lf", &time);
if (val < 1) break;
}
if (read_cen1)
val =scanf("%lf %lf %lf", cen1, cen1+1, cen1+2);
if (read_rot1) {
val=scanf("%lf %lf %lf", &yaw1, &pitch1, &roll1);
anim_dy_p_r2mat(m_rot1, yaw1, pitch1, roll1);
}
if (read_cen2) {
val=scanf("%lf %lf %lf", cen2, cen2+1, cen2+2);
}
if (read_rot2) {
val=scanf("%lf %lf %lf", &yaw2, &pitch2, &roll2);
anim_dy_p_r2mat(m_rot2, yaw2, pitch2, roll2);
}
if (val<3) break;
if (output_mode==CASCADE_R) {
anim_tran(m_rot1);
VSUB2(rotated, cen2, cen1);
MAT4X3PNT(cen_ans, m_rot1, rotated);
bn_mat_mul(m_ans, m_rot1, m_rot2);
} else if (output_mode==CASCADE_F) {
anim_tran(m_rot2);
bn_mat_mul(m_ans, m_rot1, m_rot2);
MAT4X3PNT(rotated, m_ans, cen2);
VSUB2(cen_ans, cen1, rotated);
} else {
MAT4X3PNT(rotated, m_rot1, cen2);
VADD2(cen_ans, rotated, cen1);
bn_mat_mul(m_ans, m_rot1, m_rot2);
}
anim_mat2ypr(rad_ang_ans, m_ans);
VSCALE(ang_ans, rad_ang_ans, RTOD);
if (print_time) {
printf("%g", time);
}
printf("\t%.12g\t%.12g\t%.12g", cen_ans[0], cen_ans[1], cen_ans[2]);
printf("\t%.12g\t%.12g\t%.12g", ang_ans[0], ang_ans[1], ang_ans[2]);
printf("\n");
if (one_time) break;
}
return( 0 );
}
HIDDEN int
scloud_setup(register struct region *rp, struct bu_vls *matparm, void **dpp, const struct mfuncs *mfp, struct rt_i *rtip)
/* pointer to reg_udata in *rp */
{
register struct scloud_specific *scloud;
struct db_full_path full_path;
mat_t region_to_model;
mat_t model_to_region;
mat_t tmp;
BU_CK_VLS(matparm);
BU_GET(scloud, struct scloud_specific);
*dpp = scloud;
if (rp->reg_aircode == 0) {
bu_log("WARNING(%s): air shader '%s' applied to non-air region.\n%s\n",
rp->reg_name,
mfp->mf_name,
" Set air flag with \"edcodes\" in mged");
bu_bomb("");
}
memcpy(scloud, &scloud_defaults, sizeof(struct scloud_specific));
if (rdebug&RDEBUG_SHADE)
bu_log("scloud_setup\n");
if (bu_struct_parse(matparm, scloud_parse, (char *)scloud) < 0)
return -1;
if (rdebug&RDEBUG_SHADE)
(void)bu_struct_print(rp->reg_name, scloud_parse, (char *)scloud);
/* get transformation between world and "region" coordinates */
if (db_string_to_path(&full_path, rtip->rti_dbip, rp->reg_name)) {
/* bad thing */
bu_bomb("db_string_to_path() error");
}
if (! db_path_to_mat(rtip->rti_dbip, &full_path, region_to_model, 0, &rt_uniresource)) {
/* bad thing */
bu_bomb("db_path_to_mat() error");
}
/* get matrix to map points from model space to "region" space */
bn_mat_inv(model_to_region, region_to_model);
/* add the noise-space scaling */
MAT_IDN(tmp);
if (!EQUAL(scloud->scale, 1.0)) {
tmp[0] = tmp[5] = tmp[10] = 1.0 / scloud->scale;
} else {
tmp[0] = 1.0 / (scloud->vscale[0]);
tmp[5] = 1.0 / (scloud->vscale[1]);
tmp[10] = 1.0 / (scloud->vscale[2]);
}
bn_mat_mul(scloud->mtos, tmp, model_to_region);
/* add the translation within noise space */
MAT_IDN(tmp);
tmp[MDX] = scloud->delta[0];
tmp[MDY] = scloud->delta[1];
tmp[MDZ] = scloud->delta[2];
bn_mat_mul2(tmp, scloud->mtos);
bn_mat_inv(scloud->stom, scloud->mtos);
return 1;
}
/*
* T P _ 3 S Y M B O L
*/
void
tp_3symbol(FILE *fp, char *string, fastf_t *origin, fastf_t *rot, double scale)
/* string of chars to be plotted */
/* lower left corner of 1st char */
/* Transform matrix (WARNING: may xlate) */
/* scale factor to change 1x1 char sz */
{
register unsigned char *cp;
double offset; /* offset of char from given x, y */
int ysign; /* sign of y motion, either +1 or -1 */
vect_t temp;
vect_t loc;
mat_t xlate_to_origin;
mat_t mat;
if ( string == NULL || *string == '\0' )
return; /* done before begun! */
/*
* The point "origin" will be the center of the axis rotation.
* The text is located in a local coordinate system with the
* lower left corner of the first character at (0, 0, 0), with
* the text proceeding onward towards +X.
* We need to rotate the text around its local (0, 0, 0),
* and then translate to the user's designated "origin".
* If the user provided translation or
* scaling in his matrix, it will *also* be applied.
*/
MAT_IDN( xlate_to_origin );
MAT_DELTAS_VEC( xlate_to_origin, origin );
bn_mat_mul( mat, xlate_to_origin, rot );
/* Check to see if initialization is needed */
if ( tp_cindex[040] == 0 ) tp_setup();
/* Draw each character in the input string */
offset = 0;
for ( cp = (unsigned char *)string; *cp; cp++, offset += scale ) {
register int *p; /* pointer to stroke table */
register int stroke;
VSET( temp, offset, 0, 0 );
MAT4X3PNT( loc, mat, temp );
pdv_3move( fp, loc );
for ( p = tp_cindex[*cp]; (stroke= *p) != LAST; p++ ) {
int draw;
if ( stroke==NEGY ) {
ysign = (-1);
stroke = *++p;
} else
ysign = 1;
/* Detect & process pen control */
if ( stroke < 0 ) {
stroke = -stroke;
draw = 0;
} else
draw = 1;
/* stroke co-ordinates in string coord system */
VSET( temp, (stroke/11) * 0.1 * scale + offset,
(ysign * (stroke%11)) * 0.1 * scale, 0 );
MAT4X3PNT( loc, mat, temp );
if ( draw )
pdv_3cont( fp, loc );
else
pdv_3move( fp, loc );
}
}
}
int
main(int argc, char **argv)
{
void anim_dir2mat(fastf_t *, const fastf_t *, const fastf_t *), anim_y_p_r2mat(fastf_t *, double, double, double), anim_add_trans(fastf_t *, const fastf_t *, const fastf_t *), anim_mat_print(FILE *, const fastf_t *, int);
int get_args(int argc, char **argv), track_prep(void), val, frame, go, i, count;
fastf_t y_rot, distance, yaw, pitch, roll;
vect_t p1, p2, p3, dir, dir2, wheel_now, wheel_prev;
vect_t zero, position, vdelta, temp, to_track, to_front;
mat_t mat_v, wmat, mat_x;
FILE *stream;
int last_frame;
VSETALL(zero, 0.0);
VSETALL(to_track, 0.0);
VSETALL(centroid, 0.0);
VSETALL(rcentroid, 0.0);
init_dist = y_rot = radius= 0.0;
first_frame = num_wheels = steer = axes = cent = links_placed=0;
num_wheels = num_links = last_frame = 0;
MAT_IDN(mat_v);
MAT_IDN(mat_x);
MAT_IDN(wmat);
MAT_IDN(m_axes);
MAT_IDN(m_rev_axes);
if (!get_args(argc, argv)) {
fprintf(stderr, "anim_hardtrack: argument error.");
return(-1);
}
if (axes || cent ) {
/* vehicle has own reference frame */
anim_add_trans(m_axes, centroid, zero);
anim_add_trans(m_rev_axes, zero, rcentroid);
}
/* get track information from specified file */
if (!(stream = fopen(*(argv+bu_optind), "rb"))) {
fprintf(stderr, "Anim_hardtrack: Could not open file %s.\n", *(argv+bu_optind));
return(0);
}
num_wheels = -1;
if (radius) {
while (!feof(stream)) {
fscanf(stream, "%*f %*f %*f");
num_wheels++;
}
} else {
while (!feof(stream)) {
fscanf(stream, "%*f %*f %*f %*f");
num_wheels++;
}
}
rewind(stream);
/*allocate memory for track information*/
x = (struct all *) bu_calloc(num_wheels, sizeof(struct all), "struct all");
/*read rest of track info */
for (i=0;i<NW;i++) {
fscanf(stream, "%lf %lf %lf", temp, temp+1, temp+2);
if (radius)
x[i].w.rad = radius;
else
fscanf(stream, "%lf", & x[i].w.rad);
MAT4X3PNT(x[i].w.pos, m_rev_axes, temp);
if (i==0)
track_y = x[0].w.pos[1];
else
x[i].w.pos[1] = track_y;
}
(void) fclose(stream);
(void) track_prep();
if (get_circumf) {
printf("%.10g\n", tracklen);
return(0);
}
/* initialize to_track */
VSET(to_track, 0.0, track_y, 0.0);
VSET(to_front, 1.0, 0.0, 0.0);
if ((!print_link)&&(!print_wheel)) {
fprintf(stderr, "anim_hardtrack: no ouput requested. Use -l or -w.\n");
bu_exit(0, NULL);
}
/* main loop */
distance = 0.0;
if (!steer)
frame = first_frame;
else
frame = first_frame-1;
for (val = 3; val > 2; frame++) {
go = 1;
/*p2 is current position. p3 is next;p1 is previous*/
VMOVE(p1, p2);
VMOVE(p2, p3);
scanf("%*f");/*time stamp*/
val = scanf("%lf %lf %lf", p3, p3+1, p3 + 2);
if (!steer) {
scanf("%lf %lf %lf", &yaw, &pitch, &roll);
anim_dy_p_r2mat(mat_v, yaw, pitch, roll);
anim_add_trans(mat_v, p3, rcentroid);
}
else {
/* analyze positions for steering */
/*get useful direction unit vectors*/
if (frame == first_frame) {
/* first frame*/
VSUBUNIT(dir, p3, p2);
VMOVE(dir2, dir);
}
else if (val < 3) {
/*last frame*/
VSUBUNIT(dir, p2, p1);
VMOVE(dir2, dir);
}
else if (frame > first_frame) {
/*normal*/
VSUBUNIT(dir, p3, p1);
VSUBUNIT(dir2, p2, p1);/*needed for vertical case*/
}
else go = 0;/*first time through loop;no p2*/
/*create matrix which would move vehicle*/
anim_dir2mat(mat_v, dir, dir2);
anim_add_trans(mat_v, p2, rcentroid);
}
/*determine distance traveled*/
VMOVE(wheel_prev, wheel_now);
MAT4X3PNT(wheel_now, mat_v, to_track);
if (frame > first_frame) {
/* increment distance by distance moved */
VSUB2(vdelta, wheel_now, wheel_prev);
MAT3X3VEC(temp, mat_v, to_front);/*new front of vehicle*/
distance += VDOT(temp, vdelta);/*portion of vdelta in line with track*/
}
if (go) {
if (print_mode==TRACK_ANIM) {
printf("start %d;\nclean;\n", frame);
} else if (print_mode==TRACK_ARCED) {
if (frame != arced_frame) continue;
last_frame = 1;
}
if (print_link) {
for (count=0;count<num_links;count++) {
(void) get_link(position, &y_rot, distance+tracklen*count/num_links+init_dist);
anim_y_p_r2mat(wmat, 0.0, y_rot+r[count].ang, 0.0);
anim_add_trans(wmat, position, r[count].pos);
if ((axes || cent) && links_placed) {
/* link moved from vehicle coords */
bn_mat_mul(mat_x, wmat, m_rev_axes);
bn_mat_mul(wmat, m_axes, mat_x);
}
else if (axes || cent) {
/* link moved to vehicle coords */
MAT_MOVE(mat_x, wmat);
bn_mat_mul(wmat, m_axes, mat_x);
}
if (print_mode==TRACK_ANIM) {
printf("anim %s.%d matrix %s\n", *(argv+link_nindex), count, link_cmd);
anim_mat_printf(stdout, wmat, "%.10g ", "\n", ";\n");
} else if (print_mode==TRACK_ARCED) {
printf("arced %s.%d matrix %s ", *(argv+link_nindex), count, link_cmd);
anim_mat_printf(stdout, wmat, "%.10g ", "", "\n");
}
}
}
if (print_wheel) {
for (count = 0;count<num_wheels;count++) {
anim_y_p_r2mat(wmat, 0.0, -distance/x[count].w.rad, 0.0);
VREVERSE(temp, x[count].w.pos);
anim_add_trans(wmat, x[count].w.pos, temp);
if (axes || cent) {
bn_mat_mul(mat_x, wmat, m_rev_axes);
bn_mat_mul(wmat, m_axes, mat_x);
}
if (print_mode==TRACK_ANIM) {
printf("anim %s.%d matrix %s\n", *(argv+wheel_nindex), count, wheel_cmd);
anim_mat_printf(stdout, wmat, "%.10g ", "\n", ";\n");
} else if (print_mode==TRACK_ARCED) {
printf("arced %s.%d matrix %s ", *(argv+wheel_nindex), count, wheel_cmd);
anim_mat_printf(stdout, wmat, "%.10g ", "", "\n");
}
}
}
if (print_mode==TRACK_ANIM)
printf("end;\n");
}
if (last_frame) break;
}
if (x) {
bu_free(x, "struct all");
}
if (r) {
bu_free(r, "struct rlink");
}
return( 0 );
}
int
Getcurve(int curve, struct ptlist **curv_pts)
{
int type;
int npts = 0;
int i, j;
double pi;
struct ptlist *ptr, *prev;
pi = atan2(0.0, -1.0);
(*curv_pts) = NULL;
prev = NULL;
switch (dir[curve]->type) {
case 110: {
/* line */
point_t pt1;
Readrec(dir[curve]->param);
Readint(&type, "");
if (type != dir[curve]->type) {
bu_log("Error in Getcurve, looking for curve type %d, found %d\n" ,
dir[curve]->type, type);
npts = 0;
break;
}
BU_ALLOC((*curv_pts), struct ptlist);
ptr = (*curv_pts);
/* Read first point */
for (i = 0; i < 3; i++)
Readcnv(&pt1[i], "");
MAT4X3PNT(ptr->pt, *dir[curve]->rot, pt1);
ptr->prev = NULL;
prev = ptr;
BU_ALLOC(ptr->next, struct ptlist);
ptr = ptr->next;
/* Read second point */
for (i = 0; i < 3; i++)
Readcnv(&pt1[i], "");
MAT4X3PNT(ptr->pt, *dir[curve]->rot, pt1);
ptr->next = NULL;
ptr->prev = prev;
npts = 2;
break;
}
case 100: {
/* circular arc */
point_t center, start, stop, tmp;
fastf_t common_z, ang1, ang2, delta;
double cosdel, sindel, rx, ry;
delta = (2.0*pi)/ARCSEGS;
Readrec(dir[curve]->param);
Readint(&type, "");
if (type != dir[curve]->type) {
bu_log("Error in Getcurve, looking for curve type %d, found %d\n" ,
dir[curve]->type, type);
npts = 0;
break;
}
/* Read common Z coordinate */
Readcnv(&common_z, "");
/* Read center point */
Readcnv(¢er[X], "");
Readcnv(¢er[Y], "");
center[Z] = common_z;
/* Read start point */
Readcnv(&start[X], "");
Readcnv(&start[Y], "");
start[Z] = common_z;
/* Read stop point */
Readcnv(&stop[X], "");
Readcnv(&stop[Y], "");
stop[Z] = common_z;
ang1 = atan2(start[Y] - center[Y], start[X] - center[X]);
ang2 = atan2(stop[Y] - center[Y], stop[X] - center[X]);
while (ang2 <= ang1)
ang2 += (2.0*pi);
npts = (ang2 - ang1)/delta;
npts++;
V_MAX(npts, 3);
delta = (ang2 - ang1)/(npts-1);
cosdel = cos(delta);
sindel = sin(delta);
/* Calculate points on curve */
BU_ALLOC((*curv_pts), struct ptlist);
ptr = (*curv_pts);
prev = NULL;
MAT4X3PNT(ptr->pt, *dir[curve]->rot, start);
ptr->prev = prev;
prev = ptr;
BU_ALLOC(ptr->next, struct ptlist);
ptr = ptr->next;
ptr->prev = prev;
VMOVE(tmp, start);
for (i = 1; i < npts; i++) {
rx = tmp[X] - center[X];
ry = tmp[Y] - center[Y];
tmp[X] = center[X] + rx*cosdel - ry*sindel;
tmp[Y] = center[Y] + rx*sindel + ry*cosdel;
MAT4X3PNT(ptr->pt, *dir[curve]->rot, tmp);
prev = ptr;
BU_ALLOC(ptr->next, struct ptlist);
ptr = ptr->next;
ptr->prev = prev;
}
ptr = prev;
bu_free((char *)ptr->next, "Getcurve: ptr->next");
ptr->next = NULL;
break;
}
case 106: {
/* copius data */
int interpflag; /* interpretation flag
1 => x, y pairs (common z-coord)
2 => x, y, z coords
3 => x, y, z coords and i, j, k vectors */
int ntuples; /* number of points */
fastf_t common_z; /* common z-coordinate */
point_t pt1; /* temporary storage for incoming point */
Readrec(dir[curve]->param);
Readint(&type, "");
if (type != dir[curve]->type) {
bu_log("Error in Getcurve, looking for curve type %d, found %d\n" ,
dir[curve]->type, type);
npts = 0;
break;
}
Readint(&interpflag, "");
Readint(&ntuples, "");
switch (dir[curve]->form) {
case 1:
case 11:
case 40:
case 63: {
/* data are coordinate pairs with common z */
if (interpflag != 1) {
bu_log("Error in Getcurve for copius data entity D%07d, IP=%d, should be 1\n",
dir[curve]->direct, interpflag);
npts = 0;
break;
}
Readcnv(&common_z, "");
BU_ALLOC((*curv_pts), struct ptlist);
ptr = (*curv_pts);
ptr->prev = NULL;
for (i = 0; i < ntuples; i++) {
Readcnv(&pt1[X], "");
Readcnv(&pt1[Y], "");
pt1[Z] = common_z;
MAT4X3PNT(ptr->pt, *dir[curve]->rot, pt1);
prev = ptr;
BU_ALLOC(ptr->next, struct ptlist);
ptr = ptr->next;
ptr->prev = prev;
ptr->next = NULL;
}
ptr = ptr->prev;
bu_free((char *)ptr->next, "Getcurve: ptr->next");
ptr->next = NULL;
npts = ntuples;
break;
}
case 2:
case 12: {
/* data are coordinate triples */
if (interpflag != 2) {
bu_log("Error in Getcurve for copius data entity D%07d, IP=%d, should be 2\n",
dir[curve]->direct, interpflag);
npts = 0;
break;
}
BU_ALLOC((*curv_pts), struct ptlist);
ptr = (*curv_pts);
ptr->prev = NULL;
for (i = 0; i < ntuples; i++) {
Readcnv(&pt1[X], "");
Readcnv(&pt1[Y], "");
Readcnv(&pt1[Z], "");
MAT4X3PNT(ptr->pt, *dir[curve]->rot, pt1);
prev = ptr;
BU_ALLOC(ptr->next, struct ptlist);
ptr = ptr->next;
ptr->prev = prev;
}
ptr = ptr->prev;
bu_free((char *)ptr->next, "Getcurve: ptr->next");
ptr->next = NULL;
npts = ntuples;
break;
}
default: {
bu_log("Error in Getcurve for copius data entity D%07d, form %d is not a legal choice\n",
dir[curve]->direct, dir[curve]->form);
npts = 0;
break;
}
}
break;
}
case 112: {
/* parametric spline */
struct spline *splroot;
struct segment *seg, *seg1;
vect_t tmp;
double a;
Readrec(dir[curve]->param);
Readint(&type, "");
if (type != dir[curve]->type) {
bu_log("Error in Getcurve, looking for curve type %d, found %d\n" ,
dir[curve]->type, type);
npts = 0;
break;
}
Readint(&i, ""); /* Skip over type */
Readint(&i, ""); /* Skip over continuity */
BU_ALLOC(splroot, struct spline);
splroot->start = NULL;
Readint(&splroot->ndim, ""); /* 2->planar, 3->3d */
Readint(&splroot->nsegs, ""); /* Number of segments */
Readdbl(&a, ""); /* first breakpoint */
/* start a linked list of segments */
seg = splroot->start;
for (i = 0; i < splroot->nsegs; i++) {
if (seg == NULL) {
BU_ALLOC(seg, struct segment);
splroot->start = seg;
} else {
BU_ALLOC(seg->next, struct segment);
seg = seg->next;
}
seg->segno = i+1;
seg->next = NULL;
seg->tmin = a; /* set minimum T for this segment */
Readflt(&seg->tmax, ""); /* get maximum T for segment */
a = seg->tmax;
}
/* read coefficients for polynomials */
seg = splroot->start;
for (i = 0; i < splroot->nsegs; i++) {
for (j = 0; j < 4; j++)
Readflt(&seg->cx[j], ""); /* x coeff's */
for (j = 0; j < 4; j++)
Readflt(&seg->cy[j], ""); /* y coeff's */
for (j = 0; j < 4; j++)
Readflt(&seg->cz[j], ""); /* z coeff's */
seg = seg->next;
}
/* Calculate points */
BU_ALLOC((*curv_pts), struct ptlist);
ptr = (*curv_pts);
prev = NULL;
ptr->prev = NULL;
npts = 0;
seg = splroot->start;
while (seg != NULL) {
/* plot 9 points per segment (This should
be replaced by some logic) */
for (i = 0; i < 9; i++) {
a = (fastf_t)i/(8.0)*(seg->tmax-seg->tmin);
tmp[0] = splinef(seg->cx, a);
tmp[1] = splinef(seg->cy, a);
if (splroot->ndim == 3)
tmp[2] = splinef(seg->cz, a);
else
tmp[2] = seg->cz[0];
MAT4X3PNT(ptr->pt, *dir[curve]->rot, tmp);
for (j = 0; j < 3; j++)
ptr->pt[j] *= conv_factor;
npts++;
prev = ptr;
BU_ALLOC(ptr->next, struct ptlist);
ptr = ptr->next;
ptr->prev = prev;
}
seg = seg->next;
}
ptr = ptr->prev;
bu_free((char *)ptr->next, "Getcurve: ptr->next");
ptr->next = NULL;
/* free the used memory */
seg = splroot->start;
while (seg != NULL) {
seg1 = seg;
seg = seg->next;
bu_free((char *)seg1, "Getcurve: seg1");
}
bu_free((char *)splroot, "Getcurve: splroot");
splroot = NULL;
break;
}
case 104: {
/* conic arc */
double A, B, C, D, E, F, a, b, c, del, I, theta, dpi, t1, t2, xc, yc;
point_t v1, v2, tmp;
mat_t rot1;
int num_points;
Readrec(dir[curve]->param);
Readint(&type, "");
if (type != dir[curve]->type) {
bu_log("Error in Getcurve, looking for curve type %d, found %d\n" ,
dir[curve]->type, type);
npts = 0;
break;
}
/* read coefficients */
Readdbl(&A, "");
Readdbl(&B, "");
Readdbl(&C, "");
Readdbl(&D, "");
Readdbl(&E, "");
Readdbl(&F, "");
/* read common z-coordinate */
Readflt(&v1[2], "");
v2[2] = v1[2];
/* read start point */
Readflt(&v1[0], "");
Readflt(&v1[1], "");
/* read terminate point */
Readflt(&v2[0], "");
Readflt(&v2[1], "");
type = 0;
if (dir[curve]->form == 1) {
/* Ellipse */
if (fabs(E) < SMALL)
E = 0.0;
if (fabs(B) < SMALL)
B = 0.0;
if (fabs(D) < SMALL)
D = 0.0;
if (ZERO(B) && ZERO(D) && ZERO(E))
type = 1;
else
bu_log("Entity #%d is an incorrectly formatted ellipse\n", curve);
}
/* make coeff of X**2 equal to 1.0 */
a = A*C - B*B/4.0;
if (fabs(a) < 1.0 && fabs(a) > TOL) {
a = fabs(A);
if (fabs(B) < a && !ZERO(B))
a = fabs(B);
V_MIN(a, fabs(C));
A = A/a;
B = B/a;
C = C/a;
D = D/a;
E = E/a;
F = F/a;
a = A*C - B*B/4.0;
}
if (!type) {
/* check for type of conic */
del = A*(C*F-E*E/4.0)-0.5*B*(B*F/2.0-D*E/4.0)+0.5*D*(B*E/4.0-C*D/2.0);
I = A+C;
if (ZERO(del)) {
/* not a conic */
bu_log("Entity #%d, claims to be conic arc, but isn't\n", curve);
break;
} else if (a > 0.0 && del*I < 0.0)
type = 1; /* ellipse */
else if (a < 0.0)
type = 2; /* hyperbola */
else if (ZERO(a))
type = 3; /* parabola */
else {
/* imaginary ellipse */
bu_log("Entity #%d is an imaginary ellipse!!\n", curve);
break;
}
}
switch (type) {
double p, r1;
case 3: /* parabola */
/* make A+C == 1.0 */
if (!EQUAL(A+C, 1.0)) {
b = A+C;
A = A/b;
B = B/b;
C = C/b;
D = D/b;
E = E/b;
F = F/b;
}
/* theta is the angle that the parabola axis is rotated
about the origin from the x-axis */
theta = 0.5*atan2(B, C-A);
/* p is the distance from vertex to directrix */
p = (-E*sin(theta) - D*cos(theta))/4.0;
if (fabs(p) < TOL) {
bu_log("Cannot plot entity %d, p=%g\n", curve, p);
break;
}
/* calculate vertex (xc, yc). This is based on the
parametric representation:
x = xc + a*t*t*cos(theta) - t*sin(theta)
y = yc + a*t*t*sin(theta) + t*cos(theta)
and the fact that v1 and v2 are on the curve
*/
a = 1.0/(4.0*p);
b = ((v1[0]-v2[0])*cos(theta) + (v1[1]-v2[1])*sin(theta))/a;
c = ((v1[1]-v2[1])*cos(theta) - (v1[0]-v2[0])*sin(theta));
if (fabs(c) < TOL*TOL) {
bu_log("Cannot plot entity %d\n", curve);
break;
}
b = b/c;
t1 = (b + c)/2.0; /* value of 't' at v1 */
t2 = (b - c)/2.0; /* value of 't' at v2 */
xc = v1[0] - a*t1*t1*cos(theta) + t1*sin(theta);
yc = v1[1] - a*t1*t1*sin(theta) - t1*cos(theta);
/* Calculate points */
BU_ALLOC((*curv_pts), struct ptlist);
ptr = (*curv_pts);
ptr->prev = NULL;
prev = NULL;
npts = 0;
num_points = ARCSEGS+1;
dpi = (t2-t1)/(double)num_points; /* parameter increment */
/* start point */
VSET(tmp, xc, yc, v1[2]);
MAT4X3PNT(ptr->pt, *dir[curve]->rot, tmp);
VSCALE(ptr->pt, ptr->pt, conv_factor);
npts++;
prev = ptr;
BU_ALLOC(ptr->next, struct ptlist);
ptr = ptr->next;
ptr->prev = prev;
/* middle points */
b = cos(theta);
c = sin(theta);
for (i = 1; i < num_points-1; i++) {
r1 = t1 + dpi*i;
tmp[0] = xc + a*r1*r1*b - r1*c;
tmp[1] = yc + a*r1*r1*c + r1*b;
MAT4X3PNT(ptr->pt, *dir[curve]->rot, tmp);
VSCALE(ptr->pt, ptr->pt, conv_factor);
npts++;
prev = ptr;
BU_ALLOC(ptr->next, struct ptlist);
ptr = ptr->next;
ptr->prev = prev;
}
/* plot terminate point */
tmp[0] = v2[0];
tmp[1] = v2[1];
MAT4X3PNT(ptr->pt, *dir[curve]->rot, tmp);
for (j = 0; j < 3; j++)
ptr->pt[j] *= conv_factor;
npts++;
ptr->next = NULL;
break;
case 1: /* ellipse */
case 2: {
/* hyperbola */
double A1, C1, F1, alpha, beta;
mat_t rot2;
point_t v3;
/* calculate center of ellipse or hyperbola */
xc = (B*E/4.0 - D*C/2.0)/a;
yc = (B*D/4.0 - A*E/2.0)/a;
/* theta is angle that the curve axis is rotated about
the origin from the x-axis */
if (!ZERO(B))
theta = 0.5*atan2(B, A-C);
else
theta = 0.0;
/* calculate coeff's for same curve, but with
vertex at origin and theta = 0.0 */
A1 = A + 0.5*B*tan(theta);
C1 = C - 0.5*B*tan(theta);
F1 = F - A*xc*xc - B*xc*yc - C*yc*yc;
if (type == 2 && F1/A1 > 0.0)
theta += pi/2.0;
/* set-up matrix to translate and rotate
the start and terminate points to match
the simpler curve (A1, C1, and F1 coeff's) */
for (i = 0; i < 16; i++)
rot1[i] = idn[i];
MAT_DELTAS(rot1, -xc, -yc, 0.0);
MAT4X3PNT(tmp, rot1, v1);
VMOVE(v1, tmp);
MAT4X3PNT(tmp, rot1, v2);
VMOVE(v2, tmp);
MAT_DELTAS(rot1, 0.0, 0.0, 0.0);
rot1[0] = cos(theta);
rot1[1] = sin(theta);
rot1[4] = (-rot1[1]);
rot1[5] = rot1[0];
MAT4X3PNT(tmp, rot1, v1);
VMOVE(v1, tmp);
MAT4X3PNT(tmp, rot1, v2);
VMOVE(v2, tmp);
MAT_DELTAS(rot1, 0.0, 0.0, 0.0);
/* calculate:
alpha = start angle
beta = terminate angle
*/
beta = 0.0;
if (EQUAL(v2[0], v1[0]) && EQUAL(v2[1], v1[1])) {
/* full circle */
alpha = 0.0;
beta = 2.0*pi;
}
a = sqrt(fabs(F1/A1)); /* semi-axis length */
b = sqrt(fabs(F1/C1)); /* semi-axis length */
if (type == 1) {
/* ellipse */
alpha = atan2(a*v1[1], b*v1[0]);
if (ZERO(beta)) {
beta = atan2(a*v2[1], b*v2[0]);
beta = beta - alpha;
}
} else {
/* hyperbola */
alpha = myarcsinh(v1[1]/b);
beta = myarcsinh(v2[1]/b);
if (fabs(a*cosh(beta) - v2[0]) > 0.01)
a = (-a);
beta = beta - alpha;
}
num_points = ARCSEGS;
/* set-up matrix to translate and rotate
the simpler curve back to the original
position */
MAT_DELTAS(rot1, xc, yc, 0.0);
rot1[1] = (-rot1[1]);
rot1[4] = (-rot1[4]);
#if defined(USE_BN_MULT_)
/* o <= a X b */
bn_mat_mul(rot2, *(dir[curve]->rot), rot1);
#else
/* a X b => o */
Matmult(*(dir[curve]->rot), rot1, rot2);
#endif
/* calculate start point */
BU_ALLOC((*curv_pts), struct ptlist);
ptr = (*curv_pts);
prev = NULL;
ptr->prev = NULL;
npts = 0;
VSCALE(v3, v1, conv_factor);
MAT4X3PNT(ptr->pt, rot2, v3);
npts++;
prev = ptr;
BU_ALLOC(ptr->next, struct ptlist);
ptr = ptr->next;
ptr->prev = prev;
/* middle points */
for (i = 1; i < num_points; i++) {
point_t tmp2 = {0.0, 0.0, 0.0};
theta = alpha + (double)i/(double)num_points*beta;
if (type == 2) {
tmp2[0] = a*cosh(theta);
tmp2[1] = b*sinh(theta);
} else {
tmp2[0] = a*cos(theta);
tmp2[1] = b*sin(theta);
}
VSCALE(tmp2, tmp2, conv_factor);
MAT4X3PNT(ptr->pt, rot2, tmp2);
npts++;
prev = ptr;
BU_ALLOC(ptr->next, struct ptlist);
ptr = ptr->next;
ptr->prev = prev;
}
/* terminate point */
VSCALE(v2, v2, conv_factor);
MAT4X3PNT(ptr->pt, rot2, v2);
npts++;
ptr->next = NULL;
break;
}
}
break;
}
case 102: /* composite curve */ {
int ncurves, *curvptr;
struct ptlist *tmp_ptr;
Readrec(dir[curve]->param);
Readint(&type, "");
if (type != dir[curve]->type) {
bu_log("Error in Getcurve, looking for curve type %d, found %d\n" ,
dir[curve]->type, type);
npts = 0;
break;
}
Readint(&ncurves, "");
curvptr = (int *)bu_calloc(ncurves, sizeof(int), "Getcurve: curvptr");
for (i = 0; i < ncurves; i++) {
Readint(&curvptr[i], "");
curvptr[i] = (curvptr[i]-1)/2;
}
npts = 0;
(*curv_pts) = NULL;
for (i = 0; i < ncurves; i++) {
npts += Getcurve(curvptr[i], &tmp_ptr);
if ((*curv_pts) == NULL)
(*curv_pts) = tmp_ptr;
else {
ptr = (*curv_pts);
while (ptr->next != NULL)
ptr = ptr->next;
ptr->next = tmp_ptr;
ptr->next->prev = ptr;
if (NEAR_EQUAL(ptr->pt[X], tmp_ptr->pt[X], TOL) &&
NEAR_EQUAL(ptr->pt[Y], tmp_ptr->pt[Y], TOL) &&
NEAR_EQUAL(ptr->pt[Z], tmp_ptr->pt[Z], TOL)) {
ptr->next = ptr->next->next;
if (ptr->next != NULL)
ptr->next->prev = ptr;
bu_free((char *)tmp_ptr, "Getcurve: tmp_ptr");
npts--;
}
}
}
break;
}
case 126: {
/* rational B-spline */
int k, m, n, a, prop1, prop2, prop3, prop4;
fastf_t *t; /* knot values */
fastf_t *w; /* weights */
point_t *cntrl_pts; /* control points */
fastf_t v0, v1; /* starting and stopping parameter values */
fastf_t v; /* current parameter value */
fastf_t delv; /* parameter increment */
Readrec(dir[curve]->param);
Readint(&type, "");
if (type != dir[curve]->type) {
bu_log("Error in Getcurve, looking for curve type %d, found %d\n" ,
dir[curve]->type, type);
npts = 0;
break;
}
Readint(&k, "");
Readint(&m, "");
Readint(&prop1, "");
Readint(&prop2, "");
Readint(&prop3, "");
Readint(&prop4, "");
n = k - m + 1;
a = n + 2 * m;
t = (fastf_t *)bu_calloc(a+1, sizeof(fastf_t), "Getcurve: spline t");
for (i = 0; i < a+1; i++)
Readflt(&t[i], "");
Knot(a+1, t);
w = (fastf_t *)bu_calloc(k+1, sizeof(fastf_t), "Getcurve: spline w");
for (i = 0; i < k+1; i++)
Readflt(&w[i], "");
cntrl_pts = (point_t *)bu_calloc(k+1, sizeof(point_t), "Getcurve: spline cntrl_pts");
for (i = 0; i < k+1; i++) {
fastf_t tmp;
for (j = 0; j < 3; j++) {
Readcnv(&tmp, "");
cntrl_pts[i][j] = tmp;
}
}
Readflt(&v0, "");
Readflt(&v1, "");
delv = (v1 - v0)/((fastf_t)(3*k));
/* Calculate points */
BU_ALLOC((*curv_pts), struct ptlist);
ptr = (*curv_pts);
ptr->prev = NULL;
prev = NULL;
npts = 0;
v = v0;
while (v < v1) {
point_t tmp;
B_spline(v, k, m+1, cntrl_pts, w, tmp);
MAT4X3PNT(ptr->pt, *dir[curve]->rot, tmp);
npts++;
prev = ptr;
BU_ALLOC(ptr->next, struct ptlist);
ptr = ptr->next;
ptr->prev = prev;
v += delv;
}
VMOVE(ptr->pt, cntrl_pts[k]);
npts++;
ptr->next = NULL;
/* Free memory */
Freeknots();
bu_free((char *)cntrl_pts, "Getcurve: spline cntrl_pts");
bu_free((char *)w, "Getcurve: spline w");
bu_free((char *)t, "Getcurve: spline t");
break;
}
}
return npts;
}
/**
* Given a pointer to an internal GED database object, mirror the
* object's values about the given transformation matrix.
*/
int
rt_ell_mirror(struct rt_db_internal *ip, register const plane_t plane)
{
struct rt_ell_internal *ell;
mat_t mirmat;
mat_t rmat;
mat_t temp;
vect_t nvec;
vect_t xvec;
vect_t mirror_dir;
point_t mirror_pt;
fastf_t ang;
mat_t mat;
point_t pt;
vect_t a, b, c;
vect_t n;
static point_t origin = {0.0, 0.0, 0.0};
RT_CK_DB_INTERNAL(ip);
ell = (struct rt_ell_internal *)ip->idb_ptr;
RT_ELL_CK_MAGIC(ell);
MAT_IDN(mirmat);
VMOVE(mirror_dir, plane);
VSCALE(mirror_pt, plane, plane[W]);
/* Build mirror transform matrix, for those who need it. */
/* First, perform a mirror down the X axis */
mirmat[0] = -1.0;
/* Create the rotation matrix */
VSET(xvec, 1, 0, 0);
VCROSS(nvec, xvec, mirror_dir);
VUNITIZE(nvec);
ang = -acos(VDOT(xvec, mirror_dir));
bn_mat_arb_rot(rmat, origin, nvec, ang*2.0);
/* Add the rotation to mirmat */
MAT_COPY(temp, mirmat);
bn_mat_mul(mirmat, temp, rmat);
/* Add the translation to mirmat */
mirmat[3 + X*4] += mirror_pt[X] * mirror_dir[X];
mirmat[3 + Y*4] += mirror_pt[Y] * mirror_dir[Y];
mirmat[3 + Z*4] += mirror_pt[Z] * mirror_dir[Z];
VMOVE(pt, ell->v);
MAT4X3PNT(ell->v, mirmat, pt);
VMOVE(a, ell->a);
VUNITIZE(a);
VCROSS(n, mirror_dir, ell->a);
VUNITIZE(n);
ang = M_PI_2 - acos(VDOT(a, mirror_dir));
bn_mat_arb_rot(mat, origin, n, ang*2);
VMOVE(a, ell->a);
MAT4X3VEC(ell->a, mat, a);
VMOVE(b, ell->b);
VUNITIZE(b);
VCROSS(n, mirror_dir, ell->b);
VUNITIZE(n);
ang = M_PI_2 - acos(VDOT(b, mirror_dir));
bn_mat_arb_rot(mat, origin, n, ang*2);
VMOVE(b, ell->b);
MAT4X3VEC(ell->b, mat, b);
VMOVE(c, ell->c);
VUNITIZE(c);
VCROSS(n, mirror_dir, ell->c);
VUNITIZE(n);
ang = M_PI_2 - acos(VDOT(c, mirror_dir));
bn_mat_arb_rot(mat, origin, n, ang*2);
VMOVE(c, ell->c);
MAT4X3VEC(ell->c, mat, c);
return 0;
}
/**
* Given a pointer to an internal GED database object, mirror the
* object's values about the given transformation matrix.
*/
int
rt_nurb_mirror(struct rt_db_internal *ip, register const plane_t plane)
{
struct rt_nurb_internal *nurb;
mat_t mirmat;
mat_t rmat;
mat_t temp;
vect_t nvec;
vect_t xvec;
vect_t mirror_dir;
point_t mirror_pt;
fastf_t ang;
int i;
int j;
static point_t origin = {0.0, 0.0, 0.0};
RT_CK_DB_INTERNAL(ip);
nurb = (struct rt_nurb_internal *)ip->idb_ptr;
RT_PG_CK_MAGIC(nurb);
MAT_IDN(mirmat);
VMOVE(mirror_dir, plane);
VSCALE(mirror_pt, plane, plane[W]);
/* Build mirror transform matrix, for those who need it. */
/* First, perform a mirror down the X axis */
mirmat[0] = -1.0;
/* Create the rotation matrix */
VSET(xvec, 1, 0, 0);
VCROSS(nvec, xvec, mirror_dir);
VUNITIZE(nvec);
ang = -acos(VDOT(xvec, mirror_dir));
bn_mat_arb_rot(rmat, origin, nvec, ang*2.0);
/* Add the rotation to mirmat */
MAT_COPY(temp, mirmat);
bn_mat_mul(mirmat, temp, rmat);
/* Add the translation to mirmat */
mirmat[3 + X*4] += mirror_pt[X] * mirror_dir[X];
mirmat[3 + Y*4] += mirror_pt[Y] * mirror_dir[Y];
mirmat[3 + Z*4] += mirror_pt[Z] * mirror_dir[Z];
for (i=0; i<nurb->nsrf; i++) {
fastf_t *ptr;
int tmp;
int orig_size[2];
int ncoords;
int m;
int l;
/* swap knot vectors between u and v */
ptr = nurb->srfs[i]->u.knots;
tmp = nurb->srfs[i]->u.k_size;
nurb->srfs[i]->u.knots = nurb->srfs[i]->v.knots;
nurb->srfs[i]->u.k_size = nurb->srfs[i]->v.k_size;
nurb->srfs[i]->v.knots = ptr;
nurb->srfs[i]->v.k_size = tmp;
/* swap order */
tmp = nurb->srfs[i]->order[0];
nurb->srfs[i]->order[0] = nurb->srfs[i]->order[1];
nurb->srfs[i]->order[1] = tmp;
/* swap mesh size */
orig_size[0] = nurb->srfs[i]->s_size[0];
orig_size[1] = nurb->srfs[i]->s_size[1];
nurb->srfs[i]->s_size[0] = orig_size[1];
nurb->srfs[i]->s_size[1] = orig_size[0];
/* allocate memory for a new control mesh */
ncoords = RT_NURB_EXTRACT_COORDS(nurb->srfs[i]->pt_type);
ptr = (fastf_t *)bu_calloc(orig_size[0]*orig_size[1]*ncoords, sizeof(fastf_t), "rt_mirror: ctl mesh ptr");
/* mirror each control point */
for (j=0; j<orig_size[0]*orig_size[1]; j++) {
point_t pt;
VMOVE(pt, &nurb->srfs[i]->ctl_points[j*ncoords]);
MAT4X3PNT(&nurb->srfs[i]->ctl_points[j*ncoords], mirmat, pt);
}
/* copy mirrored control points into new mesh
* while swapping u and v */
m = 0;
for (j=0; j<orig_size[0]; j++) {
for (l=0; l<orig_size[1]; l++) {
VMOVEN(&ptr[(l*orig_size[0]+j)*ncoords], &nurb->srfs[i]->ctl_points[m*ncoords], ncoords);
m++;
}
}
/* free old mesh */
bu_free((char *)nurb->srfs[i]->ctl_points, "rt_mirror: ctl points");
/* put new mesh in place */
nurb->srfs[i]->ctl_points = ptr;
}
return 0;
}
/**
* Given a pointer to a GED database record, and a transformation
* matrix, determine if this is a valid superellipsoid, and if so,
* precompute various terms of the formula.
*
* Returns -
* 0 SUPERELL is OK
* !0 Error in description
*
* Implicit return - A struct superell_specific is created, and its
* address is stored in stp->st_specific for use by rt_superell_shot()
*/
int
rt_superell_prep(struct soltab *stp, struct rt_db_internal *ip, struct rt_i *rtip)
{
struct superell_specific *superell;
struct rt_superell_internal *eip;
fastf_t magsq_a, magsq_b, magsq_c;
mat_t R, TEMP;
vect_t Au, Bu, Cu; /* A, B, C with unit length */
fastf_t f;
eip = (struct rt_superell_internal *)ip->idb_ptr;
RT_SUPERELL_CK_MAGIC(eip);
/* Validate that |A| > 0, |B| > 0, |C| > 0 */
magsq_a = MAGSQ(eip->a);
magsq_b = MAGSQ(eip->b);
magsq_c = MAGSQ(eip->c);
if (magsq_a < rtip->rti_tol.dist_sq || magsq_b < rtip->rti_tol.dist_sq || magsq_c < rtip->rti_tol.dist_sq) {
bu_log("rt_superell_prep(): superell(%s) near-zero length A(%g), B(%g), or C(%g) vector\n",
stp->st_name, magsq_a, magsq_b, magsq_c);
return 1; /* BAD */
}
if (eip->n < rtip->rti_tol.dist || eip->e < rtip->rti_tol.dist) {
bu_log("rt_superell_prep(): superell(%s) near-zero length <n, e> curvature (%g, %g) causes problems\n",
stp->st_name, eip->n, eip->e);
/* BAD */
}
if (eip->n > 10000.0 || eip->e > 10000.0) {
bu_log("rt_superell_prep(): superell(%s) very large <n, e> curvature (%g, %g) causes problems\n",
stp->st_name, eip->n, eip->e);
/* BAD */
}
/* Create unit length versions of A, B, C */
f = 1.0/sqrt(magsq_a);
VSCALE(Au, eip->a, f);
f = 1.0/sqrt(magsq_b);
VSCALE(Bu, eip->b, f);
f = 1.0/sqrt(magsq_c);
VSCALE(Cu, eip->c, f);
/* Validate that A.B == 0, B.C == 0, A.C == 0 (check dir only) */
f = VDOT(Au, Bu);
if (! NEAR_ZERO(f, rtip->rti_tol.dist)) {
bu_log("rt_superell_prep(): superell(%s) A not perpendicular to B, f=%f\n", stp->st_name, f);
return 1; /* BAD */
}
f = VDOT(Bu, Cu);
if (! NEAR_ZERO(f, rtip->rti_tol.dist)) {
bu_log("rt_superell_prep(): superell(%s) B not perpendicular to C, f=%f\n", stp->st_name, f);
return 1; /* BAD */
}
f = VDOT(Au, Cu);
if (! NEAR_ZERO(f, rtip->rti_tol.dist)) {
bu_log("rt_superell_prep(): superell(%s) A not perpendicular to C, f=%f\n", stp->st_name, f);
return 1; /* BAD */
}
/* Solid is OK, compute constant terms now */
BU_GET(superell, struct superell_specific);
stp->st_specific = (void *)superell;
superell->superell_n = eip->n;
superell->superell_e = eip->e;
VMOVE(superell->superell_V, eip->v);
VSET(superell->superell_invsq, 1.0/magsq_a, 1.0/magsq_b, 1.0/magsq_c);
VMOVE(superell->superell_Au, Au);
VMOVE(superell->superell_Bu, Bu);
VMOVE(superell->superell_Cu, Cu);
/* compute the inverse magnitude square for equations during shot */
superell->superell_invmsAu = 1.0 / magsq_a;
superell->superell_invmsBu = 1.0 / magsq_b;
superell->superell_invmsCu = 1.0 / magsq_c;
/* compute the rotation matrix */
MAT_IDN(R);
VMOVE(&R[0], Au);
VMOVE(&R[4], Bu);
VMOVE(&R[8], Cu);
bn_mat_trn(superell->superell_invR, R);
/* computer invRSSR */
MAT_IDN(superell->superell_invRSSR);
MAT_IDN(TEMP);
TEMP[0] = superell->superell_invsq[0];
TEMP[5] = superell->superell_invsq[1];
TEMP[10] = superell->superell_invsq[2];
bn_mat_mul(TEMP, TEMP, R);
bn_mat_mul(superell->superell_invRSSR, superell->superell_invR, TEMP);
/* compute Scale(Rotate(vect)) */
MAT_IDN(superell->superell_SoR);
VSCALE(&superell->superell_SoR[0], eip->a, superell->superell_invsq[0]);
VSCALE(&superell->superell_SoR[4], eip->b, superell->superell_invsq[1]);
VSCALE(&superell->superell_SoR[8], eip->c, superell->superell_invsq[2]);
/* Compute bounding sphere */
VMOVE(stp->st_center, eip->v);
f = magsq_a;
if (magsq_b > f)
f = magsq_b;
if (magsq_c > f)
f = magsq_c;
stp->st_aradius = stp->st_bradius = sqrt(f);
/* Compute bounding RPP */
if (rt_superell_bbox(ip, &(stp->st_min), &(stp->st_max), &rtip->rti_tol)) return 1;
return 0; /* OK */
}
int
ged_orotate(struct ged *gedp, int argc, const char *argv[])
{
struct directory *dp;
struct _ged_trace_data gtd;
struct rt_db_internal intern;
/* intentionally double for scan */
double xrot, yrot, zrot;
mat_t rmat;
mat_t pmat;
mat_t emat;
mat_t tmpMat;
mat_t invXform;
point_t rpp_min;
point_t rpp_max;
point_t keypoint;
static const char *usage = "obj rX rY rZ [kX kY kZ]";
GED_CHECK_DATABASE_OPEN(gedp, GED_ERROR);
GED_CHECK_READ_ONLY(gedp, GED_ERROR);
GED_CHECK_ARGC_GT_0(gedp, argc, GED_ERROR);
/* initialize result */
bu_vls_trunc(gedp->ged_result_str, 0);
/* must be wanting help */
if (argc == 1) {
bu_vls_printf(gedp->ged_result_str, "Usage: %s %s", argv[0], usage);
return GED_HELP;
}
if (argc != 5 && argc != 8) {
bu_vls_printf(gedp->ged_result_str, "Usage: %s %s", argv[0], usage);
return GED_ERROR;
}
if (sscanf(argv[2], "%lf", &xrot) != 1) {
bu_vls_printf(gedp->ged_result_str, "%s: bad rX value - %s", argv[0], argv[2]);
return GED_ERROR;
}
if (sscanf(argv[3], "%lf", &yrot) != 1) {
bu_vls_printf(gedp->ged_result_str, "%s: bad rY value - %s", argv[0], argv[3]);
return GED_ERROR;
}
if (sscanf(argv[4], "%lf", &zrot) != 1) {
bu_vls_printf(gedp->ged_result_str, "%s: bad rZ value - %s", argv[0], argv[4]);
return GED_ERROR;
}
if (argc == 5) {
/* Use the object's center as the keypoint. */
if (_ged_get_obj_bounds2(gedp, 1, argv+1, >d, rpp_min, rpp_max) == GED_ERROR)
return GED_ERROR;
dp = gtd.gtd_obj[gtd.gtd_objpos-1];
if (!(dp->d_flags & RT_DIR_SOLID)) {
if (_ged_get_obj_bounds(gedp, 1, argv+1, 1, rpp_min, rpp_max) == GED_ERROR)
return GED_ERROR;
}
VADD2(keypoint, rpp_min, rpp_max);
VSCALE(keypoint, keypoint, 0.5);
} else {
double scan[3];
/* The user has provided the keypoint. */
MAT_IDN(gtd.gtd_xform);
if (sscanf(argv[5], "%lf", &scan[X]) != 1) {
bu_vls_printf(gedp->ged_result_str, "%s: bad kX value - %s", argv[0], argv[5]);
return GED_ERROR;
}
if (sscanf(argv[6], "%lf", &scan[Y]) != 1) {
bu_vls_printf(gedp->ged_result_str, "%s: bad kY value - %s", argv[0], argv[6]);
return GED_ERROR;
}
if (sscanf(argv[7], "%lf", &scan[Z]) != 1) {
bu_vls_printf(gedp->ged_result_str, "%s: bad kZ value - %s", argv[0], argv[7]);
return GED_ERROR;
}
VSCALE(keypoint, scan, gedp->ged_wdbp->dbip->dbi_local2base);
if ((dp = db_lookup(gedp->ged_wdbp->dbip, argv[1], LOOKUP_QUIET)) == RT_DIR_NULL) {
bu_vls_printf(gedp->ged_result_str, "%s: %s not found", argv[0], argv[1]);
return GED_ERROR;
}
}
bn_mat_angles(rmat, xrot, yrot, zrot);
bn_mat_xform_about_pt(pmat, rmat, keypoint);
bn_mat_inv(invXform, gtd.gtd_xform);
bn_mat_mul(tmpMat, invXform, pmat);
bn_mat_mul(emat, tmpMat, gtd.gtd_xform);
GED_DB_GET_INTERNAL(gedp, &intern, dp, emat, &rt_uniresource, GED_ERROR);
RT_CK_DB_INTERNAL(&intern);
GED_DB_PUT_INTERNAL(gedp, dp, &intern, &rt_uniresource, GED_ERROR);
return GED_OK;
}
int
ged_otranslate(struct ged *gedp, int argc, const char *argv[])
{
struct directory *dp;
struct _ged_trace_data gtd;
struct rt_db_internal intern;
vect_t delta;
double scan[3];
mat_t dmat;
mat_t emat;
mat_t tmpMat;
mat_t invXform;
point_t rpp_min;
point_t rpp_max;
static const char *usage = "obj dx dy dz";
GED_CHECK_DATABASE_OPEN(gedp, GED_ERROR);
GED_CHECK_READ_ONLY(gedp, GED_ERROR);
GED_CHECK_ARGC_GT_0(gedp, argc, GED_ERROR);
/* initialize result */
bu_vls_trunc(gedp->ged_result_str, 0);
/* must be wanting help */
if (argc == 1) {
bu_vls_printf(gedp->ged_result_str, "Usage: %s %s", argv[0], usage);
return GED_HELP;
}
if (argc != 5) {
bu_vls_printf(gedp->ged_result_str, "Usage: %s %s", argv[0], usage);
return GED_ERROR;
}
if (_ged_get_obj_bounds2(gedp, 1, argv+1, >d, rpp_min, rpp_max) == GED_ERROR)
return GED_ERROR;
dp = gtd.gtd_obj[gtd.gtd_objpos-1];
if (!(dp->d_flags & RT_DIR_SOLID)) {
if (_ged_get_obj_bounds(gedp, 1, argv+1, 1, rpp_min, rpp_max) == GED_ERROR)
return GED_ERROR;
}
if (sscanf(argv[2], "%lf", &scan[X]) != 1) {
bu_vls_printf(gedp->ged_result_str, "%s: bad x value - %s", argv[0], argv[2]);
return GED_ERROR;
}
if (sscanf(argv[3], "%lf", &scan[Y]) != 1) {
bu_vls_printf(gedp->ged_result_str, "%s: bad y value - %s", argv[0], argv[3]);
return GED_ERROR;
}
if (sscanf(argv[4], "%lf", &scan[Z]) != 1) {
bu_vls_printf(gedp->ged_result_str, "%s: bad z value - %s", argv[0], argv[4]);
return GED_ERROR;
}
MAT_IDN(dmat);
VSCALE(delta, scan, gedp->ged_wdbp->dbip->dbi_local2base);
MAT_DELTAS_VEC(dmat, delta);
bn_mat_inv(invXform, gtd.gtd_xform);
bn_mat_mul(tmpMat, invXform, dmat);
bn_mat_mul(emat, tmpMat, gtd.gtd_xform);
GED_DB_GET_INTERNAL(gedp, &intern, dp, emat, &rt_uniresource, GED_ERROR);
RT_CK_DB_INTERNAL(&intern);
GED_DB_PUT_INTERNAL(gedp, dp, &intern, &rt_uniresource, GED_ERROR);
return GED_OK;
}
/*
* Returns -
* -2 unknown keyword
* -1 error in processing keyword
* 0 OK
*/
int
multi_words( char *words[], int nwords )
{
if ( strcmp( words[0], "rot" ) == 0 ) {
mat_t mat;
/* Expects rotations rx, ry, rz, in degrees */
if ( nwords < 4 ) return(-1);
MAT_IDN( mat );
bn_mat_angles( mat,
atof( words[1] ),
atof( words[2] ),
atof( words[3] ) );
out_mat( mat, stdout );
return(0);
}
if ( strcmp( words[0], "xlate" ) == 0 ) {
mat_t mat;
if ( nwords < 4 ) return(-1);
/* Expects translations tx, ty, tz */
MAT_IDN( mat );
MAT_DELTAS( mat,
atof( words[1] ),
atof( words[2] ),
atof( words[3] ) );
out_mat( mat, stdout );
return(0);
}
if ( strcmp( words[0], "rot_at" ) == 0 ) {
mat_t mat;
mat_t mat1;
mat_t mat2;
mat_t mat3;
/* JG - Expects x, y, z, rx, ry, rz */
/* Translation back to the origin by (-x, -y, -z) */
/* is done first, then the rotation, and finally */
/* back into the original position by (+x, +y, +z). */
if ( nwords < 7 ) return(-1);
MAT_IDN( mat1 );
MAT_IDN( mat2 );
MAT_IDN( mat3 );
MAT_DELTAS( mat1,
-atof( words[1] ),
-atof( words[2] ),
-atof( words[3] ) );
bn_mat_angles( mat2,
atof( words[4] ),
atof( words[5] ),
atof( words[6] ) );
MAT_DELTAS( mat3,
atof( words[1] ),
atof( words[2] ),
atof( words[3] ) );
bn_mat_mul( mat, mat2, mat1 );
bn_mat_mul2( mat3, mat );
out_mat( mat, stdout );
return(0);
}
if ( strcmp( words[0], "orient" ) == 0 ) {
register int i;
mat_t mat;
double args[8];
/* Expects tx, ty, tz, rx, ry, rz, [scale]. */
/* All rotation is done first, then translation */
/* Note: word[0] and args[0] are the keyword */
if ( nwords < 6+1 ) return(-1);
for ( i=1; i<6+1; i++ )
args[i] = 0;
args[7] = 1.0; /* optional arg, default to 1 */
for ( i=1; i<nwords; i++ )
args[i] = atof( words[i] );
MAT_IDN( mat );
bn_mat_angles( mat, args[4], args[5], args[6] );
MAT_DELTAS( mat, args[1], args[2], args[3] );
if ( NEAR_ZERO( args[7], VDIVIDE_TOL ) ) {
/* Nearly zero, signal error */
fprintf(stderr, "Orient scale arg is near zero ('%s')\n",
words[7] );
return(-1);
} else {
mat[15] = 1 / args[7];
}
out_mat( mat, stdout );
return(0);
}
if ( strcmp( words[0], "ae" ) == 0 ) {
mat_t mat;
fastf_t az, el;
if ( nwords < 3 ) return(-1);
/* Expects azimuth, elev, optional twist */
az = atof(words[1]);
el = atof(words[2]);
#if 0
if ( nwords == 3 )
twist = 0.0;
else
twist = atof(words[3]);
#endif
MAT_IDN( mat );
/* XXX does not take twist, for now XXX */
bn_mat_ae( mat, az, el );
out_mat( mat, stdout );
return(0);
}
if ( strcmp( words[0], "arb_rot_pt" ) == 0 ) {
mat_t mat;
point_t pt1, pt2;
vect_t dir;
fastf_t ang;
if ( nwords < 1+3+3+1 ) return(-1);
/* Expects point1, point2, angle */
VSET( pt1, atof(words[1]), atof(words[2]), atof(words[3]) );
VSET( pt2, atof(words[4]), atof(words[5]), atof(words[6]) );
ang = atof(words[7]) * bn_degtorad;
VSUB2( dir, pt2, pt2 );
VUNITIZE(dir);
MAT_IDN( mat );
bn_mat_arb_rot( mat, pt1, dir, ang );
out_mat( mat, stdout );
return(0);
}
if ( strcmp( words[0], "arb_rot_dir" ) == 0 ) {
mat_t mat;
point_t pt1;
vect_t dir;
fastf_t ang;
if ( nwords < 1+3+3+1 ) return(-1);
/* Expects point1, dir, angle */
VSET( pt1, atof(words[1]), atof(words[2]), atof(words[3]) );
VSET( dir, atof(words[4]), atof(words[5]), atof(words[6]) );
ang = atof(words[7]) * bn_degtorad;
VUNITIZE(dir);
MAT_IDN( mat );
bn_mat_arb_rot( mat, pt1, dir, ang );
out_mat( mat, stdout );
return(0);
}
if ( strcmp( words[0], "quat" ) == 0 ) {
mat_t mat;
quat_t quat;
/* Usage: quat x, y, z, w */
if ( nwords < 5 ) return -1;
QSET( quat, atof(words[1]), atof(words[2]),
atof(words[3]), atof(words[4]) );
quat_quat2mat( mat, quat );
out_mat( mat, stdout);
return 0;
}
if ( strcmp( words[0], "fromto" ) == 0 ) {
mat_t mat;
point_t cur;
point_t next;
vect_t from;
vect_t to;
/* Usage: fromto +Z cur_xyz next_xyz */
if ( nwords < 8 ) return -1;
if ( strcmp( words[1], "+X" ) == 0 ) {
VSET( from, 1, 0, 0 );
} else if ( strcmp( words[1], "-X" ) == 0 ) {
VSET( from, -1, 0, 0 );
} else if ( strcmp( words[1], "+Y" ) == 0 ) {
VSET( from, 0, 1, 0 );
} else if ( strcmp( words[1], "-Y" ) == 0 ) {
VSET( from, 0, -1, 0 );
} else if ( strcmp( words[1], "+Z" ) == 0 ) {
VSET( from, 0, 0, 1 );
} else if ( strcmp( words[1], "-Z" ) == 0 ) {
VSET( from, 0, 0, -1 );
} else {
fprintf(stderr, "fromto '%s' is not +/-XYZ\n", words[1]);
return -1;
}
VSET( cur, atof(words[2]), atof(words[3]), atof(words[4]) );
VSET( next, atof(words[5]), atof(words[6]), atof(words[7]) );
VSUB2( to, next, cur );
VUNITIZE(to);
bn_mat_fromto( mat, from, to );
/* Check to see if it worked. */
{
vect_t got;
MAT4X3VEC( got, mat, from );
if ( VDOT( got, to ) < 0.9 ) {
bu_log("\ntabsub ERROR: At t=%s, bn_mat_fromto failed!\n", chanwords[0] );
VPRINT("\tfrom", from);
VPRINT("\tto", to);
VPRINT("\tgot", got);
}
}
out_mat( mat, stdout );
return 0;
}
return(-2); /* Unknown keyword */
}
bool STEPWrapper::convert(BRLCADWrapper *dot_g)
{
MAP_OF_PRODUCT_NAME_TO_ENTITY_ID name2id_map;
MAP_OF_ENTITY_ID_TO_PRODUCT_NAME id2name_map;
MAP_OF_ENTITY_ID_TO_PRODUCT_ID id2productid_map;
MAP_OF_PRODUCT_NAME_TO_ENTITY_ID::iterator niter = name2id_map.end();
if (!dot_g) {
return false;
}
this->dotg = dot_g;
int num_ents = instance_list->InstanceCount();
for (int i = 0; i < num_ents; i++) {
SDAI_Application_instance *sse = instance_list->GetSTEPentity(i);
if (sse == NULL) {
continue;
}
std::string name = sse->EntityName();
std::transform(name.begin(), name.end(), name.begin(), (int(*)(int))std::tolower);
if ((sse->STEPfile_id > 0) && (sse->IsA(SCHEMA_NAMESPACE::e_shape_definition_representation))) {
ShapeDefinitionRepresentation *sdr = dynamic_cast<ShapeDefinitionRepresentation *>(Factory::CreateObject(this, (SDAI_Application_instance *)sse));
if (!sdr) {
bu_exit(1, "ERROR: unable to allocate a 'ShapeDefinitionRepresentation' entity\n");
} else {
int sdr_id = sdr->GetId();
std::string pname = sdr->GetProductName();
int product_id = sdr->GetProductId();
id2productid_map[sdr_id] = product_id;
if (pname.empty()) {
std::string str = "ShapeDefinitionRepresentation@";
str = dotg->GetBRLCADName(str);
id2name_map[sdr_id] = pname;
} else {
std::string temp = pname;
int index = 2;
while ((niter=name2id_map.find(temp)) != name2id_map.end()) {
temp = pname + "_" + static_cast<ostringstream*>( &(ostringstream() << (index++)) )->str();
}
pname = temp;
if ((niter=name2id_map.find(pname)) == name2id_map.end()) {
id2name_map[sdr_id] = pname;
name2id_map[pname] = product_id;
id2name_map[product_id] = pname;
}
}
AdvancedBrepShapeRepresentation *aBrep = sdr->GetAdvancedBrepShapeRepresentation();
if (aBrep) {
if (pname.empty()) {
std::string str = "product@";
pname = dotg->GetBRLCADName(str);
id2name_map[aBrep->GetId()] = pname;
id2name_map[product_id] = pname;
} else {
id2name_map[aBrep->GetId()] = pname;
id2name_map[product_id] = pname;
}
id2productid_map[aBrep->GetId()] = product_id;
if (Verbose()) {
if (!pname.empty()) {
std::cerr << std::endl << " Generating Product -" << pname ;
} else {
std::cerr << std::endl << " Generating Product";
}
}
LocalUnits::length = aBrep->GetLengthConversionFactor();
LocalUnits::planeangle = aBrep->GetPlaneAngleConversionFactor();
LocalUnits::solidangle = aBrep->GetSolidAngleConversionFactor();
ON_Brep *onBrep = aBrep->GetONBrep();
if (!onBrep) {
delete sdr;
bu_exit(1, "ERROR: failure creating advanced boundary representation from %s\n", stepfile.c_str());
} else {
ON_TextLog tl;
if (!onBrep->IsValid(&tl)) {
bu_log("WARNING: %s is not valid\n", name.c_str());
}
//onBrep->SpSplitClosedFaces();
//ON_Brep *tbrep = TightenBrep(onBrep);
mat_t mat;
MAT_IDN(mat);
Axis2Placement3D *axis = aBrep->GetAxis2Placement3d();
if (axis != NULL) {
//assign matrix values
double translate_to[3];
const double *toXaxis = axis->GetXAxis();
const double *toYaxis = axis->GetYAxis();
const double *toZaxis = axis->GetZAxis();
mat_t rot_mat;
VMOVE(translate_to,axis->GetOrigin());
VSCALE(translate_to,translate_to,LocalUnits::length);
MAT_IDN(rot_mat);
VMOVE(&rot_mat[0], toXaxis);
VMOVE(&rot_mat[4], toYaxis);
VMOVE(&rot_mat[8], toZaxis);
bn_mat_inv(mat, rot_mat);
MAT_DELTAS_VEC(mat, translate_to);
}
dotg->WriteBrep(pname, onBrep,mat);
delete onBrep;
}
} else { // must be an assembly
if (pname.empty()) {
std::string str = "assembly@";
pname = dotg->GetBRLCADName(str);
}
ShapeRepresentation *aSR = sdr->GetShapeRepresentation();
if (aSR) {
int sr_id = aSR->GetId();
id2name_map[sr_id] = pname;
id2name_map[product_id] = pname;
id2productid_map[sr_id] = product_id;
}
}
Factory::DeleteObjects();
}
}
}
/*
* Pickup BREP related to SHAPE_REPRESENTATION through SHAPE_REPRESENTATION_RELATIONSHIP
*
* like the following found in OpenBook Part 'C':
* #21281=SHAPE_DEFINITION_REPRESENTATION(#21280,#21270);
* #21280=PRODUCT_DEFINITION_SHAPE('','SHAPE FOR C.',#21279);
* #21279=PRODUCT_DEFINITION('design','',#21278,#21275);
* #21278=PRODUCT_DEFINITION_FORMATION_WITH_SPECIFIED_SOURCE('1','LAST_VERSION',#21277,.MADE.);
* #21277=PRODUCT('C','C','NOT SPECIFIED',(#21276));
* #21270=SHAPE_REPRESENTATION('',(#21259),#21267);
* #21259=AXIS2_PLACEMENT_3D('DANTE_BX_CPU_TOP_1',#21256,#21257,#21258);
* #21267=(GEOMETRIC_REPRESENTATION_CONTEXT(3)GLOBAL_UNCERTAINTY_ASSIGNED_CONTEXT((#21266))
* GLOBAL_UNIT_ASSIGNED_CONTEXT((#21260,#21264,#21265))REPRESENTATION_CONTEXT('ID1','3'));
*
* #21271=SHAPE_REPRESENTATION_RELATIONSHIP('','',#21270,#21268);
* #21268=ADVANCED_BREP_SHAPE_REPRESENTATION('',(#21254),#21267);
* #21272=SHAPE_REPRESENTATION_RELATIONSHIP('','',#21270,#21269);
* #21269=MANIFOLD_SURFACE_SHAPE_REPRESENTATION('',(#21255),#21267);
*
*/
for (int i = 0; i < num_ents; i++) {
SDAI_Application_instance *sse = instance_list->GetSTEPentity(i);
if (sse == NULL) {
continue;
}
std::string name = sse->EntityName();
std::transform(name.begin(), name.end(), name.begin(), (int(*)(int))std::tolower);
if ((sse->STEPfile_id > 0) && (sse->IsA(SCHEMA_NAMESPACE::e_shape_representation_relationship))) {
ShapeRepresentationRelationship *srr = dynamic_cast<ShapeRepresentationRelationship *>(Factory::CreateObject(this, (SDAI_Application_instance *)sse));
if (srr) {
ShapeRepresentation *aSR = dynamic_cast<ShapeRepresentation *>(srr->GetRepresentationRelationshipRep_1());
AdvancedBrepShapeRepresentation *aBrep = dynamic_cast<AdvancedBrepShapeRepresentation *>(srr->GetRepresentationRelationshipRep_2());
if (!aBrep) { //try rep_1
aBrep = dynamic_cast<AdvancedBrepShapeRepresentation *>(srr->GetRepresentationRelationshipRep_1());
aSR = dynamic_cast<ShapeRepresentation *>(srr->GetRepresentationRelationshipRep_2());
}
if ((aSR) && (aBrep)) {
int sr_id = aSR->GetId();
MAP_OF_ENTITY_ID_TO_PRODUCT_ID::iterator it = id2productid_map.find(sr_id);
if (it != id2productid_map.end()) { // product found
int product_id = (*it).second;
int brep_id = aBrep->GetId();
it = id2productid_map.find(brep_id);
if (it == id2productid_map.end()) { // brep not loaded yet so lets do that here.
string pname = id2name_map[product_id];
id2productid_map[brep_id] = product_id;
if (Verbose()) {
if (!pname.empty()) {
std::cerr << std::endl << " Generating Product -" << pname ;
} else {
std::cerr << std::endl << " Generating Product";
}
}
LocalUnits::length = aBrep->GetLengthConversionFactor();
LocalUnits::planeangle = aBrep->GetPlaneAngleConversionFactor();
LocalUnits::solidangle = aBrep->GetSolidAngleConversionFactor();
ON_Brep *onBrep = aBrep->GetONBrep();
if (!onBrep) {
bu_exit(1, "ERROR: failure creating advanced boundary representation from %s\n", stepfile.c_str());
} else {
ON_TextLog tl;
if (!onBrep->IsValid(&tl)) {
bu_log("WARNING: %s is not valid\n", name.c_str());
}
//onBrep->SpSplitClosedFaces();
//ON_Brep *tbrep = TightenBrep(onBrep);
mat_t mat;
MAT_IDN(mat);
Axis2Placement3D *axis = aBrep->GetAxis2Placement3d();
if (axis != NULL) {
//assign matrix values
double translate_to[3];
const double *toXaxis = axis->GetXAxis();
const double *toYaxis = axis->GetYAxis();
const double *toZaxis = axis->GetZAxis();
mat_t rot_mat;
VMOVE(translate_to,axis->GetOrigin());
VSCALE(translate_to,translate_to,LocalUnits::length);
MAT_IDN(rot_mat);
VMOVE(&rot_mat[0], toXaxis);
VMOVE(&rot_mat[4], toYaxis);
VMOVE(&rot_mat[8], toZaxis);
bn_mat_inv(mat, rot_mat);
MAT_DELTAS_VEC(mat, translate_to);
}
dotg->WriteBrep(pname, onBrep,mat);
delete onBrep;
}
}
}
}
Factory::DeleteObjects();
}
}
}
if (Verbose()) {
std::cerr << std::endl << " Generating BRL-CAD hierarchy." << std::endl;
}
for (int i = 0; i < num_ents; i++) {
SDAI_Application_instance *sse = instance_list->GetSTEPentity(i);
if (sse == NULL) {
continue;
}
std::string name = sse->EntityName();
std::transform(name.begin(), name.end(), name.begin(), (int(*)(int))std::tolower);
if ((sse->STEPfile_id > 0) && (sse->IsA(SCHEMA_NAMESPACE::e_context_dependent_shape_representation))) {
ContextDependentShapeRepresentation *aCDSR = dynamic_cast<ContextDependentShapeRepresentation *>(Factory::CreateObject(this, (SDAI_Application_instance *)sse));
if (aCDSR) {
int rep_1_id = aCDSR->GetRepresentationRelationshipRep_1()->GetId();
int rep_2_id = aCDSR->GetRepresentationRelationshipRep_2()->GetId();
int pid_1 = id2productid_map[rep_1_id];
int pid_2 = id2productid_map[rep_2_id];
Axis2Placement3D *axis1 = NULL;
Axis2Placement3D *axis2 = NULL;
if ((id2name_map.find(rep_1_id) != id2name_map.end()) && (id2name_map.find(rep_2_id) != id2name_map.end())) {
string comb = id2name_map[rep_1_id];
string member = id2name_map[rep_2_id];
mat_t mat;
MAT_IDN(mat);
ProductDefinition *relatingProduct = aCDSR->GetRelatingProductDefinition();
ProductDefinition *relatedProduct = aCDSR->GetRelatedProductDefinition();
if (relatingProduct && relatedProduct) {
string relatingName = relatingProduct->GetProductName();
int relatingID = relatingProduct->GetProductId();
string relatedName = relatedProduct->GetProductName();
int relatedID = relatedProduct->GetProductId();
if ((relatingID == pid_1) && (relatedID == pid_2)) {
axis1 = aCDSR->GetTransformItem_1();
axis2 = aCDSR->GetTransformItem_2();
comb = id2name_map[rep_1_id];
member = id2name_map[rep_2_id];
} else if ((relatingID == pid_2) && (relatedID == pid_1)) {
axis1 = aCDSR->GetTransformItem_2();
axis2 = aCDSR->GetTransformItem_1();
comb = id2name_map[rep_2_id];
member = id2name_map[rep_1_id];
} else {
std::cerr << "Error: Found Representation Relationship Rep_1(name=" << comb << ",Id=" << rep_1_id << ")" << std::endl;
std::cerr << "Error: Found Representation Relationship Rep_2(name=" << member << ",Id=" << rep_2_id << ")" << std::endl;
std::cerr << "Error: but Relating ProductDefinition (name=" << relatingName << ",Id=" << relatingID << ")" << std::endl;
std::cerr << "Error: Related ProductDefinition (name=" << relatedName << ",Id=" << relatedID << ")" << std::endl;
}
}
if ((axis1 != NULL) && (axis2 != NULL)) {
mat_t to_mat;
mat_t from_mat;
mat_t toinv_mat;
//assign matrix values
double translate_to[3];
double translate_from[3];
const double *toXaxis = axis1->GetXAxis();
const double *toYaxis = axis1->GetYAxis();
const double *toZaxis = axis1->GetZAxis();
const double *fromXaxis = axis2->GetXAxis();
const double *fromYaxis = axis2->GetYAxis();
const double *fromZaxis = axis2->GetZAxis();
VMOVE(translate_to,axis1->GetOrigin());
VSCALE(translate_to,translate_to,LocalUnits::length);
VMOVE(translate_from,axis2->GetOrigin());
VSCALE(translate_from,translate_from,-LocalUnits::length);
// undo from trans/rot
MAT_IDN(from_mat);
VMOVE(&from_mat[0], fromXaxis);
VMOVE(&from_mat[4], fromYaxis);
VMOVE(&from_mat[8], fromZaxis);
MAT_DELTAS_VEC(from_mat, translate_from);
// do to trans/rot
MAT_IDN(to_mat);
VMOVE(&to_mat[0], toXaxis);
VMOVE(&to_mat[4], toYaxis);
VMOVE(&to_mat[8], toZaxis);
bn_mat_inv(toinv_mat, to_mat);
MAT_DELTAS_VEC(toinv_mat, translate_to);
bn_mat_mul(mat, toinv_mat, from_mat);
}
dotg->AddMember(comb,member,mat);
}
Factory::DeleteObjects();
}
}
}
if (!dotg->WriteCombs()) {
std::cerr << "Error writing BRL-CAD hierarchy." << std::endl;
}
return true;
}
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;
}
