6自由度机器人运动学正反解C++程序

6自由度机器人运动学正、反解C++程序，可直接运行，简单易懂！

// DrawArm.cpp: implementation of the CDrawArm class.
//
//////////////////////////////////////////////////////////////////////

#include “stdafx.h“
#include “Zen_Fan.h“
#include “DrawArm.h“
#include “math.h“

#include 
#include 



#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif


#define NDIV_MAX 30

extern double *posx*posy*posz;
float CS_Left[6];
extern int pointnumDrawFlag[2];


float scaleview;
//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

CDrawArm::CDrawArm（）
{

}

CDrawArm::~CDrawArm（）
{

}

void CDrawArm::DrawStaticGoods（）
{
	//DrawFloor（2.5）;
	DrawObstacle（）;
	
	if （1==DrawFlag[0]）
	{
		DrawScaleOutt（）;
	}
	if （1==DrawFlag[1]）
	{
		DrawScaleIn（）;
	}
}

void CDrawArm::DrawFloor（double floorSize）
{
	int i;
	
	//灰色
	GLfloat grey_ambient[]  = { 0.2f 0.2f 0.2f }; //环境色
	GLfloat grey_diffuse[]  = { 0.0f 0.0f 0.0f }; //扩散色
	GLfloat grey_specular[] = { 0.1f 0.1f 0.1f }; //境面色
	GLfloat grey_shininess[]= { 20.0f };			 //反射强度
	
	//米黄色
	GLfloat riceyellow_ambient[]  = { 0.3f 0.3f 0.2f }; //环境色
	GLfloat riceyellow_diffuse[]  = { 0.0f 0.0f 0.0f };   //扩散色
	GLfloat riceyellow_specular[] = { 0.1f 0.1f 0.1f };    //境面色
	GLfloat riceyellow_shininess[]= { 1.0f };			   //反射强度
	
	glPushMatrix（）;
	
	//立体的材质定义：白
	glMaterialfv（ GL_FRONT GL_AMBIENT grey_ambient）;
	glMaterialfv（ GL_FRONT GL_DIFFUSE grey_diffuse ）;
	glMaterialfv（ GL_FRONT GL_SPECULARgrey_specular）;
	glMaterialfv（ GL_FRONT GL_SHININESS grey_shininess ）;
	
		  //床上画线
	glBegin（GL_LINES）;
	
	for（i=-4;i<=4;i++）{
		glVertex3d（-floorSize floorSize/4.0*（double）i 0.02 ）;
		glVertex3d（ floorSize floorSize/4.0*（double）i 0.02 ）;
	}
	
	for（i=-4;i<=4;i++）{
		glVertex3d（ floorSize/4.0*（double）i -floorSize 0.02 ）;
		glVertex3d（ floorSize/4.0*（double）i  floorSize 0.02 ）;
	}
	glEnd（）;
	
	//立体材质的定义：米黄色
	glMaterialfv（ GL_FRONT GL_AMBIENT riceyellow_ambient）;
	glMaterialfv（ GL_FRONT GL_DIFFUSE riceyellow_diffuse ）;
	glMaterialfv（ GL_FRONT GL_SPECULARriceyellow_specular）;
	glMaterialfv（ GL_FRONT GL_SHININESS riceyellow_shininess ）;
	
	glBegin（GL_QUADS）;
	glVertex3d（ floorSize floorSize 0.0 ）;
	glVertex3d（-floorSize floorSize 0.0 ）;
	glVertex3d（-floorSize-floorSize 0.0 ）;
	glVertex3d（ floorSize-floorSize 0.0 ）;
	glEnd（）;
	
	glPopMatrix（）;
}

void CDrawArm::DrawDynamicGoods（）
{	
	glPushMatrix（）;
		glTranslated（ 0 0 0 ）;//确定基座在哪
			DrawArmBody（CS_Left[0]CS_Left[1]CS_Left[2]CS_Left[3]CS_Left[4]CS_Left[5]）;	
	glPopMatrix（）;

}

void CDrawArm::SolidCylinder（GLdouble radius GLdouble height int n_div）
{
	int i;
	double angle;
	
	GLfloat x y;
	GLfloat top_poly[NDIV_MAX][3] bottom_poly[NDIV_MAX][3];
	GLfloat nv[3];
	
#if !defined PI
#define PI 3.14159

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件      73746  2010-12-02 14:50  （运动学正反解）\Debug\DrawArm.obj

     文件          0  2010-12-02 14:50  （运动学正反解）\Debug\DrawArm.sbr

     文件     262516  2010-11-01 14:48  （运动学正反解）\Debug\DSIn.txt

     文件          0  2010-11-20 10:59  （运动学正反解）\Debug\DSInn.txt

     文件          0  2010-10-30 14:08  （运动学正反解）\Debug\DSOut.txt

     文件     191378  2010-11-24 17:48  （运动学正反解）\Debug\exworkspace.txt

     文件     192242  2010-11-24 17:37  （运动学正反解）\Debug\inworkspace.txt

     文件      27415  2010-11-19 15:24  （运动学正反解）\Debug\OpenGLView.obj

     文件          0  2010-11-19 15:24  （运动学正反解）\Debug\OpenGLView.sbr

     文件     107345  2010-11-19 15:24  （运动学正反解）\Debug\StdAfx.obj

     文件    1426092  2010-11-19 15:24  （运动学正反解）\Debug\StdAfx.sbr

     文件     238592  2010-12-04 09:56  （运动学正反解）\Debug\vc60.idb

     文件     389120  2010-12-02 14:50  （运动学正反解）\Debug\vc60.pdb

     文件    5743616  2010-12-02 14:50  （运动学正反解）\Debug\Zen_Fan.bsc

     文件     184388  2010-12-02 14:50  （运动学正反解）\Debug\Zen_Fan.exe

     文件     550568  2010-12-02 14:50  （运动学正反解）\Debug\Zen_Fan.ilk

     文件      12748  2010-11-19 15:24  （运动学正反解）\Debug\Zen_Fan.obj

     文件    7149204  2010-11-19 15:24  （运动学正反解）\Debug\Zen_Fan.pch

     文件     492544  2010-12-02 14:50  （运动学正反解）\Debug\Zen_Fan.pdb

     文件       7792  2010-11-30 14:22  （运动学正反解）\Debug\Zen_Fan.res

     文件          0  2010-11-19 15:24  （运动学正反解）\Debug\Zen_Fan.sbr

     文件      92787  2010-11-29 17:01  （运动学正反解）\Debug\Zen_FanDlg.obj

     文件          0  2010-11-29 17:01  （运动学正反解）\Debug\Zen_FanDlg.sbr

     文件      28807  2010-12-02 14:50  （运动学正反解）\DrawArm.cpp

     文件       1001  2010-11-29 17:00  （运动学正反解）\DrawArm.h

     文件     262516  2010-11-01 14:48  （运动学正反解）\DSIn.txt

     文件      17525  2010-11-19 16:22  （运动学正反解）\DSInn.txt

     文件    1695468  2010-10-30 11:43  （运动学正反解）\DSOut.txt

     文件     191378  2010-11-24 17:48  （运动学正反解）\exworkspace.txt

     文件     262516  2010-11-01 14:48  （运动学正反解）\INRECT.txt

............此处省略28个文件信息