/************************************************************************/ /************************************************************************/ // path1-execute.c: execute path generated by path1.c /************************************************************************/ /************************************************************************/ #include "stdafx.h" /* Windows stuff, make empty stdafx.h under Linux */ /************************************************************************/ #ifdef LINUX #include /* This allows us to do control in real time */ #endif #include #include #include #include "maze1.h" /************************************************************************/ /* DEFINES */ #ifndef LINUX #define M_PI 3.1415927 #endif /* simulation modes */ #define PAUSED 0 #define RUNNING 1 #define GAME_OVER 2 /************************************************************************/ /* General Globals */ #ifdef LINUX /* Stuff for figuring out what time it is on LINUX */ struct timeval tv_val; struct timeval tv_val_last; struct timezone tz_val; #endif /************************************************************************/ /* GLUT/OPENGL Globals */ float view[4] = { -1.0, -1.0, 1.0, 1.0 }; GLsizei window_widthi, window_heighti; GLfloat window_widthf, window_heightf; float mouse_display[2] = { 0.0, 0.0 }; #define BACKGROUND_LIST 1 #define BALL_LIST 2 #define MAX_WINDOW_SIZE 400 #define N_POINTS_IN_CIRCLE 16 /* How circular are our holes? */ /************************************************************************/ /* Globals */ BOARD a_board; BOARD *the_board; #define MAX_N_PATH1_STEPS 10000 int n_path1_steps = 0; float path1_steps[MAX_N_PATH1_STEPS][2]; float path1_duration = 0.0; float path1_start_time = 0.0; float x_d = 0; float y_d = 0; float force_display_scale = 1.0; float force[2]; int simulation_mode = PAUSED; /************************************************************************/ int read_path1( char *filename ) { FILE *stream; stream = fopen( filename, "r" ); if ( stream == NULL ) { fprintf( stderr, "Can't open file %s for read.\n", filename ); exit( -1 ); } n_path1_steps = 0; if ( fscanf( stream, "%g", &path1_duration ) < 1 ) { fprintf( stderr, "No numbers in path1 file %s\n", filename ); exit( -1 ); } for( ; ; ) { if ( fscanf( stream, "%g%g\n", &(path1_steps[n_path1_steps][XX]), &(path1_steps[n_path1_steps][YY]) ) < 2 ) break; n_path1_steps++; if ( n_path1_steps >= MAX_N_PATH1_STEPS ) { fprintf( stderr, "Need to increase MAX_N_PATH1_STEPS: %d\n", MAX_N_PATH1_STEPS ); exit( -1 ); } } printf( "read %s: %g seconds duration, %d steps.\n", filename, path1_duration, n_path1_steps ); } /************************************************************************/ /* Generate forces on the ball */ int controller( BOARD *b ) { int path1_index; float path1_time; float timestep; float position_gain = 10.0; float velocity_gain = 5.0; float t1, t2, w1, w2; if ( simulation_mode == RUNNING ) { path1_time = b->global_time - path1_start_time; timestep = path1_duration/(n_path1_steps - 1); path1_index = path1_time/timestep; if ( path1_index >= n_path1_steps - 1 ) { /* Done */ path1_index = n_path1_steps - 1; x_d = (b->goal.p[0] + b->goal.p[2])/2; y_d = (b->goal.p[1] + b->goal.p[3])/2; /* x_d = path1_steps[path1_index][XX]; y_d = path1_steps[path1_index][YY]; */ } else { /* Let's interpolate */ t1 = path1_index*timestep; t2 = t1 + timestep; w1 = (t2 - path1_time)/timestep; w2 = (path1_time - t1)/timestep; x_d = w1*path1_steps[path1_index][XX] + w2*path1_steps[path1_index+1][XX]; y_d = w1*path1_steps[path1_index][YY] + w2*path1_steps[path1_index+1][YY]; } force[XX] = -position_gain*(b->puck[XX] - x_d) - velocity_gain*( b->puck[XD] ); force[YY] = -position_gain*(b->puck[YY] - y_d) - velocity_gain*( b->puck[YD] ); b->force[XX] = force[XX]; b->force[YY] = force[YY]; return TRUE; } force[XX] = 0.0; force[YY] = 0.0; b->force[XX] = force[XX]; b->force[YY] = force[YY]; return TRUE; } /************************************************************************/ /************************************************************************/ int simulation_count = 0; void simulate( BOARD *b ) { float time_step, time_done; int i; controller( b ); #ifdef LINUX gettimeofday( &tv_val, &tz_val ); if ( tv_val.tv_sec == tv_val_last.tv_sec ) { time_step = (tv_val.tv_usec - tv_val_last.tv_usec)*(1.0e-6); } else if ( tv_val.tv_sec == tv_val_last.tv_sec + 1 ) { time_step = (1000000 + tv_val.tv_usec - tv_val_last.tv_usec)*(1.0e-6); } else { fprintf( stderr, "Real time clock error: %ld %ld\n", tv_val.tv_sec, tv_val_last.tv_sec ); exit( -1 ); } tv_val_last.tv_sec = tv_val.tv_sec; tv_val_last.tv_usec = tv_val.tv_usec; #else time_step = 0.01; #endif /* if ( simulation_count > 3 ) simulation_mode = GAME_OVER; simulation_count++; */ if ( simulation_mode == RUNNING ) { /* printf( "simulate: %g: %g %g %g %g\n", time_step, b->puck[XX], b->puck[YY], b->puck[XD], b->puck[YD] ); */ for( i = 0; i < 10000; i++ ) /* limit number of iterations */ { time_done = do_simulation( b, time_step ); time_step -= time_done; if ( in_hole( b, b->puck ) ) { simulation_mode = GAME_OVER; printf( "GAME OVER\n" ); break; } if ( time_step < 1e-4 ) break; if ( i > 100 ) printf( "%d integration steps/step.\n", i ); } } } /************************************************************************/ void display() { BOARD *b; b = the_board; simulate( b ); glClear( GL_COLOR_BUFFER_BIT ); glCallList( BACKGROUND_LIST ); if ( simulation_mode != GAME_OVER ) { glMatrixMode(GL_MODELVIEW); glLoadIdentity(); glTranslatef( b->puck[XX], b->puck[YY], 0.0 ); glCallList( BALL_LIST ); glLoadIdentity(); } /* Show desired position */ if ( simulation_mode != GAME_OVER ) { glColor3f( 1.0, 0.0, 0.0 ); glRectf( x_d - b->puck_radius/2, y_d - b->puck_radius/2, x_d + b->puck_radius/2, y_d + b->puck_radius/2 ); } /* overlay force visualization */ glColor3f( 0.0, 1.0, 0.0 ); glLineWidth( 2.0 ); glBegin( GL_LINES ); glVertex2f( (view[LEFT] + view[RIGHT])/2, (view[TOP] + view[BOTTOM])/2 ); glVertex2f( (view[LEFT] + view[RIGHT])/2 + force[XX]*force_display_scale, (view[TOP] + view[BOTTOM])/2 + force[YY]*force_display_scale ); glEnd(); glutSwapBuffers(); } /************************************************************************/ void init_display( BOARD *b ) { int i, j; float increment, angle; view[LEFT] = b->board.p[LEFT]; view[BOTTOM] = b->board.p[BOTTOM]; view[RIGHT] = b->board.p[RIGHT]; view[TOP] = b->board.p[TOP]; glMatrixMode( GL_PROJECTION ); glLoadIdentity(); gluOrtho2D( view[LEFT], view[RIGHT], view[BOTTOM], view[TOP] ); /* create background */ glNewList( BACKGROUND_LIST, GL_COMPILE_AND_EXECUTE ); glPushAttrib( GL_CURRENT_BIT ); /* board */ glColor3f( 255.0/255.0, 255.0/255.0, 198.0/255.0 ); glRectf( b->board.p[LEFT], b->board.p[BOTTOM], b->board.p[RIGHT], b->board.p[TOP] ); /* goal */ glColor3f( 1.0, 0.0, 1.0 ); glRectf( b->goal.p[LEFT], b->goal.p[BOTTOM], b->goal.p[RIGHT], b->goal.p[TOP] ); /* holes */ glColor3f( 0.0, 0.0, 0.0 ); increment = 2*M_PI/N_POINTS_IN_CIRCLE; for( i = 0; i < b->n_holes; i++ ) { glBegin( GL_TRIANGLE_FAN ); glVertex2f( b->holes[i].p[XX], b->holes[i].p[YY] ); glVertex2f( b->holes[i].p[XX] + b->holes[i].p[RADIUS], b->holes[i].p[YY] ); angle = increment; for( j = 0; j < N_POINTS_IN_CIRCLE-1; j++ ) { glVertex2f( b->holes[i].p[XX] + b->holes[i].p[RADIUS]*cos(angle), b->holes[i].p[YY] + b->holes[i].p[RADIUS]*sin(angle) ); angle += increment; } glVertex2f( b->holes[i].p[XX] + b->holes[i].p[RADIUS], b->holes[i].p[YY] ); glEnd(); } /* obstacles */ glColor3f( 225.0/255.0, 125.0/255.0, 50.0/255.0 ); for( i = 0; i < b->n_obstacles; i++ ) glRectf( b->obstacles[i].p[LEFT], b->obstacles[i].p[BOTTOM], b->obstacles[i].p[RIGHT], b->obstacles[i].p[TOP] ); /* end background */ glPopAttrib(); glEndList(); /* create the ball */ glNewList( BALL_LIST, GL_COMPILE ); glPushAttrib( GL_CURRENT_BIT ); glColor3f( 0.6, 0.6, 0.6 ); increment = 2*M_PI/N_POINTS_IN_CIRCLE; glBegin( GL_TRIANGLE_FAN ); glVertex2f( 0.0, 0.0 ); glVertex2f( b->puck_radius, 0.0 ); angle = increment; for( j = 0; j < N_POINTS_IN_CIRCLE-1; j++ ) { glVertex2f( b->puck_radius*cos(angle), b->puck_radius*sin(angle) ); angle += increment; } glVertex2f( b->puck_radius, 0.0 ); glEnd(); glPopAttrib(); glEndList(); /* end ball */ #ifdef LINUX gettimeofday( &tv_val_last, &tz_val ); #endif } /************************************************************************/ /* This needs to be fixed. */ void reshape( GLsizei w, GLsizei h ) { window_widthi = w; window_heighti = h; window_widthf = (GLfloat) w; window_heightf = (GLfloat) h; glMatrixMode( GL_PROJECTION ); glLoadIdentity(); /* if ( w <= h ) gluOrtho2D( view[LEFT], view[RIGHT], view[BOTTOM]*window_heightf/window_widthf, view[TOP]*window_heightf/window_widthf ); else gluOrtho2D( view[LEFT]*window_widthf/window_heightf, view[RIGHT]*window_widthf/window_heightf, view[BOTTOM], view[TOP] ); */ gluOrtho2D( view[LEFT], view[RIGHT], view[BOTTOM], view[TOP] ); /* glMatrixMode( GL_MODELVIEW ); */ glViewport( 0, 0, w, h ); } /************************************************************************/ void idle() { glutPostRedisplay(); } /************************************************************************/ /************************************************************************/ void keypress( unsigned char key, int x, int y ) { if ( key == 'q' || key == 'Q' || key == '\27' ) exit( 0 ); mouse_display[XX] = x; mouse_display[YY] = y; simulate( the_board ); } /************************************************************************/ void mouse( int button, int state, int x, int y ) { char b, d; if ( button == GLUT_LEFT_BUTTON ) b = 'L'; else if ( button == GLUT_RIGHT_BUTTON ) b = 'R'; else b = 'M'; if ( state == GLUT_UP ) d = 'U'; else d = 'D'; mouse_display[XX] = x; mouse_display[YY] = y; /* If you want to start it using the mouse. */ if ( simulation_mode == PAUSED && state == GLUT_DOWN ) { simulation_mode = RUNNING; path1_start_time = the_board->global_time; } simulate( the_board ); } /************************************************************************/ void mouse_motion( int x, int y ) { mouse_display[XX] = x; mouse_display[YY] = y; simulate( the_board ); } /************************************************************************/ int init_glut( int argc, char **argv, float ratio ) { int wx, wy; if ( ratio < 1.0 ) { wx = MAX_WINDOW_SIZE; wy = wx*ratio; } else { wy = MAX_WINDOW_SIZE; wx = wy/ratio; } /* printf( "wx: %d; wy: %d\n", wx, wy ); */ glutInit( &argc, argv ); glutInitDisplayMode( GLUT_RGB | GLUT_DOUBLE ); glutInitWindowSize( wx, wy ); glutInitWindowPosition( 0, 0 ); glutCreateWindow( "simple" ); glutDisplayFunc( display ); glutReshapeFunc( reshape ); glutIdleFunc( idle ); glutKeyboardFunc( keypress ); glutMouseFunc( mouse ); glutMotionFunc( mouse_motion ); glutPassiveMotionFunc( mouse_motion ); return TRUE; } /************************************************************************/ int main( int argc, char **argv ) { int wx, wy; BOARD *b; /* Set up a board */ b = the_board = &a_board; initialize_maze( "board2", b ); read_path1( "path1-execute.path" ); /* Should really call set_state() */ x_d = b->puck[XX] = b->start[0]; y_d = b->puck[YY] = b->start[1]; init_glut( argc, argv, ( b->board.p[TOP] - b->board.p[BOTTOM] ) /( b->board.p[RIGHT] - b->board.p[LEFT] ) ); init_display( b ); simulation_mode = RUNNING; path1_start_time = the_board->global_time; glutMainLoop(); return 0; } /************************************************************************/