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));
}
}
static int
test_bn_mat_yrot(int argc, char *argv[])
{
mat_t expected, actual;
double siny, cosy;
if (argc != 5) {
bu_exit(1, "<args> format: siny cosy <expected_result> [%s]\n", argv[0]);
}
sscanf(argv[2], "%lg", &siny);
sscanf(argv[3], "%lg", &cosy);
scan_mat_args(argv, 4, &expected);
bn_mat_yrot(actual, siny, cosy);
return !mat_equal(expected, actual);
}
