Need a unique gift idea?
A Pastebin account makes a great Christmas gift
SHARE
TWEET

Untitled

a guest Aug 14th, 2014 520 Never
Upgrade to PRO!
ENDING IN00days00hours00mins00secs
 
  1.  
  2. #define PI           3.14159265358979323846  /* pi */
  3. #define DEG2RADs(x) ((PI / 180.0) * x)
  4. #define RAD2DEGs(x) ((180.0 / PI) * x)
  5. #define SQR(x) (x * x)
  6.  
  7.  
  8. class Coordinate {
  9. public:
  10.     public: double lat;
  11.     public: double lon;
  12.     public: double rad;
  13.    
  14.     public: Coordinate(double lat, double lon, double rad){
  15.         this->lat=lat; this->lon=lon; this->rad=rad;
  16.     }
  17.  
  18. };
  19.  
  20.  
  21.     Coordinate triangulate(Coordinate a, Coordinate b, Coordinate c){
  22.        
  23.         //Constants
  24.         double earthR = 6371;
  25.         double LatA = a.lat;
  26.         double LonA = a.lon;
  27.         double DistA = a.rad;
  28.         double LatB = b.lat;
  29.         double LonB = b.lon;
  30.         double DistB = b.rad;
  31.         double LatC = c.lat;
  32.         double LonC = c.lon;
  33.         double DistC = c.rad;
  34.        
  35.         //using authalic sphere
  36.         //if using an ellipsoid this step is slightly different
  37.         //Convert geodetic Lat/Long to ECEF xyz
  38.         //   1. Convert Lat/Long to radians
  39.         //   2. Convert Lat/Long(radians) to ECEF
  40.         double xA = earthR *(cos(DEG2RADs(LatA)) * cos(DEG2RADs(LonA)));
  41.         double yA = earthR *(cos(DEG2RADs(LatA)) * sin(DEG2RADs(LonA)));
  42.         double zA = earthR *(sin(DEG2RADs(LatA)));
  43.        
  44.         double xB = earthR *(cos(DEG2RADs(LatB)) * cos(DEG2RADs(LonB)));
  45.         double yB = earthR *(cos(DEG2RADs(LatB)) * sin(DEG2RADs(LonB)));
  46.         double zB = earthR *(sin(DEG2RADs(LatB)));
  47.        
  48.         double xC = earthR *(cos(DEG2RADs(LatC)) * cos(DEG2RADs(LonC)));
  49.         double yC = earthR *(cos(DEG2RADs(LatC)) * sin(DEG2RADs(LonC)));
  50.         double zC = earthR *(sin(DEG2RADs(LatC)));
  51.        
  52.         //REQUIRE C++ 11
  53.         vector<double> P1 = {xA,yA,zA};
  54.         vector<double> P2 = {xB, yB, zB};
  55.         vector<double> P3 = {xC, yC, zC};
  56.        
  57.        
  58.         //ex = (P2 - P1)/(numpy.linalg.norm(P2 - P1))
  59.         //i = dot(ex, P3 - P1)
  60.         //ey = (P3 - P1 - i*ex)/(numpy.linalg.norm(P3 - P1 - i*ex))
  61.         //ez = numpy.cross(ex,ey)
  62.         //d = numpy.linalg.norm(P2 - P1)
  63.         //j = dot(ey, P3 - P1)
  64.         vector<double> ex = vectorDiv( vectorDiff(P2, P1) , vectorNorm(vectorDiff(P2, P1)));
  65.         double i = vectorDot(ex, vectorDiff(P3, P1)); //PROB
  66.         vector<double> diff = vectorDiff(vectorDiff(P3, P1), vectorMul(i, ex));
  67.         vector<double> ey = vectorDiv(diff, vectorNorm(diff));
  68.         vector<double> ez = vectorCross(ex, ey);
  69.         double d = vectorNorm( vectorDiff(P2, P1) );
  70.         double j = vectorDot(ey, vectorDiff(P3, P1));
  71.        
  72.         //from wikipedia
  73.         //plug and chug using above values
  74.         double x = (pow(DistA,2) - pow(DistB,2) + pow(d,2))/(2*d);
  75.         double y = ((pow(DistA,2) - pow(DistC,2) + pow(i,2) + pow(j,2))/(2*j)) - ((i/j)*x);
  76.        
  77.         //only one case shown here
  78.         double z = sqrt(pow(DistA,2) - pow(x,2) - pow(y,2));
  79.        
  80.         //triPt is an array with ECEF x,y,z of trilateration point
  81.         //triPt = P1 + x*ex + y*ey + z*ez
  82.         vector<double> triPt = vectorAdd(vectorAdd(P1, vectorMul(x, ex)), vectorAdd(vectorMul(y, ey), vectorMul(z, ez)));
  83.        
  84.         //convert back to lat/long from ECEF
  85.         //convert to degrees
  86.         double lat = RAD2DEGs(asin(triPt[2] / earthR));
  87.         double lon = RAD2DEGs(atan2(triPt[1],triPt[0]));
  88.        
  89.         cout << lat << endl;
  90.         cout << lon << endl;
  91.         Coordinate result(lat,lon,0);
  92.         return result;
  93.     }
  94.    
  95.     private: vector<double> vectorMul(vector<double> a,vector<double> b){
  96.         vector<double> result = {0,0,0};
  97.         result[0]=a[0]*b[0]; result[1]=a[1]*b[1]; result[2]=a[2]*b[2];
  98.         return result;
  99.     }
  100.    
  101.     private: vector<double> vectorMul(double a,vector<double> b){
  102.         vector<double> result = {0,0,0};
  103.         result[0]=a*b[0]; result[1]=a*b[1]; result[2]=a*b[2];
  104.         return result;
  105.     }
  106.    
  107.     private: vector<double> vectorDiv(vector<double> a,vector<double> b){
  108.         vector<double> result = {0,0,0};
  109.         result[0]=a[0]/b[0]; result[1]=a[1]/b[1]; result[2]=a[2]/b[2];
  110.         return result;
  111.     }
  112.    
  113.     private: vector<double> vectorDiv(vector<double> a,double b){
  114.         vector<double> result = {0,0,0};
  115.         result[0]=a[0]/b; result[1]=a[1]/b; result[2]=a[2]/b;
  116.         return result;
  117.     }
  118.    
  119.     private: vector<double> vectorDiff(vector<double> a,vector<double> b){
  120.         vector<double> result = {0,0,0};
  121.         result[0]=a[0]-b[0]; result[1]=a[1]-b[1]; result[2]=a[2]-b[2];
  122.         return result;
  123.     }
  124.    
  125.     private: vector<double> vectorAdd(vector<double> a,vector<double> b){
  126.         vector<double> result = {0,0,0};
  127.         result[0]=a[0]+b[0]; result[1]=a[1]+b[1]; result[2]=a[2]+b[2];
  128.         return result;
  129.     }
  130.    
  131.     private: double vectorDot(vector<double> a,vector<double> b){
  132.         return (a[0] * b[0]) + (a[1] * b[1]) + (a[2] * b[2]);
  133.     }
  134.    
  135.     private: vector<double> vectorCross(vector<double> a,vector<double> b){
  136.         vector<double> result = {0,0,0};
  137.         result[0] = a[1]*b[2]-a[2]*b[1];
  138.         result[1] = a[2]*b[0]-a[0]*b[2];
  139.         result[2] = a[0]*b[1]-a[1]*b[0];
  140.         return result;
  141.     }
  142.    
  143.     private: double vectorNorm(vector<double> a){
  144.         return sqrt(vectorDot(a,a));
  145.     }
  146.    
  147.     private: vector<double> vectorNormalize(vector<double> a){
  148.         vector<double> result = {0,0,0};
  149.         double mag = sqrt(vectorDot(a,a));
  150.         result[0] = a[0]/mag;
  151.         result[1] = a[1]/mag;
  152.         result[2] = a[2]/mag;
  153.         return result;
  154.     }
  155.    
  156. };
RAW Paste Data
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
 
Top