bool CCharShape::TranslateNode (size_t node_name, const TVector3& vec) { TCharNode *node = GetNode (node_name); if (node == NULL) return false; TMatrix TransMatrix; MakeTranslationMatrix (TransMatrix, vec.x, vec.y, vec.z); MultiplyMatrices (node->trans, node->trans, TransMatrix); MakeTranslationMatrix (TransMatrix, -vec.x, -vec.y, -vec.z); MultiplyMatrices (node->invtrans, TransMatrix, node->invtrans); if (newActions && useActions) AddAction (node_name, 0, vec, 0); return true; }
/*********************************************************************** ComputeNextFrame() - this function moves the end effector along a predefined path, calls HandleMoveStructure to recompute the joint angles and redraw the structure. ***********************************************************************/ int ComputeNextFrame(MyProgram *data) { matrixType translation; int last_link, f, i; last_link = data->num_links - 1; /* move end effector along predefined path based on motion type */ switch(data->end_effect.motion) { case ENDMOT_LINEAR: translation = MakeTranslationMatrix(data->anim.linear_vel.x, data->anim.linear_vel.y, data->anim.linear_vel.z); data->anim.new_position = TransformWPoint(&data->links[last_link].w_end, &translation); /* recompute joint angles and redraw */ HandleMoveStructure(data); break; case ENDMOT_CIRCLE: /* recompute joint angles and redraw */ HandleMoveStructure(data); break; case ENDMOT_RANDOM: data->anim.random_vel.x = nrand(); data->anim.random_vel.y = nrand(); data->anim.random_vel.z = 0.0; /* recompute joint angles and redraw */ HandleMoveStructure(data); break; case ENDMOT_FW_TABLE: /* forward kinematics example */ /* erase previous drawing of links */ if (data->redraw == TRUE) DrawLinks(data); f = data->frame_count % data->anim.num_frames_per_cycle; /* read new state vector from table */ for (i = 0; i < data->num_links; i++) { data->links[i].zrot_deg = data->anim.table[f][i]; ComputeInitLink(i, data); } ComputeInitEndEffector(data); /* draw new links */ DrawLinks(data); break; case ENDMOT_FW_RANDOM: /* forward kinematics example */ /* erase previous drawing of links */ if (data->redraw == TRUE) DrawLinks(data); /* generate a randomized state vector */ for (i = 0; i < data->num_links; i++) { data->links[i].zrot_deg = data->links[i].zrot_deg + (5 * nrands()); ComputeInitLink(i, data); } ComputeInitEndEffector(data); /* draw new links */ DrawLinks(data); break; default: break; } /* increment frame count */ data->frame_count++; return(0); } /* end of ComputeNextFrame() */
/*********************************************************************** ComputeInitLink() - this function computes the initial end points, u,v,n vectors, and rotation matrix for the specified link. for the given link set visible end to origin + vislen set actual end to origin + totlen rotate both points by theta about z axis translate both points to actual end of previous link compute u,v,n vectors map end points to screen coordinates ***********************************************************************/ int ComputeInitLink(int link, MyProgram *data) { vectorType o2, o2prime, o3, o3prime; vectorType p1, p2, p3; vectorType u, v, vprime, n; double world[4]; double screen[4]; matrixType translation; matrixType rotation; matrixType screen_map; pointType MapWPointToScreen(wpointType *p, matrixType *map); /* set actual end to origin + totlen */ o3.x = data->links[link].total_length; o3.y = 0.0; o3.z = 0.0; /* set visible end to origin + vislen */ o2.x = data->links[link].visible_length; o2.y = 0.0; o2.z = 0.0; /* make rotation matrix */ rotation = MakeRotationMatrix('z', data->links[link].zrot_deg); MatrixCopy(&rotation, &data->links[link].rot_mat); /* rotate actual and visible end points around z axis */ o3prime = TransformVector(&o3, &rotation); /* o3prime = o3 * Rz(theta) */ o2prime = TransformVector(&o2, &rotation); /* o2prime = o2 * Rz(theta) */ /* set start of link to actual end of previous link if not root link */ if (data->links[link].type == LINK_ROOT) { /* root link */ p1.x = data->links[link].w_start.x; p1.y = data->links[link].w_start.y; p1.z = data->links[link].w_start.z; } else { /* normal link */ p1.x = data->links[link - 1].w_end.x; p1.y = data->links[link - 1].w_end.y; p1.z = data->links[link - 1].w_end.z; } /* make translation matrix */ translation = MakeTranslationMatrix(p1.x, p1.y, p1.z); /* translate actual end and visible end to final locations */ p3 = TransformVector(&o3prime, &translation); /* p3 = o3prime * T(ox,oy,oz) */ p2 = TransformVector(&o2prime, &translation); /* p2 = o2prime * T(ox,oy,oz) */ /* copy final end locations to link data */ data->links[link].w_end.x = p3.x; /* actual end */ data->links[link].w_end.y = p3.y; data->links[link].w_end.z = p3.z; data->links[link].w_visend.x = p2.x; /* visible end */ data->links[link].w_visend.y = p2.y; data->links[link].w_visend.z = p2.z; data->links[link].w_start.x = p1.x; /* start point */ data->links[link].w_start.y = p1.y; data->links[link].w_start.z = p1.z; /* compute u vector */ u = VecSub(&p1, &p3); /* u = p3 - p1 */ VecNormalize(&u); /* u = u / mag(u) */ /* compute n vector */ vprime.x = 0.0; vprime.y = 1.0; vprime.z = 0.0; n = VecCrossproduct(&u, &vprime); /* n = u X vprime */ /* compute v vector */ v = VecCrossproduct(&n, &u); /* v = n X u */ /* copy u,v,n vectors to link data */ data->links[link].u_vec.x = u.x; data->links[link].u_vec.y = u.y; data->links[link].u_vec.z = u.z; data->links[link].v_vec.x = v.x; data->links[link].v_vec.y = v.y; data->links[link].v_vec.z = v.z; data->links[link].n_vec.x = n.x; data->links[link].n_vec.y = n.y; data->links[link].n_vec.z = n.z; /* map start and end points to screen coordinates */ world[0] = WB_MINX; world[1] = WB_MINY; world[2] = WB_MAXX; world[3] = WB_MAXY; screen[0] = 0.0; screen[1] = 0.0; screen[2] = SCREEN_X_SIZE; screen[3] = SCREEN_Y_SIZE; screen_map = MakeMappingMatrix(world, screen); data->links[link].start = MapWPointToScreen( &data->links[link].w_start, &screen_map); data->links[link].end = MapWPointToScreen( &data->links[link].w_end, &screen_map); data->links[link].visend = MapWPointToScreen( &data->links[link].w_visend, &screen_map); #ifdef INVKINE_DEBUG OutputLink(stdout, data, link); #endif return(0); } /* end of ComputeInitLink() */
void CCharShape::RefreshNode (size_t idx) { if (idx >= numNodes) return; TMatrix TempMatrix; char caxis; double angle; TCharNode *node = Nodes[idx]; TCharAction *act = node->action; if (act == NULL) return; if (act->num < 1) return; MakeIdentityMatrix (node->trans); MakeIdentityMatrix (node->invtrans); for (size_t i=0; i<act->num; i++) { int type = act->type[i]; const TVector3& vec = act->vec[i]; double dval = act->dval[i]; switch (type) { case 0: MakeTranslationMatrix (TempMatrix, vec.x, vec.y, vec.z); MultiplyMatrices (node->trans, node->trans, TempMatrix); MakeTranslationMatrix (TempMatrix, -vec.x, -vec.y, -vec.z); MultiplyMatrices (node->invtrans, TempMatrix, node->invtrans); break; case 1: caxis = 'x'; angle = dval; MakeRotationMatrix (TempMatrix, angle, caxis); MultiplyMatrices (node->trans, node->trans, TempMatrix); MakeRotationMatrix (TempMatrix, -angle, caxis); MultiplyMatrices (node->invtrans, TempMatrix, node->invtrans); break; case 2: caxis = 'y'; angle = dval; MakeRotationMatrix (TempMatrix, angle, caxis); MultiplyMatrices (node->trans, node->trans, TempMatrix); MakeRotationMatrix (TempMatrix, -angle, caxis); MultiplyMatrices (node->invtrans, TempMatrix, node->invtrans); break; case 3: caxis = 'z'; angle = dval; MakeRotationMatrix (TempMatrix, angle, caxis); MultiplyMatrices (node->trans, node->trans, TempMatrix); MakeRotationMatrix (TempMatrix, -angle, caxis); MultiplyMatrices (node->invtrans, TempMatrix, node->invtrans); break; case 4: MakeIdentityMatrix (TempMatrix); MultiplyMatrices (node->trans, node->trans, TempMatrix); MakeIdentityMatrix (TempMatrix); MultiplyMatrices (node->invtrans, TempMatrix, node->invtrans); MakeScalingMatrix (TempMatrix, vec.x, vec.y, vec.z); MultiplyMatrices (node->trans, node->trans, TempMatrix); MakeScalingMatrix (TempMatrix, 1.0 / vec.x, 1.0 / vec.y, 1.0 / vec.z); MultiplyMatrices (node->invtrans, TempMatrix, node->invtrans); MakeIdentityMatrix (TempMatrix); MultiplyMatrices (node->trans, node->trans, TempMatrix); MakeIdentityMatrix (TempMatrix); MultiplyMatrices (node->invtrans, TempMatrix, node->invtrans); break; case 5: VisibleNode (node->node_name, dval); break; default: break; } } }