#include <stdio.h> #include <math.h> #define P 3.1415926 #define E1 1e-3 #define E0 1e-9 #define m 4 #define N0 1000 //控制牛顿迭代次数 double F(double x[m],double t,int n) //目标位移方程组 { double p[m]; p[0]=440*cos(t*P/180)+490*cos(x[0])+249*cos(x[1]-20*P/180)-x[3]; p[1]=440*sin(t*P/180)+490*sin(x[0])-249*sin(x[1]-20*P/180); p[2]=500*cos(t*P/180+20*P/180)+370*cos(x[2])+540.2285*cos(x[1])-x[3]; p[3]=500*sin(t*P/180+20*P/180)+370*sin(x[2])-540.2285*sin(x[1]); return p[n]; } double A(double x[m],int i,int j) //建立目标方程的雅可比矩阵 { double p[m][m]; p[0][0]=-490*sin(x[0]);p[0][1]=-249*sin(x[1]-20*P/180);p[0][2]=0;p[0][3]=-1; p[1][0]=490*cos(x[0]);p[1][1]=-249*cos(x[1]-20*P/180);p[1][2]=0;p[1][3]=0; p[2][0]=0;p[2][1]=-540.2285*sin(x[1]);p[2][2]=-370*sin(x[2]);p[2][3]=-1; p[3][0]=0;p[3][1]=-540.2285*cos(x[1]);p[3][2]=370*cos(x[2]);p[3][3]=0; return p[i][j]; } double G(double a[m][m],int n,double b[m]) //用高斯消去法求解方程组（系数矩阵为方阵） { int i,j,k,i1,j1; double temp; int row_maxmod; double element_maxmod; for(j=0;j<m;j++) { element_maxmod=a[j][j]; row_maxmod=j; for(i=j+1;i<m;i++) //找出第j列最大元素 if(fabs(element_maxmod)<fabs(a[i][j])) { element_maxmod=a[i][j]; row_maxmod=i; } if(fabs(element_maxmod)<E0) { printf(系数行列式为零，线性方程组无解!\n); return NULL; } if(row_maxmod!=j) { for(k=j;k<m;k++) //将主行与第j行交换 { temp=a[j][k]; a[j][k]=a[row_maxmod][k]; a[row_maxmod][k]=temp; } temp=b[j]; b[j]=b[row_maxmod]; b[row_maxmod]=temp; } for(k=j;k<m;k++) //高斯消元 a[j][k]/=element_maxmod; b[j]/=element_maxmod; for(i1=j+1;i1<4;i1++) { temp=a[i1][j]; for(j1=j;j1<4;j1++) a[i1][j1]-=a[j][j1]*temp; b[i1]-=b[j]*temp; } } for(i=m-2;i>=0;i--) //逆序求方程组的解 for(j=3;j>i;j--) b[i]-=a[i][j]*b[j]; return b[n]; } double C(double x[m],double v[m], double t,int n) //建立加速度方程常数项 { double p[m]; p[0]=440*2*P*2*P*cos(t*P/180)+490*v[0]*v[0]*cos(x[0])+249*v[1]*v[1]*cos(x[1]-20*P/180); p[1]=440*2*P*2*P*sin(t*P/180)+490*v[0]*v[0]*sin(x[0])-249*v[1]*v[1]*sin(x[1]-20*P/180); p[2]=500*2*P*2*P*cos(t*P/180+20*P/180)+370*(v[2])*(v[2])*cos(x[2])+540.2285*(v[1])*(v[1])*cos(x[1]); p[3]=500*2*P*2*P*sin(t*P/180+20*P/180)+370*(v[2])*(v[2])*sin(x[2])-540.2285*(v[1])*(v[1])*sin(x[1]); return p[n]; } int main() { int i1=0,i,j; double x[4],a[4][4],b[4]; double s[4],v[4],p[4],c[4]; double t; printf(求解：\t t2(度) t4(度) t5(度) XD(滑块位移)\n); x[0]=13.02531*P/180; //t=0时的初始值。 x[1]=46.32880*P/180; x[2]=36.43463*P/180; x[3]=1140.56222; for(t=0.0;t<=360.0;t+=5.0) { for(i=0;i<4;i++) b[i]=F(x,t,i); while((fabs(b[0])>E1 || fabs(b[1])>E1 || fabs(b[2])>E1 || fabs(b[3])>E1) && i1<=N0) { for(i=0;i<4;i++) for(j=0;j<4;j++) a[i][j]=A(x,i,j); for(i=0;i<4;i++) s[i]=G(a,i,b); for(j=0;j<4;j++) x[j]-=s[j]; for(i=0;i<4;i++) b[i]=F(x,t,i); i1++; } printf(t=%lf\n,t); if(i1>N0) printf(位移无解i1=%d\n,i1); else { printf(位移精确值为： ); for(i=0;i<3;i++) printf(%-15.5f,x[i]*(180/P)); for(i=3;i<4;i++) printf(%-15.5f,x[i]); printf(\n); } for(i=0;i<4;i++) for(j=0;j<4;j++) a[i][j]=A(x,i,j);//建立目标方程组速度的常数项 b[0]=440*2*P*sin(t*P/180); b[1]=-440*2*P*cos(t*P/180); b[2]=500*2*P*sin(t*P/180+20*P/180); b[3]=-500*2*P*cos(t*P/180+20*P/180); for(i=0;i<4;i++) v[i]=G(a,i,b); printf(速度精确值为： ); for(j=0;j<4;j++) printf(%-15.5f,v[j]); printf(\n); for(j=0;j<4;j++) { c[j]=C(x,v,t,j); } printf(加速度精确值： ); for(i=0;i<4;i++) p[i]=G(a,i,c); for(j=0;j<4;j++) printf(%-15.5f,p[j]); printf(\n); } return 0; } | 
