int
main(int argc, char *argv[])
{
int val;
fastf_t yaw, pitch, roll;
vect_t temp, point, zero;
mat_t mat;
/* intentionally double for scan */
double timeval;
double scan[3];
VSETALL(temp, 0.0);
VSETALL(point, 0.0);
VSETALL(zero, 0.0);
if (argc == 1 && isatty(fileno(stdin)) && isatty(fileno(stdout))) {
usage();
return 0;
}
if (!get_args(argc, argv)) {
usage();
return 0;
}
while (1) {
/*read line from table */
val = scanf("%lf%*[^-0123456789]", &timeval); /*read time, ignore garbage*/
if (val < 1) {
break;
}
val = scanf("%lf %lf %lf", &scan[0], &scan[1], &scan[2]);
if (val < 3) {
break;
}
/* double to fastf_t */
VMOVE(point, scan);
val = scanf("%lf %lf %lf", &scan[0], &scan[1], &scan[2]);
if (val < 3) {
break;
}
/* double to fastf_t */
yaw = scan[0];
pitch = scan[1];
roll = scan[2];
anim_dy_p_r2mat(mat, yaw, pitch, roll);
anim_add_trans(mat, point, zero);
MAT4X3PNT(temp, mat, offset);
printf("%.10g\t%.10g\t%.10g\t%.10g", timeval, temp[0], temp[1], temp[2]);
if (full_print)
printf("\t%.10g\t%.10g\t%.10g", yaw, pitch, roll);
printf("\n");
}
return 0;
}
int
main(int argc, char **argv)
{
void anim_dir2mat(fastf_t *, const fastf_t *, const fastf_t *), anim_y_p_r2mat(fastf_t *, double, double, double), anim_add_trans(fastf_t *, const fastf_t *, const fastf_t *), anim_mat_print(FILE *, const fastf_t *, int);
int get_args(int argc, char **argv), track_prep(void), val, frame, go, i, count;
fastf_t y_rot, distance, yaw, pitch, roll;
vect_t p1, p2, p3, dir, dir2, wheel_now, wheel_prev;
vect_t zero, position, vdelta, temp, to_track, to_front;
mat_t mat_v, wmat, mat_x;
FILE *stream;
int last_frame;
VSETALL(zero, 0.0);
VSETALL(to_track, 0.0);
VSETALL(centroid, 0.0);
VSETALL(rcentroid, 0.0);
init_dist = y_rot = radius= 0.0;
first_frame = num_wheels = steer = axes = cent = links_placed=0;
num_wheels = num_links = last_frame = 0;
MAT_IDN(mat_v);
MAT_IDN(mat_x);
MAT_IDN(wmat);
MAT_IDN(m_axes);
MAT_IDN(m_rev_axes);
if (!get_args(argc, argv)) {
fprintf(stderr, "anim_hardtrack: argument error.");
return(-1);
}
if (axes || cent ) {
/* vehicle has own reference frame */
anim_add_trans(m_axes, centroid, zero);
anim_add_trans(m_rev_axes, zero, rcentroid);
}
/* get track information from specified file */
if (!(stream = fopen(*(argv+bu_optind), "rb"))) {
fprintf(stderr, "Anim_hardtrack: Could not open file %s.\n", *(argv+bu_optind));
return(0);
}
num_wheels = -1;
if (radius) {
while (!feof(stream)) {
fscanf(stream, "%*f %*f %*f");
num_wheels++;
}
} else {
while (!feof(stream)) {
fscanf(stream, "%*f %*f %*f %*f");
num_wheels++;
}
}
rewind(stream);
/*allocate memory for track information*/
x = (struct all *) bu_calloc(num_wheels, sizeof(struct all), "struct all");
/*read rest of track info */
for (i=0;i<NW;i++) {
fscanf(stream, "%lf %lf %lf", temp, temp+1, temp+2);
if (radius)
x[i].w.rad = radius;
else
fscanf(stream, "%lf", & x[i].w.rad);
MAT4X3PNT(x[i].w.pos, m_rev_axes, temp);
if (i==0)
track_y = x[0].w.pos[1];
else
x[i].w.pos[1] = track_y;
}
(void) fclose(stream);
(void) track_prep();
if (get_circumf) {
printf("%.10g\n", tracklen);
return(0);
}
/* initialize to_track */
VSET(to_track, 0.0, track_y, 0.0);
VSET(to_front, 1.0, 0.0, 0.0);
if ((!print_link)&&(!print_wheel)) {
fprintf(stderr, "anim_hardtrack: no ouput requested. Use -l or -w.\n");
bu_exit(0, NULL);
}
/* main loop */
distance = 0.0;
if (!steer)
frame = first_frame;
else
frame = first_frame-1;
for (val = 3; val > 2; frame++) {
go = 1;
/*p2 is current position. p3 is next;p1 is previous*/
VMOVE(p1, p2);
VMOVE(p2, p3);
scanf("%*f");/*time stamp*/
val = scanf("%lf %lf %lf", p3, p3+1, p3 + 2);
if (!steer) {
scanf("%lf %lf %lf", &yaw, &pitch, &roll);
anim_dy_p_r2mat(mat_v, yaw, pitch, roll);
anim_add_trans(mat_v, p3, rcentroid);
}
else {
/* analyze positions for steering */
/*get useful direction unit vectors*/
if (frame == first_frame) {
/* first frame*/
VSUBUNIT(dir, p3, p2);
VMOVE(dir2, dir);
}
else if (val < 3) {
/*last frame*/
VSUBUNIT(dir, p2, p1);
VMOVE(dir2, dir);
}
else if (frame > first_frame) {
/*normal*/
VSUBUNIT(dir, p3, p1);
VSUBUNIT(dir2, p2, p1);/*needed for vertical case*/
}
else go = 0;/*first time through loop;no p2*/
/*create matrix which would move vehicle*/
anim_dir2mat(mat_v, dir, dir2);
anim_add_trans(mat_v, p2, rcentroid);
}
/*determine distance traveled*/
VMOVE(wheel_prev, wheel_now);
MAT4X3PNT(wheel_now, mat_v, to_track);
if (frame > first_frame) {
/* increment distance by distance moved */
VSUB2(vdelta, wheel_now, wheel_prev);
MAT3X3VEC(temp, mat_v, to_front);/*new front of vehicle*/
distance += VDOT(temp, vdelta);/*portion of vdelta in line with track*/
}
if (go) {
if (print_mode==TRACK_ANIM) {
printf("start %d;\nclean;\n", frame);
} else if (print_mode==TRACK_ARCED) {
if (frame != arced_frame) continue;
last_frame = 1;
}
if (print_link) {
for (count=0;count<num_links;count++) {
(void) get_link(position, &y_rot, distance+tracklen*count/num_links+init_dist);
anim_y_p_r2mat(wmat, 0.0, y_rot+r[count].ang, 0.0);
anim_add_trans(wmat, position, r[count].pos);
if ((axes || cent) && links_placed) {
/* link moved from vehicle coords */
bn_mat_mul(mat_x, wmat, m_rev_axes);
bn_mat_mul(wmat, m_axes, mat_x);
}
else if (axes || cent) {
/* link moved to vehicle coords */
MAT_MOVE(mat_x, wmat);
bn_mat_mul(wmat, m_axes, mat_x);
}
if (print_mode==TRACK_ANIM) {
printf("anim %s.%d matrix %s\n", *(argv+link_nindex), count, link_cmd);
anim_mat_printf(stdout, wmat, "%.10g ", "\n", ";\n");
} else if (print_mode==TRACK_ARCED) {
printf("arced %s.%d matrix %s ", *(argv+link_nindex), count, link_cmd);
anim_mat_printf(stdout, wmat, "%.10g ", "", "\n");
}
}
}
if (print_wheel) {
for (count = 0;count<num_wheels;count++) {
anim_y_p_r2mat(wmat, 0.0, -distance/x[count].w.rad, 0.0);
VREVERSE(temp, x[count].w.pos);
anim_add_trans(wmat, x[count].w.pos, temp);
if (axes || cent) {
bn_mat_mul(mat_x, wmat, m_rev_axes);
bn_mat_mul(wmat, m_axes, mat_x);
}
if (print_mode==TRACK_ANIM) {
printf("anim %s.%d matrix %s\n", *(argv+wheel_nindex), count, wheel_cmd);
anim_mat_printf(stdout, wmat, "%.10g ", "\n", ";\n");
} else if (print_mode==TRACK_ARCED) {
printf("arced %s.%d matrix %s ", *(argv+wheel_nindex), count, wheel_cmd);
anim_mat_printf(stdout, wmat, "%.10g ", "", "\n");
}
}
}
if (print_mode==TRACK_ANIM)
printf("end;\n");
}
if (last_frame) break;
}
if (x) {
bu_free(x, "struct all");
}
if (r) {
bu_free(r, "struct rlink");
}
return( 0 );
}
