物理实验E04：声源定位和GPS模拟，纠结了好久，竟然还要交数据处理的源程序
程序倒不难，关键是不知道数据怎么处理的话就……于是我就杯具了……
网上竟然没有什么现成的程序，看样大家还不够团结啊～
贴在这里，完全开源，请遵守GPL协议，造福后人……
/*
*ID:38211214
*Program:声源定位
*/
/*
*in.txt
* 45.4 73.7 0 42.7
* 0 55.0 107.8 126.3
*100.2 112.0 0 25.8
*106.0 78.7 53.0 0
* 0 34.6 9.3 34.6
* 0 25.6 74.0 85.5
* 81.5 52.6 50.1 0
* 0 62.3 134.8 151.6
* 1.3 0 94.3 91.6
*/
/*
*out.txt
*( 53.29,153.49)
*( 60.26,393.04)
*( 98.92, 49.82)
*(254.10,103.05)
*( 89.57,364.71)
*( 98.04,317.42)
*(249.78,142.83)
*( 50.46,403.15)
*(151.00,405.37)
*/
#include <iostream>
#include <iomanip>
#include <cmath>
using namespace std;
const double sensor_x[4] = { 0, 298, 0, 298}; //传感器横坐标
const double sensor_y[4] = { 446, 446, 0, 0}; //传感器纵坐标
const double c = 2970000; //常数，声音的传播速度，单位"mm/s"
const double pi = acos(-1.0); //常数π
double det_t[4] = {0}; //时间间隔
double A, B, D, phi; //计算公式中间量A, B, D, Φ
double radius, theta; //声源坐标，极坐标表示(r,θ)
double source_x, source_y; //声源坐标，直角坐标表示
void cal(int sign, double t1, double t2, double x1, double y1, double x2, double y2, double px, double py) //计算公式的实现
{
double det1 = c * t1, det2 = c * t2;
//中间量的计算
A = x2 * (x1 * x1 + y1 * y1 - det1 * det1) - x1 * (x2 * x2 + y2 * y2 - det2 * det2);
B = y2 * (x1 * x1 + y1 * y1 - det1 * det1) - y1 * (x2 * x2 + y2 * y2 - det2 * det2);
D = det1 * (x2 * x2 + y2 * y2 - det2 * det2) - det2 * (x1 * x1 + y1 * y1 - det1 * det1);
phi = atan(B / A);
theta = phi + sign * acos(D / sqrt(A * A + B * B));
radius = (x1 * x1 + y1 * y1 - det1 * det1 ) / (2 * ( x1 * cos(theta) + y1 * sin(theta) + det1 ));
//最终结果及输出
source_x = cos (theta) * radius;
source_y = sin (theta) * radius;
cout < <fixed <<setprecision(2)
<<'(' <<setw(6) <<source_x + px << ',' <<setw(6) <<source_y + py <<')' <<endl;
}
int main()
{
freopen("in.txt", "r", stdin); //打开输入文件
freopen("out.txt", "w", stdout); //打开输出文件
for (int i = 0; i < 9; ++ i)
{
for (int j = 0; j < 4; ++ j) //数据读入
{
cin >>det_t[j];
det_t[j] /= 1e6;
}
for (int j = 0; j < 4; ++ j)
{
if (fabs(det_t[j] - 0) < 1e-6) //判断最先到达的传感器编号
{
switch (j)
{
case 0:
cal(-1, det_t[1], det_t[3], 298, 0, 298, -446, 0, 446);
break;
case 1:
cal(-1, det_t[0], det_t[2], -298, 0, -298, -446, 298, 446);
break;
case 2:
cal(1, det_t[0], det_t[1], 0, 446, 298, 446, 0, 0);
break;
case 3:
cal(1, det_t[0], det_t[1], -298, 446, 0, 446, 298, 0);
break;
}
}
}
}
return 0;
}
/*
*ID:38211214
*Program:GPS仿真
*/
/*
*in.txt
* 87.7 113.7 88.1 85.0 224.0
* 89.9 67.7 124.9 203.0 303.0
*147.5 123.5 95.9 240.0 109.0
*112.2 91.3 106.0 208.0 216.0
*112.4 140.7 56.0 49.0 137.0
* 90.3 79.9 112.1 179.0 269.0
*128.8 88.7 121.0 272.0 209.0
* 67.2 62.5 142.3 158.0 369.0
* 94.3 111.3 98.2 128.0 228.0
* 78.5 79.8 118.5 149.0 297.0
*/
/*
*out.txt
*( -9.51,458.73)
*(313.00,460.32)
*( -8.63, -7.67)
*/
#include <iostream>
#include <iomanip>
#include <cmath>
using namespace std;
const double c = 2970; //常数：声音传播速度
double det_t[3][10] = {0}; //三个接收器的时间差
double trans_x[10],trans_y[10]; //发送器（transmitter）坐标;
double a1, a2, a3, a4, b1, b2, mult, D, E; //算法过程中间量
void gps(double t[], double x1, double y1, int num)
{
D = E = 0;
do
{
a1 = a2 = a3 = a4 = b1 = b2 = 0;
for (int i = 0; i < 10; ++ i)
{
double m1 = trans_x[i] * 1e-3 - x1;
double m2 = y1 - trans_y[i] * 1e-3;
double m3 = (c * t[i] * 1e-6) * (c * t[i] * 1e-6);
a1 = a1 - 3 * (trans_x[i] * 1e-3 - x1) * (trans_x[i] * 1e-3 - x1) + m2 * m2 - m3;
a2 = a2 + 2 * m1 * m2;
a3 = a2;
a4 = a4 - 3 * (y1 - trans_y[i] * 1e-3) * (y1 - trans_y[i] * 1e-3) + m1 * m1 - m3;
b1 = b1 + (m1 * m1 + m2 * m2 - m3) * (- m1);
b2 = b2 + (m1 * m1 + m2 * m2 - m3) * m2;
}
mult = a3 / a1;
a4 = a4 - a2 * mult;
b2 = b2 - b1 * mult;
E = b2 / a4;
D = (b1 - a2 * E) / a1;
x1 += D;
y1 += E;
}
while (D * D + E * E <= 1e-9); //达到精度之后停止
cout <<fixed <<setprecision(2)
<<'(' <<setw(6) <<x1 * 1e3 <<',' <<setw(6) <<y1 * 1e3 <<')' <<endl;
}
int main()
{
freopen("in.txt", "r", stdin); //打开输入文件
freopen("out.txt", "w", stdout); //打开输出文件
for (int i = 0; i < 10; ++ i) //读入数据
cin >>det_t[0][i] >>det_t[1][i] >>det_t[2][i] >>trans_x[i] >>trans_y[i];
//三次调用计算过程
gps(det_t[0], 0, 0.45, 1);
gps(det_t[1], 0.3, 0.45, 2);
gps(det_t[2], 0, 0, 3);
return 0;
}