#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);
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);
printf("x0=%lf,y0=%lf,f=%f\n",x0,y0,f);
scanf("%lf%lf%lf",&x0,&y0,&f);
printf("Xs=%lf,Ys=%lf,Zs=%f\n",Xs,Ys,Zs);
scanf("%lf%lf%lf",&Xs,&Ys,&Zs);
printf("x1=%lf,y1=%lf\n",x1,y1);
scanf("%lf%lf",&x1,&y1);
printf("x2=%lf,y2=%lf\n",x2,y2);
scanf("%lf%lf",&x2,&y2);
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*b1+(x2-x0)*a3;
l8=f*a1+(x2-x0)*b3;
l9=f*c1+(x2-x0)*c3;
lx2=f*a1*Xs+f*b1*Ys+f*c1*Zs+(x2-x0)*a3*Xs+(x2-x0)*b3*Ys+(x2-x0)*c3*Zs;
l10=f*a2+(y2-y0)*a3;
l11=f*b2+(y2-y0)*b3;
l12=f*c2+(y2-y0)*c3;
ly2=f*a2*Xs+f*b2*Ys+f*c2*Zs+(y2-y0)*a3*Xs+(y2-y0)*b3*Ys+(y2-y0)*c3*Zs;
transpose(A,AT,3,4);
multi(AT,A,ATA,3,4,3);
inverse(ATA);
multi(AT,l,ATl,3,4,1);
multi(ATA,ATl,XG,3,3,1);
Xs=Xs+XG[0][0]; Ys=Ys+XG[1][0]; Zs=Zs+XG[2][0];
printf("X=%lf,Y=%lf,Z=%lf\n",X,Y,Z);
}
template<typename T1,typename T2>void transpose(T1*mat1,T2*mat2,int a,int b)
{ int i,j;
for(i=0;i<b;i++)
for(j=0;j<a;j++)
mat2[j][i]=mat1[i][j];
return;
}
template<typename T1,typename T2>void multi(T1*mat1,T2 * mat2,T2 * result,int a,int b,int c)
{ int i,j,k;
for(i=0;i<a;i++)
{for(j=0;j<c;j++)
{result[i][j]=0;
for(k=0;k<b;k++)
result[i][j]+=mat1[i][k]*mat2[k][j];
}
}
return;
}
qfjh.zip_前方交会_前方交会 测量_前方交会C程序代码_前方交会测量
版权申诉
197 浏览量
2022-09-24
08:01:10
上传
评论
收藏 1KB ZIP 举报
小波思基
- 粉丝: 74
- 资源: 1万+
最新资源
- javaweb网上购物系统(源码+数据库+文档)高分毕业设计&期末大作业
- NX二次开发UF-CURVE-free-wrap-parms 函数介绍
- 详解计算机网络经典面试题.pdf
- NX二次开发UF-CURVE-free-trim 函数介绍
- app1111111111111111111111.py
- Docker常用命令合集.pdf
- NX二次开发UF-CURVE-free-curve-struct 函数介绍
- NX二次开发UF-CURVE-fix-spline-data 函数介绍
- NX二次开发UF-CURVE-evaluate-curve-structure 函数介绍
- 软件测试面试题-基础知识问题&测试方法和策略&工具和技术
资源上传下载、课程学习等过程中有任何疑问或建议,欢迎提出宝贵意见哦~我们会及时处理!
点击此处反馈