int
main (int argc, char **argv)
{
int val;
fastf_t time, yaw1, pitch1, roll1, yaw2, pitch2, roll2;
vect_t cen1, cen2, cen_ans, ang_ans, rad_ang_ans, rotated;
mat_t m_rot1, m_rot2, m_ans;
int one_time, read_cen1, read_cen2, read_rot1, read_rot2;
read_cen1 = read_cen2 = read_rot1 = read_rot2 = 1;
if (!get_args(argc, argv))
fprintf(stderr, "anim_cascade: Argument error.");
switch (output_mode) {
case CASCADE_A:
if (cmd_fcen) {
VMOVE(cen1, fcenter);
read_cen1 = 0;
}
if (cmd_rcen) {
VMOVE(cen2, rcenter);
read_cen2 = 0;
}
if (cmd_fypr) {
anim_dy_p_r2mat(m_rot1, fypr[0], fypr[1], fypr[2]);
read_rot1 = 0;
}
if (cmd_rypr) {
anim_dy_p_r2mat(m_rot2, rypr[0], rypr[1], rypr[2]);
read_rot2 = 0;
}
break;
case CASCADE_R:
if (cmd_fcen) {
VMOVE(cen1, fcenter);
read_cen1 = 0;
}
if (cmd_acen) {
VMOVE(cen2, acenter);
read_cen2 = 0;
}
if (cmd_fypr) {
anim_dy_p_r2mat(m_rot1, fypr[0], fypr[1], fypr[2]);
read_rot1 = 0;
}
if (cmd_aypr) {
anim_dy_p_r2mat(m_rot2, aypr[0], aypr[1], aypr[2]);
read_rot2 = 0;
}
break;
case CASCADE_F:
if (cmd_acen) {
VMOVE(cen1, acenter);
read_cen1 = 0;
}
if (cmd_rcen) {
VMOVE(cen2, rcenter);
read_cen2 = 0;
}
if (cmd_aypr) {
anim_dy_p_r2mat(m_rot1, aypr[0], aypr[1], aypr[2]);
read_rot1 = 0;
}
if (cmd_rypr) {
anim_dy_p_r2mat(m_rot2, rypr[0], rypr[1], rypr[2]);
read_rot2 = 0;
}
break;
default:
break;
}
one_time = (!(read_cen1||read_cen2||read_rot1||read_rot2));
read_time = one_time ? 0 : print_time;
time = 0.0;
val = 3;
while (1) {
if (read_time) {
val=scanf("%lf", &time);
if (val < 1) break;
}
if (read_cen1)
val =scanf("%lf %lf %lf", cen1, cen1+1, cen1+2);
if (read_rot1) {
val=scanf("%lf %lf %lf", &yaw1, &pitch1, &roll1);
anim_dy_p_r2mat(m_rot1, yaw1, pitch1, roll1);
}
if (read_cen2) {
val=scanf("%lf %lf %lf", cen2, cen2+1, cen2+2);
}
if (read_rot2) {
val=scanf("%lf %lf %lf", &yaw2, &pitch2, &roll2);
anim_dy_p_r2mat(m_rot2, yaw2, pitch2, roll2);
}
if (val<3) break;
if (output_mode==CASCADE_R) {
anim_tran(m_rot1);
VSUB2(rotated, cen2, cen1);
MAT4X3PNT(cen_ans, m_rot1, rotated);
bn_mat_mul(m_ans, m_rot1, m_rot2);
} else if (output_mode==CASCADE_F) {
anim_tran(m_rot2);
bn_mat_mul(m_ans, m_rot1, m_rot2);
MAT4X3PNT(rotated, m_ans, cen2);
VSUB2(cen_ans, cen1, rotated);
} else {
MAT4X3PNT(rotated, m_rot1, cen2);
VADD2(cen_ans, rotated, cen1);
bn_mat_mul(m_ans, m_rot1, m_rot2);
}
anim_mat2ypr(rad_ang_ans, m_ans);
VSCALE(ang_ans, rad_ang_ans, RTOD);
if (print_time) {
printf("%g", time);
}
printf("\t%.12g\t%.12g\t%.12g", cen_ans[0], cen_ans[1], cen_ans[2]);
printf("\t%.12g\t%.12g\t%.12g", ang_ans[0], ang_ans[1], ang_ans[2]);
printf("\n");
if (one_time) break;
}
return( 0 );
}
int
main (int argc, char *argv[])
{
int val;
/* intentionally double for scan */
double elapsed, yaw1, pitch1, roll1, yaw2, pitch2, roll2;
double cen1[3], cen2[3];
vect_t rad_ang_ans, cen_ans, ang_ans, rotated = VINIT_ZERO;
mat_t m_rot1, m_rot2, m_ans;
int one_time, read_cen1, read_cen2, read_rot1, read_rot2;
if (argc == 1 && isatty(fileno(stdin)) && isatty(fileno(stdout))) {
usage();
return 0;
}
if (!get_args(argc, argv)) {
usage();
return 0;
}
read_cen1 = read_cen2 = read_rot1 = read_rot2 = 1;
switch (output_mode) {
case CASCADE_A:
if (cmd_fcen) {
VMOVE(cen1, fcenter);
read_cen1 = 0;
}
if (cmd_rcen) {
VMOVE(cen2, rcenter);
read_cen2 = 0;
}
if (cmd_fypr) {
anim_dy_p_r2mat(m_rot1, fypr[0], fypr[1], fypr[2]);
read_rot1 = 0;
}
if (cmd_rypr) {
anim_dy_p_r2mat(m_rot2, rypr[0], rypr[1], rypr[2]);
read_rot2 = 0;
}
break;
case CASCADE_R:
if (cmd_fcen) {
VMOVE(cen1, fcenter);
read_cen1 = 0;
}
if (cmd_acen) {
VMOVE(cen2, acenter);
read_cen2 = 0;
}
if (cmd_fypr) {
anim_dy_p_r2mat(m_rot1, fypr[0], fypr[1], fypr[2]);
read_rot1 = 0;
}
if (cmd_aypr) {
anim_dy_p_r2mat(m_rot2, aypr[0], aypr[1], aypr[2]);
read_rot2 = 0;
}
break;
case CASCADE_F:
if (cmd_acen) {
VMOVE(cen1, acenter);
read_cen1 = 0;
}
if (cmd_rcen) {
VMOVE(cen2, rcenter);
read_cen2 = 0;
}
if (cmd_aypr) {
anim_dy_p_r2mat(m_rot1, aypr[0], aypr[1], aypr[2]);
read_rot1 = 0;
}
if (cmd_rypr) {
anim_dy_p_r2mat(m_rot2, rypr[0], rypr[1], rypr[2]);
read_rot2 = 0;
}
break;
default:
break;
}
one_time = (!(read_cen1||read_cen2||read_rot1||read_rot2));
read_time = one_time ? 0 : print_time;
elapsed = 0.0;
val = 3;
while (1) {
if (read_time) {
val=scanf("%lf", &elapsed);
if (val < 1) break;
}
if (read_cen1)
val =scanf("%lf %lf %lf", cen1, cen1+1, cen1+2);
if (read_rot1) {
val=scanf("%lf %lf %lf", &yaw1, &pitch1, &roll1);
anim_dy_p_r2mat(m_rot1, yaw1, pitch1, roll1);
}
if (read_cen2) {
val=scanf("%lf %lf %lf", cen2, cen2+1, cen2+2);
}
if (read_rot2) {
val=scanf("%lf %lf %lf", &yaw2, &pitch2, &roll2);
anim_dy_p_r2mat(m_rot2, yaw2, pitch2, roll2);
}
if (val<3) break;
if (output_mode==CASCADE_R) {
anim_tran(m_rot1);
VSUB2(rotated, cen2, cen1);
MAT4X3PNT(cen_ans, m_rot1, rotated);
bn_mat_mul(m_ans, m_rot1, m_rot2);
} else if (output_mode==CASCADE_F) {
anim_tran(m_rot2);
bn_mat_mul(m_ans, m_rot1, m_rot2);
MAT4X3PNT(rotated, m_ans, cen2);
VSUB2(cen_ans, cen1, rotated);
} else {
MAT4X3PNT(rotated, m_rot1, cen2);
VADD2(cen_ans, rotated, cen1);
bn_mat_mul(m_ans, m_rot1, m_rot2);
}
anim_mat2ypr(m_ans, rad_ang_ans);
VSCALE(ang_ans, rad_ang_ans, RAD2DEG);
if (print_time) {
printf("%g", elapsed);
}
printf("\t%.12g\t%.12g\t%.12g", cen_ans[0], cen_ans[1], cen_ans[2]);
printf("\t%.12g\t%.12g\t%.12g", ang_ans[0], ang_ans[1], ang_ans[2]);
printf("\n");
if (one_time) break;
}
return 0;
}