C语言实现三维空间前方交会算法

#include<stdio.h>
#include<math.h>
const int n=3;
void inverse (double c[n][n]);
template<typename T1,typename T2>void transpose (T1*mat1,T2*mat2,int a,int b);
template<typename T1,typename T2>void multi(T1*mat1,T2 * mat2,T2 * result,int a,int b,int c);
template<typename T>void input (T*mat,int a,int b);
template<typename T>void output(T*mat,char*s,int a,int b);


 void main()
 { 
      int i,j,m; 
float r,w,k;
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,x0,y0,f,x1,y1,x2,y2,Xs,Ys,Zs,l1,l2,l3,l4,l5,l6,l7,l8,l9,l10,l11,l12,lx1,ly1,lx2,ly2;
double A[4][3]={l1,l2,l3,l4,l5,l6,l7,l8,l9,l10,l11,l12};
double l[4][1]={lx1,ly1,lx2,ly2};
printf("输入r:\n");
scanf("%f",&r);
printf("输入w:\n");
scanf("%f",&w);
printf("输入k:\n");
scanf("%f",&k);
printf("输入(x1,y1,z1):\n");
    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);