graphics.c

这个代码是policy iteration算法关于强化学习的. 请您用winzip 解压缩

#ifdef GRAPHICS

#include <GL/glut.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>

#include "graphics.h"
#include "path.h"
#include "learn.h"
#include "gaussian.h"
#include "misc.h"

extern int step;
extern double **s_hist;

extern int dim;
extern double *goal_s;
extern double *start_s;


extern int num_of_gaussians;
extern double **gauss_center, **gauss_var;


// graphics globals

#define NUM_OF_POLYGONS 100
double goal_poly_vert[NUM_OF_POLYGONS][2];
int Update_Boundaries = 0;
double ***boundary_polygons;
// ****************************



void init_graphics(int *argc, char** argv)
{
	glutInit(argc, argv);
	glutInitDisplayMode (GLUT_DOUBLE | GLUT_RGB);
	glutInitWindowSize (400, 400); 
	glutInitWindowPosition (100, 100);

	glutCreateWindow ("Continuous RL Simulation");
	glClearColor (1.0, 1.0, 1.0, 0.0);
	glShadeModel (GL_FLAT);
	
	Define_Goal_Polygons();

	Define_Boundary_Polygons();

	glutDisplayFunc(display); 
	glutReshapeFunc(reshape); 
	glutMouseFunc(mouse);
	glutKeyboardFunc (keyboard);
	glutIdleFunc(NULL);
	glutMainLoop();
}

void Define_Boundary_Polygons(void)
{
	int i,j;
	boundary_polygons = (double ***)My_Malloc((long)(num_of_gaussians-1)  * sizeof(double**));
	for ( i = 0; i < num_of_gaussians-1; i++ )
	{
		boundary_polygons[i] = (double **)My_Malloc((long)NUM_OF_POLYGONS  * sizeof(double*));
		for ( j = 0; j < NUM_OF_POLYGONS; j++ )
		{
			boundary_polygons[i][j] = (double *)My_Malloc((long)2  * sizeof(double));
		}
	}

	Update_Boundary_Location();
}

void display_graphics(void)
{
	Update_Sim_Display();
}


void display(void)
{
	int i,j;
	glClear(GL_COLOR_BUFFER_BIT);

	/* draw the goal position */
	glColor3d(0.0,1.0,0.0);
	
	glPushMatrix();
	
	glBegin(GL_POLYGON);
	
	for ( j = 0; j < NUM_OF_POLYGONS; j++ )
	{
		glVertex2d(goal_poly_vert[j][0],goal_poly_vert[j][1]);
	}
	
	glEnd();
	
	glPopMatrix();
	/*************************/

	/* dras the boundaries */
	glColor3d(1.0,0.0,0.0);
	glLineWidth(6.0);
	for ( i = 0; i < num_of_gaussians-1; i++ )
	{
		glPushMatrix();

		glBegin(GL_LINE_LOOP);
		
		for ( j = 0; j < NUM_OF_POLYGONS; j++ )
		{
			glVertex2d(boundary_polygons[i][j][0],boundary_polygons[i][j][1]);
		}

		glEnd();
		
		glPopMatrix();
		
	}
	/*********************/

	/* draw the robot path */
	glLineWidth(3.0);
	glColor3d(0.0, 0.0, 1.0);
	glPushMatrix();
	
	glEnable(GL_POINT_SMOOTH);
	glPointSize(8.0);
	glBegin(GL_POINTS);
	
	glVertex2d(start_s[0],start_s[1]);
	
	glEnd();
	
	glPopMatrix();
	
	glPushMatrix();
	
	glBegin(GL_LINE_STRIP);
	
	for ( j = 0; j < step; j++ )
	{
		glVertex2d(s_hist[j][0],s_hist[j][1]);
	}
	glEnd();
	
	glPopMatrix();
	
	
	glutSwapBuffers();
}

void Update_Sim_Display(void)
{
	path();
	learn();
	if ( Update_Boundaries == 1 )
	{
		Update_Boundaries = 0;
		Update_Boundary_Location();
	}
	glutPostRedisplay();
}


double Max_G(int exclude, double *x)
{
	int j;
	double max_g = -1,g;
	
	for ( j = 0; j < num_of_gaussians; j++ )
	{
		if ( j != exclude )
		{
			g = evaluate_gauss(dim, x, gauss_center[j], gauss_var[j]);
			if ( g > max_g )
			{
				max_g = g;
			}
		}
	}

	return(max_g);
	
}

void Update_Boundary_Location(void)
{
	int i, pcnt, cont;
	
	double pi = 3.1415926535;
	double *x,xc[2];
	double rad, rad_min, rad_max, g, gtmp;

	x = (double *)My_Malloc((long)dim * sizeof(double));
	for ( i = 0; i < dim; i++ )
	{
		x[i] = 0.0;
	}
	
	for (pcnt=1; pcnt < num_of_gaussians; pcnt++)
	{
		for (i=0; i < NUM_OF_POLYGONS; i++)
		{
			xc[0] = gauss_center[pcnt][0];
			xc[1] = gauss_center[pcnt][1];
			// bracket the boundary by first finding rad_min
			rad_min = 0.0001;
			x[0] = xc[0] + (cos( (((double)i)/(NUM_OF_POLYGONS-1)) * 2.0 * pi) * rad_min);
			x[1] = xc[1] + (sin( (((double)i)/(NUM_OF_POLYGONS-1)) * 2.0 * pi) * rad_min);
			g = evaluate_gauss(dim, x, gauss_center[pcnt], gauss_var[pcnt]);
			gtmp = Max_G(pcnt, x);
			if ( g < gtmp )
			{
				cont = 1;
			}
			else
			{
				cont = 0;
			}
			while (cont)
			{
				rad_min = rad_min / 2.0;
				x[0] = xc[0] + (cos( (((double)i)/(NUM_OF_POLYGONS-1)) * 2.0 * pi) * rad_min);
				x[1] = xc[1] + (sin( (((double)i)/(NUM_OF_POLYGONS-1)) * 2.0 * pi) * rad_min);
				g = evaluate_gauss(dim, x, gauss_center[pcnt], gauss_var[pcnt]);
				gtmp = Max_G(pcnt, x);
				if ( g > gtmp )
				{
					cont = 0;
				}
			}
			// next find rad_max
			rad_max = 0.000001;
			x[0] = xc[0] + (cos( (((double)i)/(NUM_OF_POLYGONS-1)) * 2.0 * pi) * rad_max);
			x[1] = xc[1] + (sin( (((double)i)/(NUM_OF_POLYGONS-1)) * 2.0 * pi) * rad_max);
			g = evaluate_gauss(dim, x, gauss_center[pcnt], gauss_var[pcnt]);
			gtmp = Max_G(pcnt, x);
			if ( g > gtmp )
			{
				cont = 1;
			}
			else
			{
				cont = 0;
			}
			while (cont)
			{
				rad_max = rad_max * 2.0;
				x[0] = xc[0] + (cos( (((double)i)/(NUM_OF_POLYGONS-1)) * 2.0 * pi) * rad_max);
				x[1] = xc[1] + (sin( (((double)i)/(NUM_OF_POLYGONS-1)) * 2.0 * pi) * rad_max);
				g = evaluate_gauss(dim, x, gauss_center[pcnt], gauss_var[pcnt]);
				gtmp = Max_G(pcnt, x);
				if ( g < gtmp )
				{
					cont = 0;
				}
			}
			
			while ( rad_max - rad_min > 0.001 )
			{
				rad = rad_min + 0.5 * (rad_max - rad_min);
				x[0] = xc[0] + (cos( (((double)i)/(NUM_OF_POLYGONS-1)) * 2.0 * pi) * rad);
				x[1] = xc[1] + (sin( (((double)i)/(NUM_OF_POLYGONS-1)) * 2.0 * pi) * rad);
				g = evaluate_gauss(dim, x, gauss_center[pcnt], gauss_var[pcnt]);
				gtmp = Max_G(pcnt, x);

				if ( gtmp > g )
				{
					rad_max = rad;
				}
				else
				{
					rad_min = rad;
				}
			}
			boundary_polygons[pcnt-1][i][0] = x[0];
			boundary_polygons[pcnt-1][i][1] = x[1];
		}
	}
	
	free(x);
}

void reshape(int w, int h)
{
	glViewport (0, 0, (GLsizei) w, (GLsizei) h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluOrtho2D(-1.0, 1.0, -1.0, 1.0);
	//glOrtho(-50.0, 50.0, -50.0, 50.0, -1.0, 1.0);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
}

void mouse(int button, int state, int x, int y) 
{
	switch (button) {
	case GLUT_LEFT_BUTTON:
		if (state == GLUT_DOWN)
            glutIdleFunc(Update_Sim_Display);
		break;
	case GLUT_MIDDLE_BUTTON:
	case GLUT_RIGHT_BUTTON:
		if (state == GLUT_DOWN)
            glutIdleFunc(NULL);
		break;
	default:
		break;
	}
}

void keyboard(unsigned char key, int x, int y)
{
	switch (key) {
	case 27:
		//Save_Simulation(&(gra.sim));
		exit(0);
		break;
	}
}


void Define_Goal_Polygons(void)
{
	int i;
	double rad = 0.1;
	double pi = 3.1415926535;
	double x,y;
	
	for ( i = 0; i < NUM_OF_POLYGONS; i++ )
	{
		x = goal_s[0] + (cos( (((double)i)/(NUM_OF_POLYGONS-1)) * 2.0 * pi) * rad);
		y = goal_s[1] + (sin( (((double)i)/(NUM_OF_POLYGONS-1)) * 2.0 * pi) * rad);
		goal_poly_vert[i][0] = x;
		goal_poly_vert[i][1] = y;
	}
}


#endif