Seuoj--155（数学，高斯消元）

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

提示

 友情提示：四颗卫星可能共面也可能共线，甚至部分卫星位置重合，但我们确保任何数据中都只有唯一解.

来源

windhunter
 
思路：这是“华为杯”复赛的一道高斯消元题，时隔数月，终于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 }

 

 
posted @ 2014-09-17 21:34  Naturain  阅读(278)  评论(0)  编辑  收藏  举报