Author: Xuan Hu

物理实验E04：声源定位和GPS模拟，纠结了好久，竟然还要交数据处理的源程序
程序倒不难，关键是不知道数据怎么处理的话就……于是我就杯具了……
网上竟然没有什么现成的程序，看样大家还不够团结啊～
贴在这里，完全开源，请遵守GPL协议，造福后人……

[cpp]
/*
*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
#include
#include
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 < >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; } [/cpp] [cpp] /* *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
#include
#include
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 <>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;
}
[/cpp]