API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Nov 29th, 2015
57
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 20.86 KB | None | 0 0
raw download clone embed print report
  1. //0: xyz grid reference
  2. //in Km, and nanoseconds
  3. using namespace std;
  4.  
  5. #include <iostream>
  6. #include <string>
  7. #include <cmath>
  8. #include <vector>
  9. #include <ctime>
  10. #include <cstdlib>
  11. #include <cstdio>
  12. #include <unistd.h>
  13. #include <fstream>
  14.  
  15. long double C = 300000000.0;
  16. long double R = 6400.0;
  17. long double TIMERESOLUTION = 100000.0;
  18. // in picoseconds, simulation-time for a tick
  19. long double RTIMERESOLUTION = 100000.0;
  20. // in microseconds, real-time for a tick
  21. #define ERRORRATE 1000
  22. #define ALTCOEFF 200
  23. #define ERRORTOLORATE 10000
  24. #define PRECISIONDEFINITION cout.precision(15)
  25. #define PRINTLINE cout << "---------------------------------" << endl
  26.  
  27. //string FILENAME = "log";
  28. //ofstream fout( FILENAME.c_str(), std::ios::app);
  29.  
  30. //class sate;
  31. //class center;
  32. //class user;
  33.  
  34. class sate;
  35. class center;
  36.  
  37. vector< sate > vSate;
  38. vector< center > vCenter;
  39.  
  40. long double tpr_x(long double theta, long double phi, long double rad){
  41.   return rad*sin(theta)*cos(phi);
  42. }
  43. long double tpr_y(long double theta, long double phi, long double rad){
  44.   return rad*sin(theta)*sin(phi);
  45. }
  46. long double tpr_z(long double theta, long double phi, long double rad){
  47.   return rad*cos(theta);
  48. }
  49. long double randomFloat(long double a, long double b) {
  50.     long double random = ((long double) rand()) / (long double) RAND_MAX;
  51.     long double diff = b - a;
  52.     long double r = random * diff;
  53.     return a + r;
  54. }
  55. long double dist(long double x, long double y, long double z, long double a, long double b, long double c){
  56.   return sqrt((x-a)*(x-a)+(y-b)*(y-b)+(z-c)*(z-c));
  57. }
  58. long double cDist(long double x, long double y, long double z, long double a, long double b, long double c, long long error){
  59.   return dist(x,y,z,a,b,c)-(((long double)(C * error)+0.0)/1000000000000);
  60. }
  61. int chkComm(long double x,long double y,long double z,long double a,long double b, long double c){
  62.   long double check=(a*x-x*x+b*y-y*y+c*z-z*z)
  63.                     /(sqrt((a-x)*(a-x)+(b-y)*(b-y)+(c-z)*(c-z))*sqrt(x*x+y*y+z*z));
  64. //  cout << "DEBUG: COS(THETA) = "<< check << endl;
  65. //  cout << "DEBUG: A = "<< (a*x-x*x+b*y-y*y+c*z-z*z) << endl;
  66. //  cout << "DEBUG: /B = "<< (sqrt((a-x)*(a-x)+(b-y)*(b-y)+(c-z)*(c-z))*sqrt(x*x+y*y+z*z)) << endl;
  67. //  cout << "DEBUG: /p = "<< sqrt((a-x)*(a-x)+(b-y)*(b-y)+(c-z)*(c-z)) << endl;
  68. //  cout << "DEBUG: /q = "<< sqrt(x*x+y*y+z*z) << endl;
  69. //  cout << "DEBUG: A/B = "<< (a*x-x*x+b*y-y*y+c*z-z*z)/(sqrt((a-x)*(a-x)+(b-y)*(b-y)+(c-z)*(c-z))*sqrt(x*x+y*y+z*z)) << endl;
  70.   if(0.0<=check){
  71.     return 1;
  72.   }
  73.   else{
  74.     return 0;
  75.   }
  76. }
  77. class xyz{
  78. public:
  79.   long double x_;
  80.   long double y_;
  81.   long double z_;
  82.  
  83.   xyz(){}
  84. };
  85.  
  86. class sate{
  87. private:
  88.   long double x_;
  89.   long double y_;
  90.   long double z_;
  91.  
  92.   long double theta_;
  93.   long double phi_;
  94.   long double rad_;
  95.  
  96.   long long error_;
  97.  
  98. public:
  99.   sate(){}
  100.   void setSateposition(){
  101.     theta_ = randomFloat(0.0, M_PI);
  102.     phi_ =  randomFloat(0.0, 2*M_PI);
  103.     rad_ = R+randomFloat(100,100+ALTCOEFF);
  104.  
  105.     x_=tpr_x(theta_, phi_, rad_);
  106.     y_=tpr_y(theta_, phi_, rad_);
  107.     z_=tpr_z(theta_, phi_, rad_);
  108. //    cout << "TPR: "<<theta_<<" "<<phi_<<" "<<rad_<<" XYZ: "<<x_<<" "<<y_<<" "<<z_<<endl;
  109.   }
  110.   void setSateposition(int i, long double p, long double q);
  111.   void setSateposition(long double a, long double b, long double c){
  112.       theta_ = a;
  113.       phi_ = b;
  114.       rad_ = c;
  115.  
  116.       x_=tpr_x(theta_, phi_, rad_);
  117.       y_=tpr_y(theta_, phi_, rad_);
  118.       z_=tpr_z(theta_, phi_, rad_);
  119. //      cout << "TPR: "<<theta_<<" "<<phi_<<" "<<rad_<<" XYZ: "<<x_<<" "<<y_<<" "<<z_<<endl;
  120.     }
  121.   void setError(){
  122.     error_=error_+TIMERESOLUTION*ERRORRATE*(rand()%100);
  123.   }
  124.   void setError(long long error){
  125.     error_=error;
  126.   }
  127.   long double getx(void){
  128.     return x_;
  129.   }
  130.   long double gety(void){
  131.     return y_;
  132.   }
  133.   long double getz(void){
  134.     return z_;
  135.   }
  136.   long double gett(void){
  137.     return theta_;
  138.   }
  139.   long double getp(void){
  140.     return phi_;
  141.   }
  142.   long double getr(void){
  143.     return rad_;
  144.   }
  145.   long long getError(void){
  146.     return error_;
  147.   }
  148.  
  149. };
  150. class center{
  151. private:
  152.   long double x_;
  153.   long double y_;
  154.   long double z_;
  155.  
  156.   long double theta_;
  157.   long double phi_;
  158.   long double rad_;
  159.  
  160.   long long tError_;
  161.  
  162.   vector <sate*> sate_;
  163.  
  164. public:
  165.   center(long long tError){
  166.     tError_ = tError;
  167.   }
  168.   void setCenterposition(long double a, long double b, long double c){
  169.       theta_ = a;
  170.       phi_ = b;
  171.       rad_ = c;
  172.  
  173.       x_=tpr_x(theta_, phi_, rad_);
  174.       y_=tpr_y(theta_, phi_, rad_);
  175.       z_=tpr_z(theta_, phi_, rad_);
  176.  //     cout << "TPR: "<<theta_<<" "<<phi_<<" "<<rad_<<" XYZ: "<<x_<<" "<<y_<<" "<<z_<<endl;
  177.     }
  178.   void setCenterposition(){
  179.  
  180.     theta_ = randomFloat(0.0, M_PI);
  181.     phi_ =  randomFloat(0.0, 2*M_PI);
  182.     rad_ = R;
  183.  
  184.     x_=tpr_x(theta_, phi_, rad_);
  185.     y_=tpr_y(theta_, phi_, rad_);
  186.     z_=tpr_z(theta_, phi_, rad_);
  187.  
  188. //    cout << "TPR: "<<theta_<<" "<<phi_<<" "<<rad_<<" XYZ: "<<x_<<" "<<y_<<" "<<z_<<endl;
  189.   }
  190.  
  191.   void init(){
  192.     cout << "DEBUG: INITIALIZING CENTER WITH (NEW) SATES SET" << endl;
  193.     sate_.clear();
  194.     for(int i=0; i < vSate.size(); i++){
  195.       //if coummunicatable,
  196.       sate* Sate = &vSate[i];
  197.       if(chkComm(x_,y_,z_,Sate->getx(),Sate->gety(),Sate->getz())){
  198.         sate_.push_back(Sate);
  199.         cout << "DEBUG: A COMMUNCATABLE SATE FOUND WITH XYZ: "<<Sate->getx()<<" "<<Sate->gety()<<" "<<Sate->getz()<<endl;
  200.       }
  201.       else{
  202.         cout << "DEBUG: AN UNCOMMUNICATABLE SATE FOUND WITH XYZ: "<<Sate->getx()<<" "<<Sate->gety()<<" "<<Sate->getz()<<endl;
  203.       }
  204.     }
  205.   }
  206.   void syncTime(){
  207.     for(int i=0; i < sate_.size(); i++){
  208.       sate* Sate = sate_[i];
  209.       if(abs(Sate->getError())>tError_){
  210.         Sate->setError(.0);
  211.       }
  212.     }
  213.   }
  214.   long double getx(void){
  215.     return x_;
  216.   }
  217.   long double gety(void){
  218.     return y_;
  219.   }
  220.   long double getz(void){
  221.     return z_;
  222.   }
  223.   long double gett(void){
  224.     return theta_;
  225.   }
  226.   long double getp(void){
  227.     return phi_;
  228.   }
  229.   long double getr(void){
  230.     return rad_;
  231.   }
  232. };
  233.  
  234. class user{
  235. private:
  236.   long double x_;
  237.   long double y_;
  238.   long double z_;
  239.  
  240.   long double theta_;
  241.   long double phi_;
  242.   long double rad_;
  243.  
  244.   vector<sate*> commSate_;
  245.  
  246. public:
  247.   user(){}
  248.   void init(){
  249.     commSate_.clear();
  250.     for(int i=0;i<vSate.size();i++){
  251.       sate* Sate = &vSate[i];
  252.       if(chkComm(x_,y_,z_,Sate->getx(),Sate->gety(),Sate->getz())){
  253.         commSate_.push_back(Sate);
  254.  //       cout << "DEBUG: COMMUNICATABLE SATE FOUUND WITH TPR: "<<Sate->getx()<<" "<<Sate->gety()<<" "<<Sate->getz()<<endl;
  255.       }
  256.     }
  257.   }
  258.   void setUserposition(long double a, long double b, long double c){
  259.     theta_ = a;
  260.     phi_ = b;
  261.     rad_ = c;
  262.  
  263.     x_=tpr_x(theta_, phi_, rad_);
  264.     y_=tpr_y(theta_, phi_, rad_);
  265.     z_=tpr_z(theta_, phi_, rad_);
  266.  //   cout << "TPR: "<<theta_<<" "<<phi_<<" "<<rad_<<" XYZ: "<<x_<<" "<<y_<<" "<<z_<<endl;
  267.   }
  268.   void setUserposition(){
  269.     theta_ = randomFloat(0.0, M_PI);
  270.     phi_ =  randomFloat(0.0, 2*M_PI);
  271.     rad_ = R;
  272.  
  273.     x_=tpr_x(theta_, phi_, rad_);
  274.     y_=tpr_y(theta_, phi_, rad_);
  275.     z_=tpr_z(theta_, phi_, rad_);
  276.  //   cout << "TPR: "<<theta_<<" "<<phi_<<" "<<rad_<<" XYZ: "<<x_<<" "<<y_<<" "<<z_<<endl;
  277.   }
  278.  
  279.   xyz* printCoordinate(void){
  280.     if(commSate_.size() < 3){
  281.       cout << "FATAL ERROR: NOT ENOUGH SATES" << endl;
  282.       cout << "DEBUG: YOU HAVE "<<commSate_.size()<<" COMMUNICATABLE SATES NOW" << endl;
  283.     }
  284.     else{
  285.       int i, j, k, n;
  286.       long double ax, ay, az, at, ap, ar;
  287.       ax = ay = az = at = ap = ar = 0.0;
  288.       for(i=0, n=0;i<commSate_.size()-2;i++){
  289.         for(j=i+1;j<commSate_.size()-1;j++){
  290.           for(k=j+1;k<commSate_.size();k++){
  291.             // getCoordinate();
  292.             // vector[i]와 j, k를 이용하여 getCoordinate한 것들을, 평균
  293.               sate* S1 = commSate_[i];
  294.               sate* S2 = commSate_[j];
  295.               sate* S3 = commSate_[k];
  296. //              cout << "NOW CALCULATING WITH "<<i<<", "<<j<<", "<<k<<" SATES" << endl;
  297.               xyz* XYZ = getCoordinate(S1->getx(),S1->gety(),S1->getz(),
  298.                                        S2->getx(),S2->gety(),S2->getz(),
  299.                                        S3->getx(),S3->gety(),S3->getz(),
  300.                                        cDist(x_,y_,z_,S1->getx(),S1->gety(),S1->getz(),S1->getError()),
  301.                                        cDist(x_,y_,z_,S2->getx(),S2->gety(),S2->getz(),S2->getError()),
  302.                                        cDist(x_,y_,z_,S3->getx(),S3->gety(),S3->getz(),S3->getError()));
  303.             //A의 좌표, B의 좌표, C의 좌표, A까지 추정 거리, B까지 추정 거리, C까지 추정 거리
  304. //            cout << XYZ->x_<<" "<< XYZ->y_<<" "<< XYZ->z_<<" "<< endl;
  305.             ax = ax + XYZ->x_;
  306.             ay = ay + XYZ->y_;
  307.             az = az + XYZ->z_;
  308.  
  309.             delete XYZ;
  310.             n++;
  311.           }
  312.         }
  313.       }
  314.  
  315.       ax = ax / (n+.0);
  316.       ay = ay / (n+.0);
  317.       az = az / (n+.0);
  318. /*
  319.       PRINTLINE;
  320.       cout << "AVERAGE X ASSUMPTION: " << ax << endl;
  321.       cout << "AVERAGE Y ASSUMPTION: " << ay << endl;
  322.       cout << "AVERAGE Z ASSUMPTION: " << az << endl;
  323.       PRINTLINE;
  324. */
  325.       at = acos(az/sqrt(ax*ax+ay*ay+az*az));
  326.       ap = atan2(ay,ax);
  327.       ar = sqrt(ax*ax+ay*ay+az*az);
  328. /*
  329.       cout << "AVERAGE THETA ASSUMPTION: " << at << endl;
  330.       cout << "AVERAGE PHI ASSUMPTION: " << ap << endl;
  331.       cout << "AVERAGE RHO ASSUMPTION: " << ar << endl;
  332. */
  333.  
  334.       xyz* XYZ = new xyz();
  335.       XYZ->x_ = ax;
  336.       XYZ->y_ = ay;
  337.       XYZ->z_ = az;
  338.  
  339.       return XYZ;
  340.     }
  341.   }
  342.  
  343.   xyz* getCoordinate(long double a, long double b, long double c, long double p, long double q, long double r, long double i, long double j, long double k, long double d, long double s, long double l){
  344.  
  345.     long double x_, y_, z_;
  346.     x_ = -(((4*c*q - 4*b*r)*(2*k*(a*a + b*b + c*c - d*d + R*R) -
  347.                2*c*(i*i + j*j + k*k - l*l + R*R)) - (4*c*j -
  348.                4*b*k)*(2*r*(a*a + b*b + c*c - d*d + R*R) -
  349.                2*c*(p*p + q*q + r*r + R*R - s*s)))/((-16*(c*c))*j*p +
  350.             16*b*c*k*p + 16*(c*c)*i*q - 16*a*c*k*q - 16*b*c*i*r +
  351.             16*a*c*j*r));
  352.  
  353.     y_ = -((c*i*i*p + c*j*j*p - a*a*k*p - b*b*k*p - c*c*k*p + d*d*k*p +
  354.           c*k*k*p - c*l*l*p - c*i*p*p + a*k*p*p - c*i*q*q + a*k*q*q +
  355.           a*a*i*r + b*b*i*r + c*c*i*r - d*d*i*r - a*i*i*r - a*j*j*r -
  356.           a*k*k*r + a*l*l*r - c*i*r*r + a*k*r*r - c*i*R*R + a*k*R*R +
  357.           c*p*R*R - k*p*R*R - a*r*R*R + i*r*R*R + c*i*s*s -
  358.           a*k*s*s)/(2*(-c*j*p + b*k*p + c*i*q - a*k*q - b*i*r +
  359.             a*j*r)));
  360.  
  361.     z_ = (-b*i*i*p + a*a*j*p + b*b*j*p + c*c*j*p -d*d*j*p - b*j*j*p -
  362.         b *k*k* p + b*l*l*p + b*i*p*p - a*j*p*p - a*a*i*q - b*b*i*q -
  363.         c*c* i* q + d*d*i*q + a*i*i*q + a*j*j*q + a*k*k*q - a*l*l*q +
  364.         b*i*q*q - a*j*q*q + b*i*r*r - a*j*r*r + b*i*R*R - a*j*R*R -
  365.         b*p*R*R + j*p*R*R + a*q*R*R - i*q*R*R - b*i*s*s +
  366.         a*j*s*s)/(2*(c*j*p - b*k*p - c*i*q + a*k*q + b*i*r - a*j*r));
  367.     xyz* XYZ = new xyz();
  368.     XYZ->x_=x_;
  369.     XYZ->y_=y_;
  370.     XYZ->z_=z_;
  371.  
  372.     return XYZ;
  373.   }
  374.   long double getx(void){
  375.     return x_;
  376.   }
  377.   long double gety(void){
  378.     return y_;
  379.   }
  380.   long double getz(void){
  381.     return z_;
  382.   }
  383.   long double gett(void){
  384.     return theta_;
  385.   }
  386.   long double getp(void){
  387.     return phi_;
  388.   }
  389.   long double getr(void){
  390.     return rad_;
  391.   }
  392. };
  393.  
  394. /* REFERENCE TO CHECK WHETHER IMPLEMENTATION WAS CORRECTLY DONE
  395.  
  396. Solve[{(x - a)^2 + (y - b)^2 + (z - c)^2 - (x^2 + y^2 + z^2) ==
  397.    d^2 - R^2, (x - p)^2 + (y - q)^2 + (z - r)^2 - (x^2 + y^2 + z^2) ==
  398.     s^2 - R^2, (x - i)^2 + (y - j)^2 + (z - k)^2 - (x^2 + y^2 +
  399.       z^2) == l^2 - R^2}, {x, y, z}]
  400.  
  401. {{x -> -(((4 c q - 4 b r) (2 k (a^2 + b^2 + c^2 - d^2 + R^2) -
  402.            2 c (i^2 + j^2 + k^2 - l^2 + R^2)) - (4 c j -
  403.            4 b k) (2 r (a^2 + b^2 + c^2 - d^2 + R^2) -
  404.            2 c (p^2 + q^2 + r^2 + R^2 - s^2)))/(-16 c^2 j p +
  405.         16 b c k p + 16 c^2 i q - 16 a c k q - 16 b c i r +
  406.         16 a c j r)),
  407.   y -> -((c i^2 p + c j^2 p - a^2 k p - b^2 k p - c^2 k p + d^2 k p +
  408.         c k^2 p - c l^2 p - c i p^2 + a k p^2 - c i q^2 + a k q^2 +
  409.         a^2 i r + b^2 i r + c^2 i r - d^2 i r - a i^2 r - a j^2 r -
  410.         a k^2 r + a l^2 r - c i r^2 + a k r^2 - c i R^2 + a k R^2 +
  411.         c p R^2 - k p R^2 - a r R^2 + i r R^2 + c i s^2 -
  412.         a k s^2)/(2 (-c j p + b k p + c i q - a k q - b i r +
  413.           a j r))),
  414.   z -> (-b i^2 p + a^2 j p + b^2 j p + c^2 j p - d^2 j p - b j^2 p -
  415.       b k^2 p + b l^2 p + b i p^2 - a j p^2 - a^2 i q - b^2 i q -
  416.       c^2 i q + d^2 i q + a i^2 q + a j^2 q + a k^2 q - a l^2 q +
  417.       b i q^2 - a j q^2 + b i r^2 - a j r^2 + b i R^2 - a j R^2 -
  418.       b p R^2 + j p R^2 + a q R^2 - i q R^2 - b i s^2 +
  419.       a j s^2)/(2 (c j p - b k p - c i q + a k q + b i r - a j r))}}
  420.  
  421.  
  422.  
  423. coordinate      distance
  424. (a,b,c)        d
  425. (p,q,r)        s
  426. (i,j,k)        l
  427.  
  428. */
  429.  
  430. void sate::setSateposition(int i, long double p, long double q){
  431.   if(i > (vCenter.size()-1)){
  432.     cout << "FATAL ERROR: THERE ARE UP TO #" << vCenter.size()-1 << " CENTERS" << endl;
  433.   }
  434.   else{
  435.     long double st, sp, sr, sx, sy, sz;
  436.     center* Center = &vCenter[i];
  437.  
  438.     while(1){
  439.       sr = randomFloat(p, q);
  440.       st = randomFloat(0, 2*M_PI);
  441.       sp = randomFloat(0, M_PI);
  442.  
  443.       sx = tpr_x(st, sp, sr);
  444.       sy = tpr_y(st, sp, sr);
  445.       sz = tpr_z(st, sp, sr);
  446.  
  447. //    cout << "DEBUG: A TEMP SATE GENERATED WITH TPR: "<<st<<" "<<sp<<" "<<sr<<endl;
  448.       if(chkComm(Center->getx(),Center->gety(),Center->getz(),sx,sy,sz)){
  449.         rad_=sr;
  450.         theta_=st;
  451.         phi_=sp;
  452.  
  453.         x_=sx;
  454.         y_=sy;
  455.         z_=sz;
  456.  
  457. //        cout << "DEBUG: A SATE GENERATED  WITH ";
  458.         break;
  459.       }
  460.       else{
  461.  
  462.       }
  463.     }
  464. //    cout << "TPR: "<<theta_<<" "<<phi_<<" "<<rad_<<endl;
  465. //    cout << "XYZ: "<<x_<<" "<<y_<<" "<<z_<<endl;
  466.   }
  467. }
  468.  
  469.  
  470. int main(){
  471.  
  472.   PRECISIONDEFINITION;
  473.   srand(time(NULL));
  474.   int i;
  475.   long double a, b, c;
  476. /*
  477.   system("clear");
  478.   cout << "THIS IS A TEST PROGRAMME" << endl;
  479.   cout << "SETTINGS: "<< endl << "TIME IN SIMULATION FOR A TICK(n): " << TIMERESOLUTION << endl;
  480.   cout << "TIME IN REAL WORLD FOR A TICK(u): " << RTIMERESOLUTION << endl;
  481. */
  482. /*
  483.   //USER POSITION ASSUMING
  484.   center* Center;
  485.   sate* Sate;
  486.  
  487.   cout << "HOW MANY CENTERS ARE THERE IN THIS SYSTEM: " << endl;
  488.   cin >> i;
  489.   if(i<1){
  490.     system("clear");
  491.     return 0;
  492.   }
  493.  
  494.   for(int k=0;k<i;k++){
  495.     PRINTLINE;
  496.     vCenter.push_back(center(ERRORTOLORATE));
  497.     Center = &vCenter[k];
  498.     cout << "INPUT CENTER "<<k<<"'S POSITION IN TPR <THETA, PHI>: "<<endl;
  499. //    cin >> a >> b >> c;
  500.     cin >> a >> b;
  501.     Center->setCenterposition(a,b,R);
  502.     cout <<"CENTER "<<k<<" GENERATED" << endl;
  503.   }
  504.   for(int k=0;;k++){
  505.     PRINTLINE;
  506.  
  507.     cout << "INPUT SATE "<<k<<"'S TARGET CENTER, MIN RAD, MAX RAD: "<<endl;
  508.     cout << "TO SKIP, INPUT ANY NEGATIVE INTEGERS" << endl;
  509.     vSate.push_back(sate());
  510.     Sate = &vSate[k];
  511.  
  512.     int n;
  513.     cin >> n >> a >> b;
  514.     if(n < 0 || b < 0 || c < 0){
  515.       break;
  516.     }
  517.     Sate->setSateposition(n, a, b);
  518.     Sate->setError(0.0);
  519.  
  520.     for(int j=0;j<vCenter.size();j++){
  521.       cout << "CENTER #"<<j<<" SELECTED"<<endl;
  522.       Center = &vCenter[j];
  523.       Center->init();
  524.     }
  525.   }
  526.  
  527.   user* User = new user();
  528.   PRINTLINE;
  529.   cout << "INPUT USER'S POS IN TPR <THETA, PHI>: ";
  530. //  cin >> a >> b >> c;
  531.   cin >> a >> b;
  532.   User->setUserposition(a, b ,R);
  533.   User->init();
  534.   User->printCoordinate();
  535. */
  536.  
  537. /*
  538. //Test2
  539.  
  540. sate* S1;
  541. sate* S2;
  542. sate* S3;
  543.  
  544. long double at, ap, ar, ax, ay, az, eDist;
  545. int count;
  546.  
  547. for(unsigned long i=0;i<1000000000;i=i+10){
  548.     count = 0;
  549.     for(int j=0;j<10000;j++){
  550.  //       PRINTLINE;
  551.  //       cout << "DEBUG: ERROR IS : " << i << ", " << j << " TRY" << endl;
  552.  //       PRINTLINE;
  553.         vCenter.push_back(center(0.0));
  554.         center* Center = &vCenter[0];
  555.         Center->setCenterposition();
  556.  
  557.         user* User = new user();
  558.         User->setUserposition(Center->gett(),Center->getp(),Center->getr());
  559. //        cout << "DEBUG: USER WITH TPR: " << User->gett() << " " << User->getp() << " " << User->getr() << " GENERATED" << endl;
  560.  
  561.         sate* Sate;
  562.         for(int k=0;k<3;k++){
  563.             vSate.push_back(sate());
  564.             Sate = &vSate[k];
  565.             Sate->setError(i);
  566.             Sate->setSateposition(0,R+ALTCOEFF,R+ALTCOEFF*(rand()%2));
  567. //            cout << "DEBUG: SATE WITH TPR: " << Sate->gett() << " " << Sate->getp() << " " << Sate->getr() << " GENERATED" << endl;
  568.         }
  569.  
  570.         S1=&vSate[0];
  571.         S2=&vSate[1];
  572.         S3=&vSate[3];
  573.  
  574.  //       cout << "USER XYZ: " << User->getx() << " " << User->gety() << " " << User->getz() << endl;
  575.  
  576.         xyz* XYZ;
  577.         User->init();
  578.         XYZ = User->printCoordinate();
  579.  
  580.         ax = XYZ->x_;
  581.         ay = XYZ->y_;
  582.         az = XYZ->z_;
  583.  
  584.         at = acos(az/sqrt(ax*ax+ay*ay+az*az));
  585.         ap = atan2(ay,ax);
  586.         ar = sqrt(ax*ax+ay*ay+az*az);
  587.  
  588.         ar = 6400.0;
  589.         ax = tpr_x(at,ap,ar);
  590.         ay = tpr_y(at,ap,ar);
  591.  
  592.  
  593. //        cout << "DEBUG: XYZ ASSUMTION: " << ax << " " << ay << " " << az << endl;
  594. //        cout << "DEBUG: TPR ASSUMTION: " << at << " " << ap << " " << ar << endl;
  595.  
  596.         eDist=dist(ax,ay,az,User->getx(),User->gety(),User->getz());
  597. //        cout << "error in meter: "<< eDist*1000 << endl;
  598. //        cout << "error count: " << count << endl;
  599.  
  600.         if(eDist>5.0){
  601.             count++;
  602.         }
  603.  
  604.         delete User;
  605.         delete XYZ;
  606.         vSate.clear();
  607.         vCenter.clear();
  608.     }
  609.         cout << i <<" ps : " << count << endl;
  610.  
  611. }
  612. */
  613.  
  614. //Test3
  615. ofstream fout;
  616. fout.open("data.txt");
  617. sate* S1;
  618. sate* S2;
  619. sate* S3;
  620. sate* S4;
  621.  
  622. long double at, ap, ar, ax, ay, az, eDist, tE, fE;
  623. int tcount, fcount;
  624.  
  625. for(unsigned long i=0;i<1000000000;i=i+10){
  626.     tE = fE = 0.0;
  627.     tcount = fcount = 0;
  628.     for(int j=0;j<10000;j++){
  629.  //       PRINTLINE;
  630.  //         cout << "DEBUG: ERROR IS : " << i << ", " << j << " TRY" << endl;
  631.  //       PRINTLINE;
  632.         vCenter.push_back(center(0.0));
  633.         center* Center = &vCenter[0];
  634.         Center->setCenterposition();
  635.  
  636.         user* User = new user();
  637.         User->setUserposition(Center->gett(),Center->getp(),Center->getr());
  638. //        cout << "DEBUG: USER WITH TPR: " << User->gett() << " " << User->getp() << " " << User->getr() << " GENERATED" << endl;
  639.  
  640.         sate* Sate;
  641.         for(int k=0;k<3;k++){
  642.             vSate.push_back(sate());
  643.             Sate = &vSate[k];
  644.             Sate->setError(i);
  645.             Sate->setSateposition(0,R+ALTCOEFF,R+ALTCOEFF*(rand()%2));
  646. //            cout << "DEBUG: SATE WITH TPR: " << Sate->gett() << " " << Sate->getp() << " " << Sate->getr() << " GENERATED" << endl;
  647.         }
  648.  
  649.         S1=&vSate[0];
  650.         S2=&vSate[1];
  651.         S3=&vSate[2];
  652.  
  653.  //       cout << "USER XYZ: " << User->getx() << " " << User->gety() << " " << User->getz() << endl;
  654.  
  655.         xyz* XYZ;
  656.         User->init();
  657.         XYZ = User->printCoordinate();
  658.  
  659.         ax = XYZ->x_;
  660.         ay = XYZ->y_;
  661.         az = XYZ->z_;
  662.  
  663.         at = acos(az/sqrt(ax*ax+ay*ay+az*az));
  664.         ap = atan2(ay,ax);
  665.         ar = sqrt(ax*ax+ay*ay+az*az);
  666.  
  667.         ar = 6400.0;
  668.         ax = tpr_x(at,ap,ar);
  669.         ay = tpr_y(at,ap,ar);
  670.  
  671.  
  672. //        cout << "DEBUG: XYZ ASSUMTION: " << ax << " " << ay << " " << az << endl;
  673. //        cout << "DEBUG: TPR ASSUMTION: " << at << " " << ap << " " << ar << endl;
  674.  
  675.         eDist=dist(ax,ay,az,User->getx(),User->gety(),User->getz());
  676.         if(eDist>5.0){
  677.             tcount++;
  678.         }
  679. //        cout << "error in meter: "<< eDist*1000 << endl;
  680. //        cout << "error count: " << count << endl;
  681.  
  682.         tE=tE+eDist;
  683.  
  684.         vSate.push_back(sate());
  685.         Sate = &vSate[3];
  686.         Sate->setError(i);
  687.         Sate->setSateposition(0,R+ALTCOEFF,R+ALTCOEFF*(rand()%2));
  688.  
  689.         S4=&vSate[3];
  690.  
  691.         User->init();
  692.         XYZ = User->printCoordinate();
  693.  
  694.         ax = XYZ->x_;
  695.         ay = XYZ->y_;
  696.         az = XYZ->z_;
  697.  
  698.         at = acos(az/sqrt(ax*ax+ay*ay+az*az));
  699.         ap = atan2(ay,ax);
  700.         ar = sqrt(ax*ax+ay*ay+az*az);
  701.  
  702.         ar = 6400.0;
  703.         ax = tpr_x(at,ap,ar);
  704.         ay = tpr_y(at,ap,ar);
  705.  
  706.         eDist=dist(ax,ay,az,User->getx(),User->gety(),User->getz());
  707.         if(eDist>5.0){
  708.             fcount++;
  709.         }
  710.         fE=fE+eDist;
  711.  
  712.         delete User;
  713.         delete XYZ;
  714.         vSate.clear();
  715.         vCenter.clear();
  716.     }
  717.         cout << i <<" ps : " << tE/10000 << " " << fE/10000 << endl;
  718.         cout << i <<" ps : " << tcount << " " << fcount << endl;
  719.         fout << i << " " << tcount << " " << fcount << endl;
  720.  
  721. }
  722. fout.close();
  723.  
  724. /*
  725.   //Time Simualtion
  726.   long double elapsed = .0;
  727.   while(1){
  728.     cout << "TIME OF SIMULATION ELAPSED IN SECONDS: "<<(long double)elapsed<<endl;
  729.     elapsed=elapsed+(long double)(0.0+TIMERESOLUTION/1000000000);
  730.     usleep(RTIMERESOLUTION);
  731.   }
  732.  
  733.  */
  734. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!