static int
test_bn_mat_trn(int argc, char *argv[])
{
mat_t m, expected, actual;
if (argc != 4) {
bu_exit(1, "<args> format: M <expected_result> [%s]\n", argv[0]);
}
scan_mat_args(argv, 2, &m);
scan_mat_args(argv, 3, &expected);
bn_mat_trn(actual, m);
return !mat_equal(expected, actual);
}
/**
* R E C _ P R E P
*
* Given a pointer to a GED database record, and a transformation matrix,
* determine if this is a valid REC,
* and if so, precompute various terms of the formulas.
*
* Returns -
* 0 REC is OK
* !0 Error in description
*
* Implicit return - A struct rec_specific is created, and its
* address is stored in stp->st_specific for use by rt_rec_shot(). If
* the TGC is really an REC, stp->st_id is modified to ID_REC.
*/
int
rt_rec_prep(struct soltab *stp, struct rt_db_internal *ip, struct rt_i *rtip)
{
struct rt_tgc_internal *tip;
struct rec_specific *rec;
double magsq_h, magsq_a, magsq_b;
double mag_h, mag_a, mag_b;
mat_t R;
mat_t Rinv;
mat_t S;
vect_t invsq; /* [ 1/(|A|**2), 1/(|B|**2), 1/(|Hv|**2) ] */
vect_t work;
fastf_t f;
if (!stp || !ip)
return -1;
RT_CK_SOLTAB(stp);
RT_CK_DB_INTERNAL(ip);
if (rtip) RT_CK_RTI(rtip);
tip = (struct rt_tgc_internal *)ip->idb_ptr;
RT_TGC_CK_MAGIC(tip);
/* Validate that |H| > 0, compute |A| |B| |C| |D| */
mag_h = sqrt(magsq_h = MAGSQ(tip->h));
mag_a = sqrt(magsq_a = MAGSQ(tip->a));
mag_b = sqrt(magsq_b = MAGSQ(tip->b));
/* Check for |H| > 0, |A| > 0, |B| > 0 */
if (NEAR_ZERO(mag_h, RT_LEN_TOL) || NEAR_ZERO(mag_a, RT_LEN_TOL)
|| NEAR_ZERO(mag_b, RT_LEN_TOL)) {
return 1; /* BAD, too small */
}
/* Make sure that A == C, B == D */
VSUB2(work, tip->a, tip->c);
f = MAGNITUDE(work);
if (! NEAR_ZERO(f, RT_LEN_TOL)) {
return 1; /* BAD, !cylinder */
}
VSUB2(work, tip->b, tip->d);
f = MAGNITUDE(work);
if (! NEAR_ZERO(f, RT_LEN_TOL)) {
return 1; /* BAD, !cylinder */
}
/* Check for A.B == 0, H.A == 0 and H.B == 0 */
f = VDOT(tip->a, tip->b) / (mag_a * mag_b);
if (! NEAR_ZERO(f, RT_DOT_TOL)) {
return 1; /* BAD */
}
f = VDOT(tip->h, tip->a) / (mag_h * mag_a);
if (! NEAR_ZERO(f, RT_DOT_TOL)) {
return 1; /* BAD */
}
f = VDOT(tip->h, tip->b) / (mag_h * mag_b);
if (! NEAR_ZERO(f, RT_DOT_TOL)) {
return 1; /* BAD */
}
/*
* This TGC is really an REC
*/
stp->st_id = ID_REC; /* "fix" soltab ID */
stp->st_meth = &rt_functab[ID_REC];
BU_GET(rec, struct rec_specific);
stp->st_specific = (genptr_t)rec;
VMOVE(rec->rec_Hunit, tip->h);
VUNITIZE(rec->rec_Hunit);
VMOVE(rec->rec_V, tip->v);
VMOVE(rec->rec_A, tip->a);
VMOVE(rec->rec_B, tip->b);
rec->rec_iAsq = 1.0/magsq_a;
rec->rec_iBsq = 1.0/magsq_b;
VSET(invsq, 1.0/magsq_a, 1.0/magsq_b, 1.0/magsq_h);
/* Compute R and Rinv matrices */
MAT_IDN(R);
f = 1.0/mag_a;
VSCALE(&R[0], tip->a, f);
f = 1.0/mag_b;
VSCALE(&R[4], tip->b, f);
f = 1.0/mag_h;
VSCALE(&R[8], tip->h, f);
bn_mat_trn(Rinv, R); /* inv of rot mat is trn */
/* Compute S */
MAT_IDN(S);
S[ 0] = sqrt(invsq[0]);
S[ 5] = sqrt(invsq[1]);
S[10] = sqrt(invsq[2]);
/* Compute SoR and invRoS */
bn_mat_mul(rec->rec_SoR, S, R);
bn_mat_mul(rec->rec_invRoS, Rinv, S);
/* Compute bounding sphere and RPP */
{
fastf_t dx, dy, dz; /* For bounding sphere */
if (stp->st_meth->ft_bbox(ip, &(stp->st_min), &(stp->st_max), &(rtip->rti_tol))) return 1;
VSET(stp->st_center,
(stp->st_max[X] + stp->st_min[X])/2,
(stp->st_max[Y] + stp->st_min[Y])/2,
(stp->st_max[Z] + stp->st_min[Z])/2);
dx = (stp->st_max[X] - stp->st_min[X])/2;
f = dx;
dy = (stp->st_max[Y] - stp->st_min[Y])/2;
if (dy > f) f = dy;
dz = (stp->st_max[Z] - stp->st_min[Z])/2;
if (dz > f) f = dz;
stp->st_aradius = f;
stp->st_bradius = sqrt(dx*dx + dy*dy + dz*dz);
}
return 0; /* OK */
}
/**
* 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 */
}
void
bn_mat_fromto(
mat_t m,
const fastf_t *from,
const fastf_t *to,
const struct bn_tol *tol)
{
vect_t test_to;
vect_t unit_from, unit_to;
fastf_t dot;
point_t origin = VINIT_ZERO;
/**
* The method used here is from Graphics Gems, A. Glassner, ed.
* page 531, "The Use of Coordinate Frames in Computer Graphics",
* by Ken Turkowski, Example 6.
*/
mat_t Q, Qt;
mat_t R;
mat_t A;
mat_t temp;
vect_t N, M;
vect_t w_prime; /* image of "to" ("w") in Qt */
VMOVE(unit_from, from);
VUNITIZE(unit_from); /* aka "v" */
VMOVE(unit_to, to);
VUNITIZE(unit_to); /* aka "w" */
/* If from and to are the same or opposite, special handling is
* needed, because the cross product isn't defined. asin(0.00001)
* = 0.0005729 degrees (1/2000 degree)
*/
if (bn_lseg3_lseg3_parallel(origin, from, origin, to, tol)) {
dot = VDOT(from, to);
if (dot > SMALL_FASTF) {
MAT_IDN(m);
return;
} else {
bn_vec_perp(N, unit_from);
}
} else {
VCROSS(N, unit_from, unit_to);
VUNITIZE(N); /* should be unnecessary */
}
VCROSS(M, N, unit_from);
VUNITIZE(M); /* should be unnecessary */
/* Almost everything here is done with pre-multiplys: vector * mat */
MAT_IDN(Q);
VMOVE(&Q[0], unit_from);
VMOVE(&Q[4], M);
VMOVE(&Q[8], N);
bn_mat_trn(Qt, Q);
/* w_prime = w * Qt */
MAT4X3VEC(w_prime, Q, unit_to); /* post-multiply by transpose */
MAT_IDN(R);
VMOVE(&R[0], w_prime);
VSET(&R[4], -w_prime[Y], w_prime[X], w_prime[Z]);
VSET(&R[8], 0, 0, 1); /* is unnecessary */
bn_mat_mul(temp, R, Q);
bn_mat_mul(A, Qt, temp);
bn_mat_trn(m, A); /* back to post-multiply style */
/* Verify that it worked */
MAT4X3VEC(test_to, m, unit_from);
if (UNLIKELY(!bn_lseg3_lseg3_parallel(origin, unit_to, origin, test_to, tol))) {
dot = VDOT(unit_to, test_to);
bu_log("bn_mat_fromto() ERROR! from (%g, %g, %g) to (%g, %g, %g) went to (%g, %g, %g), dot=%g?\n",
V3ARGS(from), V3ARGS(to), V3ARGS(test_to), dot);
}
}
