walker.c

学习c++必备

#include <GL/glut.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#ifdef _WIN32
#include "win32_dirent.h"
/* Have to #undef LoadMenu or else Microsoft VC++ won't allow us to
   redefine it. */
#undef LoadMenu
#else
#include <dirent.h>
#endif
#include <assert.h>

#include "walker.h"



#define MAX(x,y) ((x) > (y) ? (x) : (y))
#define MIN(x,y) ((x) < (y) ? (x) : (y))

#define CYCLE_SIZE 100
#define CYCLE_STEP 1.0/CYCLE_SIZE
#define OVERSAMPLE 10

#define MAX_CPOINTS 34   /* 2 end point ones and 10 in the middle */

#define MAX_CSETNAMELEN 25

#define NUM_JOINTS 5

#define CSET_EXT ".cset"
#define CSET_EXTLEN 5

#include "walkviewer.h"


typedef enum { CMENU_QUIT, CMENU_CURVE, CMENU_HORIZ, CMENU_RESET,
               CMENU_WALK, CMENU_DONEEDIT, CMENU_SAVE, CMENU_LOAD,
	       CMENU_MIRROR
              } CurveMenuChoices;

typedef enum { WIRECUBE, SOLIDCUBE, CYLINDER1, CYLINDER2 } ModelTypes;

int GuyModel = SOLIDCUBE;

/***************************************************************/
/*************************** GLOBALS ***************************/
/***************************************************************/

GLuint HorizontalList, AxesList,     /* Display lists */
       CurveLists, ControlPtsLists;  /* Firsts of groups of display lists */

int CurveAsPoints   =  0,    /* Display curve as points? */
    DrawHorizontals =  0,    /* Draw horizontal lines?   */
    EditingCurve    = -1;    /* Editing what curve, -1 means none */

int CurveWindow,   /* Glut window id's to two top level windows */
    GuyWindow;

int MirrorLegs = 0;

int CurveMenu     = -1,
    CurveEditMenu = -1,
    StepSizeMenu  = -1,
    LoadMenu      = -1,
    SaveMenu      = -1;

char *CSetNames[MAX_CSETNAMELEN];

int CurrentCurve = -1;  /* Curve loaded, index to CSetNames */

GLfloat Walk_cycle[2][NUM_JOINTS][CYCLE_SIZE];  /* array of computed angles */

int Step = CYCLE_SIZE/2;  /* Position in cycle, start in middle */

float fStep = CYCLE_SIZE/2;   /* floating point for non-integer steping */
float IncStep = 1.0;

typedef struct ControlPts {
  int numpoints;
  float xcoords[MAX_CPOINTS];
  float angles[MAX_CPOINTS];
} tControlPts;

tControlPts RotCurve[NUM_JOINTS];   /* series of cntrl points for ea joint */

int Walking         = 0,     /* Guy is walking? */
    ViewPerspective = 1,     /* Perspective or orthographic views */
    DrawAxes        = 0;     /* Draw axes for alignment */

int CurveWWidth,             /* Dimensions of curve window */
    CurveWHeight;  

int CurveDownBtn = -1,               /* mouse stuff, for editing curves */
    WasWalking,
    CurvePickedPoint = -1,
    CurveLastX,
    CurveLastY;

int CurveWindowVisible = 1;


  /* prototypes */
void RedisplayBoth(void);
void IncrementStep(void);
void CurveCPointDrag(void);
void CurveHandleMenu(int value);
void StopWalking(void);
void CurveHandleEditMenu(int curve);
void ComputeCSetAndMakeLists(void);
int MakeLoadAndSaveMenus(void);
void CurveMenuInit(void);
void SetWindowTitles(char *csetname);

/***************************************************************/
/**************************** BEZIER ***************************/
/***************************************************************/

  /* Matrix times a vector  dest = m*v */
void MultMV(float m[3][4], float v[4], float dest[3])
{
  int i, j;

  for (i = 0; i < 3; i++) {
    dest[i] = 0;
    for (j = 0; j < 4; j++)
      dest[i] += m[i][j] * v[j];
  }
}

  /* Matrix multiplication, dest = m1*m2 */
void MultM(float m1[3][4], float m2[4][4], float dest[3][4])
{
  int i, j, k;

  for (i = 0; i < 3; i++)
    for (j = 0; j < 4; j++) {
      dest[i][j] = 0;
      for (k = 0; k < 4; k++)
	  dest[i][j] += (m1[i][k] * m2[k][j]);
    }
}

void ComputeCurve(int joint)
{
  float prod[3][4], tm[4], pos[3];
  float t = 0, tinc = (float)CYCLE_STEP/OVERSAMPLE;
  int ctlpoint, i;
  float BBasis[4][4] = {{-1, 3, -3, 1}, {3, -6, 3, 0},
                        {-3, 3,  0, 0}, {1,  0, 0, 0}};
  int lastindex, newindex;

  float pointset[3][4];
  for (i = 0; i < 4; i++)   /* z's are always zero, only 2-d */
    pointset[2][i] = 0; 

  lastindex = -1;
  for(ctlpoint = 0; ctlpoint < RotCurve[joint].numpoints; ctlpoint += 3) {
    t = 0;
    for (i = 0; i < 4; i++)
      pointset[0][i] = RotCurve[joint].xcoords[ctlpoint + i];
    for (i = 0; i < 4; i++)
      pointset[1][i] = RotCurve[joint].angles[ctlpoint + i];

    MultM(pointset, BBasis, prod);

    while (t <= 1) {
      tm[0] = t*t*t;
      tm[1] = t*t;
      tm[2] = t;
      tm[3] = 1;
      MultMV(prod, tm, pos);
      newindex = (int)(pos[0]*(CYCLE_SIZE-1));
      if ((int)(newindex > lastindex))  {  /* go at least one */
	Walk_cycle[0][joint][newindex] = pos[1];
	lastindex++;
      } 
      t += tinc;
    }
  }

  for (i = 0; i < CYCLE_SIZE; i++) {      /* copy to other leg, out-o-phase */
    if (MirrorLegs)
      Walk_cycle[1][joint][i] =
        Walk_cycle[0][joint][i];
    else
      Walk_cycle[1][joint][i] =
        Walk_cycle[0][joint][(i+(CYCLE_SIZE/2))%CYCLE_SIZE];
  }
}

/***************************************************************/
/************************* CURVE I/O ***************************/
/***************************************************************/

void FlatCSet(void)
{
  int joint;

  for (joint = 0; joint < NUM_JOINTS; joint++) {
    RotCurve[joint].numpoints = 4;
    RotCurve[joint].xcoords[0] = 0.0;
    RotCurve[joint].angles[0]  = 0.0;
    RotCurve[joint].xcoords[1] = 0.2;
    RotCurve[joint].angles[1]  = 0.0;
    RotCurve[joint].xcoords[2] = 0.8;
    RotCurve[joint].angles[2]  = 0.0;
    RotCurve[joint].xcoords[3] = 1.0;
    RotCurve[joint].angles[3]  = 0.0;
  }
}

int ReadCSetFromFile(char *filename)
{
  FILE *infile = fopen(filename, "r");
  int numjoints, numpoints, joint, point, mirrorlegs;
  float value;

  if (infile == NULL)
    goto abort;
  
  if (!fscanf(infile, " %d", &numjoints) || numjoints != NUM_JOINTS)
    goto abort;

  if (!fscanf(infile, " %d", &mirrorlegs) || (mirrorlegs != 0 &&
                                              mirrorlegs != 1))
    goto abort;

  MirrorLegs = mirrorlegs;

  for (joint = 0; joint < NUM_JOINTS; joint++) {
    if (!fscanf(infile, " %d", &numpoints) || numpoints < 4 || 
                                              numpoints > MAX_CPOINTS)
      goto abort;
    RotCurve[joint].numpoints = numpoints;
    for (point = 0; point < numpoints; point++) {
      if (!fscanf(infile, " %f", &value))
	goto abort;
      RotCurve[joint].xcoords[point] = value;
    }
    for (point = 0; point < numpoints; point++) {
      if (!fscanf(infile, " %f", &value))
	goto abort;
      RotCurve[joint].angles[point] = value;
    }
  }

  fclose(infile);
  return 0;

  abort:
    fclose(infile);
    fprintf(stderr, "Something went wrong while reading file %s\n", filename);
    FlatCSet();
    return -1;
}

void WriteCSetToFile(char *filename)
{
  FILE *outfile = fopen(filename, "w+");
  int joint, point;

  if (outfile == NULL) {
    fprintf(stderr, "Error: could not create file %s\n", filename);
    return;
  }

  fprintf(outfile, "%d\n", NUM_JOINTS);

  fprintf(outfile, "%d\n", MirrorLegs);

  for (joint = 0; joint < NUM_JOINTS; joint++) {

    fprintf(outfile, "%d\n", RotCurve[joint].numpoints);

    for (point = 0; point < RotCurve[joint].numpoints; point++) {
      fprintf(outfile, "%f ", RotCurve[joint].xcoords[point]);
    }
    fprintf(outfile, "\n");

    for (point = 0; point < RotCurve[joint].numpoints; point++) {
      fprintf(outfile, "%f ", RotCurve[joint].angles[point]);
    }
    fprintf(outfile, "\n");
  }

  fclose(outfile);
}

void HandleLoadMenu(int cset)
{
  char filename[MAX_CSETNAMELEN + CSET_EXTLEN + 1];

  if (cset == -1) {
    MakeLoadAndSaveMenus();
    CurveMenuInit();
  } else {
    (void)strcpy(filename, CSetNames[cset]);
    (void)strcat(filename, CSET_EXT);
    if (ReadCSetFromFile(filename) == 0) {
      glutSetMenu(SaveMenu);
      glutChangeToMenuEntry(1, CSetNames[cset], cset);
      ComputeCSetAndMakeLists();
      SetWindowTitles(CSetNames[cset]);
      RedisplayBoth();
    }
  }
}

void HandleSaveMenu(int cset)
{
  char filename[MAX_CSETNAMELEN + CSET_EXTLEN + 1];

  (void)strcpy(filename, CSetNames[cset]);
  (void)strcat(filename, CSET_EXT);
  WriteCSetToFile(filename);
  ComputeCSetAndMakeLists();
  RedisplayBoth();
}

int MakeLoadAndSaveMenus(void)
{
  DIR *dirp = opendir(".");
  struct dirent *direntp;
  int csetnum = 0;
  char *newcsetname;

  if (LoadMenu != -1)
    glutDestroyMenu(LoadMenu);
  if (SaveMenu != -1)
    glutDestroyMenu(SaveMenu);

  SaveMenu = glutCreateMenu(HandleSaveMenu);
  LoadMenu = glutCreateMenu(HandleLoadMenu);

  if (dirp == NULL) {
    fprintf(stderr, "Error opening current dir in MakeLoadAndSaveMenus\n");
    return(0);
  }

  while ((direntp = readdir(dirp)) != NULL) {
    char *ext = direntp->d_name + (strlen(direntp->d_name) - CSET_EXTLEN);
    if (!strcmp(ext, CSET_EXT)) {
      newcsetname = malloc(strlen(direntp->d_name) - CSET_EXTLEN + 1);
      strncpy(newcsetname, direntp->d_name,
	      strlen(direntp->d_name) - CSET_EXTLEN);
      newcsetname[strlen(direntp->d_name) - CSET_EXTLEN] = 0;
      CSetNames[csetnum] = newcsetname;
      glutAddMenuEntry(newcsetname, csetnum++);
    }
  }
  closedir(dirp);
  glutSetMenu(LoadMenu);
  glutAddMenuEntry("-> Rescan Directory <-", -1);
  glutSetMenu(SaveMenu);
  CSetNames[csetnum] = "NewCurve0";
  glutAddMenuEntry("NewCurve0", csetnum++);
  CSetNames[csetnum] = "NewCurve1";
  glutAddMenuEntry("NewCurve1", csetnum++);
  CSetNames[csetnum] = "NewCurve2";
  glutAddMenuEntry("NewCurve2", csetnum++);

  return (csetnum - 2);  /* just indicate curves in Load menu */
}


/***************************************************************/
/********************* DISPLAY LISTS ***************************/
/***************************************************************/


void MakeCurveList(int joint)
{
  int i;

  glNewList(CurveLists+joint, GL_COMPILE);
  glColor3f(1, 1, 1);
  for (i = 0; i < CYCLE_SIZE; i++) {
    glVertex3f((GLfloat)i/CYCLE_SIZE, Walk_cycle[0][joint][i]/180, 0);
  }
  glEndList();
}

void MakeCPointList(int joint)
{
  int point;
 
  glNewList(ControlPtsLists+joint, GL_COMPILE);

    glColor3f(0, 0.4, 0);
    glBegin(GL_LINE_STRIP);
    for (point = 0; point < RotCurve[joint].numpoints; point++) {
      if (!((point-2) % 3)) {
        glEnd();
        glBegin(GL_LINE_STRIP);
      }
      glVertex3f(RotCurve[joint].xcoords[point],
	         (RotCurve[joint].angles[point])/180.0, 0.0);
    }
    glEnd();

    glBegin(GL_POINTS);
    for (point = 0; point < RotCurve[joint].numpoints; point++) {
      if (point % 3)
        glColor3f(0, 0.7, 0);
      else
        glColor3f(0.7, 0.0, 0);
      glVertex3f(RotCurve[joint].xcoords[point],
	         (RotCurve[joint].angles[point])/180, 0);
    }
    glEnd();

  glEndList();
}

void MakeJointLists(int joint)
{
  MakeCurveList(joint);
  MakeCPointList(joint);
}


void ComputeCSetAndMakeLists(void)
{
  int joint;

  for(joint = 0; joint < NUM_JOINTS; joint++) {
    ComputeCurve(joint);
    MakeJointLists(joint);
  }
}

void MakeLists(void)
{
  HorizontalList = glGenLists(1);
  glNewList(HorizontalList, GL_COMPILE);
  {
    float line1 = 25.0/180,
          line2 = 35.0/180,
          line3 = 45.0/180;
    glColor3f(0, 0, 0.7);
    glPushAttrib(GL_ENABLE_BIT);
    glEnable(GL_LINE_STIPPLE);
    glBegin(GL_LINES);
      glVertex3f(0, 0, 0.5);
      glVertex3f(1, 0, 0.5);
      glVertex3f(0, line1, 0.5);  glVertex3f(1, line1, 0.5);
      glVertex3f(0, -line1, 0.5); glVertex3f(1, -line1, 0.5);
      glVertex3f(0, line2, 0.5);  glVertex3f(1, line2, 0.5);
      glVertex3f(0, -line2, 0.5); glVertex3f(1, -line2, 0.5);
      glVertex3f(0, line3, 0.5);  glVertex3f(1, line3, 0.5);
      glVertex3f(0, -line3, 0.5); glVertex3f(1, -line3, 0.5);
    glEnd();
    glPopAttrib();
  }
  glEndList();


  CurveLists = glGenLists(NUM_JOINTS);
  assert(CurveLists != 0);

  ControlPtsLists = glGenLists(NUM_JOINTS);
  assert(ControlPtsLists != 0);

  ComputeCSetAndMakeLists();
}

/***************************************************************/
/********************* curve WINDOW ****************************/
/***************************************************************/

void CurveReshape(int w, int h)
{
  glViewport(0,0,w,h);
  CurveWWidth = w; 
  CurveWHeight = h;
  glFlush();
}

void CurveDisplay(void)
{
  int joint, otherlegstep;

  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

  glPushMatrix();
  glTranslatef(0, 0.5, 0);

  for (joint = NUM_JOINTS-1; joint >= 0; joint--) {

    if (DrawHorizontals) 
      glCallList(HorizontalList);

    (CurveAsPoints) ?
    glBegin(GL_POINTS) :
    glBegin(GL_LINE_STRIP);
      glCallList(CurveLists+joint);
    glEnd();

    if (joint == EditingCurve) { 
      glPointSize(5.0);
      glCallList(ControlPtsLists+EditingCurve);
      glPointSize(1.0);
    }
  glTranslatef(0, 1, 0);
  }

  glPopMatrix();

  otherlegstep = (Step+50) % CYCLE_SIZE;
    /* draw vertical line */
  glColor3f(1, 1, 1);
  glBegin(GL_LINES);
    glVertex3f((GLfloat)Step/CYCLE_SIZE, 0, 0);
    glVertex3f((GLfloat)Step/CYCLE_SIZE, NUM_JOINTS, 0);
    if (!MirrorLegs) {
      glVertex3f((GLfloat)otherlegstep/CYCLE_SIZE, 0, 0);
      glVertex3f((GLfloat)otherlegstep/CYCLE_SIZE, NUM_JOINTS, 0);
    }
  glEnd();

  glFlush();
  glutSwapBuffers();
}


void CurveGLInit(void)
{
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  glOrtho(0,1,0,NUM_JOINTS,1,-1);

  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();

  glLineStipple(1, 0x00FF);

  glEnable(GL_DEPTH_TEST);
  glDepthFunc(GL_LEQUAL);


  glFlush();

} 


/* ARGSUSED2 */
void CurveHandleButton(int button, int state, int x, int y)
{
  if (button == GLUT_RIGHT_BUTTON )
    return;

  if (state == GLUT_DOWN && CurveDownBtn == -1) {
    if (button == GLUT_MIDDLE_BUTTON)
      CurveHandleMenu(CMENU_WALK);