robot.cpp

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

								"OpenGL",							// Class Name
								title,								// Window Title
								dwStyle |							// Defined Window Style
								WS_CLIPSIBLINGS |					// Required Window Style
								WS_CLIPCHILDREN,					// Required Window Style
								0, 0,								// Window Position
								WindowRect.right-WindowRect.left,	// Calculate Window Width
								WindowRect.bottom-WindowRect.top,	// Calculate Window Height
								NULL,								// No Parent Window
								hMenu,								// No Menu
								hInstance,							// Instance
								NULL)))								// Dont Pass Anything To WM_CREATE
	{
		KillGLWindow();								// Reset The Display
		MessageBox(NULL,"Window Creation Error.","ERROR",MB_OK|MB_ICONEXCLAMATION);
		return FALSE;								// Return FALSE
	}

	static	PIXELFORMATDESCRIPTOR pfd=				// pfd Tells Windows How We Want Things To Be
	{
		sizeof(PIXELFORMATDESCRIPTOR),				// Size Of This Pixel Format Descriptor
		1,											// Version Number
		PFD_DRAW_TO_WINDOW |						// Format Must Support Window
		PFD_SUPPORT_OPENGL |						// Format Must Support OpenGL
		PFD_DOUBLEBUFFER,							// Must Support Double Buffering
		PFD_TYPE_RGBA,								// Request An RGBA Format
		bits,										// Select Our Color Depth
		0, 0, 0, 0, 0, 0,							// Color Bits Ignored
		0,											// No Alpha Buffer
		0,											// Shift Bit Ignored
		0,											// No Accumulation Buffer
		0, 0, 0, 0,									// Accumulation Bits Ignored
		16,											// 16Bit Z-Buffer (Depth Buffer)  
		0,											// No Stencil Buffer
		0,											// No Auxiliary Buffer
		PFD_MAIN_PLANE,								// Main Drawing Layer
		0,											// Reserved
		0, 0, 0										// Layer Masks Ignored
	};
	
	if (!(hDC=GetDC(hWnd)))							// Did We Get A Device Context?
	{
		KillGLWindow();								// Reset The Display
		MessageBox(NULL,"Can't Create A GL Device Context.","ERROR",MB_OK|MB_ICONEXCLAMATION);
		return FALSE;								// Return FALSE
	}

	if (!(PixelFormat=ChoosePixelFormat(hDC,&pfd)))	// Did Windows Find A Matching Pixel Format?
	{
		KillGLWindow();								// Reset The Display
		MessageBox(NULL,"Can't Find A Suitable PixelFormat.","ERROR",MB_OK|MB_ICONEXCLAMATION);
		return FALSE;								// Return FALSE
	}

	if(!SetPixelFormat(hDC,PixelFormat,&pfd))		// Are We Able To Set The Pixel Format?
	{
		KillGLWindow();								// Reset The Display
		MessageBox(NULL,"Can't Set The PixelFormat.","ERROR",MB_OK|MB_ICONEXCLAMATION);
		return FALSE;								// Return FALSE
	}

	if (!(hRC=wglCreateContext(hDC)))				// Are We Able To Get A Rendering Context?
	{
		KillGLWindow();								// Reset The Display
		MessageBox(NULL,"Can't Create A GL Rendering Context.","ERROR",MB_OK|MB_ICONEXCLAMATION);
		return FALSE;								// Return FALSE
	}

	if(!wglMakeCurrent(hDC,hRC))					// Try To Activate The Rendering Context
	{
		KillGLWindow();								// Reset The Display
		MessageBox(NULL,"Can't Activate The GL Rendering Context.","ERROR",MB_OK|MB_ICONEXCLAMATION);
		return FALSE;								// Return FALSE
	}

	ShowWindow(hWnd,SW_SHOW);						// Show The Window
	SetForegroundWindow(hWnd);						// Slightly Higher Priority
	SetFocus(hWnd);									// Sets Keyboard Focus To The Window
	ReSizeGLScene(width, height);					// Set Up Our Perspective GL Screen

	if (!InitGL())									// Initialize Our Newly Created GL Window
	{
		KillGLWindow();								// Reset The Display
		MessageBox(NULL,"Can't Activate The GL Rendering Context.","ERROR",MB_OK|MB_ICONEXCLAMATION);
		return FALSE;								// Return FALSE
	}

	return TRUE;									// Success
}

LRESULT CALLBACK WndProc(	HWND	hWnd,			// Handle For This Window
							UINT	uMsg,			// Message For This Window
							WPARAM	wParam,			// Additional Message Information
							LPARAM	lParam)			// Additional Message Information
{
	HMENU hMenu;
	switch (uMsg)									// Check For Windows Messages
	{
		case WM_ACTIVATE:							// Watch For Window Activate Message
		{
			if (!HIWORD(wParam))					// Check Minimization State
			{
				active=TRUE;						// Program Is Active
			}
			else
			{
				active=FALSE;						// Program Is No Longer Active
			}

			return 0;								// Return To The Message Loop
		}

		case WM_COMMAND:
		{
			hMenu = GetMenu(hWnd);
			switch(wParam)
			{
			case ID_GET_Q1:
				{
					int t;
					t=q1/360;
					q1=q1-t*360;
					MessageBoxPrintf(TEXT("绕基座旋转的角度值"),TEXT("%f"),q1);
					return(0);
				}
			case ID_GET_q2:
				{
					int t;
					t=q2/360;
					q2=q2-t*360;
					MessageBoxPrintf(TEXT("绕关节1旋转的角度值"),TEXT("%f"),q2);
					return(0);
				}
			case ID_GET_q3:
				{
					int t;
					t=q3/360;
					q3=q3-t*360;
					MessageBoxPrintf(TEXT("绕关节3旋转的角度值"),TEXT("%f"),q3);
					return(0);
				}
			case ID_GET_q4:
				{
					int t;
					t=q4/360;
					q4=q4-t*360;
					MessageBoxPrintf(TEXT("绕手坐标系x轴旋转的角度值"),TEXT("%f"),q4);
					return(0);
				}
			case ID_GET_q5:
				{
					int t;
					t=q5/360;
					q5=q5-t*360;
					MessageBoxPrintf(TEXT("绕手坐标系y轴旋转的角度值"),TEXT("%f"),q5);
					return(0);
				}
			case ID_GET_q6:
				{
					int t;
					t=q6/360;
					q6=q6-t*360;
					MessageBoxPrintf(TEXT("绕手坐标系z轴旋转的角度值"),TEXT("%f"),q6);
					return(0);
				}


			case ID_CAL_MATRIX:                                  //绕基座旋转
				{
					float theta;
					const float pi=3.1415926;
					theta=q1*pi/180;
					float M[3][3];
					CalMatrixY(M,theta);
					MessageBoxPrintf(TEXT("绕基座旋转的变换矩阵"),TEXT("%f %f %f\n%f %f %f\n%f %f %f"),M[0][0],M[0][1],M[0][2],M[1][0],M[1][1],M[1][2],M[2][0],M[2][1],M[2][2]);
					return(0);
				}

			case ID_CAL_MATRIX2:                               //绕关节1旋转                              
				{
					float theta;
					const float pi=3.1415926;
					theta=q2*pi/180;
					float M[3][3];
					CalMatrixZ(M,theta);
					MessageBoxPrintf(TEXT("绕关节1旋转的变换矩阵"),TEXT("%f %f %f\n%f %f %f\n%f %f %f"),M[0][0],M[0][1],M[0][2],M[1][0],M[1][1],M[1][2],M[2][0],M[2][1],M[2][2]);
					return(0);
				}

			case ID_CAL_MATRIX3:                             //绕关节3旋转
				{
					float theta;					
					theta=q3*pi/180;
					float M[3][3];
					CalMatrixZ(M,theta);
					MessageBoxPrintf(TEXT("绕关节3旋转的变换矩阵"),TEXT("%f %f %f\n%f %f %f\n%f %f %f"),M[0][0],M[0][1],M[0][2],M[1][0],M[1][1],M[1][2],M[2][0],M[2][1],M[2][2]);
					return(0);
				}
			case ID_CAL_TRANSMATRIX:                       //计算坐标变换矩阵
				{
					int i,j;
					float M1[3][3],M2[3][3],M3[3][3];
					float RotateMatrix[3][3];
					float AidedMatrix[3][3];//辅助矩阵
					for(i=0;i<3;i++)
						for(j=0;j<3;j++)
						{
							AidedMatrix[i][j]=0.0;
							RotateMatrix[i][j]=0.0;
						}
                    CalMatrixY(M1,q1*pi/180);
					CalMatrixZ(M2,q2*pi/180);
					CalMatrixZ(M3,q3*pi/180);
					MatrixMultiply(M1,M2,AidedMatrix);
                    MatrixMultiply(AidedMatrix,M3,RotateMatrix);//相乘次序:M3M2M1
					MessageBoxPrintf(TEXT("旋转变换矩阵"),TEXT("%f %f %f\n%f %f %f\n%f %f %f"),RotateMatrix[0][0],RotateMatrix[0][1],RotateMatrix[0][2],RotateMatrix[1][0],RotateMatrix[1][1],RotateMatrix[1][2],RotateMatrix[2][0],RotateMatrix[2][1],RotateMatrix[2][2]);
					return(0);
				}
			}
			break;
		}
			
		case WM_SYSCOMMAND:							// Intercept System Commands
		{
			switch (wParam)							// Check System Calls
			{
				case SC_SCREENSAVE:					// Screensaver Trying To Start?
				case SC_MONITORPOWER:				// Monitor Trying To Enter Powersave?
				return 0;							// Prevent From Happening
			}
			break;									// Exit
		}

		case WM_CLOSE:								// Did We Receive A Close Message?
		{
			PostQuitMessage(0);						// Send A Quit Message
			return 0;								// Jump Back
		}

		case WM_KEYDOWN:							// Is A Key Being Held Down?
		{
			keys[wParam] = TRUE;					// If So, Mark It As TRUE
			return 0;								// Jump Back
		}

		case WM_KEYUP:								// Has A Key Been Released?
		{
			keys[wParam] = FALSE;					// If So, Mark It As FALSE
			return 0;								// Jump Back
		}

		case WM_LBUTTONDOWN:
			{
					return(0);
			}

		case WM_SIZE:								// Resize The OpenGL Window
		{
			ReSizeGLScene(LOWORD(lParam),HIWORD(lParam));  // LoWord=Width, HiWord=Height
			return 0;								// Jump Back
		}
	}

	// Pass All Unhandled Messages To DefWindowProc
	return DefWindowProc(hWnd,uMsg,wParam,lParam);
}

int WINAPI WinMain(	HINSTANCE	hInstance,			// Instance
					HINSTANCE	hPrevInstance,		// Previous Instance
					LPSTR		lpCmdLine,			// Command Line Parameters
					int			nCmdShow)			// Window Show State
{
	MSG		msg;									// Windows Message Structure
	BOOL	done=FALSE;	

	if (!CreateGLWindow("Unimation Puma560 机器人动力学仿真与轨迹规划",640,480,16,false))
	{
		return 0;									// Quit If Window Was Not Created
	}

	while(!done)									// Loop That Runs While done=FALSE
	{
		if (PeekMessage(&msg,NULL,0,0,PM_REMOVE))	// Is There A Message Waiting?
		{
			if (msg.message==WM_QUIT)				// Have We Received A Quit Message?
			{
				done=TRUE;							// If So done=TRUE
			}
			else									// If Not, Deal With Window Messages
			{
				TranslateMessage(&msg);				// Translate The Message
				DispatchMessage(&msg);				// Dispatch The Message
			}
		}
		else										// If There Are No Messages
		{
			// Draw The Scene.  Watch For ESC Key And Quit Messages From DrawGLScene()
			if (active)								// Program Active?
			{
		    	if (keys[VK_ESCAPE])				// Was ESC Pressed?
				{
					done=TRUE;						// ESC Signalled A Quit
				}
				else								// Not Time To Quit, Update Screen
				{
					DrawGLScene();					// Draw The Scene
					SwapBuffers(hDC);				// Swap Buffers (Double Buffering)
				}
			}
		//////////////////////////////////////////////////////////////////////////////
		////////////////     响应键盘动作              //////////////////////////////
			if(keys[VK_LEFT])//连杆1左右旋转（y轴）
			{				
				q1-=1.5f;			
			}
			if(keys[VK_RIGHT])
			{
				q1+=1.5f;
			}

			//////////////////////连杆2绕z轴旋转
			if(keys[VK_PRIOR])
			{				
				q2+=2.0f;			
			}
            if(keys[VK_NEXT])
			{				
				q2-=2.0f;			
			}

			///////////////////////////连杆4绕z轴旋转
			if(keys[VK_UP])
			{
				q3+=1.5f;				
			}
            if(keys[VK_DOWN])
			{
				q3-=1.5f;				
			}

			////////////////////////手坐标系的旋转
			if(keys[VK_MULTIPLY])//绕y轴
			{
				q4+=1.5f;			
			}

			if(keys[VK_DIVIDE])//绕x轴
			{
				q5+=1.5f;			
			}

			if(keys[VK_SUBTRACT])//绕z轴
			{
				q6+=1.5f;				
			}


			if(keys[VK_HOME])
			{
				q1=q2=q3=0;
				q4=0.0f;
				q5=q6=0.0f;
			}
		}
	}
	

	KillGLWindow();									// Kill The Window
	return (msg.wParam);							// Exit The Program
}