4d19b880549b953587cf6d0bbed2307f64e81124
[libecef.git] / test / ecef-wgs84_test.cc
1 /*
2   Earth Centered Earth Fixed (ECEF) WGS84 Navigation Utility Copyright © 2013-2017  Infinite Delta Corp
3
4   This program is free software: you can redistribute it and/or modify
5   it under the terms of the GNU Lesser General Public License as published by
6   the Free Software Foundation, either version 2 of the License, or
7   (at your option) any later version.
8
9   This program is distributed in the hope that it will be useful,
10   but WITHOUT ANY WARRANTY; without even the implied warranty of
11   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12   GNU General Public License for more details.
13
14   You should have received a copy of the GNU Lesser General Public License
15   along with this program.  If not, see <http://www.gnu.org/licenses/>.
16
17 */
18
19 #include <iostream>
20 #include <ecef-quaternion.h>
21 #include <ecef-wgs84.h>
22
23 namespace ecef
24 {
25 static bool tst(const double i, const double j=0, const double t=0.0000001)
26   {if (i>=j ? i-j <= t : j-i <= t) return true;
27   std::cout << i << " " << j << " " << i-j << " " << t << " ";
28   return false;
29   }
30
31 #define ERR(e,m) {std::cout << "Test Error: \"" << m << "\" in " << __FILE__ << ":" << __LINE__ << std::endl; return e;}
32
33 #define LL_INC wgs84Const::maxNorth/1000.0
34
35 int tst1()
36 {
37   vec cent;
38   wgs84 pos;
39
40   if (pos.x != 6378137.0) ERR(1, "Default X is not Wgs84 Radius");
41   if (pos.y != 0.0) ERR(1, "Default Y is not zero");
42   if (pos.z != 0.0) ERR(1, "Default Z is not zero");
43   // std::cout << pos.x << std::endl;
44   // std::cout << pos.y << std::endl;
45   // std::cout << pos.z << std::endl;
46
47   if (vec(1.0, 0.0, 0.0) + vec(0.0, 2.0, 0.0) + vec(0.0, 0.0, 3.0) !=
48       vec(1.0, 2.0, 3.0)) ERR(1, "Bad vector addition");
49
50   if (vec(1.0, 0.0, 0.0) - vec(2.0, 3.0, 0.0) - vec(0.0, 0.0, 4.0) !=
51       vec(-1.0, -3.0, -4.0)) ERR(1, "Bad vector subtraction");
52
53   if (vec(1.0, 2.0, 3.0).dot(vec(4.0, 5.0, 6.0)) != 32.0) ERR(1, "Bad vector dot product");
54
55   if (vec(1.0, 0.0, 0.0).cross(vec(0.0, 1.0, 0.0)) != vec(0.0, 0.0, 1.0))
56         ERR(1, "Bad vector cross product");
57
58   if (vec(0.0, 1.0, 0.0).cross(vec(0.0, 1.0, 0.0)) != vec(0.0, 0.0, 0.0))
59         ERR(1, "Bad vector cross product");
60
61   if (vec(0.0, 0.0, 1.0).cross(vec(0.0, 1.0, 0.0)) != vec(-1.0, 0.0, 0.0))
62         ERR(1, "Bad vector cross product");
63
64   // std::cout << pos.dist(vec(6378137.0, 0.0, 0.0)) << std::endl;
65
66   if (!(pos == vec(6378137.0, 0.0, 0.0))) ERR(1, "Default pos is not prime meridian");
67   if (pos != vec(6378137.0, 0.0, 0.0)) ERR(1, "Default pos is not prime meridian");
68
69   // std::cout << cent.dist(pos)- 6378137.0 << std::endl;
70
71   if (cent.dist(pos) != 6378137.0) ERR(1, "Equator Radius not Wgs84");
72   if (wgs84Const::c != 6356752.3142451792954l) ERR(1, "Minor axis not Wgs84");
73
74   // std::cout << wgs84(wgs84Const::maxNorth, 0.0).x << std::endl;
75   // std::cout << wgs84(wgs84Const::maxNorth, 0.0).y << std::endl;
76   // std::cout << wgs84(wgs84Const::maxNorth, 0.0).z << std::endl;
77
78   // std::cout << a * (1-1/fr) << std::endl;
79
80   if (wgs84(wgs84Const::maxNorth, 0.0).z != wgs84Const::c) ERR(1, "Bad North");
81   if (!tst(wgs84(wgs84Const::maxNorth, 0.0).z, 6356752.31424518l)) ERR(1, "Bad North");
82
83   for (double lon=wgs84Const::maxEast; lon > wgs84Const::maxWest; lon -= LL_INC)
84     {
85     if (!tst(cent.dist(wgs84(0.0, lon)), 6378137.0))
86       {
87       std::cout << lon << ": " << cent.dist(wgs84(0.0, lon)) - 6378137.0 << std::endl;
88
89       ERR(1, "World Equator Radius not Wgs84");
90       }
91     }
92
93   return 0;
94 }
95
96 int tst2()
97 {
98   int total=0;
99   wgs84Line line;
100   // line l1(vec(6378137.0/2,0,0));
101   // line l1(vec(6000000.0,0,0));
102   // line l2(count, l1);
103
104   // std::cout << count << std::endl;
105   // std::cout << l2.p1.x << std::endl;
106   // std::cout << l2.p1.y << std::endl;
107   // std::cout << l2.p1.z << std::endl;
108   // std::cout << l2.p2.x << std::endl;
109   // std::cout << l2.p2.y << std::endl;
110   // std::cout << l2.p2.z << std::endl;
111
112   if (line.trimToEllipsoid() != 2) ERR(2, "WGS84 Crossing count");
113   if (line.p[0] != vec(-6378137.0,0,0)) ERR(2, "WGS84 Crossing");
114   if (line.p[1] != vec( 6378137.0,0,0)) ERR(2, "WGS84 Crossing");
115
116   line = wgs84Line(vec(0,1,0));
117   if (line.trimToEllipsoid() != 2) ERR(2, "WGS84 Crossing count");
118   // std::cout << " " << line.p[0].x << std::endl;
119   // std::cout << " " << line.p[0].y << std::endl;
120   // std::cout << " " << line.p[0].z << std::endl;
121   // std::cout << std::endl;
122   if (line.p[0] != vec(0,-6378137.0,0)) ERR(2, "WGS84 Crossing");
123   if (line.p[1] != vec(0, 6378137.0,0)) ERR(2, "WGS84 Crossing");
124
125   line = wgs84Line(vec(0,0,1));
126   if (line.trimToEllipsoid() != 2) ERR(2, "WGS84 Crossing count");
127   if (line.p[0] != vec(0,0,-6356752.31424518)) ERR(2, "WGS84 Crossing");
128   if (line.p[1] != vec(0,0, 6356752.31424518)) ERR(2, "WGS84 Crossing");
129
130   for (double lon=wgs84Const::maxEast; lon > wgs84Const::maxWest; lon -= LL_INC)
131     {
132     for (double lat=wgs84Const::maxNorth; lat > wgs84Const::maxSouth; lat -= LL_INC)
133       {
134       wgs84Line l1(wgs84(lat,lon), wgs84(wgs84Const::maxNorth/2, wgs84Const::maxNorth));
135       wgs84Line l2(l1);
136       int count = l2.trimToEllipsoid();
137       if (count > 0 && l1.p[0] != l2.p[0]) ERR(2, "WGS84 robust");
138       if (count > 1 && l1.p[1] != l2.p[1]) 
139         {
140         // ERR(2, "WGS84 robust");
141
142         std::cout << "Second point trim error at lat/lon " << lat * 180.0 / M_PIl;
143         std::cout << "/" << lon * 180.0 / M_PIl;
144         // std::cout << std::endl;
145         // std::cout << " " << l1.p[0].x;
146         // std::cout << " " << l1.p[0].y;
147         // std::cout << " " << l1.p[0].z;
148         // std::cout << " " << l1.p[1].x;
149         // std::cout << " " << l1.p[1].y;
150         // std::cout << " " << l1.p[1].z;
151         // std::cout << std::endl;
152         // std::cout << " " << l2.p[0].x;
153         // std::cout << " " << l2.p[0].y;
154         // std::cout << " " << l2.p[0].z;
155         // std::cout << " " << l2.p[1].x;
156         // std::cout << " " << l2.p[1].y;
157         // std::cout << " " << l2.p[1].z;
158         std::cout << std::endl;
159         }
160
161
162       wgs84Line l3(wgs84(lat,lon));
163       wgs84Line l5(l3.p[1]/33.0);
164       l5.trimToEllipsoid();
165
166       if (l3.p[1]*-1 != l5.p[0]) ERR(2, "WGS84 trim robust");
167       if (l3.p[1] != l5.p[1]) ERR(2, "WGS84 trim robust");
168
169 #ifdef tlz
170       vec p1(wgs84(lat,lon));
171       vec p2(p1);
172       p2.y = 0;
173       line l6(count, line(p1, p2));
174       if (l6.p1 != p1)
175         {
176         std::cout << p1.x << std::endl;
177         std::cout << p1.y << std::endl;
178         std::cout << p1.z << std::endl;
179         std::cout << l6.p1.x << std::endl;
180         std::cout << l6.p1.y << std::endl;
181         std::cout << l6.p1.z << std::endl;
182         ERR(2, "WGS84 robust");
183         }
184 #endif
185       ++total;
186       }
187     }
188   std::cout << "Total " << total << std::endl;
189   return 0;
190 }
191
192 int tstLon()
193 {
194   for (double lon=wgs84Const::maxEast; lon > wgs84Const::maxWest; lon -= LL_INC)
195     {
196     wgs84 npole(wgs84Const::maxNorth,lon);
197     if (!tst(0, npole.lon()*1800/M_PIl)) ERR(4, "Bad Longitude");
198     wgs84 spole(wgs84Const::maxSouth,lon);
199     if (!tst(0, spole.lon()*1800/M_PIl)) ERR(4, "Bad Longitude");
200
201     for (double lat=wgs84Const::maxNorth-LL_INC; lat >= wgs84Const::maxSouth+LL_INC; lat -= LL_INC)
202       {
203       wgs84 t(lat,lon);
204       if (!tst(lon*1800/M_PIl, t.lon()*1800/M_PIl)) ERR(4, "Bad Longitude");
205       }
206     }
207   return 0;
208 }
209
210 int tstCos()
211 {
212   vec t(1,0,0);
213
214   if (!tst(acos(t.cos(vec(1,1,0)))*180/M_PIl, 45)) ERR(4, "Bad Cos");
215   if (!tst(acos(t.cos(vec(1,0,1)))*180/M_PIl, 45)) ERR(4, "Bad Cos");
216   if (!tst(acos(t.cos(vec(1,0,0)))*180/M_PIl, 0)) ERR(4, "Bad Cos");
217   if (!tst(acos(t.cos(vec(1,-1,0)))*180/M_PIl, 45)) ERR(4, "Bad Cos");
218   if (!tst(acos(t.cos(vec(1,0,-1)))*180/M_PIl, 45)) ERR(4, "Bad Cos");
219   if (!tst(acos(t.cos(vec(-1,0,0)))*180/M_PIl, 180)) ERR(4, "Bad Cos");
220   if (!tst(acos(t.cos(vec(0,1,0)))*180/M_PIl, 90)) ERR(4, "Bad Cos");
221   if (!tst(acos(t.cos(vec(0,0,1)))*180/M_PIl, 90)) ERR(4, "Bad Cos");
222   if (!tst(acos(t.cos(vec(0,-1,0)))*180/M_PIl, 90)) ERR(4, "Bad Cos");
223   if (!tst(acos(t.cos(vec(0,0,-1)))*180/M_PIl, 90)) ERR(4, "Bad Cos");
224
225   for (double lon=wgs84Const::maxEast; lon > wgs84Const::maxWest; lon -= LL_INC*100)
226     {
227     // std::cout << lon*1800/M_PIl << std::endl;
228     for (double lat=wgs84Const::maxNorth; lat >= wgs84Const::maxSouth; lat -= LL_INC)
229       {
230       wgs84Fast t(lat,lon), n(t.north());
231       if (!tst(lat*1800/M_PIl, t.lat()*1800/M_PIl)) ERR(4, "Bad Lat");
232
233       if (!tst(abs(t.sinLat()), n.distZ()/n.dist())) ERR(4, "Bad North vector");
234
235       wgs84Fast t2(lat,-85,10000);
236       if (!tst(lat*1800/M_PIl, t2.lat()*1800/M_PIl, 0.0035)) ERR(4, "Bad Lat at alt");
237       }
238     }
239
240   return 0;
241 }
242
243 int tst3()
244 {
245   for (double lat=wgs84Const::maxNorth; lat > wgs84Const::maxSouth; lat -= LL_INC)
246     {
247     wgs84Fast pnt(lat,0);
248     // std::cout << " " << lat*1800/M_PIl;
249     // std::cout << pnt.x - FindX2D(pnt.z);
250     // std::cout << " " << Slope2D(pnt.z);
251     // std::cout << " " << tan(lat);
252     // std::cout << " " << tan(lat) - Slope2D(pnt.z);
253     // if (!tst(pnt.x, pnt.rotationRadius(), 0.00001)) ERR(4, "Bad radius");
254     if (!tst(pnt.x, pnt.distZ())) ERR(4, "Bad radius");
255     //tlz if (!tst(tan(lat), pnt.tanLat())) ERR(4, "Bad Tan");
256     //tlz if (!tst(pnt.x, tan(lat)*(pnt.z - TanZCrossing(pnt.z)), 0.000001)) ERR(4, "Bad Z0");
257     if (!tst(lat, asin(pnt.sinLat()), 0.0000001)) ERR(4, "Bad WGS84 Sin");
258
259     pnt = wgs84(lat,0,10000);
260     // std::cout << " " << lat*1800/M_PIl;
261     // std::cout << " " << pnt.x;
262     // std::cout << " " << pnt.y;
263     // std::cout << " " << pnt.z;
264     // std::cout << " " << TanZCrossing(pnt.z);
265     // std::cout << " " << tan(lat);
266     // std::cout << " " << (find2dX(pnt.z) - find2dX(pnt.z+0.1))/0.1;
267     // std::cout << " " << pnt.x / (pnt.z - TanZCrossing(pnt.z));
268     // std::cout << " " << pnt.z;
269     // std::cout << " " << WGS84EarthCenter(pnt.z);
270     // std::cout << std::endl;
271     if (!tst(lat, asin(pnt.sinLat()), 0.00001)) ERR(4, "Bad WGS84 Sin");
272
273     // if (lat > wgs84Const::maxNorth-0.2) continue;  // Skip near polls TanZCrossing is better!
274     // if (lat < wgs84Const::maxSouth+0.2) continue;  // Skip near polls TanZCrossing is better!
275
276     // if (!tst(pnt.x, tan(lat)*(pnt.z - TanZCrossing(pnt.z)), 0.1)) ERR(4, "Bad Z0");
277     }
278
279   return 0;
280 }
281
282 int qtst()
283 {
284   quat q;
285
286   q = quat(0, vec(0,0,0)).versor();
287   if (!tst(q.a, 0)) ERR(5, "Bad Quat Versor Init");
288   if (!tst(q.v.x, 0)) ERR(5, "Bad Quat Versor Init");
289   if (!tst(q.v.y, 0)) ERR(5, "Bad Quat Versor Init");
290   if (!tst(q.v.z, 1)) ERR(5, "Bad Quat Versor Init");
291
292   q = quat(3, vec(4,5,6)).conj();
293   if (!tst(q.a, 3)) ERR(5, "Bad Quat Conj Angle");
294   if (!tst(q.v.x, -4)) ERR(5, "Bad Quat Conj X");
295   if (!tst(q.v.y, -5)) ERR(5, "Bad Quat Conj Y");
296   if (!tst(q.v.z, -6)) ERR(5, "Bad Quat Conj Z");
297   if (!tst(q.mag(), sqrt(86.0))) ERR(5, "Bad Quat Magnitude");
298   if (!tst(q.versor().mag(), 1)) ERR(5, "Bad Quat Versor");
299
300   q = quat(0, zAxis) * quat(0, zAxis);
301   if (!tst(q.a, -1)) ERR(5, "Bad Quat Angle");
302   if (!tst(q.v.x, 0)) ERR(5, "Bad Quat X");
303   if (!tst(q.v.y, 0)) ERR(5, "Bad Quat Y");
304   if (!tst(q.v.z, 0)) ERR(5, "Bad Quat Z");
305
306   q = quat(0, zAxis) * quat(0, vec(1,0,0));
307   if (!tst(q.a, 0)) ERR(5, "Bad Quat Angle");
308   if (!tst(q.v.x, 0)) ERR(5, "Bad Quat X");
309   if (!tst(q.v.y, 1)) ERR(5, "Bad Quat Y");
310   if (!tst(q.v.z, 0)) ERR(5, "Bad Quat Z");
311
312   // q = quat(cos(M_PI_2l/2), zAxis) * quat(cos(M_PI_2l/2), zAxis);
313   // q = quat(cos(M_PI_2l/2), zAxis * sin(M_PI_2l/2));
314
315   q = quat(cos(M_PI_4l/2), zAxis).normVec();
316   q = q * q;
317
318   if (!tst(q.a, cos(M_PI_4l))) ERR(5, "Bad Quat Hamilton Multiple Angle");
319   if (!tst(q.v.x, 0)) ERR(5, "Bad Quat Mult X");
320   if (!tst(q.v.y, 0)) ERR(5, "Bad Quat Mult Y");
321   if (!tst(q.v.norm().z, 1)) ERR(5, "Bad Quat Z");
322
323   q = quat(cos(M_PI_4l/4), zAxis).normVec() * quat(cos(M_PI_4l/2), zAxis).normVec();
324   if (!tst(q.a, cos(M_PI_4l * 3/4))) ERR(5, "Bad Quat Hamilton Multiple Angle");
325   if (!tst(q.v.x, 0)) ERR(5, "Bad Quat Mult X");
326   if (!tst(q.v.y, 0)) ERR(5, "Bad Quat Mult Y");
327   if (!tst(q.v.norm().z, 1)) ERR(5, "Bad Quat Z");
328
329   q = quat(cos(M_PI_2l), zAxis).normVec() * quat(cos(M_PI_2l), vec(0,1,0)).normVec();
330   if (!tst(q.a, cos(M_PI_2l))) ERR(5, "Bad Quat Hamilton Multiple Angle");
331   if (!tst(q.v.x, -1)) ERR(5, "Bad Quat Mult X");
332   if (!tst(q.v.y, 0)) ERR(5, "Bad Quat Mult Y");
333   if (!tst(q.v.z, 0)) ERR(5, "Bad Quat Mult Z");
334
335   q = quat(cos(M_PI_4l), zAxis).normVec() * quat(cos(M_PI_4l), vec(0,1,0)).normVec();
336   if (!tst(q.a, cos(M_PIl/3))) ERR(5, "Bad Quat Hamilton Multiple Angle");
337   if (!tst(q.v.x, -0.5)) ERR(5, "Bad Quat Mult X");
338   if (!tst(q.v.y, 0.5)) ERR(5, "Bad Quat Mult Y");
339   if (!tst(q.v.z, 0.5)) ERR(5, "Bad Quat Mult Z");
340
341   // std::cout << " " << q.a;
342   // std::cout << " " << acos(q.a) * 180 / M_PIl;
343   // std::cout << " " << q.v.x;
344   // std::cout << " " << q.v.y;
345   // std::cout << " " << q.v.z;
346   // std::cout << std::endl;
347
348   return 0;
349 }
350
351 int qvtst()
352 {
353   vec v;
354   quat q(cos(M_PI_4l), zAxis);
355   q = q.normVec();
356
357   v = q * vec(1,0,0);
358   if (!tst(v.x, 0)) ERR(5, "Bad Quat rotate X");
359   if (!tst(v.y, 1)) ERR(5, "Bad Quat rotate Y");
360   if (!tst(v.z, 0)) ERR(5, "Bad Quat rotate Z");
361
362   v = q * vec(0,1,0);
363   if (!tst(v.x, -1)) ERR(5, "Bad Quat rotate X");
364   if (!tst(v.y, 0)) ERR(5, "Bad Quat rotate Y");
365   if (!tst(v.z, 0)) ERR(5, "Bad Quat rotate Z");
366
367   v = q * vec(0,0,1);
368   if (!tst(v.x, 0)) ERR(5, "Bad Quat rotate X");
369   if (!tst(v.y, 0)) ERR(5, "Bad Quat rotate Y");
370   if (!tst(v.z, 1)) ERR(5, "Bad Quat rotate Z");
371
372   v = q * vec(1,1,0);
373   if (!tst(v.x, -1)) ERR(5, "Bad Quat rotate X");
374   if (!tst(v.y, 1)) ERR(5, "Bad Quat rotate Y");
375   if (!tst(v.z, 0)) ERR(5, "Bad Quat rotate Z");
376
377   q = quat(cos(M_PI_4l/2), zAxis).normVec();
378   v = q * vec(1,0,0);
379   if (!tst(v.x, sqrt(0.5))) ERR(5, "Bad Quat rotate X");
380   if (!tst(v.y, sqrt(0.5))) ERR(5, "Bad Quat rotate Y");
381   if (!tst(v.z, 0)) ERR(5, "Bad Quat rotate Z");
382
383   q = quat(cos(30.0 * M_PI_2l/180), zAxis).normVec();
384   v = q * vec(5,0,0);
385   //if (!tst(v.x, sqrt(0.5))) ERR(5, "Bad Quat rotate X");
386   if (!tst(v.y, 2.5)) ERR(5, "Bad Quat rotate Y");
387   if (!tst(v.z, 0)) ERR(5, "Bad Quat rotate Z");
388
389   /*
390   std::cout << " " << v.x;
391   std::cout << " " << v.y;
392   std::cout << " " << v.z;
393   std::cout << std::endl;
394   */
395
396   return 0;
397 }
398
399 int quaternionAroundWorldTest()
400 {
401   wgs84 start(42.89, 85.56), p; // 42.89°N 85.56°W
402   quat q = quat(cos(M_PI_2l/18000), zAxis).normVec();           // Around the world at this latitude.
403
404   p = start;
405   for (int i=0; i<36000; ++i) p = q * p;
406   if ((start - p).dist() > 0.1) ERR(6, "Around world on latitude Quaternion test");
407
408   q = quat(cos(M_PI_2l/18000), zAxis.cross(start)).normVec();   // Polar around the world
409   p = start;
410   for (int i=0; i<36000; ++i) p = q * p;
411   if ((start - p).dist() > 0.25) ERR(6, "Polar around world Quaternion test");
412
413   q = quat(cos(M_PI_2l/18000), wgs84(0,0).cross(start)).normVec();      // Around world through N0, E0
414   p = start;
415   for (int i=0; i<36000; ++i) p = q * p;
416   if ((start - p).dist() > 0.25) ERR(6, "Around world through N0, W0 Quaternion test");
417
418   /*
419   std::cout << " " << p.x;
420   std::cout << " " << p.y;
421   std::cout << " " << p.z;
422   std::cout << " " << (start - p).dist();
423   std::cout << std::endl;
424   */
425
426   return 0;
427 }
428
429
430 } // namespace ecef
431
432 int main()
433 {
434   int err=0;
435
436   std::cout.precision(14);
437   std::cout << std::scientific;
438
439   std::cout << "starting ecef-wgs84_test..." << std::endl;
440
441   err |= ecef::tstLon();
442   err |= ecef::tst1();
443   err |= ecef::tst2();
444   err |= ecef::tstCos();
445   err |= ecef::tst3();
446   err |= ecef::qtst();
447   err |= ecef::qvtst();
448   err |= ecef::quaternionAroundWorldTest();
449
450   std::cout << "finishing ecef-wgs84_test." << std::endl;
451
452   return err;
453 }