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;
}
}
}