GPS模拟C语言

GPS模拟# include <iostream># include <fstream># include <math.h>using namespace std;ifstream fin ("1.txt");//ofstream fout ("data_out.txt");// 1 2 3 4double x[5] = {-1, 0, 300, 0, 300};double y[5] = {-1, 450, 450, 0, 0};double t[5];double A, B, D,c = 2982000, p, r;double pi = 3.141592653;double xx, yy;//三角形算法void cal (int sign, double t1, double t2, double x1, double y1, double x2, double y2, double px, double py){double tt1 = c * t1, tt2 = c * t2;double q, ac;A = x2 * (x1*x1 + y1*y1 - tt1*tt1) - x1 * (x2*x2 + y2*y2 - tt2*tt2);B = y2 * (x1*x1 + y1*y1 - tt1*tt1) - y1 * (x2*x2 + y2*y2 - tt2*tt2);D = tt1 * (x2*x2 + y2*y2 - tt2*tt2) - tt2 * (x1*x1 + y1*y1 - tt1*tt1);q = atan (B / A);ac = acos (D / sqrt (A*A + B*B));if (sign > 0)p = q + ac;elsep = q - ac;r = (x1*x1 + y1*y1 - tt1*tt1) / (2 * (x1 * cos (p) + y1 * sin (p) + tt1));xx = cos (p) * r;yy = sin (p) * r;cout << xx + px << endl << yy + py<< endl << endl;}int main (){int i, j, k;while (fin >> t[1]){t[1] /= 1e6;for (int h = 2; h < 5; h++){fin >> t[h];t[h] /= 1e6;}for (int h = 1; h < 5; h++)if (fabs (t[h] - 0) < 1e-6){switch (h){case 1:cal (-1, t[2], t[4], 300, 0, 300, -450, 0, 450);break;case 2: cal (-1, t[1], t[3], -300, 0, -300, -450,300, 450);break;case 3: cal (1, t[1], t[2], 0, 450, 300, 450, 0, 0);break;case 4: cal (1, t[1], t[2], -300, 450, 0, 450, 300, 0);break;}break;}}system ("pause");return 0;}声源定位#include <iostream>#include <cmath>using namespace std;#define Max 50int N=10;double xi[10];double yi[10];double ti[10][3];double det(double a[Max][Max],int n){int i,j,start;double k,temp,result=1;for(start=0;start<=n-2;start++){i=1;while(a[start][start]==0&&start+i<=n-1){for(j=start;j<=n-1;j++){temp=-1*a[start][j];a[start][j]=a[start+i][j];a[start+i][j]=temp;}i++;}if(start+i==n) continue;for(i=start+1;i<=n-1;i++){k=a[i][start]/a[start][start];for(j=start;j<=n-1;j++)a[i][j]=a[i][j]-k*a[start][j];}}for(i=0;i<=n-1;i++)result*=a[i][i];return result;}int equalation(double a[Max][Max+1],double r[Max],int n){double det(double a[Max][Max],int n);double det0[Max][Max],det1[Max][Max];int i,j,k;for(i=0;i<=n-1;i++)for(j=0;j<=n-1;j++)det0[i][j]=a[i][j];if(det(det0,n)==0) return 0;for(k=0;k<=n-1;k++){for(i=0;i<=n-1;i++)for(j=0;j<=n-1;j++){det0[i][j]=a[i][j];det1[i][j]=a[i][j];}for(i=0;i<=n-1;i++)det1[i][k]=a[i][n];r[k]=det(det1,n)/det(det0,n);}return 1;}double sqr(double x){return x*x;}double g(double x,double y,double c,double xi,double yi,double ti) {return sqr(xi-x)+sqr(yi-y)-sqr(c*ti);}double fx(double x,double y,double c,int k){int i;double sum=0;for (i=0;i<=N-1;i++)sum+=g(x,y,c,xi[i],yi[i],ti[i][k-1])*(xi[i]-x);return sum;}double fy(double x,double y,double c,int k){int i;double sum=0;for (i=0;i<=N-1;i++)sum+=g(x,y,c,xi[i],yi[i],ti[i][k-1])*(yi[i]-y);return sum;}double fc(double x,double y,double c,int k){int i;double sum=0;for (i=0;i<=N-1;i++)sum+=g(x,y,c,xi[i],yi[i],ti[i][k-1])*sqr(ti[i][k-1]);return sum;}double fxx(double x,double y,double c,int k){int i;double sum=0;for (i=0;i<=N-1;i++)sum=sum-2*sqr(xi[i]-x)-g(x,y,c,xi[i],yi[i],ti[i][k-1]);return sum;}double fxy(double x,double y,double c,int k){int i;double sum=0;for (i=0;i<=N-1;i++)sum=sum-2*(yi[i]-y)*(xi[i]-x);return sum;}double fxc(double x,double y,double c,int k){int i;double sum=0;for (i=0;i<=N-1;i++)sum=sum-2*c*sqr(ti[i][k-1])*(xi[i]-x);return sum;}double fyx(double x,double y,double c,int k){int i;double sum=0;for (i=0;i<=N-1;i++)sum=sum-2*(xi[i]-x)*(yi[i]-y);return sum;}double fyy(double x,double y,double c,int k){int i;double sum=0;for (i=0;i<=N-1;i++)sum=sum-2*sqr(yi[i]-y)-g(x,y,c,xi[i],yi[i],ti[i][k-1]);return sum;}double fyc(double x,double y,double c,int k) {int i;double sum=0;for (i=0;i<=N-1;i++)sum=sum-2*c*sqr(ti[i][k-1])*(yi[i]-y);return sum;}double fcx(double x,double y,double c,int k) {int i;double sum=0;for (i=0;i<=N-1;i++)sum=sum-2*(xi[i]-x)*sqr(ti[i][k-1]);return sum;}double fcy(double x,double y,double c,int k) {int i;double sum=0;for (i=0;i<=N-1;i++)sum=sum-2*(yi[i]-y)*sqr(ti[i][k-1]);return sum;}double fcc(double x,double y,double c,int k) {int i;double sum=0;for (i=0;i<=N-1;i++)sum=sum-2*c*sqr(sqr(ti[i][k-1]));return sum;}int main(){int i=0,k;double x,y,c,x0,y0,c0;double a[Max][Max+1],r[Max];freopen ("1.txt", "r", stdin);freopen ("1_out.txt", "w", stdout);for (i=0;i<=9;i++){cin>>xi[i]>>yi[i];xi[i] /= 10;yi[i] /= 10;cin>>ti[i][0]>>ti[i][1]>>ti[i][2];}for(k=1;k<=3;k++){cin>>x>>y;c=0.2;do{x0=x;y0=y;c0=c;i++;a[0][0]=fxx(x,y,c,k);a[0][1]=fxy(x,y,c,k);a[0][2]=fxc(x,y,c,k);a[1][0]=fyx(x,y,c,k);a[1][1]=fyy(x,y,c,k);a[1][2]=fyc(x,y,c,k);a[2][0]=fcx(x,y,c,k);a[2][1]=fcy(x,y,c,k);a[2][2]=fcc(x,y,c,k);a[0][3]=x*a[0][0]+y*a[0][1]+c*a[0][2]-fx(x,y,c,k);a[1][3]=x*a[1][0]+y*a[1][1]+c*a[1][2]-fy(x,y,c,k);a[2][3]=x*a[2][0]+y*a[2][1]+c*a[2][2]-fc(x,y,c,k);if(equalation(a,r,3)==0){cout<<"第"<<k<<"个用户的坐标求解错误，请降低精度！"<<endl;return 0;}x=r[0];y=r[1];c=r[2];}while(abs(x-x0)+abs(y-y0)+abs(c-c0)>=1e-6);cout<<"第"<<k<<"个用户的坐标为："<<endl<<"x="<<x<<"cm"<<endl<<"y="<<y<<"cm"<<endl<<"参考波速为：c="<<c*1e4<<"m/s"<<endl;}getchar();return 0; }。