北航物理实验E04——Code

物理实验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;
}

• June 4, 2010 -- 怨念的外场申请 (7)
• April 18, 2010 -- 浑浑噩噩ing (8)
• April 6, 2010 -- Sleepless Night (6)
• February 13, 2010 -- Hello World Press！ (6)
• September 19, 2009 -- 觉醒…… (4)