C语言实现三自由度机械臂轨迹规划源程序

C语言实现三自由度机械臂轨迹规划源程序,输入为空间三维坐标，输出为相应电机需要旋转的角度

#include
#include
#include
#include
#include
#include 
#include 
const double p =3.1415926;
using namespace std;
double l1=100.0l2=100.0;


inline void swap（double &adouble &b）
{
	double c=a;a=b;b=c;
}
//求逆函数
void MatrixInV（double M[20][20]int ndouble* Q）   
{
	int ijk;
	double d;
	int JS[20]IS[20];
	for （k=0;k		d=0;
		for （i=k;i			for （j=k;j				if （fabs（M[i][j]）>d）{
					d=fabs（M[i][j]）;
					IS[k]=i;
					JS[k]=j;
                }
            }
		}
		//出错处理 
		if （d+1.0==1.0） 
			return;
		if （IS[k]!=k）
			for （j=0;j				swap（M[k][j]M[IS[k]][j]）;
		if （JS[k]!=k）
			for （i=0;i				swap（M[i][k]M[i][JS[k]]）;
		M[k][k]=1/M[k][k];
		for （j=0;j			if （j!=k） 
				M[k][j]=M[k][j]*M[k][k];
		for （i=0;i			if （i!=k）
				for （j=0;j					if （j!=k） 
						M[i][j]=M[i][j]-M[i][k]*M[k][j];
		for （i=0;i			if （i!=k） 
				M[i][k]=-M[i][k]*M[k][k];
	}
	for （k=n-1;k>=0;k--）{
		for （j=0;j			if （JS[k]!=k） 
				swap（M[k][j]M[JS[k]][j]）;
		for （i=0;i			if （IS[k]!=k） 
				swap（M[i][k]M[i][IS[k]]）;
    }
	int t=0;
	for （int c=0;c<20;c++）{
		for （int d=0;d<20;d++）{ 
			Q[t]=M[c][d];
			t++;
	    }
    }
}
//求系数函数
void coefficient（double A[6]double* Z）            
{
	double N[20]={A[0]0A[1]A[1]00A[2]A[2]00A[3]A[3]00A[4]A[4]00A[5]0};
	//求系数矩阵
	double M[20][20]={{10000000000000000000}
	                  {01000000000000000000}
	                  {11110000000000000000}
	                  {00001000000000000000}
	                  {01230-100000000000000}
	                  {002600-20000000000000}
	                  {00001111000000000000}
	                  {00000000100000000000}
	                  {000001230-10000000000}
	                  {0000002600-2000000000}
	                  {00000000111100000000}
	                  {00000000000010000000}
	                  {0000000001230-1000000}
	                  {00000000002600-200000}
	                  {00000000000011110000}
	                  {00000000000000001000}
	                  {00000000000001230-100}
	                  {000000000000002600-20}
	                  {00000000000000001111}
	                  {00000000000000000123}};   
	double Q[20*20]={0};
	double B[20]={0};
	double G[20]={0};
	int t=0;
	int k=0;
	MatrixInV（M20Q）;
	for （int i=0;i<20;i++）{           
		for （int j=0;j<20;j++）{      
			B[j]=Q[t]*N[j];     
			G[i]+=B[j];
			t++;
		}	
	}

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

     文件       8328  2011-09-25 16:13  31\31\31.cpp

     文件       3906  2011-09-21 10:20  31\31\31.vcproj

     文件       1407  2011-09-28 18:53  31\31\31.vcproj.THINK.IBM.user

     文件        663  2011-09-21 10:20  31\31\Debug\31.exe.embed.manifest

     文件        728  2011-09-21 10:20  31\31\Debug\31.exe.embed.manifest.res

     文件        621  2011-09-25 16:13  31\31\Debug\31.exe.intermediate.manifest

     文件      80299  2011-09-25 16:13  31\31\Debug\31.obj

     文件       5828  2011-09-25 16:13  31\31\Debug\BuildLog.htm

     文件         65  2011-09-25 16:13  31\31\Debug\mt.dep

     文件     183296  2011-09-25 16:13  31\31\Debug\vc90.idb

     文件     217088  2011-09-25 16:13  31\31\Debug\vc90.pdb

     文件    1780736  2011-09-28 18:53  31\31.ncb

     文件        872  2011-09-21 10:10  31\31.sln

    ..A..H.     13824  2011-09-28 18:53  31\31.suo

     文件      56320  2011-09-25 16:13  31\Debug\31.exe

     文件     440784  2011-09-25 16:13  31\Debug\31.ilk

     文件     625664  2011-09-25 16:13  31\Debug\31.pdb

     目录          0  2011-09-25 16:13  31\31\Debug

     目录          0  2011-09-25 16:13  31\31

     目录          0  2011-09-25 16:07  31\Debug

     目录          0  2011-09-21 10:20  31

----------- ---------  ---------- -----  ----

              3420429                    21