void
bn_wrt_point_direc(mat_t out, const mat_t change, const mat_t in, const point_t point, const vect_t direc, const struct bn_tol *tol)
{
static mat_t t1;
static mat_t pt_to_origin, origin_to_pt;
static mat_t d_to_zaxis, zaxis_to_d;
static vect_t zaxis;
/* build "point to origin" matrix */
MAT_IDN(pt_to_origin);
MAT_DELTAS_VEC_NEG(pt_to_origin, point);
/* build "origin to point" matrix */
MAT_IDN(origin_to_pt);
MAT_DELTAS_VEC_NEG(origin_to_pt, point);
/* build "direc to zaxis" matrix */
VSET(zaxis, 0.0, 0.0, 1.0);
bn_mat_fromto(d_to_zaxis, direc, zaxis, tol);
/* build "zaxis to direc" matrix */
bn_mat_inv(zaxis_to_d, d_to_zaxis);
/* apply change matrix...
* t1 = change * d_to_zaxis * pt_to_origin * in
*/
bn_mat_mul4(t1, change, d_to_zaxis, pt_to_origin, in);
/* apply origin_to_pt matrix:
* out = origin_to_pt * zaxis_to_d *
* change * d_to_zaxis * pt_to_origin * in
*/
bn_mat_mul3(out, origin_to_pt, zaxis_to_d, t1);
}
static int
test_bn_mat_mul3(int argc, char *argv[])
{
mat_t m1, m2, m3, expected, actual;
if (argc != 6) {
bu_exit(1, "<args> format: M1 M2 M3 <expected_result> [%s]\n", argv[0]);
}
scan_mat_args(argv, 2, &m1);
scan_mat_args(argv, 3, &m2);
scan_mat_args(argv, 4, &m3);
scan_mat_args(argv, 5, &expected);
bn_mat_mul3(actual, m1, m2, m3);
return !mat_equal(expected, actual);
}