/*---------------------------------------------------------------------------- * rot.c * * This file contains the routines that handle the users input through * requester gadgets. Joint angles can be altered with proportional * slider gadgets. * Routines to update local link rotation transformations, concatenate * all the local transformations with a viewing transformation, and then * transform all the arm vertices, are also in this file. *---------------------------------------------------------------------------- */ #include #include #include #include "defs.h" #include "ds.h" #include "input.h" extern ROTATION_INFO rot_info ; extern VIEW_TRANS view_trans ; extern REF_AXES ref_axes [NUM_LINKS] ; handle_gadget (gadget) struct Gadget *gadget ; { static short first_time = YES ; /*---------------------------------------------------------------------------- * Identify which gadget was selected by the user and take appropriate * action. *--------------------------------------------------------------------------- */ switch (gadget->GadgetID) { case X_SHOULDER_ANGLE: save_angle (SH, X, MAX_SH_X, MIN_SH_X, gadget) ; break ; case Y_SHOULDER_ANGLE: save_angle (SH, Y, MAX_SH_Y, MIN_SH_Y, gadget) ; break ; case Z_SHOULDER_ANGLE: save_angle (SH, Z, MAX_SH_Z, MIN_SH_Z, gadget) ; break ; case X_WRIST_ANGLE: save_angle (WR, X, MAX_WR_X, MIN_WR_X, gadget) ; break ; case Y_WRIST_ANGLE: save_angle (WR, Y, MAX_WR_Y, MIN_WR_Y, gadget) ; break ; case Z_WRIST_ANGLE: save_angle (WR, Z, MAX_WR_Z, MIN_WR_Z, gadget) ; break ; case X_ELBOW_ANGLE: save_angle (EL, X, MAX_EL_X, MIN_EL_X, gadget) ; break ; case REQ_SHOULDER_CANCEL: cancel_rotation (SH) ; if (menu_choices.goaltype == SET_ALL_JOINTS) Request (&get_elbow_angle, window) ; break ; case REQ_SHOULDER_TEST: test_rotate_joint (SH) ; break ; case REQ_SHOULDER_OK: if (menu_choices.goaltype == SET_ALL_JOINTS) Request (&get_elbow_angle, window) ; else rotate_joint (SH) ; break ; case REQ_WRIST_CANCEL: cancel_rotation (WR) ; if (menu_choices.goaltype == SET_ALL_JOINTS) { process_set_all_joints_request () ; if (first_time) { first_time = NO ; Request (&anim_info, window) ; } } break ; case REQ_WRIST_TEST: test_rotate_joint (WR) ; break ; case REQ_WRIST_OK: if (menu_choices.goaltype == SET_ALL_JOINTS) { process_set_all_joints_request () ; if (first_time) { first_time = NO ; Request (&anim_info, window) ; } } else rotate_joint (WR) ; break ; case REQ_ELBOW_CANCEL: cancel_rotation (EL) ; if (menu_choices.goaltype == SET_ALL_JOINTS) Request (&get_wrist_angles, window) ; break ; case REQ_ELBOW_TEST: test_rotate_joint (EL) ; break ; case REQ_ELBOW_OK: if (menu_choices.goaltype == SET_ALL_JOINTS) Request (&get_wrist_angles, window) ; else rotate_joint (EL) ; break ; case ANIM_INFO: break ; case NO_CHANGE: break ; case ANIM_CANCEL_SET_YES: cancel_all_joint_rotations () ; Request (&get_shoulder_angles, window) ; break ; case ANIM_CANCEL_SET_NO: break ; case ANIM_CANCEL_ROT_YES: cancel_all_joint_rotations () ; menu_choices.goaltype = ROTATE ; Request (&get_shoulder_angles, window) ; break ; case ANIM_CANCEL_ROT_NO: break ; case ROT_INFO: break ; default: close_down ("Error: Bad gadget id\n") ; break ; } } save_angle (joint, axis, max_angle, min_angle, gadget) int joint, axis, max_angle, min_angle ; struct Gadget *gadget ; { int slider_value ; int new_angle ; USHORT temp ; /*--------------------------------------------------------------------------- * Store the new joint angle selected by the user for future use. * The value retrieved from the HorizPot field of the proportional gadget is * converted to a usable floating point (FFP) value. * Store the slider value as well, in case the user cancels this joint * alteration later with the cancel gadget. *--------------------------------------------------------------------------- */ temp = ((struct PropInfo *) gadget->SpecialInfo)->HorizPot ; rot_info.angle [joint][axis].new_pot = temp ; slider_value = temp ; new_angle = min_angle + (((max_angle - min_angle) * slider_value) / 0xffff) ; rot_info.angle [joint][axis].final = SPFlt (new_angle) ; rot_info.angle [joint][axis].current = rot_info.angle [joint][axis].final ; rot_info.angle [joint][axis].changed = YES ; } rotate_joint (joint) int joint ; { /*--------------------------------------------------------------------------- * The user has selected a "use" gadget for a joint angle. * Calculate a new local tranformation for this link and redraw the display. *--------------------------------------------------------------------------- */ SetRast (window->RPort, COLOR0) ; fix_joint_transformation (joint) ; transform_links () ; set_screen_coords () ; draw_image (window->RPort) ; if (menu_choices.viewtype != PARALLEL) change_persp_option (ON, YES) ; reset_angle_info (joint) ; } fix_joint_transformation (joint) int joint ; { int trans [4] [4][4] ; int i ; /*--------------------------------------------------------------------------- * Calculate a new local tranformation for an arm link using the altered * joint angle(s). Include the transformations to and from the origin for * this particular link. *--------------------------------------------------------------------------- */ if (joint != EL) { for (i = X; i <= Z; ++i) get_trans (rot_info.angle [joint][i].current, i, trans [i]) ; matrix_mult (trans [Z], trans [Y], trans [TEMP]) ; matrix_mult (trans [TEMP], trans [X], rot_info.arm_trans [joint]) ; } else get_trans (rot_info.angle [EL][X].current, X, rot_info.arm_trans [EL]) ; matrix_mult (rot_info.trans_to_origin [joint], rot_info.arm_trans [joint], trans [TEMP]) ; matrix_mult (trans [TEMP], rot_info.trans_back [joint], rot_info.arm_trans [joint]) ; } transform_links () { int temp_trans [4][4], trans_EL [4][4], trans_WR [4][4] ; /*--------------------------------------------------------------------------- * Concatenate all the required transformations to display the arm, using * the current local transformations for each link. * Tranformations are cumulative for each link. The elbow transformation * is post-multiplied by the shoulder transformation, and the wrist * transformation is post-multiplied by the elbow, and then the shoulder * transformation. * One link's points are transformed at a time using these concatenated * transformations. *--------------------------------------------------------------------------- */ one_link_transform (SH, rot_info.arm_trans [SH]) ; matrix_mult (rot_info.arm_trans [EL], rot_info.arm_trans [SH], trans_EL) ; one_link_transform (EL, trans_EL) ; matrix_mult (rot_info.arm_trans [WR], rot_info.arm_trans [EL], temp_trans) ; matrix_mult (temp_trans, rot_info.arm_trans [SH], trans_WR) ; one_link_transform (WR, trans_WR) ; } one_link_transform (joint, trans) int joint ; int trans [4][4] ; { FACE_PTR f ; int final_trans [4][4] ; FFP_POINT_PTR p_curr, p_orig ; int i, j, ; /*--------------------------------------------------------------------------- * Add in the current viewing transformation then transform the vertices * (and reference axes points) for a particular link. *--------------------------------------------------------------------------- */ matrix_mult (trans, rot_info.curr_view_trans, final_trans) ; f = &arm.link[joint].face[0] ; for (i = 0; i < arm.link [joint].num_faces; ++i) { p_orig = &f->ffp_orig_vertex [0] ; p_curr = &f->ffp_curr_vertex [0] ; for (j = 0; j < f->num_vertices; ++j) transform_link_point (p_curr++, p_orig++, final_trans) ; ++f ; } transform_ref_axes (joint, final_trans) ; } transform_link_point (p_curr, p_orig, trans) int p_curr [], p_orig [] ; int trans [4][4] ; { int i, j ; /*--------------------------------------------------------------------------- * Transform a single point by the specified transformation. *--------------------------------------------------------------------------- */ for (i = X; i <= W; ++i) p_curr [i] = ffp.zero.i ; for (i = X; i <= W; ++i) for (j = X; j <= W; ++j) p_curr [i] = SPAdd (p_curr [i], SPMul (p_orig [j], trans [j][i])) ; } transform_ref_axes (joint, trans) int joint ; int trans [4][4] ; { FFP_POINT_PTR p_orig, p_curr ; int i ; /*--------------------------------------------------------------------------- * Transform all the reference axes points with the final transformation. *--------------------------------------------------------------------------- */ p_curr = &ref_axes [joint].curr_coord [0] ; p_orig = &ref_axes [joint].orig_coord [0] ; for (i = X; i <= ORIGIN; ++i) transform_link_point (p_curr++, p_orig++, trans) ; } get_trans (angle, axis, trans) int angle, axis ; int trans [4][4] ; { int cos, sin ; /*--------------------------------------------------------------------------- * Calculate a rotation transformation for a specified angle and axis. * The Motorola FFP SPSincos routine calculates both a sine and cosine for * an angle (in radians) in less time than doing both separately. *--------------------------------------------------------------------------- */ sin = SPSincos (&cos, SPMul (ffp.radians, angle)) ; set_to_identity_matrix (trans) ; if (axis == X) { trans [1][1] = cos ; trans [2][2] = cos ; trans [2][1] = sin ; trans [1][2] = SPNeg (sin) ; } else { if (axis == Y) { trans [0][0] = cos ; trans [2][2] = cos ; trans [2][0] = SPNeg (sin) ; trans [0][2] = sin ; } else { /* axis == Z */ trans [0][0] = cos ; trans [1][1] = cos ; trans [1][0] = sin ; trans [0][1] = SPNeg (sin) ; } } } cancel_all_joint_rotations () { int i ; /*--------------------------------------------------------------------------- * The user has aborted all joint settings specified with "set all joints". * Restore the previous values. *--------------------------------------------------------------------------- */ for (i = 0; i < NUM_LINKS; ++i) cancel_rotation (i) ; rot_info.num_frames = ffp.zero.i ; rot_info.max_angle_change = ffp.zero.i ; } cancel_rotation (joint) int joint ; { int i ; /*--------------------------------------------------------------------------- * Cancel the joint changes for one joint and restore the old values. * The proportional gadget slider pots are restored as well. *--------------------------------------------------------------------------- */ for (i = X; i <= Z; ++i) { if (rot_info.angle [joint][i].changed) { rot_info.angle [joint][i].final = rot_info.angle [joint][i].start ; rot_info.angle [joint][i].current = rot_info.angle [joint][i].start ; rot_info.angle [joint][i].incr = ffp.zero.i ; rot_info.angle [joint][i].changed = NO ; restore_pot_value (joint, i, rot_info.angle [joint][i].old_pot) ; } } } update_joint_transformations () { int i ; /*--------------------------------------------------------------------------- * After an animation sequence is over, update or reset all angle and * rotation parameters. The final angle specified for the animation * by the user is used to redraw the window display, rather than the * incremented current angle used in the animation. Congruency of display * appearance and slider pot values is preserved. *--------------------------------------------------------------------------- */ for (i = 0; i < NUM_LINKS; ++i) reset_angle_info (i) ; rot_info.num_frames = ffp.zero.i ; rot_info.max_angle_change = ffp.zero.i ; fix_all_joint_transformations () ; } reset_angle_info (joint) int joint ; { int i ; /*--------------------------------------------------------------------------- * After an animation sequence is over, update or reset all angle and * rotation parameters for a particular joint. *--------------------------------------------------------------------------- */ for (i = X; i <= Z; ++i) { if (rot_info.angle [joint][i].changed) { rot_info.angle [joint][i].changed = NO ; rot_info.angle [joint][i].current = rot_info.angle [joint][i].final ; rot_info.angle [joint][i].incr = ffp.zero.i ; rot_info.angle [joint][i].start = rot_info.angle [joint][i].final ; rot_info.angle [joint][i].old_pot = rot_info.angle [joint][i].new_pot ; } } } fix_all_joint_transformations () { /*--------------------------------------------------------------------------- * The current joint angles have been altered. Re-calculate all the * joint rotation transformations that depend on them. *--------------------------------------------------------------------------- */ fix_joint_transformation (SH) ; fix_joint_transformation (EL) ; fix_joint_transformation (WR) ; } process_set_all_joints_request () { int i, j ; /*--------------------------------------------------------------------------- * The user is finished with "set all joints". Calculate the number of * frames, and joint angle increments required by each joint axis, in order * to animate the joint angle changes. * The number of frames of animation is equal to the maximum angle change * on all joint axes altered by the user, divided by a minimum angle * change per animation frame (currently set at 5 degrees). *--------------------------------------------------------------------------- */ find_max_angle_change () ; if (SPCmp (rot_info.max_angle_change, ffp.zero.i) != 0) { rot_info.num_frames = SPDiv (ffp.min_angle_change.i, rot_info.max_angle_change) ; for (i = 0; i < NUM_LINKS; ++i) { for (j = X; j <= Z; ++j) { rot_info.angle [i][j].incr = SPDiv (rot_info.num_frames, rot_info.angle [i][j].change) ; rot_info.angle [i][j].current = SPSub (rot_info.angle [i][j].incr, rot_info.angle [i][j].start) ; } } } } find_max_angle_change () { int temp, max_change = ffp.neg_one.i ; int i, j ; /*--------------------------------------------------------------------------- * Find the maximum angle change specified by the user in "set all joints". *--------------------------------------------------------------------------- */ for (i = 0; i < NUM_LINKS; ++i) { for (j = X; j <= Z; ++j) { temp = SPSub (rot_info.angle [i][j].start, rot_info.angle [i][j].final) ; rot_info.angle [i][j].change = temp ; temp = SPAbs (temp) ; if (SPCmp (temp, max_change) == 1) max_change = temp ; } } rot_info.max_angle_change = max_change ; } set_screen_coords () { LINK_PTR l ; FACE_PTR f ; FFP_POINT_PTR p, prf ; int i, j, k ; int *x, *y, *c, *crf ; int scale_x, scale_y, center_x, center_y ; /*--------------------------------------------------------------------------- * Calculate the X and Y actual screen coordinates (ints) for writing * the arm image to a rastport. The arm image is centered on the screen * using the current maximum and minimum X and Y values for the whole arm. * Parameters to scale and center the image are calculated. *--------------------------------------------------------------------------- */ find_max_min_coords (X) ; find_max_min_coords (Y) ; compute_display_parameters (&scale_x, &scale_y, ¢er_x, ¢er_y) ; l = &arm.link [UPPER] ; for (k = 0; k < NUM_LINKS; ++k) { f = &l->face[0] ; for (i = 0; i < l->num_faces; ++i) { p = &f->ffp_curr_vertex [0] ; x = &f->x [0] ; y = &f->y [0] ; for (j = 0; j < f->num_vertices; ++j) { c = (int *) p++ ; *x++ = SPFix (SPMul (*(c+X), scale_x)) + center_x ; *y++ = SPFix (SPMul (*(c+Y), scale_y)) + center_y ; } ++f ; } prf = &ref_axes [k].curr_coord [0] ; x = &ref_axes [k].scrn_x [0] ; y = &ref_axes [k].scrn_y [0] ; for (j = X; j <= ORIGIN; ++j) { crf = (int *) prf++ ; *x++ = SPFix (SPMul (*(crf+X), scale_x)) + center_x ; *y++ = SPFix (SPMul (*(crf+Y), scale_y)) + center_y ; } ++l ; } } compute_display_parameters (scale_x, scale_y, center_x, center_y) int *scale_x, *scale_y ; int *center_x, *center_y ; { int range_x, range_y ; /*--------------------------------------------------------------------------- * Compute parameters to center the arm image on the screen. The * different vertical and horizontal pixel widths (640 x 200 pixels in a * 10 inch x 9 inch area) must be adjusted for as well. *--------------------------------------------------------------------------- */ range_x = SPSub (arm.amin [X], arm.amax [X]) ; range_y = SPSub (arm.amin [Y], arm.amax [Y]) ; if (SPCmp (range_x, range_y) == 1) { *scale_x = SPDiv (range_x, ffp.screen_fill_x.i) ; *scale_y = SPDiv (ffp.screen_scale_x.i, *scale_x) ; *center_x = SCREEN_START_X - SPFix (SPMul (*scale_x, arm.amin [X])) ; *center_y = SCREEN_CENTER_Y - SPFix (SPMul (SPAdd (SPDiv (ffp.two.i, range_y), arm.amin [Y]), *scale_y)) ; } else { *scale_y = SPDiv (range_y, ffp.screen_fill_y.i) ; *scale_x = SPMul (*scale_y, ffp.screen_scale_x.i) ; *center_y = SCREEN_START_Y - SPFix (SPMul (*scale_y, arm.amin [Y])) ; *center_x = SCREEN_CENTER_X - SPFix (SPMul (SPAdd (SPDiv (ffp.two.i, range_x), arm.amin [X]), *scale_x)) ; } } find_max_min_coords (axis) int axis ; { LINK_PTR l ; FACE_PTR f ; FFP_POINT_PTR p ; int *c ; int fmax, fmin, lmax, lmin, amax, amin ; int i, j, k, ; /*--------------------------------------------------------------------------- * Find the maximum amd minimum face, link and arm coordinates for the * specified axis. *--------------------------------------------------------------------------- */ amax = ffp.small_num.i ; amin = ffp.large_num.i ; l = &arm.link [UPPER] ; for (k = 0; k < NUM_LINKS; ++k) { lmax = ffp.small_num.i ; lmin = ffp.large_num.i ; f = &l->face[0] ; for (i = 0; i < l->num_faces; ++i) { fmax = ffp.small_num.i ; fmin = f->ffp_curr_vertex [0][axis] ; p = &f->ffp_curr_vertex [0] ; for (j = 0; j < f->num_vertices; ++j) { c = ((int *) p)+axis ; if (SPCmp (*c, fmax) == 1) fmax = *c ; else if (SPCmp (fmin, *c) == 1) fmin = *c ; ++p ; } f->fmax [axis] = fmax ; f->fmin [axis] = fmin ; if (SPCmp (fmax, lmax) == 1) lmax = fmax ; if (SPCmp (lmin, fmin) == 1) lmin = fmin ; ++f ; } l->lmax [axis] = lmax ; l->lmin [axis] = lmin ; if (SPCmp (lmax, amax) == 1) amax = lmax ; if (SPCmp (amin, lmin) == 1) amin = lmin ; ++l ; } arm.amax [axis] = amax ; arm.amin [axis] = amin ; } test_rotate_joint (joint) int joint ; { /* dummy routine - not yet implemented */ }