• API
• FAQ
• Tools
• Archive
A Pastebin account makes a great Christmas gift
SHARE
TWEET

# Untitled

a guest Aug 14th, 2014 520 Never
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;
13.
14.     public: Coordinate(double lat, double lon, double 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;
28.         double LatB = b.lat;
29.         double LonB = b.lon;
31.         double LatC = c.lat;
32.         double LonC = c.lon;
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
42.         double zA = earthR *(sin(DEG2RADs(LatA)));
43.
46.         double zB = earthR *(sin(DEG2RADs(LatB)));
47.
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
83.
84.         //convert back to lat/long from ECEF
85.         //convert to degrees
86.         double lat = RAD2DEGs(asin(triPt[2] / earthR));
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.

Top