2014-09-17 21:08:40
115: GPS空间定位
时间限制: 1 Sec 内存限制: 128 MB提交: 288 解决: 16
[提交][状态][讨论版]
题目描述
空中有四颗测距卫星,现在对三维空间中一个点定位,4颗卫星各自输出自己与测量点间距离;求被测量点的三维坐标。
输入
多组数据输入。
第一行一个数
CASENUM,表示有CASENUM组数据输入。
之后每组数据有
5行,
前四行,每行个三个整数
Xi,Yi,Zi,(i=1,2,3,4,表示第i颗卫星的位置坐标)
第五行,四个整数
D1,D2,D3,D4,表示空间中的点到这四个卫星距离的平方。
(所有输入在
106内)
输出
每组case输出一行,包含三个整数,X Y Z 即测试点的坐标。
(我们确保每一问题只有唯一解,且该解为整点)
样例输入
1 0 0 0 0 0 1 1 0 0 0 1 0 4 1 5 5
样例输出
0 0 2
提示
友情提示:四颗卫星可能共面也可能共线,甚至部分卫星位置重合,但我们确保任何数据中都只有唯一解.
来源
思路:这是“华为杯”复赛的一道高斯消元题,时隔数月,终于AC。通过4个距离,建立4条3元2次方程,然后把(1)-(2),(1)-(3),(1)-(4),逐个想减,得到三条3元1次方程,是线性方程组,于是可以建立系数矩阵。有点坑的是:di==0 要特判,不然解方程会出现无穷解的情况。还有答案出来要特别注意0的情况,如果fabs(答案)<eps,就应该把答案赋值为0。
1 /************************************************************************* 2 > File Name: 115.cpp 3 > Author: Nature 4 > Mail: 564374850@qq.com 5 > Created Time: Wed 17 Sep 2014 03:29:59 PM CST 6 ************************************************************************/ 7 8 #include <cstdio> 9 #include <cstring> 10 #include <cstdlib> 11 #include <cmath> 12 #include <iostream> 13 #include <algorithm> 14 using namespace std; 15 typedef long long ll; 16 const int INF = 1 << 29; 17 const double eps = 1e-9; 18 19 int T; 20 double x[5],y[5],z[5]; 21 double d[5]; 22 double ans[10][10],X[10]; 23 24 void Gauss(){ 25 int equ,var; equ = var = 3; 26 int i,j,k,r,key,col = 0; 27 for(i = 0; i < equ && col < var; ++i,++col){ 28 r = i; 29 for(j = i + 1; j < equ; ++j) 30 if(fabs(ans[j][col]) > fabs(ans[r][col])) r = j; 31 if(r != i) 32 for(j = 0; j <= var; ++j) swap(ans[i][j],ans[r][j]); 33 if(fabs(ans[i][col]) < eps){ 34 --i; 35 continue; 36 } 37 for(k = i + 1; k < equ; ++k){ 38 double f = ans[k][col] / ans[i][col]; 39 for(j = col; j <= var; ++j) ans[k][j] -= f * ans[i][j]; 40 } 41 for(j = var; j >= col; --j) ans[i][j] /= ans[i][col]; //将该行主元系数化为1 42 } 43 for(i = var - 1; i >= 0; --i){ //从最后行开始找主元位置 44 for(key = 0; fabs(ans[i][key]) < eps; ++key); //主元位置 45 for(j = key + 1; j < 3; ++j) ans[i][3] -= ans[j][3] * ans[i][j]; 46 } 47 } 48 49 int main(){ 50 scanf("%d",&T); 51 while(T--){ 52 for(int i = 1; i <= 4; ++i) 53 scanf("%lf%lf%lf",&x[i],&y[i],&z[i]); 54 for(int i = 1; i <= 4; ++i) 55 scanf("%lf",&d[i]); 56 int flag = 0; 57 for(int i = 1; i <= 4; ++i) 58 if(fabs(d[i]) < eps){ 59 printf("%.0f %.0f %.0f\n",x[i],y[i],z[i]); 60 flag = 1; 61 break; 62 } 63 if(flag) 64 continue; 65 memset(ans,0,sizeof(ans)); 66 memset(X,0,sizeof(X)); 67 ans[0][0] = 2.0*(x[2]-x[1]); ans[0][1] = 2.0*(y[2]-y[1]); ans[0][2] = 2.0*(z[2]-z[1]); 68 ans[1][0] = 2.0*(x[3]-x[1]); ans[1][1] = 2.0*(y[3]-y[1]); ans[1][2] = 2.0*(z[3]-z[1]); 69 ans[2][0] = 2.0*(x[4]-x[1]); ans[2][1] = 2.0*(y[4]-y[1]); ans[2][2] = 2.0*(z[4]-z[1]); 70 double t[5]; 71 for(int i = 1; i <= 4; ++i) 72 t[i] = x[i]*x[i] + y[i]*y[i] + z[i]*z[i]; 73 ans[0][3] = d[1] - d[2] - t[1] + t[2]; 74 ans[1][3] = d[1] - d[3] - t[1] + t[3]; 75 ans[2][3] = d[1] - d[4] - t[1] + t[4]; 76 Gauss(); 77 for(int i = 0 ; i < 3; ++i) if(fabs(ans[i][3]) < eps) ans[i][3] = 0.0; 78 printf("%.0f %.0f %.0f\n",ans[0][3],ans[1][3],ans[2][3]); 79 } 80 return 0; 81 }