/* new window size or exposure */
static void
reshape(int width, int height)
{
GLfloat h = (GLfloat) height / (GLfloat) width;
glViewport(0, 0, (GLint) width, (GLint) height);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glFrustum(-1.0, 1.0, -h, h, 1.0, 320000.0);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
Start_Log();
RDRAM=databuffer;
if(mode==0)
{
ProcessDisplayListSW(entrypoint);
}
else if(mode>0)
{
DrawLinks();
}
Stop_Log();
}
static void
draw(void)
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glEnable(GL_DEPTH_TEST);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
gluLookAt(camera[3*4+0], camera[3*4+1], camera[3*4+2],
camera[3*4+0]+camera[0*4+0],
camera[3*4+1]+camera[0*4+1],
camera[3*4+2]+camera[0*4+2],
camera[2*4+0],
camera[2*4+1],
camera[2*4+2]);
glPushMatrix();
// draw z64 stuff
RDRAM=databuffer;
if(mode==0)
{
ProcessDisplayListSW(entrypoint);
}
else if(mode>0)
{
DrawLinks();
}
glPopMatrix();
SDL_GL_SwapBuffers();
waitTime(20);
Frames++;
}
/***********************************************************************
HandleMoveStructure() - this function computes the new positions of all
the links in the hierarchy using the inverse kinematic
approach.
***********************************************************************/
int
HandleMoveStructure(MyProgram *data)
{
/* compute positional and angular velocities of end effector
*/
/* propagate velocities down the hierarchy
*/
/* redraw the link hierarchy
*/
DrawLinks(data);
return(0);
} /* end of HandleMoveStructure() */
/***********************************************************************
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() */
/***********************************************************************
LoadAnimFile() - this function loads the definition input file which
describes the link hierarchy. This function also
precomputes the initial link data. The format of the file
is as follows:
format:
num_links
type_link ox oy oz tl vl rx ry rz
end_type
motion_type
num_frames_per_cycle
l1_zrot ... ln_zrot
where
num_links - total number of links in hierarchy (int)
type_link - 4 character keyword (root, link, or brch)
ox, oy, oz - origin of link (only valid if root) (float triplet)
tl - total length of link (float)
vl - visible length of link (float)
rx, ry, rz - rotation about each axis (float degrees)
end_type - 6 character keyword (square, circle, or triang)
motion_type - 6 character keyword (manual, linear, circle, or random)
num_frames - number of frames per animation cycle (0 = no table)
ln_zrot - z axis rotation for nth link (one value per link)
example:
4
root -20.0 0 0 10.0 10.0 0 0 0.0
link 0 0 0 10.0 10.0 0 0 10.0
link 0 0 0 10.0 10.0 0 0 20.0
link 0 0 0 10.0 10.0 0 0 30.0
square
manual
6
0.0 10.0 20.0 30.0
2.0 14.0 26.0 38.0
4.0 18.0 32.0 46.0
6.0 22.0 38.0 54.0
4.0 18.0 32.0 46.0
2.0 14.0 26.0 38.0
This specifies a single branch hierarchy of 4 links where each link is
the same length, each link is fully visible, and each link is only rotated
about the z axis by a relative amount of 10 degrees. The end effector is
a square and will be manually manipulated. A predefined animation cycle
will consist of 6 frames, which will generate a wagging effect.
***********************************************************************/
int
LoadAnimFile(char *filename, MyProgram *data)
{
FILE *infile;
int i, parse_count;
int n; /* number of links in hierarchy */
int nfpc; /* number frames per cycle */
char tempbuf[MAX_BUF_LEN]; /* temporary text buffer */
char *buf; /* pointer to primitive string */
char *parm_array[NUM_LINK_PARMS]; /* array of parameter strings */
char linktype[4];
float parms[8];
pointType MapWPointToScreen();
/* open the input file
*/
infile = fopen(filename, "r");
if (infile == (FILE *) NULL) {
fprintf(stderr, "LoadAnimFile: couldn't open input file\n");
return(1);
}
/* get number of links
*/
fscanf(infile, "%d\n", &n);
data->num_links = n;
printf("read n = %d\n", n);
/* read in all the links
*/
for (i = 0; i < n; i++) {
buf = fgets(tempbuf, MAX_BUF_LEN, infile); /* read line from file */
data->links[i].type = LINK_UNKN;
/* determine link type */
if (strncmp(buf, "root", 4) == 0) /* root link */
data->links[i].type = LINK_ROOT;
if (strncmp(buf, "link", 4) == 0) /* normal link */
data->links[i].type = LINK_LINK;
if (strncmp(buf, "brch", 4) == 0) /* branch link */
data->links[i].type = LINK_BRCH;
if (data->links[i].type != LINK_UNKN) {
/* parse buffer for link parameters */
/* parse_count = ParseString(buf, parm_array);
*/
sscanf(buf, "%4c %f %f %f %f %f %f %f %f %f\n", linktype, &parms[0],
&parms[1], &parms[2], &parms[3], &parms[4], &parms[5],
&parms[6], &parms[7]);
data->links[i].w_start.x = parms[0];
data->links[i].w_start.y = parms[1];
data->links[i].w_start.z = parms[2];
data->links[i].total_length = parms[3];
data->links[i].visible_length = parms[4];
data->links[i].xrot_deg = parms[5];
data->links[i].yrot_deg = parms[6];
data->links[i].zrot_deg = parms[7];
/* save link parameters for this link */
/* if (parse_count != NUM_LINK_PARMS) {
data->links[i].w_start.x = atof(parm_array[1]);
data->links[i].w_start.y = atof(parm_array[2]);
data->links[i].w_start.z = atof(parm_array[3]);
data->links[i].total_length = atof(parm_array[4]);
data->links[i].visible_length = atof(parm_array[5]);
data->links[i].xrot_deg = atof(parm_array[6]);
data->links[i].yrot_deg = atof(parm_array[7]);
data->links[i].zrot_deg = atof(parm_array[8]);
}
else {
fprintf(stderr, "LoadAnimFile: incorrect number of parameters\n");
return(1);
}
*/
/* compute other needed parameters for this link */
ComputeInitLink(i, data);
}
else {
fprintf(stderr, "LoadAnimFile: unknown link type read\n");
return(1);
}
}
/* read in and determine the end effector type
*/
buf = fgets(tempbuf, MAX_BUF_LEN, infile); /* read line from file */
if (strncmp(buf, "square", 6) == 0) /* square end effector */
data->end_effect.type = ENDEFF_SQUARE;
else if (strncmp(buf, "circle", 6) == 0) /* circle end effector */
data->end_effect.type = ENDEFF_CIRCLE;
else if (strncmp(buf, "triang", 6) == 0) /* triangle end effector */
data->end_effect.type = ENDEFF_TRIANGLE;
else {
data->end_effect.type = ENDEFF_UNKNOWN;
fprintf(stderr, "LoadAnimFile: unknown end effector type read\n");
return(1);
}
/* read in the end effector motion type
*/
buf = fgets(tempbuf, MAX_BUF_LEN, infile); /* read line from file */
if (strncmp(buf, "manual", 6) == 0) /* manual motion type */
data->end_effect.motion = ENDMOT_MANUAL;
else if (strncmp(buf, "linear", 6) == 0) /* linear motion type */
data->end_effect.motion = ENDMOT_LINEAR;
else if (strncmp(buf, "circle", 6) == 0) /* circle motion type */
data->end_effect.motion = ENDMOT_CIRCLE;
else if (strncmp(buf, "random", 6) == 0) /* random motion type */
data->end_effect.motion = ENDMOT_RANDOM;
else {
data->end_effect.motion = ENDMOT_UNKNOWN;
fprintf(stderr, "LoadAnimFile: unknown end effector motion read\n");
return(1);
}
/* read in number of frames per cycle
*/
fscanf(infile, "%d\n", &nfpc);
data->anim.num_frames_per_cycle = nfpc;
/* if table exists, read it in
*/
if (nfpc > 0) {
for (i = 0; i < nfpc; i++) {
buf = fgets(tempbuf, MAX_BUF_LEN, infile);
sscanf(buf, "%f %f %f %f\n", &parms[0], &parms[1], &parms[2], &parms[3]);
data->anim.table[i][0] = parms[0]; /* currently, assume only 4 links */
data->anim.table[i][1] = parms[1];
data->anim.table[i][2] = parms[2];
data->anim.table[i][3] = parms[3];
}
}
/* compute end effector related stuff
*/
ComputeInitEndEffector(data);
/* draw initial state of links
*/
ClearDrawArea();
DrawLinks(data);
/* close the input file
*/
fclose(infile);
/* set data loaded flag
*/
data->dataloaded = TRUE;
return(0);
} /* end of LoadAnimFile() */
