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