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);
}
Example #2
0
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);
}