空间前方交会程序
#include
#include
const int n=3;
void inverse (double c[n][n]);
templatevoid transpose (T1*mat1,T2*mat2,int a,int b);
templatevoid multi(T1*mat1,T2 * mat2,T2 * result,int a,int b,int c);
templatevoid input (T*mat,int a,int b);
templatevoid output(T*mat,char*s,int a,int b);
void main()
{
double X=0,Y=0,Z=0;
double AT[3][4],ATA[3][3],XG[3][1],ATl[3][1];
double a1,a2,a3,b1,b2,b3,c1,c2,c3,l1,l2,l3,l4,l5,l6,l7,l8,l9,l10,l11,l12,lx1,ly1,lx2,ly2;
double a10,a20,a30,b10,b20,b30,c10,c20,c30;
float r,w,k;
printf("输入r:\n");
scanf("%f",&r);
printf("输入w:\n");
scanf("%f",&w);
printf("输入k:\n");
scanf("%f",&k);
a1=cos(r)*cos(k)-sin(r)*sin(w)*sin(k);
a2=-cos(r)*sin(k)-sin(r)*sin(w)*cos(k);
a3=-sin(r)*cos(w);
b1=cos(w)*sin(k);
b2=cos(w)*cos(k);
b3=-sin(w);
c1=sin(r)*cos(k)+cos(r)*sin(w)*sin(k);
c2=-sin(r)*sin(k)+cos(r)*sin(w)*cos(k);
c3=cos(r)*cos(w);
float r1,w1,k1;
printf("输入r1:\n");
scanf("%f",&r1);
printf("输入w1:\n");
scanf("%f",&w1);
printf("输入k1:\n");
scanf("%f",&k1);
a10=cos(r)*cos(k)-sin(r)*sin(w)*sin(k);
a20=-cos(r)*sin(k)-sin(r)*sin(w)*cos(k);
a30=-sin(r)*cos(w);
b10=cos(w)*sin(k);
b20=cos(w)*cos(k);
b30=-sin(w);
c10=sin(r)*cos(k)+cos(r)*sin(w)*sin(k);
c20=-sin(r)*sin(k)+cos(r)*sin(w)*cos(k);
c30=cos(r)*cos(w);
double x0=0.0,y0=0.0,f=0.15324;
double x1=-86.15,y1=-68.99,x2=0.0,y2=0.0;
double Xs=393795.45,Ys=27476.46,Zs=7572.69;
l1=f*b1+(x1-x0)*a3;
l2=f*a1+(x1-x0)*b3;
l3=f*c1+(x1-x0)*c3;
lx1=f*a1*Xs+f*b1*Ys+f*c1*Zs+(x1-x0)*a3*Xs+(x1-x0)*b3*Ys+(x1-x0)*c3*Zs;
l4=f*a2+(y1-y0)*a3;
l5=f*b2+(y1-y0)*b3;
l6=f*c2+(y1-y0)*c3;
ly1=f*a2*Xs+f*b2*Ys+f*c2*Zs+(y1-y0)*a3*Xs+(y1-y0)*b3*Ys+(y1-y0)*c3*Zs;
l7=f*b10+(x2-x0)*a30;
l8=f*a10+(x2-x0)*b30;
l9=f*c10+(x2-x0)*c30;
lx2=f*a10*Xs+f*b10*Ys+f*c10*Zs+(x2-x0)*a30*Xs+(x2-x0)*b30*Ys+(x2-x0)*c30*Zs;
l10=f*a20+(y2-y0)*a30;
l11=f*b20+(y2-y0)*b30;
l12=f*c20+(y2-y0)*c30;
ly2=f*a20*Xs+f*b20*Ys+f*c20*Zs+(y2-y0)*a30*Xs+(y2-y0)*b
