robot.cpp

用于机器人开发环境的仿真程序

#include <windows.h>								// Header File For Windows
#include <gl\gl.h>								// Header File For The OpenGL32 Library
#include <gl\glu.h>								// Header File For The GLu32 Library
#include <gl\glaux.h>								// Header File For The GLaux Library
#include <gl\glut.h>
#include <stdio.h>
#include "resource.h"
#include <tchar.h>
#include <math.h>
//////////////////////////////////////////////////////////////////////////////////////////



HDC			hDC=NULL;		// Private GDI Device Context
HGLRC		hRC=NULL;		// Permanent Rendering Context
HWND		hWnd=NULL;		// Holds Our Window Handle
HINSTANCE	hInstance;		// Holds The Instance Of The Application

bool	keys[256];			// Array Used For The Keyboard Routine
bool	active=TRUE;		// Window Active Flag Set To TRUE By Default
bool	fullscreen=TRUE;	// Fullscreen Flag Set To Fullscreen Mode By Default


GLfloat q1,q2,q3,q4,q5,q6;
const float pi=3.1415926;



////////////////////////////////////////////////////////////////////////////////////////////
LRESULT	CALLBACK WndProc(HWND, UINT, WPARAM, LPARAM);	// Declaration For WndProc

void CalMatrixZ(float Matrix[3][3],GLfloat theta)//绕z轴            cos(treta)  -sin(theta)  0
{
	Matrix[0][0]=cos(theta);                     //                 sin(theta)  cos(theta)   0
	Matrix[0][1]=-sin(theta);                    //                 0           0            1 
	Matrix[1][0]=sin(theta);
	Matrix[1][1]=cos(theta);
	Matrix[0][2]=Matrix[1][2]=Matrix[2][0]=Matrix[2][1]=0.0;
	Matrix[2][2]=1.0;
}

void CalMatrixX(float Matrix[3][3],GLfloat theta)//绕x轴                 1.0      0.0          0.0                  
{
	Matrix[0][0]=1.0;Matrix[0][1]=0.0;Matrix[0][2]=0.0;              //  0.0      cos(theta)    -sin(theta) 
	Matrix[1][0]=0.0;Matrix[1][1]=cos(theta);Matrix[1][2]=-sin(theta);// 0.0      sin(theta)     cos(theta)
	Matrix[2][0]=0.0;Matrix[2][1]=sin(theta);Matrix[2][2]=cos(theta);	
}

void CalMatrixY(float Matrix[3][3],GLfloat theta)//绕y轴                  cos(theta)   0.0    -sin(theta)
{
	Matrix[0][0]=cos(theta);Matrix[0][1]=0.0;Matrix[0][2]=-sin(theta);//  0.0          1.0    0.0
	Matrix[1][0]=0.0;Matrix[1][1]=1.0;Matrix[1][2]=0.0;
	Matrix[2][0]=sin(theta);Matrix[2][1]=0.0;Matrix[2][2]=cos(theta); //  sin(theta)   0.0    cos(theta)	
}

void MatrixMultiply(float M1[3][3],float M2[3][3],float Matrix_Res[3][3])//矩阵相乘
{
	int i,j,k;	
	for(i=0;i<3;i++)
	{
		for(j=0;j<3;j++)
		{
			for(k=0;k<3;k++)
				Matrix_Res[i][j]+=M1[i][k]*M2[k][j];
		}
	}
}

///////////////////////////////////////////////////////////////////////////////////////////////////////

GLvoid ReSizeGLScene(GLsizei width, GLsizei height)		// Resize And Initialize The GL Window
{
	if (height==0)										// Prevent A Divide By Zero By
	{
		height=1;										// Making Height Equal One
	}

	glViewport(0,0,width,height);						// Reset The Current Viewport

	glMatrixMode(GL_PROJECTION);						// Select The Projection Matrix
	glLoadIdentity();									// Reset The Projection Matrix

	// Calculate The Aspect Ratio Of The Window
	gluPerspective(45.0f,(GLfloat)width/(GLfloat)height,0.1f,100.0f);

	glMatrixMode(GL_MODELVIEW);							// Select The Modelview Matrix
	glLoadIdentity();									// Reset The Modelview Matrix
}

int CDECL MessageBoxPrintf(TCHAR * szCaption,TCHAR * szFormat,...)
{
	TCHAR szBuffer[1024];
	va_list pArgList;
	va_start(pArgList,szFormat);
	_vsntprintf(szBuffer,sizeof(szBuffer)/sizeof(TCHAR),szFormat,pArgList);
	va_end(pArgList);

	return MessageBox(NULL,szBuffer,szCaption,0);
}

int InitGL(GLvoid)										// All Setup For OpenGL Goes Here
{
	glShadeModel(GL_SMOOTH);							// Enable Smooth Shading
	glClearColor(0.15f, 0.75f, 1.0f, 1.0f);				// Black Background
	glClearDepth(1.0f);									// Depth Buffer Setup
	glEnable(GL_DEPTH_TEST);							// Enables Depth Testing
	glDepthFunc(GL_LEQUAL);								// The Type Of Depth Testing To Do
	glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);	// Really Nice Perspective Calculations
	return TRUE;										// Initialization Went OK
}

int DrawGLScene(GLvoid)									// Here's Where We Do All The Drawing
{
    
	GLUquadricObj *pObj;
	pObj = gluNewQuadric();
	gluQuadricDrawStyle(pObj,GLU_FILL);

	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);	// Clear Screen And Depth Buffer	
	glLoadIdentity();									// 回到屏幕中心

	glPushMatrix();
	  glTranslatef(-1.0f,-1.2f,-6.0f);//沿x,y,z轴移动
	  glRotatef(q1,0.0f,1.0f,0.0f);
	  glRotatef(-90.0f, 1.0f, 0.0f, 0.0f);
	glPushMatrix();	     
	 glColor3f(0.90f,0.78125f,0.640625f);		 		 
	 gluCylinder(pObj, 0.4f, 0.4f, 0.3f, 26,26);//1.0
	 glTranslatef(0,0,0.3f);
     gluCylinder(pObj, 0.4f, 0.15f, 1.3f, 26,26);
	 glTranslatef(0,0,1.3f);
     glColor3f(1.0f,0.0f,0.0f);
     gluCylinder(pObj, 0.15f, 0.2f, 0.05f, 26,26);
	 glTranslatef(0,0,0.05f);
     gluDisk(pObj, 0, 0.2f, 26, 13);
	glPopMatrix();

	glPushMatrix();	  
	  glRotatef(90.0f, 1.0f, 0.0f, 0.0f);
	  glTranslatef(0,1.85f,0);   
      glRotatef(q2,0.0f,0.0f,1.0f);    
	  glTranslatef(0.0f,0.0f,-0.3f);
	  glColor3f(0.0f,1.0f,0.0f);
	  gluCylinder(pObj, 0.2f, 0.2f, 0.5f, 26,13);
	  glTranslatef(0.0f,0.0f,0.5f);
      gluDisk(pObj, 0, 0.2f, 26, 13);	  
	  glTranslatef(0.0f,0.0f,-0.5f);
	  glColor3f(1.0f,0.0f,0.0f);
	  gluCylinder(pObj, 0.25f, 0.20f, 0.05f, 26,13);//0.8
      gluDisk(pObj, 0, 0.25f, 26, 13);

	  glPushMatrix();
        glColor3f(0.5f,1.0f,0.7f);
		glBegin(GL_QUADS);						
		   glVertex3f(-0.3f, -0.3f, 0.0f);			
		   glVertex3f(-0.3f, 0.3f, 0.0f);		
		   glVertex3f( 1.0f,0.3f, 0.0f);		
		   glVertex3f( 1.0f,-0.3f, 0.0f);		
	    glEnd();
		glTranslatef(0.0f,0.0f,-0.15f);
		glBegin(GL_QUADS);					
		   glVertex3f(-0.3f, -0.3f, 0.0f);		
		   glVertex3f(-0.3f, 0.3f, 0.0f);		
		   glVertex3f( 1.0f,0.3f, 0.0f);		
		   glVertex3f( 1.0f,-0.3f, 0.0f);		
	    glEnd();
        glTranslatef(0.0f,0.3f,0.15f);
		glBegin(GL_QUADS);					
		   glVertex3f(-0.3f, 0.0, 0.0f);
		   glVertex3f(-0.3f,0.0f, -0.15f);
		   glVertex3f(1.0f,0.0f, -0.15f);		
		   glVertex3f(1.0f, 0.0f, 0.0f);		   		
	    glEnd();
		glTranslatef(0.0f,-0.6f,0.0f);
		glBegin(GL_QUADS);					
		   glVertex3f(-0.3f, 0.0, 0.0f);
		   glVertex3f(-0.3f,0.0f, -0.15f);
		   glVertex3f(1.0f,0.0f, -0.15f);		
		   glVertex3f(1.0f, 0.0f, 0.0f);		   		
	    glEnd();
		glTranslatef(-0.3f,0.0f,0.0f);
		glBegin(GL_QUADS);          				
		   glVertex3f(0.0f, 0.0, 0.0f);
		   glVertex3f(0.0f,0.0f, -0.15f);
		   glVertex3f(0.0f,0.6f, -0.15f);		
		   glVertex3f(0.0f,0.6f, 0.0f);		   		
	    glEnd();

		glTranslatef(1.0f,0.0f,0.0f);
		   
        glPushMatrix();
		     glTranslatef(0.3f,0.3f,-0.15f);
             gluCylinder(pObj, 0.3f, 0.3f, 0.15f, 26,13);
             gluDisk(pObj, 0, 0.3f, 26, 13);
             glTranslatef(0.0f,0.0f,0.15f);
             gluDisk(pObj, 0, 0.3f, 26, 13);

             glPushMatrix();
			    glColor3f(0.0f,1.0f,0.0f);
			    glRotatef(q3,0.0f,0.0f,1.0f);
			    glBegin(GL_QUADS);						
		          glVertex3f(-0.3f, 0.2f, 0.0f);			
		          glVertex3f(-0.3f, -0.2f, 0.0f);		
		          glVertex3f(0.5f,-0.2f, 0.0f);		
		          glVertex3f(0.5f,0.2f, 0.0f);		
	            glEnd();
				glTranslatef(0.0f,0.0f,0.15f);
                glBegin(GL_QUADS);						
		          glVertex3f(-0.3f, 0.2f, 0.0f);			
		          glVertex3f(-0.3f, -0.2f, 0.0f);		
		          glVertex3f(0.5f,-0.2f, 0.0f);		
		          glVertex3f(0.5f,0.2f, 0.0f);		
	            glEnd();
                glTranslatef(0.0f,0.0f,-0.15f);
				glBegin(GL_QUADS);						
		          glVertex3f(-0.3f, 0.2f, 0.0f);			
		          glVertex3f(-0.3f, -0.2f, 0.0f);		
		          glVertex3f(-0.3f,-0.2f, 0.15f);		
		          glVertex3f(-0.3f,0.2f, 0.15f);		
	            glEnd();
				glBegin(GL_QUADS);						
		          glVertex3f(-0.3f,0.2f, 0.0f);			
		          glVertex3f(-0.3f,0.2f, 0.15f);		
		          glVertex3f(0.5f,0.2f, 0.15f);		
		          glVertex3f(0.5f,0.2f, 0.0f);		
	            glEnd();
				glBegin(GL_QUADS);				  
		          glVertex3f(-0.3f,-0.2f, 0.0f);			
		          glVertex3f(-0.3f,-0.2f, 0.15f);		
		          glVertex3f(0.5f,-0.2f, 0.15f);		
		          glVertex3f(0.5f,-0.2f, 0.0f);		
	            glEnd();
				glTranslatef(0.5f,-0.20f,0.0f);
				glBegin(GL_QUADS);
				  glColor3f(1.0f,0.8,0.0);
		          glVertex3f(0.0f,0.0f,0.0f);			
		          glVertex3f(0.0f,0.0f,0.15f);		
		          glVertex3f(0.7f,0.1f, 0.15f);		
		          glVertex3f(0.7f,0.1f, 0.0f);		
	            glEnd();
				glBegin(GL_QUADS);
				  glVertex3f(0.0f,0.4f,0.0f);			
		          glVertex3f(0.7f,0.3f,0.0f);		
		          glVertex3f(0.7f,0.3f,0.15f);		
		          glVertex3f(0.0f,0.4f,0.15f);		
	            glEnd();
				glBegin(GL_QUADS);
				  glVertex3f(0.0f,0.0f,0.0f);			
		          glVertex3f(0.0f,0.4f,0.0f);		
		          glVertex3f(0.7f,0.3f,0.0f);		
		          glVertex3f(0.7f,0.1f,0.0f);		
	            glEnd();
				glTranslatef(0.0,0.0f,0.15f);
				glBegin(GL_QUADS);
				  glVertex3f(0.0f,0.0f,0.0f);			
		          glVertex3f(0.0f,0.4f,0.0f);		
		          glVertex3f(0.7f,0.3f,0.0f);		
		          glVertex3f(0.7f,0.1f,0.0f);		
	            glEnd();
				glTranslatef(0.7,0.0f,-0.15f);
				glBegin(GL_QUADS);
				  glVertex3f(0.0f,0.1f,0.0f);			
		          glVertex3f(0.0f,0.3f,0.0f);		
		          glVertex3f(0.0f,0.3f,0.15f);		
		          glVertex3f(0.0f,0.1f,0.15f);		
	            glEnd();			
			glPushMatrix();
			  glTranslatef(0.0f,0.2,0.075);
			  glColor3f(1.0f,0.0,0.0);
			  glRotatef(q4,1.0f,0.0f,0.0f);
			  glRotatef(q5,0.0f,1.0f,0.0f);
			  glRotatef(q6,0.0f,0.0f,1.0f);
              glRotatef(90,0.0,1.0f,0.0f);
			  gluCylinder(pObj, 0.075f, 0.075f, 0.15f, 26,13);
			  gluDisk(pObj, 0, 0.075f, 26, 13);
              glTranslatef(0.0,0.0f,0.15f);
              gluDisk(pObj, 0, 0.075f, 26, 13);
			glPopMatrix();
	  glPopMatrix();	    
	glPopMatrix();
  glPopMatrix();

	return TRUE;										// Everything Went OK
}

GLvoid KillGLWindow(GLvoid)								// Properly Kill The Window
{
	if (fullscreen)										// Are We In Fullscreen Mode?
	{
		ChangeDisplaySettings(NULL,0);					// If So Switch Back To The Desktop
		ShowCursor(TRUE);								// Show Mouse Pointer
	}

	if (hRC)											// Do We Have A Rendering Context?
	{
		if (!wglMakeCurrent(NULL,NULL))					// Are We Able To Release The DC And RC Contexts?
		{
			MessageBox(NULL,"Release Of DC And RC Failed.","SHUTDOWN ERROR",MB_OK | MB_ICONINFORMATION);
		}

		if (!wglDeleteContext(hRC))						// Are We Able To Delete The RC?
		{
			MessageBox(NULL,"Release Rendering Context Failed.","SHUTDOWN ERROR",MB_OK | MB_ICONINFORMATION);
		}
		hRC=NULL;										// Set RC To NULL
	}

	if (hDC && !ReleaseDC(hWnd,hDC))					// Are We Able To Release The DC
	{
		MessageBox(NULL,"Release Device Context Failed.","SHUTDOWN ERROR",MB_OK | MB_ICONINFORMATION);
		hDC=NULL;										// Set DC To NULL
	}

	if (hWnd && !DestroyWindow(hWnd))					// Are We Able To Destroy The Window?
	{
		MessageBox(NULL,"Could Not Release hWnd.","SHUTDOWN ERROR",MB_OK | MB_ICONINFORMATION);
		hWnd=NULL;										// Set hWnd To NULL
	}

	if (!UnregisterClass("OpenGL",hInstance))			// Are We Able To Unregister Class
	{
		MessageBox(NULL,"Could Not Unregister Class.","SHUTDOWN ERROR",MB_OK | MB_ICONINFORMATION);
		hInstance=NULL;									// Set hInstance To NULL
	}
}
 
BOOL CreateGLWindow(char* title, int width, int height, int bits, bool fullscreenflag)
{
	GLuint		PixelFormat;			// Holds The Results After Searching For A Match
	WNDCLASS	wc;						// Windows Class Structure
	HMENU       hMenu;

	DWORD		dwExStyle;				// Window Extended Style
	DWORD		dwStyle;				// Window Style
	RECT		WindowRect;				// Grabs Rectangle Upper Left / Lower Right Values
	WindowRect.left=(long)0;			// Set Left Value To 0
	WindowRect.right=(long)width;		// Set Right Value To Requested Width
	WindowRect.top=(long)0;				// Set Top Value To 0
	WindowRect.bottom=(long)height;		// Set Bottom Value To Requested Height

	fullscreen=fullscreenflag;			// Set The Global Fullscreen Flag

	hInstance			= GetModuleHandle(NULL);				// Grab An Instance For Our Window
	wc.style			= CS_HREDRAW | CS_VREDRAW | CS_OWNDC;	// Redraw On Size, And Own DC For Window.
	wc.lpfnWndProc		= (WNDPROC) WndProc;					// WndProc Handles Messages
	wc.cbClsExtra		= 0;									// No Extra Window Data
	wc.cbWndExtra		= 0;									// No Extra Window Data
	wc.hInstance		= hInstance;							// Set The Instance
	wc.hIcon			= LoadIcon(NULL, MAKEINTRESOURCE(IDB_BITMAP1));			// Load The Default Icon
	wc.hCursor			= LoadCursor(NULL, IDC_ARROW);			// Load The Arrow Pointer
	wc.hbrBackground	= NULL;									// No Background Required For GL
	wc.lpszMenuName		= "IDR_MENU";							
	wc.lpszClassName	= "OpenGL";								// Set The Class Name
	hMenu = LoadMenu(hInstance,MAKEINTRESOURCE(IDR_MENU));

	if (!RegisterClass(&wc))									// Attempt To Register The Window Class
	{
		MessageBox(NULL,"Failed To Register The Window Class.","ERROR",MB_OK|MB_ICONEXCLAMATION);
		return FALSE;											// Return FALSE
	}
	

	{
		dwExStyle=WS_EX_APPWINDOW | WS_EX_WINDOWEDGE;			// Window Extended Style
		dwStyle=WS_OVERLAPPEDWINDOW;							// Windows Style
	}

	AdjustWindowRectEx(&WindowRect, dwStyle, FALSE, dwExStyle);		// Adjust Window To True Requested Size

	// Create The Window
	if (!(hWnd=CreateWindowEx(	dwExStyle,							// Extended Style For The Window