1 /********************************************************************
2 ** @source JEEPS projection functions
3 **
4 ** @author Copyright (C) 1999 Alan Bleasby
5 ** @version 1.0
6 ** @modified Feb 04 2000 Alan Bleasby. First version
7 ** @@
8 **
9 ** This library is free software; you can redistribute it and/or
10 ** modify it under the terms of the GNU Library General Public
11 ** License as published by the Free Software Foundation; either
12 ** version 2 of the License, or (at your option) any later version.
13 **
14 ** This library is distributed in the hope that it will be useful,
15 ** but WITHOUT ANY WARRANTY; without even the implied warranty of
16 ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
17 ** Library General Public License for more details.
18 **
19 ** You should have received a copy of the GNU Library General Public
20 ** License along with this library; if not, write to the
21 ** Free Software Foundation, Inc., 59 Temple Place - Suite 330,
22 ** Boston, MA 02111-1307, USA.
23 ********************************************************************/
24 #include "gps.h"
25 #include <math.h>
26 #include <string.h>
27
28
29 /* @func GPS_Math_Albers_LatLon_To_EN **********************************
30 **
31 ** Convert latitude and longitude to Albers projection easting and
32 ** northing
33 **
34 ** @param [r] phi [double] latitude (deg)
35 ** @param [r] lambda [double] longitude (deg)
36 ** @param [w] E [double *] easting (metre)
37 ** @param [w] N [double *] northing (metre)
38 ** @param [r] phi1 [double] standard latitude (parallel) 1 (deg)
39 ** @param [r] phi2 [double] standard latitude (parallel) 2 (deg)
40 ** @param [r] phi0 [double] latitude of origin (deg)
41 ** @param [r] M0 [double] central meridian (deg)
42 ** @param [r] E0 [double] false easting
43 ** @param [r] N0 [double] false northing
44 ** @param [r] a [double] semi-major axis
45 ** @param [r] b [double] semi-minor axis
46 **
47 ** @return [void]
48 ************************************************************************/
GPS_Math_Albers_LatLon_To_EN(double phi,double lambda,double * E,double * N,double phi1,double phi2,double phi0,double M0,double E0,double N0,double a,double b)49 void GPS_Math_Albers_LatLon_To_EN(double phi, double lambda, double* E,
50 double* N, double phi1, double phi2,
51 double phi0, double M0, double E0,
52 double N0, double a, double b)
53
54 {
55 double dlambda;
56 double phis;
57 double phic;
58 double e;
59 double esq;
60 double esqs;
61 double omesqs2;
62
63 double a2;
64 double b2;
65 double q;
66 double q0;
67 double q1;
68 double q2;
69 double m1;
70 double m2;
71 double n;
72 double phi0s;
73 double phi1s;
74 double phi1c;
75 double phi2s;
76 double phi2c;
77 double ess;
78 double om0;
79 double m1sq;
80 double C;
81 double nq;
82 double nq0;
83 double rho;
84 double rho0;
85 double theta;
86
87 phi = GPS_Math_Deg_To_Rad(phi);
88 phi0 = GPS_Math_Deg_To_Rad(phi0);
89 phi1 = GPS_Math_Deg_To_Rad(phi1);
90 phi2 = GPS_Math_Deg_To_Rad(phi2);
91 lambda = GPS_Math_Deg_To_Rad(lambda);
92 M0 = GPS_Math_Deg_To_Rad(M0);
93
94 dlambda = lambda - M0;
95 if (dlambda > GPS_PI) {
96 dlambda -= ((double)2.0 * GPS_PI);
97 }
98 if (dlambda < -GPS_PI) {
99 dlambda += ((double)2.0 * GPS_PI);
100 }
101
102 phis = sin(phi);
103 phic = cos(phi);
104
105 a2 = a*a;
106 b2 = b*b;
107 esq = (a2-b2)/a2;
108 e = pow(esq,(double)0.5);
109
110
111 phi0s = sin(phi0);
112 ess = e * phi0s;
113 om0 = ((double)1.0 - ess*ess);
114 q0 = ((double)1.0 - esq) * (phi0s / om0-((double)1.0/(e+e)) *
115 log(((double)1.0-ess)/((double)1.0+ess)));
116 phi1s = sin(phi1);
117 phi1c = cos(phi1);
118 ess = e * phi1s;
119 om0 = ((double)1.0 - ess*ess);
120 m1 = phi1c/pow(om0,(double)0.5);
121 q1 = ((double)1.0 - esq) * (phi1s / om0-((double)1.0/(e+e)) *
122 log(((double)1.0-ess)/((double)1.0+ess)));
123
124 m1sq = m1*m1;
125 if (fabs(phi1-phi2)>1.0e-10) {
126 phi2s = sin(phi2);
127 phi2c = cos(phi2);
128 ess = e * phi2s;
129 om0 = ((double)1.0 - ess*ess);
130 m2 = phi2c/pow(om0,(double)0.5);
131 q2 = ((double)1.0 - esq) * (phi2s / om0-((double)1.0/(e+e)) *
132 log(((double)1.0-ess)/((double)1.0+ess)));
133 n = (m1sq - m2*m2) / (q2-q1);
134 } else {
135 n = phi1s;
136 }
137
138 C = m1sq + n*q1;
139 nq0 = n * q0;
140 if (C < nq0) {
141 rho0 = (double)0.;
142 } else {
143 rho0 = (a/n) * pow(C-nq0,(double)0.5);
144 }
145
146
147 esqs = e * phis;
148 omesqs2 = ((double)1.0 - esqs*esqs);
149 q = ((double)1.0 - esq) * (phis / omesqs2-((double)1.0/(e+e)) *
150 log(((double)1.0-esqs)/((double)1.0+esqs)));
151 nq = n*q;
152 if (C<nq) {
153 rho = (double)0.;
154 } else {
155 rho = (a/n) * pow(C-nq,(double)0.5);
156 }
157
158 theta = n*dlambda;
159 *E = rho * sin(theta) + E0;
160 *N = rho0 - rho * cos(theta) + N0;
161
162 return;
163 }
164
165
166
167
168 /* @func GPS_Math_Albers_EN_To_LatLon **********************************
169 **
170 ** Convert Albers easting and northing to latitude and longitude
171 **
172 ** @param [r] E [double] easting (metre)
173 ** @param [r] N [double] northing (metre)
174 ** @param [w] phi [double *] latitude (deg)
175 ** @param [w] lambda [double *] longitude (deg)
176 ** @param [r] phi1 [double] standard latitude (parallel) 1 (deg)
177 ** @param [r] phi2 [double] standard latitude (parallel) 2 (deg)
178 ** @param [r] phi0 [double] latitude of origin (deg)
179 ** @param [r] M0 [double] central meridian (deg)
180 ** @param [r] E0 [double] false easting
181 ** @param [r] N0 [double] false northing
182 ** @param [r] a [double] semi-major axis
183 ** @param [r] b [double] semi-minor axis
184 **
185 ** @return [void]
186 ************************************************************************/
GPS_Math_Albers_EN_To_LatLon(double E,double N,double * phi,double * lambda,double phi1,double phi2,double phi0,double M0,double E0,double N0,double a,double b)187 void GPS_Math_Albers_EN_To_LatLon(double E, double N, double* phi,
188 double* lambda, double phi1, double phi2,
189 double phi0, double M0, double E0,
190 double N0, double a, double b)
191 {
192 double po2;
193 double rho;
194 double rho0;
195 double C;
196 double a2;
197 double b2;
198 double esq;
199 double e;
200 double phi0s;
201 double q0;
202 double q1;
203 double q2;
204 double phi1s;
205 double phi1c;
206 double phi2s;
207 double phi2c;
208 double m1;
209 double m1sq;
210 double m2;
211 double n;
212 double nq0;
213
214 double dx;
215 double dy;
216 double rhom;
217 double q;
218 double qc;
219 double qd2;
220 double rhon;
221 double lat;
222 double dphi;
223 double phis;
224 double ess;
225 double om0;
226 double theta;
227 double tol;
228
229
230 phi0 = GPS_Math_Deg_To_Rad(phi0);
231 phi1 = GPS_Math_Deg_To_Rad(phi1);
232 phi2 = GPS_Math_Deg_To_Rad(phi2);
233 M0 = GPS_Math_Deg_To_Rad(M0);
234
235 a2 = a*a;
236 b2 = b*b;
237 esq = (a2-b2)/a2;
238 e = pow(esq,(double)0.5);
239
240
241 phi0s = sin(phi0);
242 ess = e * phi0s;
243 om0 = ((double)1.0 - ess*ess);
244 q0 = ((double)1.0 - esq) * (phi0s / om0-((double)1.0/(e+e)) *
245 log(((double)1.0-ess)/((double)1.0+ess)));
246 phi1s = sin(phi1);
247 phi1c = cos(phi1);
248 ess = e * phi1s;
249 om0 = ((double)1.0 - ess*ess);
250 m1 = phi1c/pow(om0,(double)0.5);
251 q1 = ((double)1.0 - esq) * (phi1s / om0-((double)1.0/(e+e)) *
252 log(((double)1.0-ess)/((double)1.0+ess)));
253
254 m1sq = m1*m1;
255 if (fabs(phi1-phi2)>1.0e-10) {
256 phi2s = sin(phi2);
257 phi2c = cos(phi2);
258 ess = e * phi2s;
259 om0 = ((double)1.0 - ess*ess);
260 m2 = phi2c/pow(om0,(double)0.5);
261 q2 = ((double)1.0 - esq) * (phi2s / om0-((double)1.0/(e+e)) *
262 log(((double)1.0-ess)/((double)1.0+ess)));
263 n = (m1sq - m2*m2) / (q2-q1);
264 } else {
265 n = phi1s;
266 }
267
268 C = m1sq + n*q1;
269 nq0 = n * q0;
270 if (C < nq0) {
271 rho0 = (double)0.;
272 } else {
273 rho0 = (a/n) * pow(C-nq0,(double)0.5);
274 }
275
276
277 dphi = (double) 1.0;
278 theta = (double) 0.0;
279 tol = (double) 4.85e-10;
280 po2 = (double)GPS_PI / (double)2.0;
281
282 dy = N-N0;
283 dx = E-E0;
284 rhom = rho0-dy;
285 rho = pow(dx*dx+rhom*rhom,(double)0.5);
286
287 if (n<0.0) {
288 rho *= (double)-1.0;
289 dx *= (double)-1.0;
290 dy *= (double)-1.0;
291 rhom *= (double)-1.0;
292 }
293
294 if (rho) {
295 theta = atan2(dx,rhom);
296 }
297 rhon = rho*n;
298 q = (C - (rhon*rhon) / a2) / n;
299 qc = (double)1.0 - ((double)1.0 / (e+e)) *
300 log(((double)1.0-e)/((double)1.0+e));
301 if (fabs(fabs(qc)-fabs(q))>1.9e-6) {
302 qd2 = q/(double)2.0;
303 if (qd2>1.0) {
304 *phi = po2;
305 } else if (qd2<-1.0) {
306 *phi = -po2;
307 } else {
308 lat = asin(qd2);
309 if (e<1.0e-10) {
310 *phi = lat;
311 } else {
312 while (fabs(dphi)>tol) {
313 phis = sin(lat);
314 ess = e*phis;
315 om0 = ((double)1.0 - ess*ess);
316 dphi = (om0*om0) / ((double)2.0*cos(lat))*
317 (q/((double)1.0-esq) - phis / om0 +
318 (log(((double)1.0-ess)/((double)1.0+ess)) /
319 (e+e)));
320 lat += dphi;
321 }
322 *phi = lat;
323 }
324
325 if (*phi > po2) {
326 *phi = po2;
327 } else if (*phi<-po2) {
328 *phi = -po2;
329 }
330 }
331 } else {
332 if (q>=0.0) {
333 *phi = po2;
334 } else {
335 *phi = -po2;
336 }
337 }
338
339 *lambda = M0 + theta / n;
340 if (*lambda > GPS_PI) {
341 *lambda -= GPS_PI * (double)2.0;
342 }
343 if (*lambda < -GPS_PI) {
344 *lambda += GPS_PI * (double)2.0;
345 }
346 if (*lambda>GPS_PI) {
347 *lambda = GPS_PI;
348 } else if (*lambda<-GPS_PI) {
349 *lambda = -GPS_PI;
350 }
351
352 *phi = GPS_Math_Rad_To_Deg(*phi);
353 *lambda = GPS_Math_Rad_To_Deg(*lambda);
354
355 return;
356 }
357
358
359
360 /* @func GPS_Math_LambertCC_LatLon_To_EN **********************************
361 **
362 ** Convert latitude and longitude to Lambert Conformal Conic projection
363 ** easting and northing
364 **
365 ** @param [r] phi [double] latitude (deg)
366 ** @param [r] lambda [double] longitude (deg)
367 ** @param [w] E [double *] easting (metre)
368 ** @param [w] N [double *] northing (metre)
369 ** @param [r] phi1 [double] standard latitude (parallel) 1 (deg)
370 ** @param [r] phi2 [double] standard latitude (parallel) 2 (deg)
371 ** @param [r] phi0 [double] latitude of origin (deg)
372 ** @param [r] M0 [double] central meridian (deg)
373 ** @param [r] E0 [double] false easting
374 ** @param [r] N0 [double] false northing
375 ** @param [r] a [double] semi-major axis
376 ** @param [r] b [double] semi-minor axis
377 **
378 ** @return [void]
379 ************************************************************************/
GPS_Math_LambertCC_LatLon_To_EN(double phi,double lambda,double * E,double * N,double phi1,double phi2,double phi0,double M0,double E0,double N0,double a,double b)380 void GPS_Math_LambertCC_LatLon_To_EN(double phi, double lambda, double* E,
381 double* N, double phi1, double phi2,
382 double phi0, double M0, double E0,
383 double N0, double a, double b)
384
385 {
386 double po2;
387 double po4;
388 double a2;
389 double b2;
390 double phi0s;
391 double e;
392 double esq;
393 double ed2;
394 double ess;
395 double t0;
396 double t1;
397 double t2;
398 double m1;
399 double m2;
400 double phi1s;
401 double phi1c;
402 double phi2s;
403 double phi2c;
404 double n;
405 double F;
406 double Fa;
407 double rho;
408 double rho0;
409 double phis;
410 double t;
411 double theta;
412 double dphi;
413
414 phi = GPS_Math_Deg_To_Rad(phi);
415 phi0 = GPS_Math_Deg_To_Rad(phi0);
416 phi1 = GPS_Math_Deg_To_Rad(phi1);
417 phi2 = GPS_Math_Deg_To_Rad(phi2);
418 lambda = GPS_Math_Deg_To_Rad(lambda);
419 M0 = GPS_Math_Deg_To_Rad(M0);
420
421
422 po2 = (double)GPS_PI / (double)2.0;
423 po4 = (double)GPS_PI / (double)4.0;
424 a2 = a*a;
425 b2 = b*b;
426 esq = (a2-b2)/a2;
427 e = pow(esq,(double)0.5);
428 ed2 = e / (double)2.0;
429
430 phi0s = sin(phi0);
431 ess = e * phi0s;
432 t0 = tan(po4-phi0/(double)2.0) / pow(((double)1.0-ess) /
433 ((double)1.0+ess),ed2);
434
435
436 phi1s = sin(phi1);
437 phi1c = cos(phi1);
438 ess = e * phi1s;
439 m1 = phi1c / pow(((double)1.0-ess*ess),(double)0.5);
440 t1 = tan(po4-phi1/(double)2.0) / pow(((double)1.0-ess) /
441 ((double)1.0+ess),ed2);
442
443 if (fabs(phi1-phi2)>1.0e-10) {
444 phi2s = sin(phi2);
445 phi2c = cos(phi2);
446 ess = e * phi2s;
447 m2 = phi2c / pow(((double)1.0-ess*ess),(double)0.5);
448 t2 = tan(po4-phi2/(double)2.0) / pow(((double)1.0-ess) /
449 ((double)1.0+ess),ed2);
450 n = log(m1/m2) / log(t1/t2);
451 } else {
452 n = phi1s;
453 }
454
455 F = m1 / (n*pow(t1,n));
456 Fa = F*a;
457
458 rho0 = pow(t0,n) * Fa;
459
460 if (fabs(fabs(phi)-po2)>1.0e-10) {
461 phis = sin(phi);
462 ess = e * phis;
463 t = tan(po4-phi/(double)2.0) / pow(((double)1.0-ess) /
464 ((double)1.0+ess),ed2);
465 rho = pow(t,n) * Fa;
466 } else {
467 if ((phi*n)<=(double)0.0) {
468 return;
469 }
470 rho = (double)0.0;
471 }
472
473 dphi = lambda - M0;
474 if (dphi>GPS_PI) {
475 dphi -= (double)GPS_PI * (double)2.0;
476 }
477 if (dphi<-GPS_PI) {
478 dphi += (double)GPS_PI * (double)2.0;
479 }
480 theta = dphi*n;
481
482 *E = rho * sin(theta) + E0;
483 *N = rho0 - rho * cos(theta) + N0;
484
485 return;
486 }
487
488
489
490
491 /* @func GPS_Math_LambertCC_EN_To_LatLon **********************************
492 **
493 ** Convert Lambert Conformal Conic easting and northing to latitude and
494 ** longitude
495 **
496 ** @param [r] E [double] easting (metre)
497 ** @param [r] N [double] northing (metre)
498 ** @param [w] phi [double *] latitude (deg)
499 ** @param [w] lambda [double *] longitude (deg)
500 ** @param [r] phi1 [double] standard latitude (parallel) 1 (deg)
501 ** @param [r] phi2 [double] standard latitude (parallel) 2 (deg)
502 ** @param [r] phi0 [double] latitude of origin (deg)
503 ** @param [r] M0 [double] central meridian (deg)
504 ** @param [r] E0 [double] false easting
505 ** @param [r] N0 [double] false northing
506 ** @param [r] a [double] semi-major axis
507 ** @param [r] b [double] semi-minor axis
508 **
509 ** @return [void]
510 ************************************************************************/
GPS_Math_LambertCC_EN_To_LatLon(double E,double N,double * phi,double * lambda,double phi1,double phi2,double phi0,double M0,double E0,double N0,double a,double b)511 void GPS_Math_LambertCC_EN_To_LatLon(double E, double N, double* phi,
512 double* lambda, double phi1, double phi2,
513 double phi0, double M0, double E0,
514 double N0, double a, double b)
515 {
516 double po2;
517 double po4;
518 double a2;
519 double b2;
520 double phi0s;
521 double e;
522 double esq;
523 double ed2;
524 double ess;
525 double t0;
526 double t1;
527 double t2;
528 double m1;
529 double m2;
530 double phi1s;
531 double phi1c;
532 double phi2s;
533 double phi2c;
534 double n;
535 double F;
536 double Fa;
537 double rho;
538 double rho0;
539 double phis;
540 double t;
541 double theta;
542
543 double dx;
544 double dy;
545 double rhom;
546 double lat;
547 double tlat;
548 double tol;
549
550
551
552 phi0 = GPS_Math_Deg_To_Rad(phi0);
553 phi1 = GPS_Math_Deg_To_Rad(phi1);
554 phi2 = GPS_Math_Deg_To_Rad(phi2);
555 M0 = GPS_Math_Deg_To_Rad(M0);
556
557
558 po2 = (double)GPS_PI / (double)2.0;
559 po4 = (double)GPS_PI / (double)4.0;
560 a2 = a*a;
561 b2 = b*b;
562 esq = (a2-b2)/a2;
563 e = pow(esq,(double)0.5);
564 ed2 = e / (double)2.0;
565
566 phi0s = sin(phi0);
567 ess = e * phi0s;
568 t0 = tan(po4-phi0/(double)2.0) / pow(((double)1.0-ess) /
569 ((double)1.0+ess),ed2);
570
571
572 phi1s = sin(phi1);
573 phi1c = cos(phi1);
574 ess = e * phi1s;
575 m1 = phi1c / pow(((double)1.0-ess*ess),(double)0.5);
576 t1 = tan(po4-phi1/(double)2.0) / pow(((double)1.0-ess) /
577 ((double)1.0+ess),ed2);
578
579 if (fabs(phi1-phi2)>1.0e-10) {
580 phi2s = sin(phi2);
581 phi2c = cos(phi2);
582 ess = e * phi2s;
583 m2 = phi2c / pow(((double)1.0-ess*ess),(double)0.5);
584 t2 = tan(po4-phi2/(double)2.0) / pow(((double)1.0-ess) /
585 ((double)1.0+ess),ed2);
586 n = log(m1/m2) / log(t1/t2);
587 } else {
588 n = phi1s;
589 }
590
591 F = m1 / (n*pow(t1,n));
592 Fa = F*a;
593
594 rho0 = pow(t0,n) * Fa;
595
596 tlat = theta = (double)0.0;
597 tol = (double)4.85e-10;
598
599 dx = E - E0;
600 dy = N - N0;
601 rhom = rho0 - dy;
602 rho = pow(dx*dx + rhom*rhom,(double)0.5);
603
604 if (n<0.0) {
605 rhom *= (double)-1.0;
606 dy *= (double)-1.0;
607 dx *= (double)-1.0;
608 rho *= (double)-1.0;
609 }
610
611 if (rho) {
612 theta = atan2(dx,rhom);
613 t = pow(rho/Fa,(double)1.0/n);
614 lat = po2 - (double)2.0*atan(t);
615 while (fabs(lat-tlat)>tol) {
616 tlat = lat;
617 phis = sin(lat);
618 ess = e * phis;
619 lat = po2 - (double)2.0 * atan(t*pow(((double)1.0-ess) /
620 ((double)1.0+ess),
621 e / (double)2.0));
622 }
623 *phi = lat;
624 *lambda = theta/n + M0;
625
626 if (*phi>po2) {
627 *phi=po2;
628 } else if (*phi<-po2) {
629 *phi=-po2;
630 }
631 if (*lambda>GPS_PI) {
632 *lambda -= (double)GPS_PI * (double)2.0;
633 }
634 if (*lambda<-GPS_PI) {
635 *lambda += (double)GPS_PI * (double)2.0;
636 }
637
638 if (*lambda>GPS_PI) {
639 *lambda = GPS_PI;
640 } else if (*lambda<-GPS_PI) {
641 *lambda = -GPS_PI;
642 }
643 } else {
644 if (n>0.0) {
645 *phi = po2;
646 } else {
647 *phi = -po2;
648 }
649 *lambda = M0;
650 }
651
652 *lambda = GPS_Math_Rad_To_Deg(*lambda);
653 *phi = GPS_Math_Rad_To_Deg(*phi);
654
655 return;
656 }
657
658
659
660
661 /* @func GPS_Math_Miller_LatLon_To_EN **********************************
662 **
663 ** Convert latitude and longitude to Miller Cylindrical projection easting and
664 ** northing
665 **
666 ** @param [r] phi [double] latitude (deg)
667 ** @param [r] lambda [double] longitude (deg)
668 ** @param [w] E [double *] easting (metre)
669 ** @param [w] N [double *] northing (metre)
670 ** @param [r] M0 [double] central meridian (deg)
671 ** @param [r] E0 [double] false easting
672 ** @param [r] N0 [double] false northing
673 ** @param [r] a [double] semi-major axis
674 ** @param [r] b [double] semi-minor axis
675 **
676 ** @return [void]
677 ************************************************************************/
GPS_Math_Miller_LatLon_To_EN(double phi,double lambda,double * E,double * N,double M0,double E0,double N0,double a,double b)678 void GPS_Math_Miller_LatLon_To_EN(double phi, double lambda, double* E,
679 double* N, double M0, double E0,
680 double N0, double a, double b)
681 {
682 double a2;
683 double b2;
684 double R;
685 double e2;
686 double e4;
687 double e6;
688 double p2;
689 double po2;
690 double phis;
691 double dlam;
692
693
694 phi = GPS_Math_Deg_To_Rad(phi);
695 lambda = GPS_Math_Deg_To_Rad(lambda);
696 M0 = GPS_Math_Deg_To_Rad(M0);
697
698 po2 = (double)GPS_PI / (double)2.0;
699 p2 = (double)GPS_PI * (double)2.0;
700 a2 = a*a;
701 b2 = b*b;
702 e2 = (a2-b2)/a2;
703 e4 = e2*e2;
704 e6 = e4*e2;
705
706 R = a*((double)1.0-e2/(double)6.0-(double)17.0*e4/(double)360.0-
707 (double)67.0*e6/(double)3024.0);
708
709 if (M0>GPS_PI) {
710 M0 -= p2;
711 }
712
713 phis = sin((double)0.8 * phi);
714
715 dlam = lambda - M0;
716 if (dlam>GPS_PI) {
717 dlam-=p2;
718 }
719 if (dlam<-GPS_PI) {
720 dlam+=p2;
721 }
722
723 *E = R*dlam+E0;
724 *N = (R/(double)1.6) * log(((double)1.0+phis) / ((double)1.0-phis)) + N0;
725
726 return;
727 }
728
729
730
731
732 /* @func GPS_Math_Miller_EN_To_LatLon **********************************
733 **
734 ** Convert latitude and longitude to Miller Cylindrical projection easting and
735 ** northing
736 **
737 ** @param [r] E [double] easting (metre)
738 ** @param [r] N [double] northing (metre)
739 ** @param [w] phi [double *] latitude (deg)
740 ** @param [w] lambda [double *] longitude (deg)
741 ** @param [r] M0 [double] central meridian (deg)
742 ** @param [r] E0 [double] false easting
743 ** @param [r] N0 [double] false northing
744 ** @param [r] a [double] semi-major axis
745 ** @param [r] b [double] semi-minor axis
746 **
747 ** @return [void]
748 ************************************************************************/
GPS_Math_Miller_EN_To_LatLon(double E,double N,double * phi,double * lambda,double M0,double E0,double N0,double a,double b)749 void GPS_Math_Miller_EN_To_LatLon(double E, double N, double* phi,
750 double* lambda, double M0, double E0,
751 double N0, double a, double b)
752 {
753 double a2;
754 double b2;
755 double R;
756 double e;
757 double e2;
758 double e4;
759 double e6;
760 double p2;
761 double po2;
762 double dx;
763 double dy;
764
765 dx = E - E0;
766 dy = N - N0;
767
768 M0 = GPS_Math_Deg_To_Rad(M0);
769
770 po2 = (double)GPS_PI / (double)2.0;
771 p2 = (double)GPS_PI * (double)2.0;
772 a2 = a*a;
773 b2 = b*b;
774 e2 = (a2-b2)/a2;
775 e4 = e2*e2;
776 e6 = e4*e2;
777 e = pow(e2,(double)0.5);
778
779 R = a*((double)1.0-e2/(double)6.0-(double)17.0*e4/(double)360.0-
780 (double)67.0*e6/(double)3024.0);
781 if (M0>GPS_PI) {
782 M0 -= p2;
783 }
784
785 *phi = atan(sinh((double)0.8*dy/R)) / (double)0.8;
786 *lambda = M0+dx/R;
787
788 if (*phi>po2) {
789 *phi=po2;
790 } else if (*phi<-po2) {
791 *phi=-po2;
792 }
793
794 if (*lambda>GPS_PI) {
795 *lambda-=p2;
796 }
797 if (*lambda<-GPS_PI) {
798 *lambda+=p2;
799 }
800
801 if (*lambda>GPS_PI) {
802 *lambda=GPS_PI;
803 } else if (*lambda<-GPS_PI) {
804 *lambda=-GPS_PI;
805 }
806
807 *lambda = GPS_Math_Rad_To_Deg(*lambda);
808 *phi = GPS_Math_Rad_To_Deg(*phi);
809
810 return;
811 }
812
813
814
815
816 /* @func GPS_Math_Bonne_LatLon_To_EN **********************************
817 **
818 ** Convert latitude and longitude to Bonne pseudoconic equal area projection
819 ** easting and northing
820 **
821 ** @param [r] phi [double] latitude (deg)
822 ** @param [r] lambda [double] longitude (deg)
823 ** @param [w] E [double *] easting (metre)
824 ** @param [w] N [double *] northing (metre)
825 ** @param [r] phi0 [double] latitude of origin (deg)
826 ** @param [r] M0 [double] central meridian (deg)
827 ** @param [r] E0 [double] false easting
828 ** @param [r] N0 [double] false northing
829 ** @param [r] a [double] semi-major axis
830 ** @param [r] b [double] semi-minor axis
831 **
832 ** @return [void]
833 ************************************************************************/
GPS_Math_Bonne_LatLon_To_EN(double phi,double lambda,double * E,double * N,double phi0,double M0,double E0,double N0,double a,double b)834 void GPS_Math_Bonne_LatLon_To_EN(double phi, double lambda, double* E,
835 double* N, double phi0, double M0, double E0,
836 double N0, double a, double b)
837 {
838 double p2;
839 double po2;
840 double a2;
841 double b2;
842 double e2;
843 double e4;
844 double e6;
845 double M1;
846 double m1;
847 double c0;
848 double c1;
849 double c2;
850 double c3;
851 double j;
852 double te4;
853 double E1;
854 double E2;
855 double E3;
856 double E4;
857 double x;
858 double phi0s;
859 double lat;
860 double phi0c;
861 double phi0s2;
862 double phi0s4;
863 double phi0s6;
864 double as;
865
866 double phis;
867 double phic;
868 double phis2;
869 double phis4;
870 double phis6;
871 double dlam;
872 double mm;
873 double MM;
874 double rho;
875 double EE;
876 double tol;
877
878
879 lambda = GPS_Math_Deg_To_Rad(lambda);
880 phi = GPS_Math_Deg_To_Rad(phi);
881 phi0 = GPS_Math_Deg_To_Rad(phi0);
882 M0 = GPS_Math_Deg_To_Rad(M0);
883
884 phi0s = sin(phi0);
885 p2 = (double)GPS_PI * (double)2.0;
886 po2 = (double)GPS_PI / (double)2.0;
887 if (M0>GPS_PI) {
888 M0 -= p2;
889 }
890 a2 = a*a;
891 b2 = b*b;
892 e2 = (a2-b2)/a2;
893 e4 = e2*e2;
894 e6 = e2*e4;
895
896 j = (double)45.0*e6/(double)1024.0;
897 te4 = (double)3.0 * e4;
898 c0 = (double)1.0-e2/(double)4.0-te4/(double)64.0-(double)5.0*e6/
899 (double)256.0;
900 c1 = (double)3.0*e2/(double)8.0+te4/(double)32.0+j;
901 c2 = (double)15.0*e4/(double)256.0+j;
902 c3 = (double)35.0*e6/(double)3072.0;
903
904 phi0c = cos(phi0);
905 m1 = phi0c/ pow(((double)1.0-e2*phi0s*phi0s),(double)0.5);
906 lat = c0 * phi0;
907
908 phi0s2 = c1 * sin((double)2.0*phi0);
909 phi0s4 = c2 * sin((double)4.0*phi0);
910 phi0s6 = c3 * sin((double)6.0*phi0);
911 M1 = a*(lat-phi0s2+phi0s4-phi0s6);
912
913 x = pow((double)1.0-e2,(double)0.5);
914 E1 = ((double)1.0-x) / ((double)1.0+x);
915 E2 = E1*E1;
916 E3 = E2*E1;
917 E4 = E3*E1;
918
919 if (!phi0s) {
920 as = (double)0.0;
921 } else {
922 as = a*m1/phi0s;
923 }
924
925
926 dlam = lambda - M0;
927 if (dlam>GPS_PI) {
928 dlam -= p2;
929 }
930 if (dlam<-GPS_PI) {
931 dlam += p2;
932 }
933
934 phis = sin(phi);
935 phic = cos(phi);
936
937 tol = (double)0.0001;
938 if (!(phi-phi0) && (((po2-tol)<fabs(phi)) && (fabs(phi)<po2+tol))) {
939 *E = *N = (double)0.0;
940 } else {
941 mm = phic / pow(((double)1.0-e2*phis*phis),(double)0.5);
942 lat = c0 * phi;
943 phis2 = c1 * sin((double)2.0*phi);
944 phis4 = c2 * sin((double)4.0*phi);
945 phis6 = c3 * sin((double)6.0*phi);
946 MM = a * (lat-phis2+phis4-phis6);
947
948 rho = as + M1 - MM;
949 if (!rho) {
950 EE = (double)0.0;
951 } else {
952 EE = a * mm * dlam / rho;
953 }
954
955 *E = rho * sin(EE) + E0;
956 *N = as - rho * cos(EE) + N0;
957 }
958
959 return;
960 }
961
962
963
964
965 /* @func GPS_Math_Bonne_EN_To_LatLon **********************************
966 **
967 ** Convert Bonne pseudoconic equal area easting and northing projection
968 ** to latitude and longitude
969 **
970 ** @param [r] E [double] easting (metre)
971 ** @param [r] N [double] northing (metre)
972 ** @param [w] phi [double *] latitude (deg)
973 ** @param [w] lambda [double *] longitude (deg)
974 ** @param [r] phi0 [double] latitude of origin (deg)
975 ** @param [r] M0 [double] central meridian (deg)
976 ** @param [r] E0 [double] false easting
977 ** @param [r] N0 [double] false northing
978 ** @param [r] a [double] semi-major axis
979 ** @param [r] b [double] semi-minor axis
980 **
981 ** @return [void]
982 ************************************************************************/
GPS_Math_Bonne_EN_To_LatLon(double E,double N,double * phi,double * lambda,double phi0,double M0,double E0,double N0,double a,double b)983 void GPS_Math_Bonne_EN_To_LatLon(double E, double N, double* phi,
984 double* lambda, double phi0, double M0,
985 double E0, double N0, double a, double b)
986 {
987 double p2;
988 double po2;
989 double a2;
990 double b2;
991 double e2;
992 double e4;
993 double e6;
994 double M1;
995 double m1;
996 double c0;
997 double c1;
998 double c2;
999 double c3;
1000 double A0;
1001 double A1;
1002 double A2;
1003 double A3;
1004 double j;
1005 double te4;
1006 double E1;
1007 double E2;
1008 double E3;
1009 double E4;
1010 double x;
1011 double phi0s;
1012 double lat;
1013 double phi0c;
1014 double phi0s2;
1015 double phi0s4;
1016 double phi0s6;
1017 double as;
1018
1019 double phis;
1020 double phic;
1021 double dx;
1022 double dy;
1023 double mu;
1024 double mm;
1025 double MM;
1026 double asdy;
1027 double rho;
1028 double smu2;
1029 double smu4;
1030 double smu6;
1031 double smu8;
1032 double tol;
1033
1034
1035 phi0 = GPS_Math_Deg_To_Rad(phi0);
1036 M0 = GPS_Math_Deg_To_Rad(M0);
1037
1038 phi0s = sin(phi0);
1039 p2 = (double)GPS_PI * (double)2.0;
1040 po2 = (double)GPS_PI / (double)2.0;
1041 if (M0>GPS_PI) {
1042 M0 -= p2;
1043 }
1044 a2 = a*a;
1045 b2 = b*b;
1046 e2 = (a2-b2)/a2;
1047 e4 = e2*e2;
1048 e6 = e2*e4;
1049
1050 j = (double)45.0*e6/(double)1024.0;
1051 te4 = (double)3.0 * e4;
1052 c0 = (double)1.0-e2/(double)4.0-te4/(double)64.0-(double)5.0*e6/
1053 (double)256.0;
1054 c1 = (double)3.0*e2/(double)8.0+te4/(double)32.0+j;
1055 c2 = (double)15.0*e4/(double)256.0+j;
1056 c3 = (double)35.0*e6/(double)3072.0;
1057
1058 phi0c = cos(phi0);
1059 m1 = phi0c/ pow(((double)1.0-e2*phi0s*phi0s),(double)0.5);
1060 lat = c0 * phi0;
1061
1062 phi0s2 = c1 * sin((double)2.0*phi0);
1063 phi0s4 = c2 * sin((double)4.0*phi0);
1064 phi0s6 = c3 * sin((double)6.0*phi0);
1065 M1 = a*(lat-phi0s2+phi0s4-phi0s6);
1066
1067 x = pow((double)1.0-e2,(double)0.5);
1068 E1 = ((double)1.0-x) / ((double)1.0+x);
1069 E2 = E1*E1;
1070 E3 = E2*E1;
1071 E4 = E3*E1;
1072 A0 = (double)3.0*E1/(double)2.0-(double)27.0*E3/(double)32.0;
1073 A1 = (double)21.0*E2/(double)16.0-(double)55.0*E4/(double)32.0;
1074 A2 = (double)151.0*E3/(double)96.0;
1075 A3 = (double)1097.0*E4/(double)512.0;
1076 if (!phi0s) {
1077 as = (double)0.0;
1078 } else {
1079 as = a*m1/phi0s;
1080 }
1081
1082
1083 dx = E - E0;
1084 dy = N - N0;
1085 asdy = as - dy;
1086 rho = pow(dx*dx+asdy*asdy,(double)0.5);
1087 if (phi0<(double)0.0) {
1088 rho=-rho;
1089 }
1090 MM = as+M1-rho;
1091
1092 mu = MM / (a*c0);
1093 smu2 = A0 * sin((double)2.0*mu);
1094 smu4 = A1 * sin((double)4.0*mu);
1095 smu6 = A2 * sin((double)6.0*mu);
1096 smu8 = A3 * sin((double)8.0*mu);
1097 *phi = mu+smu2+smu4+smu6+smu8;
1098
1099 tol = (double)0.00001;
1100 if (((po2-tol)<fabs(*phi)) && (fabs(*phi)<po2+tol)) {
1101 *lambda = M0;
1102 } else {
1103 phic = cos(*phi);
1104 phis = sin(*phi);
1105 mm = phic / pow(((double)1.0-e2*phis*phis),(double)0.5);
1106 if (phi0<(double)0.0) {
1107 dx = -dx;
1108 asdy = -asdy;
1109 }
1110 *lambda = M0 + rho * (atan2(dx,asdy)) / (a * mm);
1111 }
1112
1113 if (*phi>po2) {
1114 *phi = po2;
1115 } else if (*phi<-po2) {
1116 *phi = -po2;
1117 }
1118
1119 if (*lambda>GPS_PI) {
1120 *lambda -= p2;
1121 }
1122 if (*lambda<-GPS_PI) {
1123 *lambda += p2;
1124 }
1125
1126 if (*lambda>GPS_PI) {
1127 *lambda = GPS_PI;
1128 } else if (*lambda<-GPS_PI) {
1129 *lambda=-GPS_PI;
1130 }
1131
1132 *lambda = GPS_Math_Rad_To_Deg(*lambda);
1133 *phi = GPS_Math_Rad_To_Deg(*phi);
1134
1135 return;
1136 }
1137
1138
1139
1140
1141 /* @func GPS_Math_Cassini_LatLon_To_EN **********************************
1142 **
1143 ** Convert latitude and longitude to Cassini transverse cylindrical projection
1144 ** easting and northing
1145 **
1146 ** @param [r] phi [double] latitude (deg)
1147 ** @param [r] lambda [double] longitude (deg)
1148 ** @param [w] E [double *] easting (metre)
1149 ** @param [w] N [double *] northing (metre)
1150 ** @param [r] phi0 [double] latitude of origin (deg)
1151 ** @param [r] M0 [double] central meridian (deg)
1152 ** @param [r] E0 [double] false easting
1153 ** @param [r] N0 [double] false northing
1154 ** @param [r] a [double] semi-major axis
1155 ** @param [r] b [double] semi-minor axis
1156 **
1157 ** @return [void]
1158 ************************************************************************/
GPS_Math_Cassini_LatLon_To_EN(double phi,double lambda,double * E,double * N,double phi0,double M0,double E0,double N0,double a,double b)1159 void GPS_Math_Cassini_LatLon_To_EN(double phi, double lambda, double* E,
1160 double* N, double phi0, double M0,
1161 double E0, double N0, double a, double b)
1162 {
1163 double p2;
1164 double po2;
1165 double a2;
1166 double b2;
1167 double e2;
1168 double e4;
1169 double e6;
1170 double AM0;
1171 double c0;
1172 double c1;
1173 double c2;
1174 double c3;
1175 double om0;
1176 double A0;
1177 double A1;
1178 double A2;
1179 double A3;
1180 double j;
1181 double te4;
1182 double phi0s2;
1183 double phi0s4;
1184 double phi0s6;
1185 double lat;
1186 double x;
1187 double E1;
1188 double E2;
1189 double E3;
1190 double E4;
1191
1192 double phis;
1193 double phic;
1194 double phit;
1195 double phis2;
1196 double phis4;
1197 double phis6;
1198 double RD;
1199 double dlam;
1200 double NN;
1201 double TT;
1202 double WW;
1203 double WW2;
1204 double WW3;
1205 double WW4;
1206 double WW5;
1207 double CC;
1208 double MM;
1209
1210
1211 lambda = GPS_Math_Deg_To_Rad(lambda);
1212 phi0 = GPS_Math_Deg_To_Rad(phi0);
1213 phi = GPS_Math_Deg_To_Rad(phi);
1214 M0 = GPS_Math_Deg_To_Rad(M0);
1215
1216
1217 p2 = (double)GPS_PI * (double)2.;
1218 po2 = (double)GPS_PI / (double)2.;
1219
1220 a2 = a*a;
1221 b2 = b*b;
1222 e2 = (a2-b2)/a2;
1223 e4 = e2*e2;
1224 e6 = e2*e4;
1225
1226 te4 = (double)3.0 * e4;
1227 j = (double)45. * e6 / (double)1024.;
1228 c0 = (double)1.0-e2/(double)4.-te4/(double)64.-(double)5.*e6/(double)256.;
1229 c1 = (double)3.*e2/(double)8.+te4/(double)32.+j;
1230 c2 = (double)15.*e4/(double)256.+j;
1231 c3 = (double)35.*e6/(double)3072.;
1232
1233 lat = c0*phi0;
1234 phi0s2 = c1 * sin((double)2.*phi0);
1235 phi0s4 = c2 * sin((double)4.*phi0);
1236 phi0s6 = c3 * sin((double)6.*phi0);
1237 AM0 = a * (lat-phi0s2+phi0s4-phi0s6);
1238
1239 om0 = (double)1.0 - e2;
1240 x = pow(om0,(double)0.5);
1241 E1 = ((double)1.0 - x) / ((double)1.0 + x);
1242 E2 = E1*E1;
1243 E3 = E1*E2;
1244 E4 = E1*E3;
1245 A0 = (double)3.*E1/(double)2.-(double)27.*E3/(double)32.;
1246 A1 = (double)21.*E2/(double)16.-(double)55.*E4/(double)32.;
1247 A2 = (double)151.*E3/(double)96.;
1248 A3 = (double)1097.*E4/(double)512.;
1249
1250
1251 dlam = lambda - M0;
1252 if (dlam>GPS_PI) {
1253 dlam -= p2;
1254 }
1255 if (dlam<-GPS_PI) {
1256 dlam += p2;
1257 }
1258
1259 phis = sin(phi);
1260 phic = cos(phi);
1261 phit = tan(phi);
1262 RD = pow((double)1.-e2*phis*phis,(double).5);
1263 NN = a/RD;
1264 TT = phit*phit;
1265 WW = dlam*phic;
1266 WW2 = WW*WW;
1267 WW3 = WW*WW2;
1268 WW4 = WW*WW3;
1269 WW5 = WW*WW4;
1270 CC = e2*phic*phic/om0;
1271 lat = c0*phi;
1272 phis2 = c1 * sin((double)2.*phi);
1273 phis4 = c2 * sin((double)4.*phi);
1274 phis6 = c3 * sin((double)6.*phi);
1275 MM = a * (lat-phis2+phis4-phis6);
1276
1277 *E = NN*(WW-(TT*WW3/(double)6.)-((double)8.-TT+(double)8.*CC)*
1278 (TT*WW5/(double)120.)) + E0;
1279 *N = MM-AM0+NN*phit*((WW2/(double)2.)+((double)5.-TT+(double)6.*CC) *
1280 WW4/(double)24.) + N0;
1281 return;
1282 }
1283
1284
1285
1286
1287 /* @func GPS_Math_Cassini_EN_To_LatLon **********************************
1288 **
1289 ** Convert Cassini transverse cylindrical easting and northing projection
1290 ** to latitude and longitude
1291 **
1292 ** @param [r] E [double] easting (metre)
1293 ** @param [r] N [double] northing (metre)
1294 ** @param [w] phi [double *] latitude (deg)
1295 ** @param [w] lambda [double *] longitude (deg)
1296 ** @param [r] phi0 [double] latitude of origin (deg)
1297 ** @param [r] M0 [double] central meridian (deg)
1298 ** @param [r] E0 [double] false easting
1299 ** @param [r] N0 [double] false northing
1300 ** @param [r] a [double] semi-major axis
1301 ** @param [r] b [double] semi-minor axis
1302 **
1303 ** @return [void]
1304 ************************************************************************/
GPS_Math_Cassini_EN_To_LatLon(double E,double N,double * phi,double * lambda,double phi0,double M0,double E0,double N0,double a,double b)1305 void GPS_Math_Cassini_EN_To_LatLon(double E, double N, double* phi,
1306 double* lambda, double phi0, double M0,
1307 double E0, double N0, double a, double b)
1308
1309 {
1310 double p2;
1311 double po2;
1312 double a2;
1313 double b2;
1314 double e2;
1315 double e4;
1316 double e6;
1317 double AM0;
1318 double c0;
1319 double c1;
1320 double c2;
1321 double c3;
1322 double om0;
1323 double A0;
1324 double A1;
1325 double A2;
1326 double A3;
1327 double j;
1328 double te4;
1329 double phi0s2;
1330 double phi0s4;
1331 double phi0s6;
1332 double lat;
1333 double x;
1334 double E1;
1335 double E2;
1336 double E3;
1337 double E4;
1338
1339 double dx;
1340 double dy;
1341 double mu;
1342 double mus2;
1343 double mus4;
1344 double mus6;
1345 double mus8;
1346 double M1;
1347 double phi1;
1348 double phi1s;
1349 double phi1c;
1350 double phi1t;
1351 double T;
1352 double T1;
1353 double N1;
1354 double R1;
1355 double RD;
1356 double DD;
1357 double D2;
1358 double D3;
1359 double D4;
1360 double D5;
1361 double tol;
1362
1363 M0 = GPS_Math_Deg_To_Rad(M0);
1364 phi0 = GPS_Math_Deg_To_Rad(phi0);
1365
1366 p2 = (double)GPS_PI * (double)2.;
1367 po2 = (double)GPS_PI / (double)2.;
1368
1369 a2 = a*a;
1370 b2 = b*b;
1371 e2 = (a2-b2)/a2;
1372 e4 = e2*e2;
1373 e6 = e2*e4;
1374
1375 te4 = (double)3.0 * e4;
1376 j = (double)45. * e6 / (double)1024.;
1377 c0 = (double)1.0-e2/(double)4.-te4/(double)64.-(double)5.*e6/(double)256.;
1378 c1 = (double)3.*e2/(double)8.+te4/(double)32.+j;
1379 c2 = (double)15.*e4/(double)256.+j;
1380 c3 = (double)35.*e6/(double)3072.;
1381
1382 lat = c0*phi0;
1383 phi0s2 = c1 * sin((double)2.*phi0);
1384 phi0s4 = c2 * sin((double)4.*phi0);
1385 phi0s6 = c3 * sin((double)6.*phi0);
1386 AM0 = a * (lat-phi0s2+phi0s4-phi0s6);
1387
1388 om0 = (double)1.0 - e2;
1389 x = pow(om0,(double)0.5);
1390 E1 = ((double)1.0 - x) / ((double)1.0 + x);
1391 E2 = E1*E1;
1392 E3 = E1*E2;
1393 E4 = E1*E3;
1394 A0 = (double)3.*E1/(double)2.-(double)27.*E3/(double)32.;
1395 A1 = (double)21.*E2/(double)16.-(double)55.*E4/(double)32.;
1396 A2 = (double)151.*E3/(double)96.;
1397 A3 = (double)1097.*E4/(double)512.;
1398
1399
1400
1401 tol = (double)1.e-5;
1402
1403 dx = E - E0;
1404 dy = N - N0;
1405 M1 = AM0 + dy;
1406 mu = M1 / (a*c0);
1407 mus2 = A0 * sin((double)2.*mu);
1408 mus4 = A1 * sin((double)4.*mu);
1409 mus6 = A2 * sin((double)6.*mu);
1410 mus8 = A3 * sin((double)8.*mu);
1411 phi1 = mu + mus2 + mus4 + mus6 + mus8;
1412
1413 if ((((po2-tol)<phi1)&&(phi1<(po2+tol)))) {
1414 *phi = po2;
1415 *lambda = M0;
1416 } else if ((((-po2-tol)<phi1)&&(phi1<(-po2+tol)))) {
1417 *phi = -po2;
1418 *lambda = M0;
1419 } else {
1420 phi1s = sin(phi1);
1421 phi1c = cos(phi1);
1422 phi1t = tan(phi1);
1423 T1 = phi1t*phi1t;
1424 RD = pow((double)1.-e2*phi1s*phi1s,(double).5);
1425 N1 = a/RD;
1426 R1 = N1 * om0 / (RD*RD);
1427 DD = dx/N1;
1428 D2 = DD*DD;
1429 D3 = DD*D2;
1430 D4 = DD*D3;
1431 D5 = DD*D4;
1432 T = (double)1. + (double)3.*T1;
1433 *phi = phi1-(N1*phi1t/R1)*(D2/(double)2.-T*D4/(double)24.);
1434 *lambda = M0+(DD-T1*D3/(double)3.+T*T1*D5/(double)15.)/phi1c;
1435
1436 if (*phi>po2) {
1437 *phi=po2;
1438 } else if (*phi<-po2) {
1439 *phi=-po2;
1440 }
1441
1442 if (*lambda>GPS_PI) {
1443 *lambda -= p2;
1444 }
1445 if (*lambda<-GPS_PI) {
1446 *lambda += p2;
1447 }
1448
1449 if (*lambda>GPS_PI) {
1450 *lambda=GPS_PI;
1451 } else if (*lambda<-GPS_PI) {
1452 *lambda=-GPS_PI;
1453 }
1454 }
1455
1456 *lambda = GPS_Math_Rad_To_Deg(*lambda);
1457 *phi = GPS_Math_Rad_To_Deg(*phi);
1458
1459 return;
1460 }
1461
1462
1463
1464
1465 /* @func GPS_Math_Cylea_LatLon_To_EN **********************************
1466 **
1467 ** Convert latitude and longitude to Cylindrical equal area projection
1468 ** easting and northing
1469 **
1470 ** @param [r] phi [double] latitude (deg)
1471 ** @param [r] lambda [double] longitude (deg)
1472 ** @param [w] E [double *] easting (metre)
1473 ** @param [w] N [double *] northing (metre)
1474 ** @param [r] phi0 [double] latitude of origin (deg)
1475 ** @param [r] M0 [double] central meridian (deg)
1476 ** @param [r] E0 [double] false easting
1477 ** @param [r] N0 [double] false northing
1478 ** @param [r] a [double] semi-major axis
1479 ** @param [r] b [double] semi-minor axis
1480 **
1481 ** @return [void]
1482 ************************************************************************/
GPS_Math_Cylea_LatLon_To_EN(double phi,double lambda,double * E,double * N,double phi0,double M0,double E0,double N0,double a,double b)1483 void GPS_Math_Cylea_LatLon_To_EN(double phi, double lambda, double* E,
1484 double* N, double phi0, double M0,
1485 double E0, double N0, double a, double b)
1486 {
1487 double a2;
1488 double b2;
1489 double e;
1490 double e2;
1491 double e4;
1492 double e6;
1493 double k0;
1494 double ak0;
1495 double k2;
1496 double c0;
1497 double c1;
1498 double c2;
1499 double p2;
1500 double po2;
1501 double phi0s;
1502 double phi0c;
1503
1504 double dlam;
1505 double qq;
1506 double x;
1507 double phis;
1508
1509 lambda = GPS_Math_Deg_To_Rad(lambda);
1510 phi0 = GPS_Math_Deg_To_Rad(phi0);
1511 phi = GPS_Math_Deg_To_Rad(phi);
1512 M0 = GPS_Math_Deg_To_Rad(M0);
1513
1514 p2 = (double)GPS_PI * (double)2.;
1515 po2 = (double)GPS_PI / (double)2.;
1516
1517 if (M0>GPS_PI) {
1518 M0-=p2;
1519 }
1520
1521 a2 = a*a;
1522 b2 = b*b;
1523 e2 = (a2-b2)/a2;
1524 e4 = e2*e2;
1525 e6 = e2*e4;
1526 e = pow(e2,(double).5);
1527 c0 = e2/(double)3.+(double)31.*e4/(double)180.+(double)517.*
1528 e6/(double)5040.;
1529 c1 = (double)23.*e4/(double)360.+(double)251.*e6/(double)3780.;
1530 c2 = (double)761.*e6/(double)45360.;
1531
1532 phi0s = sin(phi0);
1533 phi0c = cos(phi0);
1534 k0 = phi0c / pow((double)1.-e2*phi0s*phi0s,(double).5);
1535 ak0 = a*k0;
1536 k2 = k0 * (double)2.;
1537
1538 dlam = lambda - M0;
1539 if (dlam>GPS_PI) {
1540 dlam-=p2;
1541 }
1542 if (dlam<-GPS_PI) {
1543 dlam+=p2;
1544 }
1545
1546 phis = sin(phi);
1547 x = e * phis;
1548 qq = ((double)1.-e2)*(phis/((double)1.-x*x)-((double)1./((double)2.*e))*
1549 log(((double)1.-x)/((double)1.+x)));
1550 *E = ak0 * dlam + E0;
1551 *N = a * qq / k2 + N0;
1552
1553 return;
1554 }
1555
1556
1557
1558
1559 /* @func GPS_Math_Cylea_EN_To_LatLon **********************************
1560 **
1561 ** Convert Cylindrical equal area easting and northing projection
1562 ** to latitude and longitude
1563 **
1564 ** @param [r] E [double] easting (metre)
1565 ** @param [r] N [double] northing (metre)
1566 ** @param [w] phi [double *] latitude (deg)
1567 ** @param [w] lambda [double *] longitude (deg)
1568 ** @param [r] phi0 [double] latitude of origin (deg)
1569 ** @param [r] M0 [double] central meridian (deg)
1570 ** @param [r] E0 [double] false easting
1571 ** @param [r] N0 [double] false northing
1572 ** @param [r] a [double] semi-major axis
1573 ** @param [r] b [double] semi-minor axis
1574 **
1575 ** @return [void]
1576 ************************************************************************/
GPS_Math_Cylea_EN_To_LatLon(double E,double N,double * phi,double * lambda,double phi0,double M0,double E0,double N0,double a,double b)1577 void GPS_Math_Cylea_EN_To_LatLon(double E, double N, double* phi,
1578 double* lambda, double phi0, double M0,
1579 double E0, double N0, double a, double b)
1580
1581 {
1582 double a2;
1583 double b2;
1584 double e;
1585 double e2;
1586 double e4;
1587 double e6;
1588 double k0;
1589 double ak0;
1590 double k2;
1591 double c0;
1592 double c1;
1593 double c2;
1594 double p2;
1595 double po2;
1596 double phi0s;
1597 double phi0c;
1598
1599 double dx;
1600 double dy;
1601 double qp;
1602 double bt;
1603 double phis;
1604 double i;
1605 double x;
1606 double bs2;
1607 double bs4;
1608 double bs6;
1609
1610
1611 phi0 = GPS_Math_Deg_To_Rad(phi0);
1612 M0 = GPS_Math_Deg_To_Rad(M0);
1613
1614 p2 = (double)GPS_PI * (double)2.;
1615 po2 = (double)GPS_PI / (double)2.;
1616
1617 if (M0>GPS_PI) {
1618 M0-=p2;
1619 }
1620
1621 a2 = a*a;
1622 b2 = b*b;
1623 e2 = (a2-b2)/a2;
1624 e4 = e2*e2;
1625 e6 = e2*e4;
1626 e = pow(e2,(double).5);
1627 c0 = e2/(double)3.+(double)31.*e4/(double)180.+(double)517.*
1628 e6/(double)5040.;
1629 c1 = (double)23.*e4/(double)360.+(double)251.*e6/(double)3780.;
1630 c2 = (double)761.*e6/(double)45360.;
1631
1632 phi0s = sin(phi0);
1633 phi0c = cos(phi0);
1634 k0 = phi0c / pow((double)1.-e2*phi0s*phi0s,(double).5);
1635 ak0 = a*k0;
1636 k2 = k0 * (double)2.;
1637
1638 dx = E - E0;
1639 dy = N - N0;
1640 phis = sin(po2);
1641 x = e*phis;
1642 qp = ((double)1.-e2)*(phis/((double)1.-x*x)-((double)1./((double)2.*e))*
1643 log(((double)1.-x)/((double)1.+x)));
1644 i = k2*dy/(a*qp);
1645 if (i>(double)1.) {
1646 i=(double)1.;
1647 } else if (i<(double)-1.) {
1648 i=(double)-1.;
1649 }
1650 bt = asin(i);
1651 bs2 = c0 * sin((double)2.*bt);
1652 bs4 = c1 * sin((double)4.*bt);
1653 bs6 = c2 * sin((double)6.*bt);
1654
1655 *phi = bt+bs2+bs4+bs6;
1656 *lambda = M0 + dx/ak0;
1657
1658 if (*phi>po2) {
1659 *phi=po2;
1660 } else if (*phi<-po2) {
1661 *phi=-po2;
1662 }
1663
1664 if (*lambda>GPS_PI) {
1665 *lambda -= p2;
1666 }
1667 if (*lambda<-GPS_PI) {
1668 *lambda += p2;
1669 }
1670
1671 if (*lambda>GPS_PI) {
1672 *lambda=GPS_PI;
1673 } else if (*lambda<-GPS_PI) {
1674 *lambda=-GPS_PI;
1675 }
1676
1677 *lambda = GPS_Math_Rad_To_Deg(*lambda);
1678 *phi = GPS_Math_Rad_To_Deg(*phi);
1679
1680 return;
1681 }
1682
1683
1684
1685
1686 /* @func GPS_Math_EckertIV_LatLon_To_EN **********************************
1687 **
1688 ** Convert latitude and longitude to Eckert IV equal area elliptical
1689 ** pseudocylindrical projection easting and northing
1690 **
1691 ** @param [r] phi [double] latitude (deg)
1692 ** @param [r] lambda [double] longitude (deg)
1693 ** @param [w] E [double *] easting (metre)
1694 ** @param [w] N [double *] northing (metre)
1695 ** @param [r] M0 [double] central meridian (deg)
1696 ** @param [r] E0 [double] false easting
1697 ** @param [r] N0 [double] false northing
1698 ** @param [r] a [double] semi-major axis
1699 ** @param [r] b [double] semi-minor axis
1700 **
1701 ** @return [void]
1702 ************************************************************************/
GPS_Math_EckertIV_LatLon_To_EN(double phi,double lambda,double * E,double * N,double M0,double E0,double N0,double a,double b)1703 void GPS_Math_EckertIV_LatLon_To_EN(double phi, double lambda, double* E,
1704 double* N, double M0, double E0, double N0,
1705 double a, double b)
1706 {
1707 double a2;
1708 double b2;
1709 double e2;
1710 double e4;
1711 double e6;
1712 double Ra0;
1713 double Ra1;
1714 double po2;
1715 double p2;
1716
1717 double Ra;
1718
1719 double phis;
1720 double theta;
1721 double dtheta;
1722 double thetas;
1723 double thetac;
1724 double n;
1725 double dlam;
1726 double tol;
1727
1728
1729 lambda = GPS_Math_Deg_To_Rad(lambda);
1730 phi = GPS_Math_Deg_To_Rad(phi);
1731 M0 = GPS_Math_Deg_To_Rad(M0);
1732
1733 p2 = (double)GPS_PI * (double)2.;
1734 po2 = (double)GPS_PI / (double)2.;
1735
1736 if (M0>GPS_PI) {
1737 M0-=p2;
1738 }
1739
1740
1741 a2 = a*a;
1742 b2 = b*b;
1743 e2 = (a2-b2) / a2;
1744 e4 = e2*e2;
1745 e6 = e2*e4;
1746 Ra = a*((double)1.-e2/(double)6.-(double)17.*e4/(double)360.-
1747 (double)67.*e6/(double)3024.);
1748 Ra0 = Ra * (double)0.4222382;
1749 Ra1 = Ra * (double)1.3265004;
1750
1751 theta = phi / (double)2.;
1752 dtheta = (double)1.;
1753 tol = (double)4.85e-10;
1754 phis = sin(phi);
1755
1756 dlam = lambda - M0;
1757 if (dlam>GPS_PI) {
1758 dlam -= p2;
1759 }
1760 if (dlam<-GPS_PI) {
1761 dlam += p2;
1762 }
1763
1764 while (fabs(dtheta)>tol) {
1765 thetas = sin(theta);
1766 thetac = cos(theta);
1767 n = theta+thetas*thetac+(double)2.*thetas;
1768 dtheta = -(n-((double)2.+po2)*phis) /
1769 ((double)2.*thetac*((double)1.+thetac));
1770 theta += dtheta;
1771 }
1772
1773 *E = Ra0*dlam*((double)1.+cos(theta))+E0;
1774 *N = Ra1*sin(theta)+N0;
1775
1776 return;
1777 }
1778
1779
1780
1781
1782 /* @func GPS_Math_EckertIV_EN_To_LatLon **********************************
1783 **
1784 ** Convert Eckert IV equal area elliptical pseudocylindrical projection
1785 ** easting and northing to latitude and longitude
1786 **
1787 ** @param [r] E [double] easting (metre)
1788 ** @param [r] N [double] northing (metre)
1789 ** @param [w] phi [double *] latitude (deg)
1790 ** @param [w] lambda [double *] longitude (deg)
1791 ** @param [r] M0 [double] central meridian (deg)
1792 ** @param [r] E0 [double] false easting
1793 ** @param [r] N0 [double] false northing
1794 ** @param [r] a [double] semi-major axis
1795 ** @param [r] b [double] semi-minor axis
1796 **
1797 ** @return [void]
1798 ************************************************************************/
GPS_Math_EckertIV_EN_To_LatLon(double E,double N,double * phi,double * lambda,double M0,double E0,double N0,double a,double b)1799 void GPS_Math_EckertIV_EN_To_LatLon(double E, double N, double* phi,
1800 double* lambda, double M0, double E0,
1801 double N0, double a, double b)
1802 {
1803 double a2;
1804 double b2;
1805 double e2;
1806 double e4;
1807 double e6;
1808 double Ra0;
1809 double Ra1;
1810 double po2;
1811 double p2;
1812
1813 double Ra;
1814 double theta;
1815 double thetas;
1816 double thetac;
1817 double n;
1818 double dx;
1819 double dy;
1820 double i;
1821
1822
1823 M0 = GPS_Math_Deg_To_Rad(M0);
1824
1825 p2 = (double)GPS_PI * (double)2.;
1826 po2 = (double)GPS_PI / (double)2.;
1827
1828 if (M0>GPS_PI) {
1829 M0-=p2;
1830 }
1831
1832
1833 a2 = a*a;
1834 b2 = b*b;
1835 e2 = (a2-b2) / a2;
1836 e4 = e2*e2;
1837 e6 = e2*e4;
1838 Ra = a*((double)1.-e2/(double)6.-(double)17.*e4/(double)360.-
1839 (double)67.*e6/(double)3024.);
1840 Ra0 = Ra * (double)0.4222382;
1841 Ra1 = Ra * (double)1.3265004;
1842
1843 dx = E - E0;
1844 dy = N - N0;
1845 i = dy/Ra1;
1846 if (i>(double)1.) {
1847 i=(double)1.;
1848 } else if (i<(double)-1.) {
1849 i=(double)-1.;
1850 }
1851
1852 theta = asin(i);
1853 thetas = sin(theta);
1854 thetac = cos(theta);
1855 n = theta+thetas*thetac+(double)2.*thetas;
1856
1857 *phi = asin(n/((double)2. + po2));
1858 *lambda = M0 + dx / (Ra0*((double)1.+thetac));
1859
1860 if (*phi>po2) {
1861 *phi=po2;
1862 } else if (*phi<-po2) {
1863 *phi=-po2;
1864 }
1865
1866 if (*lambda>GPS_PI) {
1867 *lambda -= p2;
1868 }
1869 if (*lambda<-GPS_PI) {
1870 *lambda += p2;
1871 }
1872
1873 if (*lambda>GPS_PI) {
1874 *lambda=GPS_PI;
1875 } else if (*lambda<-GPS_PI) {
1876 *lambda=-GPS_PI;
1877 }
1878
1879 *lambda = GPS_Math_Rad_To_Deg(*lambda);
1880 *phi = GPS_Math_Rad_To_Deg(*phi);
1881
1882 return;
1883 }
1884
1885
1886
1887
1888
1889 /* @func GPS_Math_EckertVI_LatLon_To_EN **********************************
1890 **
1891 ** Convert latitude and longitude to Eckert VI equal area
1892 ** pseudocylindrical projection easting and northing
1893 **
1894 ** @param [r] phi [double] latitude (deg)
1895 ** @param [r] lambda [double] longitude (deg)
1896 ** @param [w] E [double *] easting (metre)
1897 ** @param [w] N [double *] northing (metre)
1898 ** @param [r] M0 [double] central meridian (deg)
1899 ** @param [r] E0 [double] false easting
1900 ** @param [r] N0 [double] false northing
1901 ** @param [r] a [double] semi-major axis
1902 ** @param [r] b [double] semi-minor axis
1903 **
1904 ** @return [void]
1905 ************************************************************************/
GPS_Math_EckertVI_LatLon_To_EN(double phi,double lambda,double * E,double * N,double M0,double E0,double N0,double a,double b)1906 void GPS_Math_EckertVI_LatLon_To_EN(double phi, double lambda, double* E,
1907 double* N, double M0, double E0, double N0,
1908 double a, double b)
1909 {
1910 double a2;
1911 double b2;
1912 double e2;
1913 double e4;
1914 double e6;
1915 double Ra;
1916 double Rsq;
1917 double IRa;
1918 double po2;
1919 double p2;
1920
1921 double phis;
1922 double theta;
1923 double dtheta;
1924 double dlam;
1925 double tol;
1926
1927
1928 lambda = GPS_Math_Deg_To_Rad(lambda);
1929 phi = GPS_Math_Deg_To_Rad(phi);
1930 M0 = GPS_Math_Deg_To_Rad(M0);
1931
1932 p2 = (double)GPS_PI * (double)2.;
1933 po2 = (double)GPS_PI / (double)2.;
1934
1935 if (M0>GPS_PI) {
1936 M0-=p2;
1937 }
1938
1939
1940 a2 = a*a;
1941 b2 = b*b;
1942 e2 = (a2-b2) / a2;
1943 e4 = e2*e2;
1944 e6 = e2*e4;
1945 Ra = a*((double)1.-e2/(double)6.-(double)17.*e4/(double)360.-
1946 (double)67.*e6/(double)3024.);
1947 Rsq = Ra/pow((double)2.+GPS_PI,(double).5);
1948 IRa = (double)1./Rsq;
1949
1950 phis = sin(phi);
1951 theta = phi;
1952 dtheta = (double)1.;
1953 tol = (double)4.85e-10;
1954
1955 dlam = lambda - M0;
1956 if (dlam>GPS_PI) {
1957 dlam -= p2;
1958 }
1959 if (dlam<-GPS_PI) {
1960 dlam += p2;
1961 }
1962
1963 while (fabs(dtheta)>tol) {
1964 dtheta = -(theta+sin(theta)-((double)1.+po2)*phis) /
1965 ((double)1.+cos(theta));
1966 theta += dtheta;
1967 }
1968
1969 *E = Rsq*dlam*((double)1.+cos(theta))+E0;
1970 *N = (double)2.*Rsq*theta+N0;
1971
1972 return;
1973 }
1974
1975
1976
1977
1978 /* @func GPS_Math_EckertVI_EN_To_LatLon **********************************
1979 **
1980 ** Convert Eckert VI equal area pseudocylindrical projection
1981 ** easting and northing to latitude and longitude
1982 **
1983 ** @param [r] E [double] easting (metre)
1984 ** @param [r] N [double] northing (metre)
1985 ** @param [w] phi [double *] latitude (deg)
1986 ** @param [w] lambda [double *] longitude (deg)
1987 ** @param [r] M0 [double] central meridian (deg)
1988 ** @param [r] E0 [double] false easting
1989 ** @param [r] N0 [double] false northing
1990 ** @param [r] a [double] semi-major axis
1991 ** @param [r] b [double] semi-minor axis
1992 **
1993 ** @return [void]
1994 ************************************************************************/
GPS_Math_EckertVI_EN_To_LatLon(double E,double N,double * phi,double * lambda,double M0,double E0,double N0,double a,double b)1995 void GPS_Math_EckertVI_EN_To_LatLon(double E, double N, double* phi,
1996 double* lambda, double M0, double E0,
1997 double N0, double a, double b)
1998 {
1999 double a2;
2000 double b2;
2001 double e2;
2002 double e4;
2003 double e6;
2004 double Rsq;
2005 double IRa;
2006 double po2;
2007 double p2;
2008
2009 double Ra;
2010 double theta;
2011 double dx;
2012 double dy;
2013 double i;
2014
2015
2016 M0 = GPS_Math_Deg_To_Rad(M0);
2017
2018 p2 = (double)GPS_PI * (double)2.;
2019 po2 = (double)GPS_PI / (double)2.;
2020
2021 if (M0>GPS_PI) {
2022 M0-=p2;
2023 }
2024
2025
2026 a2 = a*a;
2027 b2 = b*b;
2028 e2 = (a2-b2) / a2;
2029 e4 = e2*e2;
2030 e6 = e2*e4;
2031 Ra = a*((double)1.-e2/(double)6.-(double)17.*e4/(double)360.-
2032 (double)67.*e6/(double)3024.);
2033 Rsq = Ra/pow((double)2.+GPS_PI,(double).5);
2034 IRa = (double)1./Rsq;
2035
2036
2037 dx = E - E0;
2038 dy = N - N0;
2039 theta = IRa * dy / (double)2.;
2040 i = (theta+sin(theta)) / ((double)1.+po2);
2041 if (i>(double)1.) {
2042 *phi = po2;
2043 } else if (i<(double)-1.) {
2044 *phi = -po2;
2045 } else {
2046 *phi= asin(i);
2047 }
2048 *lambda = M0 + IRa * dx / ((double)1.+cos(theta));
2049
2050 if (*phi>po2) {
2051 *phi=po2;
2052 } else if (*phi<-po2) {
2053 *phi=-po2;
2054 }
2055
2056 if (*lambda>GPS_PI) {
2057 *lambda -= p2;
2058 }
2059 if (*lambda<-GPS_PI) {
2060 *lambda += p2;
2061 }
2062
2063 if (*lambda>GPS_PI) {
2064 *lambda=GPS_PI;
2065 } else if (*lambda<-GPS_PI) {
2066 *lambda=-GPS_PI;
2067 }
2068
2069 *lambda = GPS_Math_Rad_To_Deg(*lambda);
2070 *phi = GPS_Math_Rad_To_Deg(*phi);
2071
2072 return;
2073 }
2074
2075
2076
2077
2078
2079 /* @func GPS_Math_Cyled_LatLon_To_EN **********************************
2080 **
2081 ** Convert latitude and longitude to cylindrical equidistant projection
2082 ** easting and northing
2083 **
2084 ** @param [r] phi [double] latitude (deg)
2085 ** @param [r] lambda [double] longitude (deg)
2086 ** @param [w] E [double *] easting (metre)
2087 ** @param [w] N [double *] northing (metre)
2088 ** @param [r] phi0 [double] latitude of origin (deg)
2089 ** @param [r] M0 [double] central meridian (deg)
2090 ** @param [r] E0 [double] false easting
2091 ** @param [r] N0 [double] false northing
2092 ** @param [r] a [double] semi-major axis
2093 ** @param [r] b [double] semi-minor axis
2094 **
2095 ** @return [void]
2096 ************************************************************************/
GPS_Math_Cyled_LatLon_To_EN(double phi,double lambda,double * E,double * N,double phi0,double M0,double E0,double N0,double a,double b)2097 void GPS_Math_Cyled_LatLon_To_EN(double phi, double lambda, double* E,
2098 double* N, double phi0, double M0, double E0,
2099 double N0, double a, double b)
2100 {
2101 double p2;
2102 double po2;
2103 double a2;
2104 double b2;
2105 double e2;
2106 double e4;
2107 double e6;
2108 double Ra;
2109 double Rac;
2110 double phi0c;
2111
2112 double dlam;
2113
2114 lambda = GPS_Math_Deg_To_Rad(lambda);
2115 phi = GPS_Math_Deg_To_Rad(phi);
2116 phi0 = GPS_Math_Deg_To_Rad(phi0);
2117 M0 = GPS_Math_Deg_To_Rad(M0);
2118
2119 p2 = (double)GPS_PI * (double)2.0;
2120 po2 = (double)GPS_PI / (double)2.0;
2121 if (M0>GPS_PI) {
2122 M0 -= p2;
2123 }
2124
2125 a2 = a*a;
2126 b2 = b*b;
2127 e2 = (a2-b2)/a2;
2128 e4 = e2*e2;
2129 e6 = e2*e4;
2130
2131 Ra = a*((double)1.-e2/(double)6.-(double)17.*e4/(double)360.-
2132 (double)67.*e6/(double)3024.);
2133 phi0c = cos(phi0);
2134 Rac = Ra * phi0c;
2135
2136 dlam = lambda - M0;
2137 if (dlam>GPS_PI) {
2138 dlam -= p2;
2139 }
2140 if (dlam<-GPS_PI) {
2141 dlam += p2;
2142 }
2143
2144 *E = Rac * dlam + E0;
2145 *N = Ra * phi + N0;
2146
2147 return;
2148 }
2149
2150
2151
2152
2153 /* @func GPS_Math_Cyled_EN_To_LatLon **********************************
2154 **
2155 ** Convert cylindrical equidistant easting and northing projection
2156 ** to latitude and longitude
2157 **
2158 ** @param [r] E [double] easting (metre)
2159 ** @param [r] N [double] northing (metre)
2160 ** @param [w] phi [double *] latitude (deg)
2161 ** @param [w] lambda [double *] longitude (deg)
2162 ** @param [r] phi0 [double] latitude of origin (deg)
2163 ** @param [r] M0 [double] central meridian (deg)
2164 ** @param [r] E0 [double] false easting
2165 ** @param [r] N0 [double] false northing
2166 ** @param [r] a [double] semi-major axis
2167 ** @param [r] b [double] semi-minor axis
2168 **
2169 ** @return [void]
2170 ************************************************************************/
GPS_Math_Cyled_EN_To_LatLon(double E,double N,double * phi,double * lambda,double phi0,double M0,double E0,double N0,double a,double b)2171 void GPS_Math_Cyled_EN_To_LatLon(double E, double N, double* phi,
2172 double* lambda, double phi0, double M0,
2173 double E0, double N0, double a, double b)
2174 {
2175 double p2;
2176 double po2;
2177 double a2;
2178 double b2;
2179 double e2;
2180 double e4;
2181 double e6;
2182 double Ra;
2183 double Rac;
2184 double phi0c;
2185
2186 double dx;
2187 double dy;
2188
2189
2190 phi0 = GPS_Math_Deg_To_Rad(phi0);
2191 M0 = GPS_Math_Deg_To_Rad(M0);
2192
2193 p2 = (double)GPS_PI * (double)2.0;
2194 po2 = (double)GPS_PI / (double)2.0;
2195 if (M0>GPS_PI) {
2196 M0 -= p2;
2197 }
2198
2199 a2 = a*a;
2200 b2 = b*b;
2201 e2 = (a2-b2)/a2;
2202 e4 = e2*e2;
2203 e6 = e2*e4;
2204
2205 Ra = a*((double)1.-e2/(double)6.-(double)17.*e4/(double)360.-
2206 (double)67.*e6/(double)3024.);
2207 phi0c = cos(phi0);
2208 Rac = Ra * phi0c;
2209
2210 dx = E - E0;
2211 dy = N - N0;
2212
2213 if (!Rac) {
2214 *lambda = (double)0.;
2215 } else {
2216 *lambda = M0 + dx / Rac;
2217 }
2218
2219 *phi = dy/Ra;
2220
2221 if (*phi>po2) {
2222 *phi = po2;
2223 } else if (*phi<-po2) {
2224 *phi = -po2;
2225 }
2226
2227 if (*lambda>GPS_PI) {
2228 *lambda -= p2;
2229 }
2230 if (*lambda<-GPS_PI) {
2231 *lambda += p2;
2232 }
2233
2234 if (*lambda>GPS_PI) {
2235 *lambda = GPS_PI;
2236 } else if (*lambda<-GPS_PI) {
2237 *lambda=-GPS_PI;
2238 }
2239
2240 *lambda = GPS_Math_Rad_To_Deg(*lambda);
2241 *phi = GPS_Math_Rad_To_Deg(*phi);
2242
2243 return;
2244 }
2245
2246
2247
2248
2249 /* @func GPS_Math_VderGrinten_LatLon_To_EN **********************************
2250 **
2251 ** Convert latitude and longitude to Van der Grinten polyconic projection
2252 ** easting and northing
2253 **
2254 ** @param [r] phi [double] latitude (deg)
2255 ** @param [r] lambda [double] longitude (deg)
2256 ** @param [w] E [double *] easting (metre)
2257 ** @param [w] N [double *] northing (metre)
2258 ** @param [r] M0 [double] central meridian (deg)
2259 ** @param [r] E0 [double] false easting
2260 ** @param [r] N0 [double] false northing
2261 ** @param [r] a [double] semi-major axis
2262 ** @param [r] b [double] semi-minor axis
2263 **
2264 ** @return [void]
2265 ************************************************************************/
GPS_Math_VderGrinten_LatLon_To_EN(double phi,double lambda,double * E,double * N,double M0,double E0,double N0,double a,double b)2266 void GPS_Math_VderGrinten_LatLon_To_EN(double phi, double lambda, double* E,
2267 double* N, double M0, double E0,
2268 double N0, double a, double b)
2269 {
2270 double po2;
2271 double p2;
2272 double a2;
2273 double b2;
2274 double e2;
2275 double e4;
2276 double e6;
2277 double Ra;
2278 double pRa;
2279
2280 double gg;
2281 double pp;
2282 double pp2;
2283 double gm0;
2284 double ppa;
2285 double thetai;
2286 double theta;
2287 double thetas;
2288 double thetac;
2289 double qq;
2290 double tol;
2291 double aa;
2292 double aa2;
2293 double dlam;
2294
2295 lambda = GPS_Math_Deg_To_Rad(lambda);
2296 phi = GPS_Math_Deg_To_Rad(phi);
2297 M0 = GPS_Math_Deg_To_Rad(M0);
2298
2299 p2 = (double)GPS_PI * (double)2.0;
2300 po2 = (double)GPS_PI / (double)2.0;
2301 if (M0>GPS_PI) {
2302 M0 -= p2;
2303 }
2304
2305
2306 a2 = a*a;
2307 b2 = b*b;
2308 e2 = (a2-b2)/a2;
2309 e4 = e2*e2;
2310 e6 = e2*e4;
2311
2312 Ra = a*((double)1.-e2/(double)6.-(double)17.*e4/(double)360.-(double)67.*
2313 e6/(double)3024.);
2314 pRa = (double)GPS_PI * Ra;
2315
2316 dlam = lambda - M0;
2317 if (dlam>GPS_PI) {
2318 dlam -= p2;
2319 }
2320 if (dlam<-GPS_PI) {
2321 dlam += p2;
2322 }
2323
2324 tol = (double)1.0e-5;
2325
2326 if (!phi) {
2327 *N = (double)0.0;
2328 *E = Ra*dlam+E0;
2329 } else if (!dlam || (((po2-tol)<tol)&&(phi<(po2+tol))) ||
2330 (((-po2-tol)<tol)&&(phi<(-po2+tol)))) {
2331 thetai = fabs(((double)2./(double)GPS_PI) * phi);
2332 if (thetai>(double)1.) {
2333 thetai=(double)1.;
2334 } else if (thetai<(double)-1.) {
2335 thetai=(double)-1.;
2336 }
2337
2338 theta = asin(thetai);
2339 *E = 0;
2340 *N = pRa * tan(theta/(double)2.) * N0;
2341 if (phi<(double)0.0) {
2342 *N *= (double)-1.;
2343 }
2344 } else {
2345 aa = (double).5*fabs((double)GPS_PI/dlam - dlam/(double)GPS_PI);
2346 thetai = fabs(((double)2./(double)GPS_PI) * phi);
2347 if (thetai>(double)1.) {
2348 thetai=(double)1.;
2349 } else if (thetai<(double)-1.) {
2350 thetai=(double)-1.;
2351 }
2352
2353 theta = asin(thetai);
2354 thetas = sin(theta);
2355 thetac = cos(theta);
2356 gg = thetac/(thetas+thetac-(double)1.);
2357 pp = gg*((double)2./thetas-(double)1.);
2358 aa2 = aa*aa;
2359 pp2 = pp*pp;
2360 gm0 = gg-pp2;
2361 ppa = pp2+aa2;
2362 qq = aa2+gg;
2363 *E = pRa*(aa*gm0+pow(aa2*gm0*gm0-ppa*(gg*gg-pp2),(double).5))/ppa+E0;
2364 if (dlam<(double)0.0) {
2365 *E *= (double)-1.;
2366 }
2367 *N = pRa*(pp*qq-aa*pow((aa2+(double)1.)*ppa-qq*qq,(double).5))/ppa+N0;
2368 if (phi<(double)0.0) {
2369 *N *= (double)-1.;
2370 }
2371 }
2372
2373 return;
2374 }
2375
2376
2377
2378
2379 /* @func GPS_Math_VderGrinten_EN_To_LatLon **********************************
2380 **
2381 ** Convert Van der Grinten polyconic easting and northing projection
2382 ** to latitude and longitude
2383 **
2384 ** @param [r] E [double] easting (metre)
2385 ** @param [r] N [double] northing (metre)
2386 ** @param [w] phi [double *] latitude (deg)
2387 ** @param [w] lambda [double *] longitude (deg)
2388 ** @param [r] M0 [double] central meridian (deg)
2389 ** @param [r] E0 [double] false easting
2390 ** @param [r] N0 [double] false northing
2391 ** @param [r] a [double] semi-major axis
2392 ** @param [r] b [double] semi-minor axis
2393 **
2394 ** @return [void]
2395 ************************************************************************/
GPS_Math_VderGrinten_EN_To_LatLon(double E,double N,double * phi,double * lambda,double M0,double E0,double N0,double a,double b)2396 void GPS_Math_VderGrinten_EN_To_LatLon(double E, double N, double* phi,
2397 double* lambda, double M0, double E0,
2398 double N0, double a, double b)
2399 {
2400 double po2;
2401 double p2;
2402 double a2;
2403 double b2;
2404 double e2;
2405 double e4;
2406 double e6;
2407 double Ra;
2408 double pRa;
2409
2410 double dx;
2411 double dy;
2412 double xx;
2413 double xx2;
2414 double yy;
2415 double yy2;
2416 double tyy2;
2417 double xpy;
2418 double c1;
2419 double c2;
2420 double c3;
2421 double c3c3;
2422 double co;
2423 double dd;
2424 double a1;
2425 double m1;
2426 double i;
2427 double theta;
2428
2429 M0 = GPS_Math_Deg_To_Rad(M0);
2430
2431 p2 = (double)GPS_PI * (double)2.0;
2432 po2 = (double)GPS_PI / (double)2.0;
2433 if (M0>GPS_PI) {
2434 M0 -= p2;
2435 }
2436
2437
2438 a2 = a*a;
2439 b2 = b*b;
2440 e2 = (a2-b2)/a2;
2441 e4 = e2*e2;
2442 e6 = e2*e4;
2443
2444 Ra = a*((double)1.-e2/(double)6.-(double)17.*e4/(double)360.-(double)67.*
2445 e6/(double)3024.);
2446 pRa = (double)GPS_PI * Ra;
2447
2448
2449 dx = E - E0;
2450 dy = N - N0;
2451 xx = dx/pRa;
2452 yy = dy/pRa;
2453 xx2 = xx*xx;
2454 yy2 = yy*yy;
2455 xpy = xx2+yy2;
2456 tyy2 = yy2*(double)2.;
2457
2458 if (!N) {
2459 *phi=(double)0.0;
2460 } else {
2461 c1 = -fabs(yy)*((double)1.+xpy);
2462 c2 = c1-tyy2+xx2;
2463 c3 = (double)-2.*c1+(double)1.+tyy2+xpy*xpy;
2464 co = c2/((double)3.*c3);
2465 c3c3 = c3*c3;
2466 dd = yy2/c3+(((double)2.*c2*c2*c2)/(c3c3*c3)-((double)9.*c1*c2)/
2467 c3c3)/(double)27.;
2468 a1 = (c1-c2*co)/c3;
2469 m1 = (double)2.* pow(-((double)1./(double)3.)*a1,(double).5);
2470 i = (double)3.*dd/(a1*m1);
2471 if ((i>(double)1.)||(i<(double)-1.)) {
2472 *phi=po2;
2473 } else {
2474 theta = ((double)1./(double)3.)*acos((double)3.*dd/(a1*m1));
2475 *phi = (double)GPS_PI*(-m1*cos(theta+(double)GPS_PI/(double)3.)-
2476 co);
2477 }
2478 }
2479
2480 if (N<(double)0.0) {
2481 *phi *= (double)-1.0;
2482 }
2483
2484 if (!xx) {
2485 *lambda = M0;
2486 } else
2487 *lambda = (double)GPS_PI * (xpy-(double)1.+
2488 pow((double)1.+((double)2.*xx2-tyy2)+xpy*xpy,(double).5)) /
2489 ((double)2.*xx) + M0;
2490 if (*phi>po2) {
2491 *phi = po2;
2492 } else if (*phi<-po2) {
2493 *phi = -po2;
2494 }
2495
2496 if (*lambda>GPS_PI) {
2497 *lambda -= p2;
2498 }
2499 if (*lambda<-GPS_PI) {
2500 *lambda += p2;
2501 }
2502
2503 if (*lambda>GPS_PI) {
2504 *lambda = GPS_PI;
2505 } else if (*lambda<-GPS_PI) {
2506 *lambda=-GPS_PI;
2507 }
2508
2509 *lambda = GPS_Math_Rad_To_Deg(*lambda);
2510 *phi = GPS_Math_Rad_To_Deg(*phi);
2511
2512 return;
2513 }
2514
2515
2516
2517
2518 /* @func GPS_Math_Bonne_LatLon_To_EN **********************************
2519 **
2520 ** Convert latitude and longitude to Bonne pseudoconic equal area projection
2521 ** easting and northing
2522 **
2523 ** @param [r] phi [double] latitude (deg)
2524 ** @param [r] lambda [double] longitude (deg)
2525 ** @param [w] E [double *] easting (metre)
2526 ** @param [w] N [double *] northing (metre)
2527 ** @param [r] phi1 [double] latitude of true scale (deg)
2528 ** @param [r] lambda1 [double] longitude from pole (deg)
2529 ** @param [r] E0 [double] false easting
2530 ** @param [r] N0 [double] false northing
2531 ** @param [r] a [double] semi-major axis
2532 ** @param [r] b [double] semi-minor axis
2533 **
2534 ** @return [void]
2535 ************************************************************************/
GPS_Math_PolarSt_LatLon_To_EN(double phi,double lambda,double * E,double * N,double phi1,double lambda1,double E0,double N0,double a,double b)2536 void GPS_Math_PolarSt_LatLon_To_EN(double phi, double lambda, double* E,
2537 double* N, double phi1, double lambda1,
2538 double E0, double N0, double a, double b)
2539 {
2540 double p2;
2541 double po2;
2542 double a2;
2543 double b2;
2544 double e2;
2545 double e4=(double)0.;
2546 double e;
2547 double eo2;
2548 double sh;
2549 double mc;
2550 double tc=(double)0.;
2551 double amc=(double)0.;
2552 double ta;
2553 double phi1s;
2554 double phi1c;
2555 double es;
2556 double op;
2557 double om;
2558 double pe;
2559 double polat;
2560 double polon;
2561
2562 double dlam;
2563 double phis;
2564 double t;
2565 double rho;
2566
2567
2568 lambda1 = GPS_Math_Deg_To_Rad(lambda1);
2569 phi1 = GPS_Math_Deg_To_Rad(phi1);
2570 lambda = GPS_Math_Deg_To_Rad(lambda);
2571 phi = GPS_Math_Deg_To_Rad(phi);
2572
2573
2574 p2 = (double)GPS_PI * (double)2.0;
2575 po2 = (double)GPS_PI / (double)2.0;
2576
2577 ta = a * (double)2.0;
2578 if (lambda1>GPS_PI) {
2579 lambda1 -= p2;
2580 }
2581 if (phi1<(double)0.0) {
2582 sh=(double)1.0;
2583 polat = -phi1;
2584 polon = -lambda1;
2585 } else {
2586 sh=(double)0.0;
2587 polat = phi1;
2588 polon = lambda1;
2589 }
2590
2591 a2 = a*a;
2592 b2 = b*b;
2593 e2 = (a2-b2)/a2;
2594 e = pow(e2,(double).5);
2595 eo2 = e/(double)2.;
2596
2597 if (fabs(fabs(polat)-po2)>(double)1.0e-10) {
2598 phi1s = sin(polat);
2599 phi1c = cos(polat);
2600 es = e*phi1s;
2601 pe = pow(((double)1.-es)/((double)1.+es),eo2);
2602 mc = phi1c / pow((double)1.-es*es,(double).5);
2603 amc = mc * a;
2604 tc = tan(((double)GPS_PI/(double)4.)-polat/(double)2.) / pe;
2605 } else {
2606 op = (double)1. + e;
2607 om = (double)1. - e;
2608 e4 = pow(pow(op,op)*pow(om,om),(double).5);
2609 }
2610
2611
2612
2613 if (fabs(fabs(phi)-po2)<(double)1.0e-10) {
2614 *E = *N = (double)0.0;
2615 } else {
2616 if (sh) {
2617 phi *= (double)-1.0;
2618 lambda *= (double)-1.0;
2619 }
2620
2621 dlam = lambda - polon;
2622 if (dlam>GPS_PI) {
2623 dlam -= p2;
2624 }
2625 if (dlam<-GPS_PI) {
2626 dlam += p2;
2627 }
2628 phis = sin(phi);
2629 es = e * phis;
2630 pe = pow(((double)1.-es)/((double)1.+es),eo2);
2631 t = tan(((double)GPS_PI/(double)4.)-phi/(double)2.) / pe;
2632
2633 if (fabs(fabs(polat)-po2)>(double)1.0e-10) {
2634 rho = amc * t / tc;
2635 } else {
2636 rho = ta * t / e4;
2637 }
2638 *E = rho * sin(dlam) + E0;
2639
2640 if (sh) {
2641 *E *= (double)-1.;
2642 *N = rho * cos(dlam) + N0;
2643 } else {
2644 *N = -rho * cos(dlam) + N0;
2645 }
2646 }
2647
2648 return;
2649 }
2650
2651
2652
2653
2654 /* @func GPS_Math_PolarSt_EN_To_LatLon **********************************
2655 **
2656 ** Convert Polar Stereographic easting and northing projection
2657 ** to latitude and longitude
2658 **
2659 ** @param [r] E [double] easting (metre)
2660 ** @param [r] N [double] northing (metre)
2661 ** @param [w] phi [double *] latitude (deg)
2662 ** @param [w] lambda [double *] longitude (deg)
2663 ** @param [r] phi1 [double] latitude of true scale (deg)
2664 ** @param [r] lambda1 [double] longitude from pole (deg)
2665 ** @param [r] E0 [double] false easting
2666 ** @param [r] N0 [double] false northing
2667 ** @param [r] a [double] semi-major axis
2668 ** @param [r] b [double] semi-minor axis
2669 **
2670 ** @return [void]
2671 ************************************************************************/
GPS_Math_PolarSt_EN_To_LatLon(double E,double N,double * phi,double * lambda,double phi1,double lambda1,double E0,double N0,double a,double b)2672 void GPS_Math_PolarSt_EN_To_LatLon(double E, double N, double* phi,
2673 double* lambda, double phi1, double lambda1,
2674 double E0, double N0, double a, double b)
2675 {
2676 double p2;
2677 double po2;
2678 double a2;
2679 double b2;
2680 double e2;
2681 double e4=(double)0.;
2682 double e;
2683 double eo2;
2684 double sh;
2685 double mc;
2686 double tc=(double)0.;
2687 double amc=(double)0.;
2688 double ta;
2689 double phi1s;
2690 double phi1c;
2691 double es;
2692 double op;
2693 double om;
2694 double pe;
2695 double polat;
2696 double polon;
2697
2698 double dx;
2699 double dy;
2700 double t;
2701 double rho;
2702 double PHI;
2703 double PHIS;
2704 double TPHI;
2705
2706
2707 lambda1 = GPS_Math_Deg_To_Rad(lambda1);
2708 phi1 = GPS_Math_Deg_To_Rad(phi1);
2709
2710
2711 p2 = (double)GPS_PI * (double)2.0;
2712 po2 = (double)GPS_PI / (double)2.0;
2713
2714 ta = a * (double)2.0;
2715 if (lambda1>GPS_PI) {
2716 lambda1 -= p2;
2717 }
2718 if (phi1<(double)0.0) {
2719 sh=(double)1.0;
2720 polat = -phi1;
2721 polon = -lambda1;
2722 } else {
2723 sh=(double)0.0;
2724 polat = phi1;
2725 polon = lambda1;
2726 }
2727
2728 a2 = a*a;
2729 b2 = b*b;
2730 e2 = (a2-b2)/a2;
2731 e = pow(e2,(double).5);
2732 eo2 = e/(double)2.;
2733
2734 if (fabs(fabs(polat)-po2)>(double)1.0e-10) {
2735 phi1s = sin(polat);
2736 phi1c = cos(polat);
2737 es = e*phi1s;
2738 pe = pow(((double)1.-es)/((double)1.+es),eo2);
2739 mc = phi1c / pow((double)1.-es*es,(double).5);
2740 amc = mc * a;
2741 tc = tan(((double)GPS_PI/(double)4.)-polat/(double)2.) / pe;
2742 } else {
2743 op = (double)1. + e;
2744 om = (double)1. - e;
2745 e4 = pow(pow(op,op)*pow(om,om),(double).5);
2746 }
2747
2748
2749 dx = E - E0;
2750 dy = N - N0;
2751 if (!dx && !dy) {
2752 *phi = po2;
2753 *lambda = polon;
2754 } else {
2755 if (sh) {
2756 dx *= (double)-1.;
2757 dy *= (double)-1.;
2758 }
2759 rho = pow(dx*dx+dy*dy,(double).5);
2760 if (fabs(fabs(polat)-po2)>(double)1.0e-10) {
2761 t = rho * tc / amc;
2762 } else {
2763 t = rho * e4 / ta;
2764 }
2765 TPHI = (double)0.0;
2766 PHI = po2 - (double)2.*atan(t);
2767 while (fabs(PHI-TPHI)>(double)1.0e-10) {
2768 TPHI=PHI;
2769 PHIS = sin(PHI);
2770 es = e * PHIS;
2771 pe = pow(((double)1.-es)/((double)1.+es),eo2);
2772 PHI = po2 - (double)2. * atan(t*pe);
2773 }
2774 *phi = PHI;
2775 *lambda = polon + atan2(dx,-dy);
2776
2777 if (*phi>po2) {
2778 *phi = po2;
2779 } else if (*phi<-po2) {
2780 *phi = -po2;
2781 }
2782
2783 if (*lambda>GPS_PI) {
2784 *lambda -= p2;
2785 }
2786 if (*lambda<-GPS_PI) {
2787 *lambda += p2;
2788 }
2789
2790 if (*lambda>GPS_PI) {
2791 *lambda = GPS_PI;
2792 } else if (*lambda<-GPS_PI) {
2793 *lambda=-GPS_PI;
2794 }
2795 }
2796 if (sh) {
2797 *phi *= (double)-1.;
2798 *lambda *= (double)1.;
2799 }
2800
2801 *lambda = GPS_Math_Rad_To_Deg(*lambda);
2802 *phi = GPS_Math_Rad_To_Deg(*phi);
2803
2804 return;
2805 }
2806
2807
2808
2809
2810 /* @func GPS_Math_Mollweide_LatLon_To_EN **********************************
2811 **
2812 ** Convert latitude and longitude to Mollweide projection easting and
2813 ** northing
2814 **
2815 ** @param [r] phi [double] latitude (deg)
2816 ** @param [r] lambda [double] longitude (deg)
2817 ** @param [w] E [double *] easting (metre)
2818 ** @param [w] N [double *] northing (metre)
2819 ** @param [r] M0 [double] central meridian (deg)
2820 ** @param [r] E0 [double] false easting
2821 ** @param [r] N0 [double] false northing
2822 ** @param [r] a [double] semi-major axis
2823 ** @param [r] b [double] semi-minor axis
2824 **
2825 ** @return [void]
2826 ************************************************************************/
GPS_Math_Mollweide_LatLon_To_EN(double phi,double lambda,double * E,double * N,double M0,double E0,double N0,double a,double b)2827 void GPS_Math_Mollweide_LatLon_To_EN(double phi, double lambda, double* E,
2828 double* N, double M0, double E0,
2829 double N0, double a, double b)
2830 {
2831 double a2;
2832 double b2;
2833 double e2;
2834 double e4;
2835 double e6;
2836 double p2;
2837 double po2;
2838 double Ra;
2839 double sRa2;
2840 double sRa8;
2841
2842 double ps;
2843 double dlam;
2844 double theta;
2845 double thetap;
2846 double d;
2847 double tol;
2848
2849 phi = GPS_Math_Deg_To_Rad(phi);
2850 lambda = GPS_Math_Deg_To_Rad(lambda);
2851 M0 = GPS_Math_Deg_To_Rad(M0);
2852
2853 po2 = (double)GPS_PI / (double)2.0;
2854 p2 = (double)GPS_PI * (double)2.0;
2855 a2 = a*a;
2856 b2 = b*b;
2857 e2 = (a2-b2)/a2;
2858 e4 = e2*e2;
2859 e6 = e4*e2;
2860
2861 Ra = a*((double)1.0-e2/(double)6.0-(double)17.0*e4/(double)360.0-
2862 (double)67.0*e6/(double)3024.0);
2863 sRa2 = pow((double)2.,(double).5) * Ra;
2864 sRa8 = pow((double)8.,(double).5) * Ra;
2865
2866 if (M0>GPS_PI) {
2867 M0 -= p2;
2868 }
2869
2870 ps = sin(phi) * (double)GPS_PI;
2871 d = (double)0.1745329;
2872 tol = (double)4.85e-10;
2873 thetap = phi;
2874
2875 dlam = lambda - M0;
2876 if (dlam>GPS_PI) {
2877 dlam-=p2;
2878 }
2879 if (dlam<-GPS_PI) {
2880 dlam+=p2;
2881 }
2882
2883 while (fabs(d)>tol) {
2884 d = -(thetap+sin(thetap)-ps)/((double)1.+cos(thetap));
2885 thetap += d;
2886 }
2887 theta = thetap / (double)2.;
2888 *E = (sRa8/(double)GPS_PI) * dlam * cos(theta) + E0;
2889 *N = sRa2 * sin(theta) + N0;
2890
2891 return;
2892 }
2893
2894
2895
2896
2897 /* @func GPS_Math_Mollweide_EN_To_LatLon **********************************
2898 **
2899 ** Convert latitude and longitude to Mollweide projection easting and
2900 ** northing
2901 **
2902 ** @param [r] E [double] easting (metre)
2903 ** @param [r] N [double] northing (metre)
2904 ** @param [w] phi [double *] latitude (deg)
2905 ** @param [w] lambda [double *] longitude (deg)
2906 ** @param [r] M0 [double] central meridian (deg)
2907 ** @param [r] E0 [double] false easting
2908 ** @param [r] N0 [double] false northing
2909 ** @param [r] a [double] semi-major axis
2910 ** @param [r] b [double] semi-minor axis
2911 **
2912 ** @return [void]
2913 ************************************************************************/
GPS_Math_Mollweide_EN_To_LatLon(double E,double N,double * phi,double * lambda,double M0,double E0,double N0,double a,double b)2914 void GPS_Math_Mollweide_EN_To_LatLon(double E, double N, double* phi,
2915 double* lambda, double M0, double E0,
2916 double N0, double a, double b)
2917 {
2918 double a2;
2919 double b2;
2920 double e2;
2921 double e4;
2922 double e6;
2923 double p2;
2924 double po2;
2925 double Ra;
2926 double sRa2;
2927 double sRa8;
2928
2929 double dx;
2930 double dy;
2931 double theta=(double)0.;
2932 double tt;
2933 double i;
2934
2935 M0 = GPS_Math_Deg_To_Rad(M0);
2936
2937 po2 = (double)GPS_PI / (double)2.0;
2938 p2 = (double)GPS_PI * (double)2.0;
2939 a2 = a*a;
2940 b2 = b*b;
2941 e2 = (a2-b2)/a2;
2942 e4 = e2*e2;
2943 e6 = e4*e2;
2944
2945 Ra = a*((double)1.0-e2/(double)6.0-(double)17.0*e4/(double)360.0-
2946 (double)67.0*e6/(double)3024.0);
2947 sRa2 = pow((double)2.,(double).5) * Ra;
2948 sRa8 = pow((double)8.,(double).5) * Ra;
2949
2950 if (M0>GPS_PI) {
2951 M0 -= p2;
2952 }
2953
2954 dx = E - E0;
2955 dy = N - N0;
2956 i = dy/sRa2;
2957 if (fabs(i)>(double)1.) {
2958 *phi = po2;
2959 if (N<(double)0.0) {
2960 *phi *= (double)-1.;
2961 }
2962 } else {
2963 theta = asin(i);
2964 tt = theta * (double)2.;
2965 *phi = asin((tt+sin(tt))/(double)GPS_PI);
2966 if (*phi>po2) {
2967 *phi=po2;
2968 } else if (*phi<-po2) {
2969 *phi=-po2;
2970 }
2971 }
2972
2973 if (fabs(fabs(*phi)-po2)<(double)1.0e-10) {
2974 *lambda = M0;
2975 } else {
2976 *lambda = M0 + (double)GPS_PI * dx / (sRa8 * cos(theta));
2977 }
2978
2979
2980 if (*lambda>GPS_PI) {
2981 *lambda-=p2;
2982 }
2983 if (*lambda<-GPS_PI) {
2984 *lambda+=p2;
2985 }
2986
2987 if (*lambda>GPS_PI) {
2988 *lambda=GPS_PI;
2989 } else if (*lambda<-GPS_PI) {
2990 *lambda=-GPS_PI;
2991 }
2992
2993 *lambda = GPS_Math_Rad_To_Deg(*lambda);
2994 *phi = GPS_Math_Rad_To_Deg(*phi);
2995
2996 return;
2997 }
2998
2999
3000
3001
3002 /* @func GPS_Math_Orthog_LatLon_To_EN **********************************
3003 **
3004 ** Convert latitude and longitude to orthographic projection
3005 ** easting and northing
3006 **
3007 ** @param [r] phi [double] latitude (deg)
3008 ** @param [r] lambda [double] longitude (deg)
3009 ** @param [w] E [double *] easting (metre)
3010 ** @param [w] N [double *] northing (metre)
3011 ** @param [r] phi0 [double] latitude of origin (deg)
3012 ** @param [r] lambda0 [double] longitude of origin (deg)
3013 ** @param [r] E0 [double] false easting
3014 ** @param [r] N0 [double] false northing
3015 ** @param [r] a [double] semi-major axis
3016 ** @param [r] b [double] semi-minor axis
3017 **
3018 ** @return [void]
3019 ************************************************************************/
GPS_Math_Orthog_LatLon_To_EN(double phi,double lambda,double * E,double * N,double phi0,double lambda0,double E0,double N0,double a,double b)3020 void GPS_Math_Orthog_LatLon_To_EN(double phi, double lambda, double* E,
3021 double* N, double phi0, double lambda0,
3022 double E0, double N0, double a, double b)
3023 {
3024 double p2;
3025 double po2;
3026 double a2;
3027 double b2;
3028 double e2;
3029 double e4;
3030 double e6;
3031 double Ra;
3032 double phi0s;
3033 double phi0c;
3034
3035 double phis;
3036 double phic;
3037 double dlam;
3038 double clc;
3039 double cc;
3040
3041
3042 lambda = GPS_Math_Deg_To_Rad(lambda);
3043 phi = GPS_Math_Deg_To_Rad(phi);
3044 phi0 = GPS_Math_Deg_To_Rad(phi0);
3045 lambda0 = GPS_Math_Deg_To_Rad(lambda0);
3046
3047 p2 = (double)GPS_PI * (double)2.0;
3048 po2 = (double)GPS_PI / (double)2.0;
3049 if (lambda0>GPS_PI) {
3050 lambda0 -= p2;
3051 }
3052 a2 = a*a;
3053 b2 = b*b;
3054 e2 = (a2-b2)/a2;
3055 e4 = e2*e2;
3056 e6 = e2*e4;
3057 Ra = a * ((double)1.-e2/(double)6.-(double)17.*e4/(double)360.-(double)67.*
3058 e6/(double)3024.);
3059 phi0s = sin(phi0);
3060 phi0c = cos(phi0);
3061
3062 dlam = lambda - lambda0;
3063 if (dlam>GPS_PI) {
3064 dlam -= p2;
3065 }
3066 if (dlam<-GPS_PI) {
3067 dlam += p2;
3068 }
3069
3070
3071 phis = sin(phi);
3072 phic = cos(phi);
3073 clc = phic * cos(dlam);
3074 cc = phi0s * phis + phi0c * clc;
3075
3076 *E = Ra * phic * sin(dlam) + E0;
3077 *N = Ra * (phi0c * phis - phi0s * clc) + N0;
3078
3079 return;
3080 }
3081
3082
3083
3084
3085 /* @func GPS_Math_Orthog_EN_To_LatLon **********************************
3086 **
3087 ** Convert Orthogonal easting and northing projection
3088 ** to latitude and longitude
3089 **
3090 ** @param [r] E [double] easting (metre)
3091 ** @param [r] N [double] northing (metre)
3092 ** @param [w] phi [double *] latitude (deg)
3093 ** @param [w] lambda [double *] longitude (deg)
3094 ** @param [r] phi0 [double] latitude of origin (deg)
3095 ** @param [r] lambda0 [double] longitude of origin (deg)
3096 ** @param [r] E0 [double] false easting
3097 ** @param [r] N0 [double] false northing
3098 ** @param [r] a [double] semi-major axis
3099 ** @param [r] b [double] semi-minor axis
3100 **
3101 ** @return [void]
3102 ************************************************************************/
GPS_Math_Orthog_EN_To_LatLon(double E,double N,double * phi,double * lambda,double phi0,double lambda0,double E0,double N0,double a,double b)3103 void GPS_Math_Orthog_EN_To_LatLon(double E, double N, double* phi,
3104 double* lambda, double phi0, double lambda0,
3105 double E0, double N0, double a, double b)
3106 {
3107 double p2;
3108 double po2;
3109 double a2;
3110 double b2;
3111 double e2;
3112 double e4;
3113 double e6;
3114 double Ra;
3115 double phi0s;
3116 double phi0c;
3117
3118 double dx;
3119 double dy;
3120 double rho;
3121 double adod;
3122 double ror;
3123 double cc;
3124 double ccs;
3125 double ccc;
3126
3127
3128
3129
3130 phi0 = GPS_Math_Deg_To_Rad(phi0);
3131 lambda0 = GPS_Math_Deg_To_Rad(lambda0);
3132
3133 p2 = (double)GPS_PI * (double)2.0;
3134 po2 = (double)GPS_PI / (double)2.0;
3135 if (lambda0>GPS_PI) {
3136 lambda0 -= p2;
3137 }
3138 a2 = a*a;
3139 b2 = b*b;
3140 e2 = (a2-b2)/a2;
3141 e4 = e2*e2;
3142 e6 = e2*e4;
3143 Ra = a * ((double)1.-e2/(double)6.-(double)17.*e4/(double)360.-(double)67.*
3144 e6/(double)3024.);
3145 phi0s = sin(phi0);
3146 phi0c = cos(phi0);
3147
3148
3149 dx = E - E0;
3150 dy = N - N0;
3151 adod = atan(dx/dy);
3152 rho = pow(dx*dx+dy*dy,(double).5);
3153 if (!rho) {
3154 *phi = phi0;
3155 *lambda = lambda0;
3156 } else {
3157 ror = rho/Ra;
3158 if (ror>(double)1.) {
3159 ror=(double)1.;
3160 } else if (ror<(double)-1.) {
3161 ror=(double)-1.;
3162 }
3163 cc = asin(ror);
3164 ccs = sin(cc);
3165 ccc = cos(cc);
3166 *phi = asin(ccc*phi0s+(dy*ccs*phi0c/rho));
3167 if (phi0==po2) {
3168 *lambda = lambda0 - adod;
3169 } else if (phi0==-po2) {
3170 *lambda = lambda0 + adod;
3171 } else {
3172 *lambda = lambda0+atan(dx*ccs/(rho*phi0c*ccc-dy*phi0s*ccs));
3173 }
3174 }
3175
3176 if (*phi>po2) {
3177 *phi = po2;
3178 } else if (*phi<-po2) {
3179 *phi = -po2;
3180 }
3181
3182 if (*lambda>GPS_PI) {
3183 *lambda -= p2;
3184 }
3185 if (*lambda<-GPS_PI) {
3186 *lambda += p2;
3187 }
3188
3189 if (*lambda>GPS_PI) {
3190 *lambda = GPS_PI;
3191 } else if (*lambda<-GPS_PI) {
3192 *lambda=-GPS_PI;
3193 }
3194
3195 *lambda = GPS_Math_Rad_To_Deg(*lambda);
3196 *phi = GPS_Math_Rad_To_Deg(*phi);
3197
3198 return;
3199 }
3200
3201
3202
3203
3204 /* @func GPS_Math_Polycon_LatLon_To_EN **********************************
3205 **
3206 ** Convert latitude and longitude to Polyconic projection
3207 ** easting and northing
3208 **
3209 ** @param [r] phi [double] latitude (deg)
3210 ** @param [r] lambda [double] longitude (deg)
3211 ** @param [w] E [double *] easting (metre)
3212 ** @param [w] N [double *] northing (metre)
3213 ** @param [r] phi0 [double] latitude of origin (deg)
3214 ** @param [r] M0 [double] central meridian (deg)
3215 ** @param [r] E0 [double] false easting
3216 ** @param [r] N0 [double] false northing
3217 ** @param [r] a [double] semi-major axis
3218 ** @param [r] b [double] semi-minor axis
3219 **
3220 ** @return [void]
3221 ************************************************************************/
GPS_Math_Polycon_LatLon_To_EN(double phi,double lambda,double * E,double * N,double phi0,double M0,double E0,double N0,double a,double b)3222 void GPS_Math_Polycon_LatLon_To_EN(double phi, double lambda, double* E,
3223 double* N, double phi0, double M0,
3224 double E0, double N0, double a, double b)
3225 {
3226 double p2;
3227 double po2;
3228 double a2;
3229 double b2;
3230 double e2;
3231 double e4;
3232 double e6;
3233 double AM0;
3234 double c0;
3235 double c1;
3236 double c2;
3237 double c3;
3238 double j;
3239 double te4;
3240 double phi0s2;
3241 double phi0s4;
3242 double phi0s6;
3243
3244 double phis;
3245 double phis2;
3246 double phis4;
3247 double phis6;
3248 double dlam;
3249 double NN;
3250 double NNot;
3251 double MM;
3252 double EE;
3253 double lat;
3254
3255
3256 lambda = GPS_Math_Deg_To_Rad(lambda);
3257 phi = GPS_Math_Deg_To_Rad(phi);
3258 phi0 = GPS_Math_Deg_To_Rad(phi0);
3259 M0 = GPS_Math_Deg_To_Rad(M0);
3260
3261 p2 = (double)GPS_PI * (double)2.0;
3262 po2 = (double)GPS_PI / (double)2.0;
3263 if (M0>GPS_PI) {
3264 M0 -= p2;
3265 }
3266 a2 = a*a;
3267 b2 = b*b;
3268 e2 = (a2-b2)/a2;
3269 e4 = e2*e2;
3270 e6 = e2*e4;
3271
3272 j = (double)45.0*e6/(double)1024.0;
3273 te4 = (double)3.0 * e4;
3274 c0 = (double)1.0-e2/(double)4.0-te4/(double)64.0-(double)5.0*e6/
3275 (double)256.0;
3276 c1 = (double)3.0*e2/(double)8.0+te4/(double)32.0+j;
3277 c2 = (double)15.0*e4/(double)256.0+j;
3278 c3 = (double)35.0*e6/(double)3072.0;
3279
3280 lat = c0 * phi0;
3281
3282 phi0s2 = c1 * sin((double)2.0*phi0);
3283 phi0s4 = c2 * sin((double)4.0*phi0);
3284 phi0s6 = c3 * sin((double)6.0*phi0);
3285 AM0 = a*(lat-phi0s2+phi0s4-phi0s6);
3286
3287
3288
3289 dlam = lambda - M0;
3290 if (dlam>GPS_PI) {
3291 dlam -= p2;
3292 }
3293 if (dlam<-GPS_PI) {
3294 dlam += p2;
3295 }
3296
3297 phis = sin(phi);
3298
3299 if (!phi) {
3300 *E = a * dlam + E0;
3301 *N = -AM0 + N0;
3302 } else {
3303 NN = a / pow((double)1.-e2*phis*phis,(double).5);
3304 NNot = NN / tan(phi);
3305 lat = c0 * phi;
3306 phis2 = c1 * sin((double)2.0*phi);
3307 phis4 = c2 * sin((double)4.0*phi);
3308 phis6 = c3 * sin((double)6.0*phi);
3309 MM = a*(lat-phis2+phis4-phis6);
3310 EE = dlam *phis;
3311 *E = NNot * sin(EE) + E0;
3312 *N = MM - AM0 + NNot * ((double)1.-cos(EE)) + N0;
3313 }
3314
3315 return;
3316 }
3317
3318
3319
3320
3321 /* @func GPS_Math_Polycon_EN_To_LatLon **********************************
3322 **
3323 ** Convert Polyconic easting and northing projection
3324 ** to latitude and longitude
3325 **
3326 ** @param [r] E [double] easting (metre)
3327 ** @param [r] N [double] northing (metre)
3328 ** @param [w] phi [double *] latitude (deg)
3329 ** @param [w] lambda [double *] longitude (deg)
3330 ** @param [r] phi0 [double] latitude of origin (deg)
3331 ** @param [r] M0 [double] central meridian (deg)
3332 ** @param [r] E0 [double] false easting
3333 ** @param [r] N0 [double] false northing
3334 ** @param [r] a [double] semi-major axis
3335 ** @param [r] b [double] semi-minor axis
3336 **
3337 ** @return [void]
3338 ************************************************************************/
GPS_Math_Polycon_EN_To_LatLon(double E,double N,double * phi,double * lambda,double phi0,double M0,double E0,double N0,double a,double b)3339 void GPS_Math_Polycon_EN_To_LatLon(double E, double N, double* phi,
3340 double* lambda, double phi0, double M0,
3341 double E0, double N0, double a, double b)
3342 {
3343 double p2;
3344 double po2;
3345 double a2;
3346 double b2;
3347 double e2;
3348 double e4;
3349 double e6;
3350 double AM0;
3351 double c0;
3352 double c1;
3353 double c2;
3354 double c3;
3355 double j;
3356 double te4;
3357 double phi0s2;
3358 double phi0s4;
3359 double phi0s6;
3360
3361 double dx;
3362 double dy;
3363 double dxoa;
3364 double AA;
3365 double BB;
3366 double CC=(double)0.;
3367 double PHIn;
3368 double PHId;
3369 double PHIs;
3370 double PHI;
3371 double PHIs2;
3372 double PHIs4;
3373 double PHIs6;
3374 double Mn;
3375 double Mnp;
3376 double Ma;
3377 double AAMa;
3378 double mpb;
3379 double AAmin;
3380 double tol;
3381 double lat;
3382
3383
3384 phi0 = GPS_Math_Deg_To_Rad(phi0);
3385 M0 = GPS_Math_Deg_To_Rad(M0);
3386
3387 p2 = (double)GPS_PI * (double)2.0;
3388 po2 = (double)GPS_PI / (double)2.0;
3389 if (M0>GPS_PI) {
3390 M0 -= p2;
3391 }
3392 a2 = a*a;
3393 b2 = b*b;
3394 e2 = (a2-b2)/a2;
3395 e4 = e2*e2;
3396 e6 = e2*e4;
3397
3398 j = (double)45.0*e6/(double)1024.0;
3399 te4 = (double)3.0 * e4;
3400 c0 = (double)1.0-e2/(double)4.0-te4/(double)64.0-(double)5.0*e6/
3401 (double)256.0;
3402 c1 = (double)3.0*e2/(double)8.0+te4/(double)32.0+j;
3403 c2 = (double)15.0*e4/(double)256.0+j;
3404 c3 = (double)35.0*e6/(double)3072.0;
3405
3406 lat = c0 * phi0;
3407
3408 phi0s2 = c1 * sin((double)2.0*phi0);
3409 phi0s4 = c2 * sin((double)4.0*phi0);
3410 phi0s6 = c3 * sin((double)6.0*phi0);
3411 AM0 = a*(lat-phi0s2+phi0s4-phi0s6);
3412
3413 tol = (double)1.0e-12;
3414
3415 dx = E - E0;
3416 dy = N - N0;
3417 dxoa = dx/a;
3418 if ((((-AM0-(double)1.)<dy)&&(dy<(-AM0+(double)1.)))) {
3419 *phi = (double)0.;
3420 *lambda = dxoa + M0;
3421 } else {
3422 AA = (AM0+dy) / a;
3423 BB = dxoa * dxoa + (AA*AA);
3424 PHIn = AA;
3425 PHId = (double)1.;
3426
3427 while (fabs(PHId)>tol) {
3428 PHIs = sin(PHIn);
3429 CC = pow((double)1.-e2*PHIs*PHIs,(double).5) * tan(PHIn);
3430 PHI = PHIn * c0;
3431 PHIs2 = c1 * sin((double)2.0*PHIn);
3432 PHIs4 = c2 * sin((double)4.0*PHIn);
3433 PHIs6 = c3 * sin((double)6.0*PHIn);
3434 Mn = a*(PHI-PHIs2+PHIs4-PHIs6);
3435 Mnp = c0 - (double)2.*c1*cos((double)2.*PHIn)+(double)4.*c2*
3436 cos((double)4.*PHIn)-(double)6.*c3*cos((double)6.*PHIn);
3437 Ma = Mn / a;
3438 AAMa = AA * Ma;
3439 mpb = Ma*Ma+BB;
3440 AAmin = AA - Ma;
3441 PHId = (AAMa*CC+AAmin-(double).5*mpb*CC)/
3442 (e2*PHIs2*(mpb-(double)2.*AAMa) /
3443 (double)4.*CC+AAmin*(CC*Mnp-(double)2./PHIs2)-Mnp);
3444 PHIn -= PHId;
3445 }
3446 *phi = PHIn;
3447
3448 if (*phi>po2) {
3449 *phi = po2;
3450 } else if (*phi<-po2) {
3451 *phi = -po2;
3452 }
3453
3454 if ((((po2-(double).00001)<fabs(*phi))&&
3455 (fabs(*phi)<(po2+(double).00001)))) {
3456 *lambda = M0;
3457 } else {
3458 *lambda = (asin(dxoa*CC)) / sin(*phi) + M0;
3459 }
3460 }
3461
3462 if (*lambda>GPS_PI) {
3463 *lambda -= p2;
3464 }
3465 if (*lambda<-GPS_PI) {
3466 *lambda += p2;
3467 }
3468
3469 if (*lambda>GPS_PI) {
3470 *lambda = GPS_PI;
3471 } else if (*lambda<-GPS_PI) {
3472 *lambda=-GPS_PI;
3473 }
3474
3475 *lambda = GPS_Math_Rad_To_Deg(*lambda);
3476 *phi = GPS_Math_Rad_To_Deg(*phi);
3477
3478 return;
3479 }
3480
3481
3482
3483
3484 /* @func GPS_Math_Sinusoid_LatLon_To_EN **********************************
3485 **
3486 ** Convert latitude and longitude to Sinusoidal projection easting and
3487 ** northing
3488 **
3489 ** @param [r] phi [double] latitude (deg)
3490 ** @param [r] lambda [double] longitude (deg)
3491 ** @param [w] E [double *] easting (metre)
3492 ** @param [w] N [double *] northing (metre)
3493 ** @param [r] M0 [double] central meridian (deg)
3494 ** @param [r] E0 [double] false easting
3495 ** @param [r] N0 [double] false northing
3496 ** @param [r] a [double] semi-major axis
3497 ** @param [r] b [double] semi-minor axis
3498 **
3499 ** @return [void]
3500 ************************************************************************/
GPS_Math_Sinusoid_LatLon_To_EN(double phi,double lambda,double * E,double * N,double M0,double E0,double N0,double a,double b)3501 void GPS_Math_Sinusoid_LatLon_To_EN(double phi, double lambda, double* E,
3502 double* N, double M0, double E0,
3503 double N0, double a, double b)
3504 {
3505 double a2;
3506 double b2;
3507 double e2;
3508 double e4;
3509 double e6;
3510 double p2;
3511 double po2;
3512 double c0;
3513 double c1;
3514 double c2;
3515 double c3;
3516 double A1;
3517 double A0;
3518 double A2;
3519 double A3;
3520 double E1;
3521 double E2;
3522 double E3;
3523 double E4;
3524 double j;
3525 double om0;
3526 double som0;
3527
3528 double phis;
3529 double phis2;
3530 double phis4;
3531 double phis6;
3532 double mm;
3533 double MM;
3534 double dlam;
3535
3536
3537 phi = GPS_Math_Deg_To_Rad(phi);
3538 lambda = GPS_Math_Deg_To_Rad(lambda);
3539 M0 = GPS_Math_Deg_To_Rad(M0);
3540
3541 po2 = (double)GPS_PI / (double)2.0;
3542 p2 = (double)GPS_PI * (double)2.0;
3543 a2 = a*a;
3544 b2 = b*b;
3545 e2 = (a2-b2)/a2;
3546 e4 = e2*e2;
3547 e6 = e4*e2;
3548
3549 j = (double)45.*e6/(double)1024.;
3550 c0 = (double)1.-e2/(double)4.-(double)3.*e4/(double)64.-(double)5.*
3551 e6/(double)256.;
3552 c1 = (double)3.*e2/(double)8.+(double)3.*e4/(double)32.+j;
3553 c2 = (double)15.*e4/(double)256.+j;
3554 c3 = (double)35.*e6/(double)3072.;
3555 om0 = (double)1. - e2;
3556 som0 = pow(om0,(double).5);
3557 E1 = ((double)1.-som0)/((double)1.+som0);
3558 E2 = E1*E1;
3559 E3 = E1*E2;
3560 E4 = E1*E3;
3561 A0 = (double)3.*E1/(double)2.-(double)27.*E3/(double)32.;
3562 A1 = (double)21.*E2/(double)16.-(double)55.*E4/(double)32.;
3563 A2 = (double)151.*E3/(double)96.;
3564 A3 = (double)1097.*E4/(double)512.;
3565
3566 if (M0>GPS_PI) {
3567 M0 -= p2;
3568 }
3569
3570 phis = sin(phi);
3571
3572 dlam = lambda - M0;
3573 if (dlam>GPS_PI) {
3574 dlam-=p2;
3575 }
3576 if (dlam<-GPS_PI) {
3577 dlam+=p2;
3578 }
3579
3580 mm = pow((double)1.-e2*phis*phis,(double).5);
3581 phis2 = c1 * sin((double)2.*phi);
3582 phis4 = c2 * sin((double)4.*phi);
3583 phis6 = c3 * sin((double)6.*phi);
3584 MM = a * (c0*phi-phis2+phis4-phis6);
3585
3586
3587
3588 *E = a*dlam*cos(phi)/mm+E0;
3589 *N = MM + N0;
3590
3591 return;
3592 }
3593
3594
3595
3596
3597 /* @func GPS_Math_Sinusoid_EN_To_LatLon **********************************
3598 **
3599 ** Convert latitude and longitude to Sinusoidal projection easting and
3600 ** northing
3601 **
3602 ** @param [r] E [double] easting (metre)
3603 ** @param [r] N [double] northing (metre)
3604 ** @param [w] phi [double *] latitude (deg)
3605 ** @param [w] lambda [double *] longitude (deg)
3606 ** @param [r] M0 [double] central meridian (deg)
3607 ** @param [r] E0 [double] false easting
3608 ** @param [r] N0 [double] false northing
3609 ** @param [r] a [double] semi-major axis
3610 ** @param [r] b [double] semi-minor axis
3611 **
3612 ** @return [void]
3613 ************************************************************************/
GPS_Math_Sinusoid_EN_To_LatLon(double E,double N,double * phi,double * lambda,double M0,double E0,double N0,double a,double b)3614 void GPS_Math_Sinusoid_EN_To_LatLon(double E, double N, double* phi,
3615 double* lambda, double M0, double E0,
3616 double N0, double a, double b)
3617 {
3618 double a2;
3619 double b2;
3620 double e2;
3621 double e4;
3622 double e6;
3623 double p2;
3624 double po2;
3625 double c0;
3626 double c1;
3627 double c2;
3628 double c3;
3629 double A1;
3630 double A0;
3631 double A2;
3632 double A3;
3633 double E1;
3634 double E2;
3635 double E3;
3636 double E4;
3637 double j;
3638 double om0;
3639 double som0;
3640
3641 double dx;
3642 double dy;
3643 double mu;
3644 double mu2s;
3645 double mu4s;
3646 double mu6s;
3647 double mu8s;
3648 double phis;
3649
3650
3651 M0 = GPS_Math_Deg_To_Rad(M0);
3652
3653 po2 = (double)GPS_PI / (double)2.0;
3654 p2 = (double)GPS_PI * (double)2.0;
3655 a2 = a*a;
3656 b2 = b*b;
3657 e2 = (a2-b2)/a2;
3658 e4 = e2*e2;
3659 e6 = e4*e2;
3660
3661 j = (double)45.*e6/(double)1024.;
3662 c0 = (double)1.-e2/(double)4.-(double)3.*e4/(double)64.-(double)5.*
3663 e6/(double)256.;
3664 c1 = (double)3.*e2/(double)8.+(double)3.*e4/(double)32.+j;
3665 c2 = (double)15.*e4/(double)256.+j;
3666 c3 = (double)35.*e6/(double)3072.;
3667 om0 = (double)1. - e2;
3668 som0 = pow(om0,(double).5);
3669 E1 = ((double)1.-som0)/((double)1.+som0);
3670 E2 = E1*E1;
3671 E3 = E1*E2;
3672 E4 = E1*E3;
3673 A0 = (double)3.*E1/(double)2.-(double)27.*E3/(double)32.;
3674 A1 = (double)21.*E2/(double)16.-(double)55.*E4/(double)32.;
3675 A2 = (double)151.*E3/(double)96.;
3676 A3 = (double)1097.*E4/(double)512.;
3677
3678
3679 dx = E - E0;
3680 dy = N - N0;
3681
3682 mu = dy/(c0*a);
3683 mu2s = A0 * sin((double)2.*mu);
3684 mu4s = A1 * sin((double)4.*mu);
3685 mu6s = A2 * sin((double)6.*mu);
3686 mu8s = A3 * sin((double)8.*mu);
3687 *phi = mu + mu2s + mu4s + mu6s + mu8s;
3688
3689 if (*phi>po2) {
3690 *phi=po2;
3691 } else if (*phi<-po2) {
3692 *phi=-po2;
3693 }
3694
3695 if ((((po2-(double)1.0e-8)<fabs(*phi))&&(fabs(*phi)<(po2+(double)1.0e-8)))) {
3696 *lambda = M0;
3697 } else {
3698 phis = sin(*phi);
3699 *lambda = M0 + dx*pow((double)1.0-e2*phis*phis,(double).5) /
3700 (a*cos(*phi));
3701 }
3702
3703 if (*lambda>GPS_PI) {
3704 *lambda-=p2;
3705 }
3706 if (*lambda<-GPS_PI) {
3707 *lambda+=p2;
3708 }
3709
3710 if (*lambda>GPS_PI) {
3711 *lambda=GPS_PI;
3712 } else if (*lambda<-GPS_PI) {
3713 *lambda=-GPS_PI;
3714 }
3715
3716 *lambda = GPS_Math_Rad_To_Deg(*lambda);
3717 *phi = GPS_Math_Rad_To_Deg(*phi);
3718
3719 return;
3720 }
3721
3722
3723
3724
3725 /* @func GPS_Math_TCylEA_LatLon_To_EN **********************************
3726 **
3727 ** Convert latitude and longitude to transverse cylindrical equal area
3728 ** projection easting and northing
3729 **
3730 ** @param [r] phi [double] latitude (deg)
3731 ** @param [r] lambda [double] longitude (deg)
3732 ** @param [w] E [double *] easting (metre)
3733 ** @param [w] N [double *] northing (metre)
3734 ** @param [r] phi0 [double] latitude of origin (deg)
3735 ** @param [r] M0 [double] central meridian (deg)
3736 ** @param [r] E0 [double] false easting
3737 ** @param [r] N0 [double] false northing
3738 ** @param [r] a [double] semi-major axis
3739 ** @param [r] b [double] semi-minor axis
3740 **
3741 ** @return [void]
3742 ************************************************************************/
GPS_Math_TCylEA_LatLon_To_EN(double phi,double lambda,double * E,double * N,double phi0,double M0,double E0,double N0,double a,double b)3743 void GPS_Math_TCylEA_LatLon_To_EN(double phi, double lambda, double* E,
3744 double* N, double phi0, double M0, double E0,
3745 double N0, double a, double b)
3746 {
3747 double p2;
3748 double po2;
3749 double a2;
3750 double b2;
3751 double e;
3752 double e2;
3753 double e4;
3754 double e6;
3755 double AM0;
3756 double qp;
3757 double om;
3758 double oo;
3759 double c0;
3760 double c1;
3761 double c2;
3762 double c3;
3763 double b0;
3764 double b1;
3765 double B2;
3766 double b3;
3767 double A0;
3768 double A1;
3769 double A2;
3770 double sf;
3771 double x;
3772 double som;
3773 double phis;
3774 double j;
3775 double te4;
3776 double lat;
3777 double phi0s2;
3778 double phi0s4;
3779 double phi0s6;
3780 double E1;
3781 double E2;
3782 double E3;
3783 double E4;
3784
3785 double dlam;
3786 double qq;
3787 double qqo;
3788 double bt;
3789 double btc;
3790 double PHI;
3791 double PHIs2;
3792 double PHIs4;
3793 double PHIs6;
3794 double bts2;
3795 double bts4;
3796 double bts6;
3797 double PHIc;
3798 double PHIcs;
3799 double Mc;
3800
3801
3802
3803 sf = (double)1.0; /* scale factor */
3804
3805 lambda = GPS_Math_Deg_To_Rad(lambda);
3806 phi = GPS_Math_Deg_To_Rad(phi);
3807 phi0 = GPS_Math_Deg_To_Rad(phi0);
3808 M0 = GPS_Math_Deg_To_Rad(M0);
3809
3810 p2 = (double)GPS_PI * (double)2.0;
3811 po2 = (double)GPS_PI / (double)2.0;
3812 if (M0>GPS_PI) {
3813 M0 -= p2;
3814 }
3815 a2 = a*a;
3816 b2 = b*b;
3817 e2 = (a2-b2)/a2;
3818 e4 = e2*e2;
3819 e6 = e2*e4;
3820 e = pow(e2,(double).5);
3821 om = (double)1.-e2;
3822 som = pow(om,(double).5);
3823 oo = (double)1./((double)2.*e);
3824
3825 phis = sin(po2);
3826 x = e * phis;
3827 qp = om*(phis/((double)1.-e2*phis*phis)-oo*
3828 log(((double)1.-x)/((double)1.+x)));
3829
3830 A0 = e2 / (double)3.+(double)31.*e4/(double)180.+(double)517.*
3831 e6/(double)5040.;
3832 A1 = (double)23.*e4/(double)360.+(double)251.*e6/(double)3780.;
3833 A2 = (double)761.*e6/(double)45360.;
3834
3835 E1 = ((double)1.0-som) / ((double)1.0+som);
3836 E2 = E1*E1;
3837 E3 = E2*E1;
3838 E4 = E3*E1;
3839
3840 b0 = (double)3.*E1/(double)2.-(double)27.*E3/(double)32.;
3841 b1 = (double)21.*E2/(double)16.-(double)55.*E4/(double)32.;
3842 B2 = (double)151.*E3/(double)96.;
3843 b3 = (double)1097.*E4/(double)512.;
3844
3845
3846 j = (double)45.0*e6/(double)1024.0;
3847 te4 = (double)3.0 * e4;
3848 c0 = (double)1.0-e2/(double)4.0-te4/(double)64.0-(double)5.0*e6/
3849 (double)256.0;
3850 c1 = (double)3.0*e2/(double)8.0+te4/(double)32.0+j;
3851 c2 = (double)15.0*e4/(double)256.0+j;
3852 c3 = (double)35.0*e6/(double)3072.0;
3853
3854 lat = c0 * phi0;
3855
3856 phi0s2 = c1 * sin((double)2.0*phi0);
3857 phi0s4 = c2 * sin((double)4.0*phi0);
3858 phi0s6 = c3 * sin((double)6.0*phi0);
3859 AM0 = a*(lat-phi0s2+phi0s4-phi0s6);
3860
3861
3862 dlam = lambda - M0;
3863 if (dlam>GPS_PI) {
3864 dlam -= p2;
3865 }
3866 if (dlam<-GPS_PI) {
3867 dlam += p2;
3868 }
3869
3870 phis = sin(phi);
3871
3872 if (phi==po2) {
3873 qq = qp;
3874 qqo = (double)1.;
3875 } else {
3876 x = e * phis;
3877 qq = om*(phis/((double)1.-e2*phis*phis)-oo*
3878 log(((double)1.-x)/((double)1.+x)));
3879 qqo = qq/qp;
3880 }
3881
3882 if (qqo>(double)1.) {
3883 qqo = (double)1.;
3884 } else if (qqo<(double)-1.) {
3885 qqo = (double)-1.;
3886 }
3887
3888 bt = asin(qqo);
3889 btc = atan(tan(bt)/cos(dlam));
3890
3891 if ((fabs(btc)-po2)>(double)1.0e-8) {
3892 PHIc = btc;
3893 } else {
3894 bts2 = A0 * sin((double)2.0*btc);
3895 bts4 = A1 * sin((double)4.0*btc);
3896 bts6 = A2 * sin((double)6.0*btc);
3897 PHIc = btc + bts2 + bts4 + bts6;
3898 }
3899
3900 PHIcs = sin(PHIc);
3901 *E = a*cos(bt)*cos(PHIc)*sin(dlam)/(sf*cos(btc)*
3902 pow((double)1.-e2*PHIcs*PHIcs,
3903 (double).5)) + E0;
3904 PHI = c0 * PHIc;
3905 PHIs2 = c1 * sin((double)2.0*PHIc);
3906 PHIs4 = c2 * sin((double)4.0*PHIc);
3907 PHIs6 = c3 * sin((double)6.0*PHIc);
3908 Mc = a*(PHI-PHIs2+PHIs4-PHIs6);
3909
3910 *N = sf * (Mc-AM0) + N0;
3911
3912 return;
3913 }
3914
3915
3916
3917
3918 /* @func GPS_Math_TCylEA_EN_To_LatLon **********************************
3919 **
3920 ** Convert transverse cylindrical equal area easting and northing projection
3921 ** to latitude and longitude
3922 **
3923 ** @param [r] E [double] easting (metre)
3924 ** @param [r] N [double] northing (metre)
3925 ** @param [w] phi [double *] latitude (deg)
3926 ** @param [w] lambda [double *] longitude (deg)
3927 ** @param [r] phi0 [double] latitude of origin (deg)
3928 ** @param [r] M0 [double] central meridian (deg)
3929 ** @param [r] E0 [double] false easting
3930 ** @param [r] N0 [double] false northing
3931 ** @param [r] a [double] semi-major axis
3932 ** @param [r] b [double] semi-minor axis
3933 **
3934 ** @return [void]
3935 ************************************************************************/
GPS_Math_TCylEA_EN_To_LatLon(double E,double N,double * phi,double * lambda,double phi0,double M0,double E0,double N0,double a,double b)3936 void GPS_Math_TCylEA_EN_To_LatLon(double E, double N, double* phi,
3937 double* lambda, double phi0, double M0,
3938 double E0, double N0, double a, double b)
3939 {
3940 double p2;
3941 double po2;
3942 double a2;
3943 double b2;
3944 double e;
3945 double e2;
3946 double e4;
3947 double e6;
3948 double AM0;
3949 double qp;
3950 double om;
3951 double oo;
3952 double c0;
3953 double c1;
3954 double c2;
3955 double c3;
3956 double b0;
3957 double b1;
3958 double B2;
3959 double b3;
3960 double A0;
3961 double A1;
3962 double A2;
3963 double sf;
3964 double x;
3965 double som;
3966 double phis;
3967 double j;
3968 double te4;
3969 double lat;
3970 double phi0s2;
3971 double phi0s4;
3972 double phi0s6;
3973 double E1;
3974 double E2;
3975 double E3;
3976 double E4;
3977
3978 double dx;
3979 double dy;
3980 double bt;
3981 double btc;
3982 double btp;
3983 double btcc;
3984 double Mc;
3985 double Muc;
3986 double mus2;
3987 double mus4;
3988 double mus6;
3989 double mus8;
3990 double bts2;
3991 double bts4;
3992 double bts6;
3993 double PHIc;
3994 double Qc;
3995 double Qco;
3996 double t;
3997
3998
3999 sf = (double)1.0; /* scale factor */
4000
4001 phi0 = GPS_Math_Deg_To_Rad(phi0);
4002 M0 = GPS_Math_Deg_To_Rad(M0);
4003
4004 p2 = (double)GPS_PI * (double)2.0;
4005 po2 = (double)GPS_PI / (double)2.0;
4006 if (M0>GPS_PI) {
4007 M0 -= p2;
4008 }
4009 a2 = a*a;
4010 b2 = b*b;
4011 e2 = (a2-b2)/a2;
4012 e4 = e2*e2;
4013 e6 = e2*e4;
4014 e = pow(e2,(double).5);
4015 om = (double)1.-e2;
4016 som = pow(om,(double).5);
4017 oo = (double)1./((double)2.*e);
4018
4019 phis = sin(po2);
4020 x = e * phis;
4021 qp = om*(phis/((double)1.-e2*phis*phis)-oo*
4022 log(((double)1.-x)/((double)1.+x)));
4023
4024 A0 = e2 / (double)3.+(double)31.*e4/(double)180.+(double)517.*
4025 e6/(double)5040.;
4026 A1 = (double)23.*e4/(double)360.+(double)251.*e6/(double)3780.;
4027 A2 = (double)761.*e6/(double)45360.;
4028
4029 E1 = ((double)1.0-som) / ((double)1.0+som);
4030 E2 = E1*E1;
4031 E3 = E2*E1;
4032 E4 = E3*E1;
4033
4034 b0 = (double)3.*E1/(double)2.-(double)27.*E3/(double)32.;
4035 b1 = (double)21.*E2/(double)16.-(double)55.*E4/(double)32.;
4036 B2 = (double)151.*E3/(double)96.;
4037 b3 = (double)1097.*E4/(double)512.;
4038
4039
4040 j = (double)45.0*e6/(double)1024.0;
4041 te4 = (double)3.0 * e4;
4042 c0 = (double)1.0-e2/(double)4.0-te4/(double)64.0-(double)5.0*e6/
4043 (double)256.0;
4044 c1 = (double)3.0*e2/(double)8.0+te4/(double)32.0+j;
4045 c2 = (double)15.0*e4/(double)256.0+j;
4046 c3 = (double)35.0*e6/(double)3072.0;
4047
4048 lat = c0 * phi0;
4049
4050 phi0s2 = c1 * sin((double)2.0*phi0);
4051 phi0s4 = c2 * sin((double)4.0*phi0);
4052 phi0s6 = c3 * sin((double)6.0*phi0);
4053 AM0 = a*(lat-phi0s2+phi0s4-phi0s6);
4054
4055
4056
4057 dx = E - E0;
4058 dy = N - N0;
4059 Mc = AM0 + dy/sf;
4060 Muc = Mc / (c0*a);
4061
4062 mus2 = b0 * sin((double)2.0*Muc);
4063 mus4 = b1 * sin((double)4.0*Muc);
4064 mus6 = B2 * sin((double)6.0*Muc);
4065 mus8 = b3 * sin((double)6.0*Muc);
4066 PHIc = Muc + mus2 + mus4 + mus6 + mus8;
4067
4068 phis = sin(PHIc);
4069 x = e * phis;
4070 Qc = om*(phis/((double)1.-e2*phis*phis)-oo*
4071 log(((double)1.-x)/((double)1.+x)));
4072 Qco = Qc/qp;
4073
4074 if (Qco>(double)1.) {
4075 Qco = (double)1.;
4076 } else if (Qco<(double)-1.) {
4077 Qco = (double)-1.;
4078 }
4079
4080 btc = asin(Qco);
4081 btcc = cos(btc);
4082 t = sf*dx*btcc*pow((double)1.-e2*phis*phis,(double).5)/(a*cos(PHIc));
4083 if (t>(double)1.) {
4084 t=(double)1.;
4085 } else if (t<(double)-1.) {
4086 t=(double)-1.;
4087 }
4088 btp = -asin(t);
4089 bt = asin(cos(btp)*sin(btc));
4090
4091 bts2 = A0 * sin((double)2.0*bt);
4092 bts4 = A1 * sin((double)4.0*bt);
4093 bts6 = A2 * sin((double)6.0*bt);
4094 *phi = bt + bts2 + bts4 + bts6;
4095 *lambda = M0 - atan(tan(btp)/btcc);
4096
4097 if (*phi>po2) {
4098 *phi = po2;
4099 } else if (*phi<-po2) {
4100 *phi = -po2;
4101 }
4102
4103 if (*lambda>GPS_PI) {
4104 *lambda -= p2;
4105 }
4106 if (*lambda<-GPS_PI) {
4107 *lambda += p2;
4108 }
4109
4110 if (*lambda>GPS_PI) {
4111 *lambda = GPS_PI;
4112 } else if (*lambda<-GPS_PI) {
4113 *lambda=-GPS_PI;
4114 }
4115
4116 *lambda = GPS_Math_Rad_To_Deg(*lambda);
4117 *phi = GPS_Math_Rad_To_Deg(*phi);
4118
4119 return;
4120 }
4121
4122
4123
4124
4125 /* @func GPS_Math_Mercator_LatLon_To_EN **********************************
4126 **
4127 ** Convert latitude and longitude to standard Mercator projection
4128 ** easting and northing
4129 **
4130 ** @param [r] phi [double] latitude (deg)
4131 ** @param [r] lambda [double] longitude (deg)
4132 ** @param [w] E [double *] easting (metre)
4133 ** @param [w] N [double *] northing (metre)
4134 ** @param [r] phi0 [double] latitude of origin (deg)
4135 ** @param [r] lambda0 [double] longitude of origin (deg)
4136 ** @param [r] E0 [double] false easting
4137 ** @param [r] N0 [double] false northing
4138 ** @param [r] a [double] semi-major axis
4139 ** @param [r] b [double] semi-minor axis
4140 **
4141 ** @return [void]
4142 ************************************************************************/
GPS_Math_Mercator_LatLon_To_EN(double phi,double lambda,double * E,double * N,double phi0,double lambda0,double E0,double N0,double a,double b)4143 void GPS_Math_Mercator_LatLon_To_EN(double phi, double lambda, double* E,
4144 double* N, double phi0, double lambda0,
4145 double E0, double N0, double a, double b)
4146 {
4147 double p2;
4148 double po2;
4149 double a2;
4150 double b2;
4151 double e2;
4152 double e4;
4153 double e3;
4154 double e;
4155 double es;
4156 double ab;
4157 double bb;
4158 double cb;
4159 double db;
4160 double ml;
4161 double phi0s;
4162 double sf;
4163
4164 double dlam;
4165 double ct;
4166 double ex;
4167 double tt;
4168 double pt;
4169
4170
4171 lambda = GPS_Math_Deg_To_Rad(lambda);
4172 phi = GPS_Math_Deg_To_Rad(phi);
4173 phi0 = GPS_Math_Deg_To_Rad(phi0);
4174 lambda0 = GPS_Math_Deg_To_Rad(lambda0);
4175
4176 ml = ((double)GPS_PI*(double)89.5)/(double)180.;
4177
4178 p2 = (double)GPS_PI * (double)2.0;
4179 po2 = (double)GPS_PI / (double)2.0;
4180 if (lambda0>GPS_PI) {
4181 lambda0 -= p2;
4182 }
4183 a2 = a*a;
4184 b2 = b*b;
4185 es = (a2-b2)/a2;
4186 e2 = es*es;
4187 e3 = e2*es;
4188 e4 = e3*es;
4189
4190 e = pow(es,(double).5);
4191 phi0s = sin(phi0);
4192 sf = (double)1. / (pow((double)1.-es*phi0s*phi0s,(double).5)/cos(phi0));
4193
4194 ab = es/(double)2.+(double)5.*e2/(double)24.+e3/(double)12.+(double)13.*
4195 e4/(double)360.;
4196 bb = (double)7.*e2/(double)48.+(double)29.*e3/(double)240.+
4197 (double)811.*e4/(double)11520.;
4198 cb = (double)7.*e3/(double)120.+(double)81.*e4/(double)1120.;
4199 db = (double)4279.*e4/(double)161280.;
4200
4201
4202
4203 if (lambda>(double)GPS_PI) {
4204 lambda -= p2;
4205 }
4206
4207 dlam = lambda - lambda0;
4208 if (dlam>GPS_PI) {
4209 dlam -= p2;
4210 }
4211 if (dlam<-GPS_PI) {
4212 dlam += p2;
4213 }
4214
4215
4216 ex = e * sin(phi);
4217 tt = tan((double)GPS_PI/(double)4.+phi/(double)2.);
4218 pt = pow((((double)1.-ex)/((double)1.+ex)),(e/(double)2.));
4219
4220 ct = tt * pt;
4221 *N = sf * a * log(ct) + N0;
4222 *E = sf * a * dlam + E0;
4223
4224 return;
4225 }
4226
4227
4228
4229
4230 /* @func GPS_Math_Mercator_EN_To_LatLon **********************************
4231 **
4232 ** Convert standard Mercator easting and northing projection
4233 ** to latitude and longitude
4234 **
4235 ** @param [r] E [double] easting (metre)
4236 ** @param [r] N [double] northing (metre)
4237 ** @param [w] phi [double *] latitude (deg)
4238 ** @param [w] lambda [double *] longitude (deg)
4239 ** @param [r] phi0 [double] latitude of origin (deg)
4240 ** @param [r] lambda0 [double] longitude of origin (deg)
4241 ** @param [r] E0 [double] false easting
4242 ** @param [r] N0 [double] false northing
4243 ** @param [r] a [double] semi-major axis
4244 ** @param [r] b [double] semi-minor axis
4245 **
4246 ** @return [void]
4247 ************************************************************************/
GPS_Math_Mercator_EN_To_LatLon(double E,double N,double * phi,double * lambda,double phi0,double lambda0,double E0,double N0,double a,double b)4248 void GPS_Math_Mercator_EN_To_LatLon(double E, double N, double* phi,
4249 double* lambda, double phi0,
4250 double lambda0, double E0, double N0,
4251 double a, double b)
4252 {
4253 double p2;
4254 double po2;
4255 double a2;
4256 double b2;
4257 double e2;
4258 double e4;
4259 double e3;
4260 double e;
4261 double es;
4262 double ab;
4263 double bb;
4264 double cb;
4265 double db;
4266 double ml;
4267 double phi0s;
4268 double sf;
4269
4270 double dx;
4271 double dy;
4272 double x;
4273
4274 phi0 = GPS_Math_Deg_To_Rad(phi0);
4275 lambda0 = GPS_Math_Deg_To_Rad(lambda0);
4276
4277 ml = ((double)GPS_PI*(double)89.5)/(double)180.;
4278
4279 p2 = (double)GPS_PI * (double)2.0;
4280 po2 = (double)GPS_PI / (double)2.0;
4281 if (lambda0>GPS_PI) {
4282 lambda0 -= p2;
4283 }
4284 a2 = a*a;
4285 b2 = b*b;
4286 es = (a2-b2)/a2;
4287 e2 = es*es;
4288 e3 = e2*es;
4289 e4 = e3*es;
4290
4291 e = pow(es,(double).5);
4292 phi0s = sin(phi0);
4293 sf = (double)1. / (pow((double)1.-es*phi0s*phi0s,(double).5)/cos(phi0));
4294
4295 ab = es/(double)2.+(double)5.*e2/(double)24.+e3/(double)12.+(double)13.*
4296 e4/(double)360.;
4297 bb = (double)7.*e2/(double)48.+(double)29.*e3/(double)240.+
4298 (double)811.*e4/(double)11520.;
4299 cb = (double)7.*e3/(double)120.+(double)81.*e4/(double)1120.;
4300 db = (double)4279.*e4/(double)161280.;
4301
4302 dx = E - E0;
4303 dy = N - N0;
4304 *lambda = lambda0 + dx / (sf*a);
4305 x = (double)GPS_PI / (double)2. -
4306 (double)2.*atan((double)1./exp(dy/(sf*a)));
4307 *phi = x+ab*sin((double)2.*x)+bb*sin((double)4.*x)+cb*sin((double)6.*x)
4308 + db*sin((double)8.*x);
4309
4310 if (*phi>po2) {
4311 *phi = po2;
4312 } else if (*phi<-po2) {
4313 *phi = -po2;
4314 }
4315
4316 if (*lambda>GPS_PI) {
4317 *lambda -= p2;
4318 }
4319 if (*lambda<-GPS_PI) {
4320 *lambda += p2;
4321 }
4322
4323 if (*lambda>GPS_PI) {
4324 *lambda = GPS_PI;
4325 } else if (*lambda<-GPS_PI) {
4326 *lambda=-GPS_PI;
4327 }
4328
4329 *lambda = GPS_Math_Rad_To_Deg(*lambda);
4330 *phi = GPS_Math_Rad_To_Deg(*phi);
4331
4332 return;
4333 }
4334
4335
4336
4337
4338 /* @func GPS_Math_TMerc_LatLon_To_EN **********************************
4339 **
4340 ** Convert latitude and longitude to transverse Mercator projection
4341 ** easting and northing
4342 **
4343 ** @param [r] phi [double] latitude (deg)
4344 ** @param [r] lambda [double] longitude (deg)
4345 ** @param [w] E [double *] easting (metre)
4346 ** @param [w] N [double *] northing (metre)
4347 ** @param [r] phi0 [double] latitude of origin (deg)
4348 ** @param [r] lambda0 [double] longitude of origin (deg)
4349 ** @param [r] E0 [double] false easting
4350 ** @param [r] N0 [double] false northing
4351 ** @param [r] F0 [double] scale factor
4352 ** @param [r] a [double] semi-major axis
4353 ** @param [r] b [double] semi-minor axis
4354 **
4355 ** @return [void]
4356 ************************************************************************/
GPS_Math_TMerc_LatLon_To_EN(double phi,double lambda,double * E,double * N,double phi0,double lambda0,double E0,double N0,double F0,double a,double b)4357 void GPS_Math_TMerc_LatLon_To_EN(double phi, double lambda, double* E,
4358 double* N, double phi0, double lambda0,
4359 double E0, double N0, double F0,
4360 double a, double b)
4361 {
4362 GPS_Math_LatLon_To_EN(E,N,phi,lambda,N0,E0,phi0,lambda0,F0,a,b);
4363
4364 return;
4365 }
4366
4367
4368
4369
4370 /* @func GPS_Math_TMerc_EN_To_LatLon **********************************
4371 **
4372 ** Convert transverse Mercator easting and northing projection
4373 ** to latitude and longitude
4374 **
4375 ** @param [r] E [double] easting (metre)
4376 ** @param [r] N [double] northing (metre)
4377 ** @param [w] phi [double *] latitude (deg)
4378 ** @param [w] lambda [double *] longitude (deg)
4379 ** @param [r] phi0 [double] latitude of origin (deg)
4380 ** @param [r] lambda0 [double] longitude of origin (deg)
4381 ** @param [r] E0 [double] false easting
4382 ** @param [r] N0 [double] false northing
4383 ** @param [r] F0 [double] scale factor
4384 ** @param [r] a [double] semi-major axis
4385 ** @param [r] b [double] semi-minor axis
4386 **
4387 ** @return [void]
4388 ************************************************************************/
GPS_Math_TMerc_EN_To_LatLon(double E,double N,double * phi,double * lambda,double phi0,double lambda0,double E0,double N0,double F0,double a,double b)4389 void GPS_Math_TMerc_EN_To_LatLon(double E, double N, double* phi,
4390 double* lambda, double phi0, double lambda0,
4391 double E0, double N0, double F0,
4392 double a, double b)
4393 {
4394 GPS_Math_EN_To_LatLon(E,N,phi,lambda,N0,E0,phi0,lambda0,F0,a,b);
4395
4396 return;
4397 }
4398
4399
4400
4401
4402 /* @func GPS_Math_Swiss_LatLon_To_EN ***********************************
4403 **
4404 ** Convert latitude and longitude to Swiss grid easting and northing
4405 **
4406 ** @param [r] phi [double] latitude (deg)
4407 ** @param [r] lambda [double] longitude (deg)
4408 ** @param [w] E [double *] easting (metre)
4409 ** @param [w] N [double *] northing (metre)
4410 ** @param [r] phi0 [double] latitude origin (deg) [normally 46.95240556]
4411 ** @param [r] lambda0 [double] longitude origin (deg) [normally 7.43958333]
4412 ** @param [r] E0 [double] false easting (metre) [normally 600000.0]
4413 ** @param [r] N0 [double] false northing (metre) [normally 200000.0]
4414 ** @param [r] a [double] semi-major axis [normally 6377397.000]
4415 ** @param [r] b [double] semi-minor axis [normally 6356078.823]
4416 **
4417 ** @return [void]
4418 ***************************************************************************/
GPS_Math_Swiss_LatLon_To_EN(double phi,double lambda,double * E,double * N,double phi0,double lambda0,double E0,double N0,double a,double b)4419 void GPS_Math_Swiss_LatLon_To_EN(double phi, double lambda, double* E,
4420 double* N,double phi0,double lambda0,
4421 double E0, double N0, double a, double b)
4422
4423 {
4424 double a2;
4425 double b2;
4426 double esq;
4427 double e;
4428 double c;
4429 double ephi0p;
4430 double phip;
4431 double sphip;
4432 double phid;
4433 double slambda2;
4434 double lambda1;
4435 double lambda2;
4436 double K;
4437 double po4;
4438 double w;
4439 double R;
4440
4441 lambda0 = GPS_Math_Deg_To_Rad(lambda0);
4442 phi0 = GPS_Math_Deg_To_Rad(phi0);
4443 lambda = GPS_Math_Deg_To_Rad(lambda);
4444 phi = GPS_Math_Deg_To_Rad(phi);
4445
4446 po4=GPS_PI/(double)4.0;
4447
4448 a2 = a*a;
4449 b2 = b*b;
4450 esq = (a2-b2)/a2;
4451 e = pow(esq,(double)0.5);
4452
4453 c = sqrt(1+((esq*pow(cos(phi0),(double)4.))/((double)1.-esq)));
4454
4455 ephi0p = asin(sin(phi0)/c);
4456
4457 K = log(tan(po4+ephi0p/(double)2.)) - c*(log(tan(po4+phi0/(double)2.)) -
4458 e/(double)2. * log(((double)1.+e*sin(phi0)) /
4459 ((double)1.-e*sin(phi0))));
4460 lambda1 = c*(lambda-lambda0);
4461 w = c*(log(tan(po4+phi/(double)2.)) - e/(double)2. *
4462 log(((double)1.+e*sin(phi)) / ((double)1.-e*sin(phi)))) + K;
4463
4464
4465 phip = (double)2. * (atan(exp(w)) - po4);
4466
4467 sphip = cos(ephi0p) * sin(phip) - sin(ephi0p) * cos(phip) * cos(lambda1);
4468 phid = asin(sphip);
4469
4470 slambda2 = cos(phip)*sin(lambda1) / cos(phid);
4471 lambda2 = asin(slambda2);
4472
4473 R = a*sqrt((double)1.-esq) / ((double)1.-esq*sin(phi0) * sin(phi0));
4474
4475 *N = R*log(tan(po4 + phid/(double)2.)) + N0;
4476 *E = R*lambda2 + E0;
4477 return;
4478 }
4479
4480
4481
4482
4483 /* @func GPS_Math_Swiss_EN_To_LatLon ************************************
4484 **
4485 ** Convert Swiss Grid easting and northing to latitude and longitude
4486 **
4487 ** @param [r] E [double] easting (metre)
4488 ** @param [r] N [double] northing (metre)
4489 ** @param [w] phi [double *] latitude (deg)
4490 ** @param [w] lambda [double *] longitude (deg)
4491 ** @param [r] phi0 [double] latitude origin (deg) [normally 46.95240556]
4492 ** @param [r] lambda0 [double] longitude origin (deg) [normally 7.43958333]
4493 ** @param [r] E0 [double] false easting (metre) [normally 600000.0]
4494 ** @param [r] N0 [double] false northing (metre) [normally 200000.0]
4495 ** @param [r] a [double] semi-major axis [normally 6377397.000]
4496 ** @param [r] b [double] semi-minor axis [normally 6356078.823]
4497 **
4498 ** @return [void]
4499 *************************************************************************/
4500
GPS_Math_Swiss_EN_To_LatLon(double E,double N,double * phi,double * lambda,double phi0,double lambda0,double E0,double N0,double a,double b)4501 void GPS_Math_Swiss_EN_To_LatLon(double E, double N, double* phi,
4502 double* lambda, double phi0, double lambda0,
4503 double E0, double N0, double a, double b)
4504 {
4505 double a2;
4506 double b2;
4507 double esq;
4508 double e;
4509 double R;
4510 double c;
4511 double po4;
4512 double phid;
4513 double phi1;
4514 double lambdad;
4515 double lambda1;
4516 double slambda1;
4517 double ephi0p;
4518 double sphip;
4519 double tol;
4520 double cr;
4521 double C;
4522 double K;
4523
4524 lambda0 = GPS_Math_Deg_To_Rad(lambda0);
4525 phi0 = GPS_Math_Deg_To_Rad(phi0);
4526
4527 po4=GPS_PI/(double)4.0;
4528 tol=(double)0.00001;
4529
4530 a2 = a*a;
4531 b2 = b*b;
4532 esq = (a2-b2)/a2;
4533 e = pow(esq,(double)0.5);
4534
4535 R = a*sqrt((double)1.-esq) / ((double)1.-esq*sin(phi0) * sin(phi0));
4536
4537 phid = (double)2.*(atan(exp((N - N0)/R)) - po4);
4538 lambdad = (E - E0)/R;
4539
4540 c = sqrt((double)1.+((esq * pow(cos(phi0), (double)4.)) /
4541 ((double)1.-esq)));
4542 ephi0p = asin(sin(phi0) / c);
4543
4544 sphip = cos(ephi0p)*sin(phid) + sin(ephi0p)*cos(phid)*cos(lambdad);
4545 phi1 = asin(sphip);
4546
4547 slambda1 = cos(phid)*sin(lambdad)/cos(phi1);
4548 lambda1 = asin(slambda1);
4549
4550 *lambda = GPS_Math_Rad_To_Deg((lambda1/c + lambda0));
4551
4552 K = log(tan(po4 + ephi0p/(double)2.)) -c*(log(tan(po4 + phi0/(double)2.))
4553 - e/(double)2. * log(((double)1.+e*sin(phi0)) /
4554 ((double)1.-e*sin(phi0))));
4555 C = (K - log(tan(po4 + phi1/(double)2.)))/c;
4556
4557 do {
4558 cr = (C + log(tan(po4 + phi1/(double)2.)) - e/(double)2. *
4559 log(((double)1.+e*sin(phi1)) / ((double)1.-e*sin(phi1)))) *
4560 ((((double)1.-esq*sin(phi1)*sin(phi1)) * cos(phi1)) /
4561 ((double)1.-esq));
4562 phi1 -= cr;
4563 } while (fabs(cr) > tol);
4564
4565 *phi = GPS_Math_Rad_To_Deg(phi1);
4566
4567 return;
4568 }
4569