1 /******************************************************************************
2 *
3 * Project: OpenCPN
4 * Purpose: OpenCPN Georef utility
5 * Author: David Register
6 *
7 ***************************************************************************
8 * Copyright (C) 2010 by David S. Register *
9 * *
10 * This program is free software; you can redistribute it and/or modify *
11 * it under the terms of the GNU General Public License as published by *
12 * the Free Software Foundation; either version 2 of the License, or *
13 * (at your option) any later version. *
14 * *
15 * This program is distributed in the hope that it will be useful, *
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
18 * GNU General Public License for more details. *
19 * *
20 * You should have received a copy of the GNU General Public License *
21 * along with this program; if not, write to the *
22 * Free Software Foundation, Inc., *
23 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. *
24 ***************************************************************************
25
26 ***************************************************************************
27 * Parts of this file were adapted from source code found in *
28 * John F. Waers (jfwaers@csn.net) public domain program MacGPS45 *
29 ***************************************************************************
30 */
31
32
33
34 #include <vector>
35 #include <utility>
36 #include <stdlib.h>
37 #include <string.h>
38 #include <ctype.h>
39 #include <math.h>
40
41 #include "georef.h"
42 #include "cutil.h"
43
44
45 #ifdef __MSVC__
46 #define snprintf mysnprintf
47 #endif
48
49 #if !defined(NAN)
50 static const long long lNaN = 0xfff8000000000000;
51 #define NAN (*(double*)&lNaN)
52 #endif
53
54
55
56 // ellipsoid: index into the gEllipsoid[] array, in which
57 // a is the ellipsoid semimajor axis
58 // invf is the inverse of the ellipsoid flattening f
59 // dx, dy, dz: ellipsoid center with respect to WGS84 ellipsoid center
60 // x axis is the prime meridian
61 // y axis is 90 degrees east longitude
62 // z axis is the axis of rotation of the ellipsoid
63
64 // The following values for dx, dy and dz were extracted from the output of
65 // the GARMIN PCX5 program. The output also includes values for da and df, the
66 // difference between the reference ellipsoid and the WGS84 ellipsoid semi-
67 // major axis and flattening, respectively. These are replaced by the
68 // data contained in the structure array gEllipsoid[], which was obtained from
69 // the Defence Mapping Agency document number TR8350.2, "Department of Defense
70 // World Geodetic System 1984."
71
72 struct DATUM const gDatum[] = {
73 // name ellipsoid dx dy dz
74 { "Adindan", 5, -162, -12, 206 }, // 0
75 { "Afgooye", 15, -43, -163, 45 }, // 1
76 { "Ain el Abd 1970", 14, -150, -251, -2 }, // 2
77 { "Anna 1 Astro 1965", 2, -491, -22, 435 }, // 3
78 { "Arc 1950", 5, -143, -90, -294 }, // 4
79 { "Arc 1960", 5, -160, -8, -300 }, // 5
80 { "Ascension Island 58", 14, -207, 107, 52 }, // 6
81 { "Astro B4 Sorol Atoll", 14, 114, -116, -333 }, // 7
82 { "Astro Beacon E ", 14, 145, 75, -272 }, // 8
83 { "Astro DOS 71/4", 14, -320, 550, -494 }, // 9
84 { "Astronomic Stn 52", 14, 124, -234, -25 }, // 10
85 { "Australian Geod 66", 2, -133, -48, 148 }, // 11
86 { "Australian Geod 84", 2, -134, -48, 149 }, // 12
87 { "Bellevue (IGN)", 14, -127, -769, 472 }, // 13
88 { "Bermuda 1957", 4, -73, 213, 296 }, // 14
89 { "Bogota Observatory", 14, 307, 304, -318 }, // 15
90 { "Campo Inchauspe", 14, -148, 136, 90 }, // 16
91 { "Canton Astro 1966", 14, 298, -304, -375 }, // 17
92 { "Cape", 5, -136, -108, -292 }, // 18
93 { "Cape Canaveral", 4, -2, 150, 181 }, // 19
94 { "Carthage", 5, -263, 6, 431 }, // 20
95 { "CH-1903", 3, 674, 15, 405 }, // 21
96 { "Chatham 1971", 14, 175, -38, 113 }, // 22
97 { "Chua Astro", 14, -134, 229, -29 }, // 23
98 { "Corrego Alegre", 14, -206, 172, -6 }, // 24
99 { "Djakarta (Batavia)", 3, -377, 681, -50 }, // 25
100 { "DOS 1968", 14, 230, -199, -752 }, // 26
101 { "Easter Island 1967", 14, 211, 147, 111 }, // 27
102 { "European 1950", 14, -87, -98, -121 }, // 28
103 { "European 1979", 14, -86, -98, -119 }, // 29
104 { "Finland Hayford", 14, -78, -231, -97 }, // 30
105 { "Gandajika Base", 14, -133, -321, 50 }, // 31
106 { "Geodetic Datum 49", 14, 84, -22, 209 }, // 32
107 { "Guam 1963", 4, -100, -248, 259 }, // 33
108 { "GUX 1 Astro", 14, 252, -209, -751 }, // 34
109 { "Hermannskogel Datum", 3, 682, -203, 480 }, // 35
110 { "Hjorsey 1955", 14, -73, 46, -86 }, // 36
111 { "Hong Kong 1963", 14, -156, -271, -189 }, // 37
112 { "Indian Bangladesh", 6, 289, 734, 257 }, // 38
113 { "Indian Thailand", 6, 214, 836, 303 }, // 39
114 { "Ireland 1965", 1, 506, -122, 611 }, // 40
115 { "ISTS 073 Astro 69", 14, 208, -435, -229 }, // 41
116 { "Johnston Island", 14, 191, -77, -204 }, // 42
117 { "Kandawala", 6, -97, 787, 86 }, // 43
118 { "Kerguelen Island", 14, 145, -187, 103 }, // 44
119 { "Kertau 1948", 7, -11, 851, 5 }, // 45
120 { "L.C. 5 Astro", 4, 42, 124, 147 }, // 46
121 { "Liberia 1964", 5, -90, 40, 88 }, // 47
122 { "Luzon Mindanao", 4, -133, -79, -72 }, // 48
123 { "Luzon Philippines", 4, -133, -77, -51 }, // 49
124 { "Mahe 1971", 5, 41, -220, -134 }, // 50
125 { "Marco Astro", 14, -289, -124, 60 }, // 51
126 { "Massawa", 3, 639, 405, 60 }, // 52
127 { "Merchich", 5, 31, 146, 47 }, // 53
128 { "Midway Astro 1961", 14, 912, -58, 1227 }, // 54
129 { "Minna", 5, -92, -93, 122 }, // 55
130 { "NAD27 Alaska", 4, -5, 135, 172 }, // 56
131 { "NAD27 Bahamas", 4, -4, 154, 178 }, // 57
132 { "NAD27 Canada", 4, -10, 158, 187 }, // 58
133 { "NAD27 Canal Zone", 4, 0, 125, 201 }, // 59
134 { "NAD27 Caribbean", 4, -7, 152, 178 }, // 60
135 { "NAD27 Central", 4, 0, 125, 194 }, // 61
136 { "NAD27 CONUS", 4, -8, 160, 176 }, // 62
137 { "NAD27 Cuba", 4, -9, 152, 178 }, // 63
138 { "NAD27 Greenland", 4, 11, 114, 195 }, // 64
139 { "NAD27 Mexico", 4, -12, 130, 190 }, // 65
140 { "NAD27 San Salvador", 4, 1, 140, 165 }, // 66
141 { "NAD83", 11, 0, 0, 0 }, // 67
142 { "Nahrwn Masirah Ilnd", 5, -247, -148, 369 }, // 68
143 { "Nahrwn Saudi Arbia", 5, -231, -196, 482 }, // 69
144 { "Nahrwn United Arab", 5, -249, -156, 381 }, // 70
145 { "Naparima BWI", 14, -2, 374, 172 }, // 71
146 { "Observatorio 1966", 14, -425, -169, 81 }, // 72
147 { "Old Egyptian", 12, -130, 110, -13 }, // 73
148 { "Old Hawaiian", 4, 61, -285, -181 }, // 74
149 { "Oman", 5, -346, -1, 224 }, // 75
150 { "Ord Srvy Grt Britn", 0, 375, -111, 431 }, // 76
151 { "Pico De Las Nieves", 14, -307, -92, 127 }, // 77
152 { "Pitcairn Astro 1967", 14, 185, 165, 42 }, // 78
153 { "Prov So Amrican 56", 14, -288, 175, -376 }, // 79
154 { "Prov So Chilean 63", 14, 16, 196, 93 }, // 80
155 { "Puerto Rico", 4, 11, 72, -101 }, // 81
156 { "Qatar National", 14, -128, -283, 22 }, // 82
157 { "Qornoq", 14, 164, 138, -189 }, // 83
158 { "Reunion", 14, 94, -948, -1262 }, // 84
159 { "Rome 1940", 14, -225, -65, 9 }, // 85
160 { "RT 90", 3, 498, -36, 568 }, // 86
161 { "Santo (DOS)", 14, 170, 42, 84 }, // 87
162 { "Sao Braz", 14, -203, 141, 53 }, // 88
163 { "Sapper Hill 1943", 14, -355, 16, 74 }, // 89
164 { "Schwarzeck", 21, 616, 97, -251 }, // 90
165 { "South American 69", 16, -57, 1, -41 }, // 91
166 { "South Asia", 8, 7, -10, -26 }, // 92
167 { "Southeast Base", 14, -499, -249, 314 }, // 93
168 { "Southwest Base", 14, -104, 167, -38 }, // 94
169 { "Timbalai 1948", 6, -689, 691, -46 }, // 95
170 { "Tokyo", 3, -128, 481, 664 }, // 96
171 { "Tristan Astro 1968", 14, -632, 438, -609 }, // 97
172 { "Viti Levu 1916", 5, 51, 391, -36 }, // 98
173 { "Wake-Eniwetok 60", 13, 101, 52, -39 }, // 99
174 { "WGS 72", 19, 0, 0, 5 }, // 100
175 { "WGS 84", 20, 0, 0, 0 }, // 101
176 { "Zanderij", 14, -265, 120, -358 }, // 102
177 { "WGS_84", 20, 0, 0, 0 }, // 103
178 { "WGS-84", 20, 0, 0, 0 }, // 104
179 { "EUROPEAN DATUM 1950", 14, -87, -98, -121 },
180 { "European 1950 (Norway Finland)", 14, -87, -98, -121},
181 { "ED50", 14, -87, -98, -121 },
182 { "RT90 (Sweden)", 3, 498, -36, 568 },
183 { "Monte Mario 1940", 14, -104, -49, 10 },
184 { "Ord Surv of Gr Britain 1936", 0, 375, -111, 431 },
185 { "South American 1969", 16, -57, 1, -41 },
186 { "PULKOVO 1942 (2)", 15, 25, -141, -79 },
187 { "EUROPEAN DATUM", 14, -87, -98, -121 },
188 { "BERMUDA DATUM 1957", 4, -73, 213, 296 },
189 { "COA", 14, -206, 172, -6 },
190 { "COABR", 14, -206, 172, -6 },
191 { "Roma 1940", 14, -225, -65, 9 },
192 { "ITALIENISCHES LANDESNETZ",14, -87, -98, -121 },
193 { "HERMANSKOGEL DATUM", 3, 682, -203, 480 },
194 { "AGD66", 2 , -128, -52, 153 },
195 { "ED", 14, -87, -98, -121 },
196 { "EUROPEAN 1950 (SPAIN AND PORTUGAL)",14,-87,-98,-121},
197 { "ED-50", 14, -87, -98, -121 },
198 { "EUROPEAN", 14, -87, -98, -121 },
199 { "POTSDAM", 14, -87, -98, -121 },
200 { "GRACIOSA SW BASE DATUM",14, -104, 167, -38 },
201 { "WGS 1984", 20, 0, 0, 0 },
202 { "WGS 1972", 19, 0, 0, 5 },
203 { "WGS", 19, 0, 0, 5 }
204
205
206 };
207
208 struct ELLIPSOID const gEllipsoid[] = {
209 // name a 1/f
210 { "Airy 1830", 6377563.396, 299.3249646 }, // 0
211 { "Modified Airy", 6377340.189, 299.3249646 }, // 1
212 { "Australian National", 6378160.0, 298.25 }, // 2
213 { "Bessel 1841", 6377397.155, 299.1528128 }, // 3
214 { "Clarke 1866", 6378206.4, 294.9786982 }, // 4
215 { "Clarke 1880", 6378249.145, 293.465 }, // 5
216 { "Everest (India 1830)", 6377276.345, 300.8017 }, // 6
217 { "Everest (1948)", 6377304.063, 300.8017 }, // 7
218 { "Modified Fischer 1960", 6378155.0, 298.3 }, // 8
219 { "Everest (Pakistan)", 6377309.613, 300.8017 }, // 9
220 { "Indonesian 1974", 6378160.0, 298.247 }, // 10
221 { "GRS 80", 6378137.0, 298.257222101 }, // 11
222 { "Helmert 1906", 6378200.0, 298.3 }, // 12
223 { "Hough 1960", 6378270.0, 297.0 }, // 13
224 { "International 1924", 6378388.0, 297.0 }, // 14
225 { "Krassovsky 1940", 6378245.0, 298.3 }, // 15
226 { "South American 1969", 6378160.0, 298.25 }, // 16
227 { "Everest (Malaysia 1969)", 6377295.664, 300.8017 }, // 17
228 { "Everest (Sabah Sarawak)", 6377298.556, 300.8017 }, // 18
229 { "WGS 72", 6378135.0, 298.26 }, // 19
230 { "WGS 84", 6378137.0, 298.257223563 }, // 20
231 { "Bessel 1841 (Namibia)", 6377483.865, 299.1528128 }, // 21
232 { "Everest (India 1956)", 6377301.243, 300.8017 }, // 22
233 { "Struve 1860", 6378298.3, 294.73 } // 23
234
235 };
236
237 short nDatums = sizeof(gDatum)/sizeof(struct DATUM);
238
239
240
241 /* define constants */
242
datumParams(short datum,double * a,double * es)243 void datumParams(short datum, double *a, double *es)
244 {
245 extern struct DATUM const gDatum[];
246 extern struct ELLIPSOID const gEllipsoid[];
247
248
249 if( datum < nDatums){
250 double f = 1.0 / gEllipsoid[gDatum[datum].ellipsoid].invf; // flattening
251 if(es)
252 *es = 2 * f - f * f; // eccentricity^2
253 if(a)
254 *a = gEllipsoid[gDatum[datum].ellipsoid].a; // semimajor axis
255 }
256 else{
257 double f = 1.0 / 298.257223563; // WGS84
258 if(es)
259 *es = 2 * f - f * f;
260 if(a)
261 *a = 6378137.0;
262 }
263 }
264
datumNameCmp(const char * n1,const char * n2)265 static int datumNameCmp(const char *n1, const char *n2)
266 {
267 while(*n1 || *n2)
268 {
269 if (*n1 == ' ')
270 n1++;
271 else if (*n2 == ' ')
272 n2++;
273 else if (toupper(*n1) == toupper(*n2))
274 n1++, n2++;
275 else
276 return 1; // No string match
277 }
278 return 0; // String match
279 }
280
isWGS84(int i)281 static int isWGS84(int i)
282 {
283 // DATUM_INDEX_WGS84 is an optimization
284 // but there's more than on in gDatum table
285 if (i == DATUM_INDEX_WGS84)
286 return i;
287
288 if (gDatum[i].ellipsoid != gDatum[DATUM_INDEX_WGS84].ellipsoid)
289 return i;
290
291 if (gDatum[i].dx != gDatum[DATUM_INDEX_WGS84].dx)
292 return i;
293
294 if (gDatum[i].dy != gDatum[DATUM_INDEX_WGS84].dy)
295 return i;
296
297 if (gDatum[i].dz != gDatum[DATUM_INDEX_WGS84].dz)
298 return i;
299
300 return DATUM_INDEX_WGS84;
301 }
302
GetDatumIndex(const char * str)303 int GetDatumIndex(const char *str)
304 {
305 int i = 0;
306 while (i < (int)nDatums)
307 {
308 if(!datumNameCmp(str, gDatum[i].name))
309 {
310 return isWGS84(i);
311 }
312 i++;
313 }
314
315 return -1;
316 }
317
318 /****************************************************************************/
319 /* Convert degrees to dd mm'ss.s" (DMS-Format) */
320 /****************************************************************************/
toDMS(double a,char * bufp,int bufplen)321 void toDMS(double a, char *bufp, int bufplen)
322 {
323 bool neg = a < 0.0;
324 a = fabs(a);
325 int n = (int) ((a - (int) a) * 36000.0);
326 int m = n / 600;
327 int s = n % 600;
328 sprintf(bufp, "%d%02d'%02d.%01d\"", (int) (neg ? -a : a), m, s / 10, s % 10);
329 }
330
331
332 /****************************************************************************/
333 /* Convert dd mm'ss.s" (DMS-Format) to degrees. */
334 /****************************************************************************/
335
fromDMS(char * dms)336 double fromDMS(char *dms)
337 {
338 int d = 0, m = 0;
339 double s = 0.0;
340 char buf[20] = {'\0'};
341
342 sscanf(dms, "%d%[ ]%d%[ ']%lf%[ \"NSWEnswe]", &d, buf, &m, buf, &s, buf);
343
344 s = (double) (abs(d)) + ((double) m + s / 60.0) / 60.0;
345
346 if (d >= 0 && strpbrk(buf, "SWsw") == NULL)
347 return s;
348
349 return -s;
350 }
351
352
353 /****************************************************************************/
354 /* Convert degrees to dd mm.mmm' (DMM-Format) */
355 /****************************************************************************/
356
todmm(int flag,double a,char * bufp,int bufplen)357 void todmm(int flag, double a, char *bufp, int bufplen)
358 {
359 bool bNeg = a < 0.0;
360 a = fabs(a);
361
362 int m = (int) ((a - (int) a) * 60000.0);
363
364 if(!flag)
365 snprintf(bufp, bufplen, "%d %02d.%03d'", (int) a, m / 1000, m % 1000);
366 else
367 {
368 if(flag == 1)
369 {
370 snprintf(bufp, bufplen, "%02d %02d.%03d %c", (int) a, m / 1000, (m % 1000), bNeg ? 'S' : 'N');
371 }
372 else if(flag == 2)
373 {
374 snprintf(bufp, bufplen, "%03d %02d.%03d %c", (int) a, m / 1000, (m % 1000), bNeg ? 'W' : 'E');
375 }
376 }
377 }
378
379
toDMM(double a,char * bufp,int bufplen)380 void toDMM(double a, char *bufp, int bufplen)
381 {
382 todmm(0, a, bufp, bufplen);
383 return;
384 }
385
386
387 /* --------------------------------------------------------------------------------- */
388 /****************************************************************************/
389 /* Convert Lat/Lon <-> Simple Mercator */
390 /****************************************************************************/
toSM(double lat,double lon,double lat0,double lon0,double * x,double * y)391 void toSM(double lat, double lon, double lat0, double lon0, double *x, double *y)
392 {
393 double xlon = lon;
394
395 /* Make sure lon and lon0 are same phase */
396
397 if((lon * lon0 < 0.) && (fabs(lon - lon0) > 180.))
398 {
399 lon < 0.0 ? xlon += 360.0 : xlon -= 360.0;
400 }
401
402 const double z = WGS84_semimajor_axis_meters * mercator_k0;
403
404 *x = (xlon - lon0) * DEGREE * z;
405
406 // y =.5 ln( (1 + sin t) / (1 - sin t) )
407 const double s = sin(lat * DEGREE);
408 const double y3 = (.5 * log((1 + s) / (1 - s))) * z;
409
410 const double s0 = sin(lat0 * DEGREE);
411 const double y30 = (.5 * log((1 + s0) / (1 - s0))) * z;
412 *y = y3 - y30;
413 }
414
toSMcache_y30(double lat0)415 double toSMcache_y30(double lat0)
416 {
417 const double z = WGS84_semimajor_axis_meters * mercator_k0;
418 const double s0 = sin(lat0 * DEGREE);
419 const double y30 = (.5 * log((1 + s0) / (1 - s0))) * z;
420 return y30;
421 }
422
toSMcache(double lat,double lon,double y30,double lon0,double * x,double * y)423 void toSMcache(double lat, double lon, double y30, double lon0, double *x, double *y)
424 {
425 double xlon = lon;
426
427 /* Make sure lon and lon0 are same phase */
428
429 if((lon * lon0 < 0.) && (fabs(lon - lon0) > 180.))
430 {
431 lon < 0.0 ? xlon += 360.0 : xlon -= 360.0;
432 }
433
434 const double z = WGS84_semimajor_axis_meters * mercator_k0;
435
436 *x = (xlon - lon0) * DEGREE * z;
437
438 // y =.5 ln( (1 + sin t) / (1 - sin t) )
439 const double s = sin(lat * DEGREE);
440 const double y3 = (.5 * log((1 + s) / (1 - s))) * z;
441
442 *y = y3 - y30;
443 }
444
445 void
fromSM(double x,double y,double lat0,double lon0,double * lat,double * lon)446 fromSM(double x, double y, double lat0, double lon0, double *lat, double *lon)
447 {
448 const double z = WGS84_semimajor_axis_meters * mercator_k0;
449
450 // lat = arcsin((e^2(y+y0) - 1)/(e^2(y+y0) + 1))
451 /*
452 double s0 = sin(lat0 * DEGREE);
453 double y0 = (.5 * log((1 + s0) / (1 - s0))) * z;
454
455 double e = exp(2 * (y0 + y) / z);
456 double e11 = (e - 1)/(e + 1);
457 double lat2 =(atan2(e11, sqrt(1 - e11 * e11))) / DEGREE;
458 */
459 // which is the same as....
460
461 const double s0 = sin(lat0 * DEGREE);
462 const double y0 = (.5 * log((1 + s0) / (1 - s0))) * z;
463
464 *lat = (2.0 * atan(exp((y0+y)/z)) - PI/2.) / DEGREE;
465
466 // lon = x + lon0
467 *lon = lon0 + (x / (DEGREE * z));
468 }
469
470 void
fromSMR(double x,double y,double lat0,double lon0,double axis_meters,double * lat,double * lon)471 fromSMR(double x, double y, double lat0, double lon0, double axis_meters, double *lat, double *lon)
472 {
473 const double s0 = sin(lat0 * DEGREE);
474 const double y0 = (.5 * log((1 + s0) / (1 - s0))) * axis_meters;
475
476 *lat = (2.0 * atan(exp((y0+y)/axis_meters)) - PI/2.) / DEGREE;
477
478 // lon = x + lon0
479 *lon = lon0 + (x / (DEGREE * axis_meters));
480 }
481
toSM_ECC(double lat,double lon,double lat0,double lon0,double * x,double * y)482 void toSM_ECC(double lat, double lon, double lat0, double lon0, double *x, double *y)
483 {
484 const double f = 1.0 / WGSinvf; // WGS84 ellipsoid flattening parameter
485 const double e2 = 2 * f - f * f; // eccentricity^2 .006700
486 const double e = sqrt(e2);
487
488 const double z = WGS84_semimajor_axis_meters * mercator_k0;
489
490 *x = (lon - lon0) * DEGREE * z;
491
492 // y =.5 ln( (1 + sin t) / (1 - sin t) )
493 const double s = sin(lat * DEGREE);
494 //const double y3 = (.5 * log((1 + s) / (1 - s))) * z;
495
496 const double s0 = sin(lat0 * DEGREE);
497 //const double y30 = (.5 * log((1 + s0) / (1 - s0))) * z;
498
499 //Add eccentricity terms
500
501 const double falsen = z *log(tan(PI/4 + lat0 * DEGREE / 2)*pow((1. - e * s0)/(1. + e * s0), e/2.));
502 const double test = z *log(tan(PI/4 + lat * DEGREE / 2)*pow((1. - e * s )/(1. + e * s ), e/2.));
503 *y = test - falsen;
504 }
505
fromSM_ECC(double x,double y,double lat0,double lon0,double * lat,double * lon)506 void fromSM_ECC(double x, double y, double lat0, double lon0, double *lat, double *lon)
507 {
508 const double f = 1.0 / WGSinvf; // WGS84 ellipsoid flattening parameter
509 const double es = 2 * f - f * f; // eccentricity^2 .006700
510 const double e = sqrt(es);
511
512 const double z = WGS84_semimajor_axis_meters * mercator_k0;
513
514 *lon = lon0 + (x / (DEGREE * z));
515
516 const double s0 = sin(lat0 * DEGREE);
517
518 const double falsen = z *log(tan(PI/4 + lat0 * DEGREE / 2)*pow((1. - e * s0)/(1. + e * s0), e/2.));
519 const double t = exp((y + falsen) / (z));
520 const double xi = (PI / 2.) - 2.0 * atan(t);
521
522 // Add eccentricity terms
523
524 double esf = (es/2. + (5*es*es/24.) + (es*es*es/12.) + (13.0 *es*es*es*es/360.)) * sin( 2 * xi);
525 esf += ((7.*es*es/48.) + (29.*es*es*es/240.) + (811.*es*es*es*es/11520.)) * sin (4. * xi);
526 esf += ((7.*es*es*es/120.) + (81*es*es*es*es/1120.) + (4279.*es*es*es*es/161280.)) * sin(8. * xi);
527
528
529 *lat = -(xi + esf) / DEGREE;
530
531 }
532
533 #define TOL 1e-10
534 #define CONV 1e-10
535 #define N_ITER 10
536 #define I_ITER 20
537 #define ITOL 1.e-12
538
539 void
toPOLY(double lat,double lon,double lat0,double lon0,double * x,double * y)540 toPOLY(double lat, double lon, double lat0, double lon0, double *x, double *y)
541 {
542 const double z = WGS84_semimajor_axis_meters * mercator_k0;
543
544 if (fabs((lat - lat0) * DEGREE) <= TOL)
545 {
546 *x = (lon - lon0) * DEGREE * z;
547 *y = 0.;
548
549 }
550 else
551 {
552 const double E = (lon - lon0) * DEGREE * sin(lat * DEGREE);
553 const double cot = 1. / tan(lat * DEGREE);
554 *x = sin(E) * cot;
555 *y = (lat * DEGREE) - (lat0 * DEGREE) + cot * (1. - cos(E));
556
557 *x *= z;
558 *y *= z;
559 }
560
561
562 /*
563 if (fabs(lp.phi) <= TOL) { xy.x = lp.lam; xy.y = P->ml0; }
564 else {
565 cot = 1. / tan(lp.phi);
566 xy.x = sin(E = lp.lam * sin(lp.phi)) * cot;
567 xy.y = lp.phi - P->phi0 + cot * (1. - cos(E));
568 }
569 */
570 }
571
572
573
574 void
fromPOLY(double x,double y,double lat0,double lon0,double * lat,double * lon)575 fromPOLY(double x, double y, double lat0, double lon0, double *lat, double *lon)
576 {
577 const double z = WGS84_semimajor_axis_meters * mercator_k0;
578
579 double yp = y - (lat0 * DEGREE * z);
580 if(fabs(yp) <= TOL)
581 {
582 *lon = lon0 + (x / (DEGREE * z));
583 *lat = lat0;
584 }
585 else
586 {
587 yp = y / z;
588 const double xp = x / z;
589
590 double lat3 = yp;
591 const double B = (xp * xp) + (yp * yp);
592 int i = N_ITER;
593 double dphi;
594 do {
595 double tp = tan(lat3);
596 dphi = (yp * (lat3 * tp + 1.) - lat3 - .5 * ( lat3 * lat3 + B) * tp) / ((lat3 - yp) / tp - 1.);
597 lat3 -= dphi;
598 } while (fabs(dphi) > CONV && --i);
599 if (! i)
600 {
601 *lon = 0.;
602 *lat = 0.;
603 }
604 else
605 {
606 *lon = asin(xp * tan(lat3)) / sin(lat3);
607 *lon /= DEGREE;
608 *lon += lon0;
609
610 *lat = lat3 / DEGREE;
611 }
612 }
613 }
614
615 /****************************************************************************/
616 /* Convert Lat/Lon <-> Transverse Mercator */
617 /****************************************************************************/
618
619
620
621 //converts lat/long to TM coords. Equations from USGS Bulletin 1532
622 //East Longitudes are positive, West longitudes are negative.
623 //North latitudes are positive, South latitudes are negative
624 //Lat and Long are in decimal degrees.
625 //Written by Chuck Gantz- chuck.gantz@globalstar.com
626 //Adapted for opencpn by David S. Register
627
toTM(float lat,float lon,float lat0,float lon0,double * x,double * y)628 void toTM(float lat, float lon, float lat0, float lon0, double *x, double *y)
629 {
630
631 // constants for WGS-84
632 const double f = 1.0 / WGSinvf; /* WGS84 ellipsoid flattening parameter */
633 const double a = WGS84_semimajor_axis_meters;
634 const double k0 = 1.; /* Scaling factor */
635
636 const double eccSquared = 2 * f - f * f;
637 const double eccPrimeSquared = (eccSquared)/(1-eccSquared);
638 const double LatRad = lat*DEGREE;
639 const double LongOriginRad = lon0 * DEGREE;
640 const double LongRad = lon*DEGREE;
641
642 const double N = a/sqrt(1-eccSquared*sin(LatRad)*sin(LatRad));
643 const double T = tan(LatRad)*tan(LatRad);
644 const double C = eccPrimeSquared*cos(LatRad)*cos(LatRad);
645 const double A = cos(LatRad)*(LongRad-LongOriginRad);
646
647 const double MM = a*((1 - eccSquared/4 - 3*eccSquared*eccSquared/64 - 5*eccSquared*eccSquared*eccSquared/256)*LatRad
648 - (3*eccSquared/8 + 3*eccSquared*eccSquared/32 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(2*LatRad)
649 + (15*eccSquared*eccSquared/256 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(4*LatRad)
650 - (35*eccSquared*eccSquared*eccSquared/3072)*sin(6*LatRad));
651
652 *x = (k0*N*(A+(1-T+C)*A*A*A/6 + (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120));
653
654 *y = (k0*(MM+N*tan(LatRad)*(A*A/2+(5-T+9*C+4*C*C)*A*A*A*A/24
655 + (61-58*T+T*T+600*C-330*eccPrimeSquared)*A*A*A*A*A*A/720)));
656
657 }
658
659 /* --------------------------------------------------------------------------------- */
660
661 //converts TM coords to lat/long. Equations from USGS Bulletin 1532
662 //East Longitudes are positive, West longitudes are negative.
663 //North latitudes are positive, South latitudes are negative
664 //Lat and Long are in decimal degrees
665 //Written by Chuck Gantz- chuck.gantz@globalstar.com
666 //Adapted for opencpn by David S. Register
667
fromTM(double x,double y,double lat0,double lon0,double * lat,double * lon)668 void fromTM (double x, double y, double lat0, double lon0, double *lat, double *lon)
669 {
670 const double rad2deg = 1./DEGREE;
671 // constants for WGS-84
672
673 const double f = 1.0 / WGSinvf; /* WGS84 ellipsoid flattening parameter */
674 const double a = WGS84_semimajor_axis_meters;
675 const double k0 = 1.; /* Scaling factor */
676
677 const double eccSquared = 2 * f - f * f;
678
679 const double eccPrimeSquared = (eccSquared)/(1-eccSquared);
680 const double e1 = (1.0 - sqrt(1.0 - eccSquared)) / (1.0 + sqrt(1.0 - eccSquared));
681
682 const double MM = y / k0;
683 const double mu = MM/(a*(1-eccSquared/4-3*eccSquared*eccSquared/64-5*eccSquared*eccSquared*eccSquared/256));
684
685 const double phi1Rad = mu + (3*e1/2-27*e1*e1*e1/32)*sin(2*mu)
686 + (21*e1*e1/16-55*e1*e1*e1*e1/32)*sin(4*mu)
687 +(151*e1*e1*e1/96)*sin(6*mu);
688
689 const double N1 = a/sqrt(1-eccSquared*sin(phi1Rad)*sin(phi1Rad));
690 const double T1 = tan(phi1Rad)*tan(phi1Rad);
691 const double C1 = eccPrimeSquared*cos(phi1Rad)*cos(phi1Rad);
692 const double R1 = a*(1-eccSquared)/pow(1-eccSquared*sin(phi1Rad)*sin(phi1Rad), 1.5);
693 const double D = x/(N1*k0);
694
695 *lat = phi1Rad - (N1*tan(phi1Rad)/R1)*(D*D/2-(5+3*T1+10*C1-4*C1*C1-9*eccPrimeSquared)*D*D*D*D/24
696 +(61+90*T1+298*C1+45*T1*T1-252*eccPrimeSquared-3*C1*C1)*D*D*D*D*D*D/720);
697 *lat = lat0 + (*lat * rad2deg);
698
699 *lon = (D-(1+2*T1+C1)*D*D*D/6+(5-2*C1+28*T1-3*C1*C1+8*eccPrimeSquared+24*T1*T1)
700 *D*D*D*D*D/120)/cos(phi1Rad);
701 *lon = lon0 + *lon * rad2deg;
702
703 }
704
705
706 /* orthographic, polar, stereographic, gnomonic and equirectangular projection routines, contributed by Sean D'Epagnier */
707 /****************************************************************************/
708 /* Convert Lat/Lon <-> Simple Polar */
709 /****************************************************************************/
cache_phi0(double lat0,double * sin_phi0,double * cos_phi0)710 void cache_phi0(double lat0, double *sin_phi0, double *cos_phi0)
711 {
712 double phi0 = lat0*DEGREE;
713 *sin_phi0 = sin(phi0);
714 *cos_phi0 = cos(phi0);
715 }
716
toORTHO(double lat,double lon,double sin_phi0,double cos_phi0,double lon0,double * x,double * y)717 void toORTHO(double lat, double lon, double sin_phi0, double cos_phi0, double lon0, double *x, double *y)
718 {
719 const double z = WGS84_semimajor_axis_meters * mercator_k0;
720
721 double xlon = lon;
722 /* Make sure lon and lon0 are same phase */
723 if((lon * lon0 < 0.) && (fabs(lon - lon0) > 180.))
724 lon < 0.0 ? xlon += 360.0 : xlon -= 360.0;
725
726 double theta = (xlon - lon0) * DEGREE;
727 double phi = lat * DEGREE;
728 double cos_phi = cos(phi);
729
730 double vy = sin(phi), vz = cos(theta)*cos_phi;
731
732 if(vy*sin_phi0 + vz*cos_phi0 < 0) { // on the far side of the earth
733 *x = *y = NAN;
734 return;
735 }
736
737 double vx = sin(theta)*cos_phi;
738 double vw = vy*cos_phi0 - vz*sin_phi0;
739
740 *x = vx*z;
741 *y = vw*z;
742 }
743
fromORTHO(double x,double y,double lat0,double lon0,double * lat,double * lon)744 void fromORTHO(double x, double y, double lat0, double lon0, double *lat, double *lon)
745 {
746 const double z = WGS84_semimajor_axis_meters * mercator_k0;
747
748 double vx = x / z;
749 double vw = y / z;
750
751 double phi0 = lat0 * DEGREE;
752 double d = 1 - vx*vx - vw*vw;
753
754 if(d < 0) { // position is outside of the earth
755 *lat = *lon = NAN;
756 return;
757 }
758
759 double sin_phi0 = sin(phi0), cos_phi0 = cos(phi0);
760 double vy = vw*cos_phi0 + sqrt(d)*sin_phi0;
761 double phi = asin(vy);
762
763 double vz = (vy*cos_phi0 - vw) / sin_phi0;
764 double theta = atan2(vx, vz);
765
766 *lat = phi / DEGREE;
767 *lon = theta / DEGREE + lon0;
768 }
769
toPOLARcache_e(double lat0)770 double toPOLARcache_e(double lat0)
771 {
772 double pole = lat0 > 0 ? 90 : -90;
773 return tan((pole - lat0) * DEGREE / 2);
774 }
775
toPOLAR(double lat,double lon,double e,double lat0,double lon0,double * x,double * y)776 void toPOLAR(double lat, double lon, double e, double lat0, double lon0, double *x, double *y)
777 {
778 const double z = WGS84_semimajor_axis_meters * mercator_k0;
779
780 double xlon = lon;
781 /* Make sure lon and lon0 are same phase */
782 if((lon * lon0 < 0.) && (fabs(lon - lon0) > 180.))
783 lon < 0.0 ? xlon += 360.0 : xlon -= 360.0;
784
785 double theta = (xlon - lon0) * DEGREE;
786 double pole = lat0 > 0 ? 90 : -90;
787
788 double d = tan((pole - lat) * DEGREE / 2);
789
790 *x = fabs(d)*sin(theta)*z;
791 *y = (e-d*cos(theta))*z;
792 }
793
794
fromPOLAR(double x,double y,double lat0,double lon0,double * lat,double * lon)795 void fromPOLAR(double x, double y, double lat0, double lon0, double *lat, double *lon)
796 {
797 const double z = WGS84_semimajor_axis_meters * mercator_k0;
798 double pole = lat0 > 0 ? 90 : -90;
799
800 double e = tan((pole - lat0) * DEGREE / 2);
801
802 double xn = x/z;
803 double yn = e - y/z;
804 double d = sqrt(xn*xn + yn*yn);
805 if(pole < 0) // south polar (negative root and correct branch from cosine)
806 d = -d, yn = -yn;
807
808 *lat = pole - atan(d) * 2 / DEGREE;
809
810 double theta = atan2(xn, yn);
811 *lon = theta / DEGREE + lon0;
812 }
813
toSTEREO1(double & u,double & v,double & w,double lat,double lon,double sin_phi0,double cos_phi0,double lon0)814 static inline void toSTEREO1(double &u, double &v, double &w, double lat, double lon,
815 double sin_phi0, double cos_phi0, double lon0)
816 {
817 double xlon = lon;
818 /* Make sure lon and lon0 are same phase */
819 if((lon * lon0 < 0.) && (fabs(lon - lon0) > 180.))
820 lon < 0.0 ? xlon += 360.0 : xlon -= 360.0;
821
822 double theta = (xlon - lon0) * DEGREE, phi = lat*DEGREE;
823 double cos_phi = cos(phi), v0 = sin(phi), w0 = cos(theta)*cos_phi;
824
825 u = sin(theta)*cos_phi;
826 v = cos_phi0*v0 - sin_phi0*w0;
827 w = sin_phi0*v0 + cos_phi0*w0;
828 }
829
fromSTEREO1(double * lat,double * lon,double lat0,double lon0,double u,double v,double w)830 static inline void fromSTEREO1(double *lat, double *lon, double lat0, double lon0,
831 double u, double v, double w)
832 {
833 double phi0 = lat0*DEGREE;
834 double v0 = sin(phi0)*w + cos(phi0)*v;
835 double w0 = cos(phi0)*w - sin(phi0)*v;
836 double phi = asin(v0);
837 double theta = atan2(u, w0);
838
839 *lat = phi / DEGREE;
840 *lon = theta / DEGREE + lon0;
841 }
842
toSTEREO(double lat,double lon,double sin_phi0,double cos_phi0,double lon0,double * x,double * y)843 void toSTEREO(double lat, double lon, double sin_phi0, double cos_phi0, double lon0, double *x, double *y)
844 {
845 const double z = WGS84_semimajor_axis_meters * mercator_k0;
846
847 double u, v, w;
848 toSTEREO1(u, v, w, lat, lon, sin_phi0, cos_phi0, lon0);
849
850 double t = 2/(w+1);
851 *x = u*t*z;
852 *y = v*t*z;
853 }
854
fromSTEREO(double x,double y,double lat0,double lon0,double * lat,double * lon)855 void fromSTEREO(double x, double y, double lat0, double lon0, double *lat, double *lon)
856 {
857 const double z = WGS84_semimajor_axis_meters * mercator_k0;
858
859 x /= z, y /= z;
860
861 double t = (x*x + y*y) / 4 + 1;
862
863 double u = x/t;
864 double v = y/t;
865 double w = 2/t - 1;
866
867 fromSTEREO1(lat, lon, lat0, lon0, u, v, w);
868 }
869
toGNO(double lat,double lon,double sin_phi0,double cos_phi0,double lon0,double * x,double * y)870 void toGNO(double lat, double lon, double sin_phi0, double cos_phi0, double lon0, double *x, double *y)
871 {
872 const double z = WGS84_semimajor_axis_meters * mercator_k0;
873
874 double u, v, w;
875 toSTEREO1(u, v, w, lat, lon, sin_phi0, cos_phi0, lon0);
876
877 if(w <= 0) {
878 *x = *y = NAN; // far side of world
879 return;
880 }
881
882 *x = u/w*z;
883 *y = v/w*z;
884 }
885
fromGNO(double x,double y,double lat0,double lon0,double * lat,double * lon)886 void fromGNO(double x, double y, double lat0, double lon0, double *lat, double *lon)
887 {
888 const double z = WGS84_semimajor_axis_meters * mercator_k0;
889
890 x /= z, y /= z;
891
892 double w = 1 / sqrt(x*x + y*y + 1);
893 double u = x*w;
894 double v = y*w;
895
896 fromSTEREO1(lat, lon, lat0, lon0, u, v, w);
897 }
898
toEQUIRECT(double lat,double lon,double lat0,double lon0,double * x,double * y)899 void toEQUIRECT(double lat, double lon, double lat0, double lon0, double *x, double *y)
900 {
901 const double z = WGS84_semimajor_axis_meters * mercator_k0;
902 double xlon = lon;
903 /* Make sure lon and lon0 are same phase */
904 if((lon * lon0 < 0.) && (fabs(lon - lon0) > 180.))
905 lon < 0.0 ? xlon += 360.0 : xlon -= 360.0;
906
907 *x = (xlon - lon0) * DEGREE * z;
908 *y = (lat - lat0) * DEGREE * z;
909 }
910
fromEQUIRECT(double x,double y,double lat0,double lon0,double * lat,double * lon)911 void fromEQUIRECT(double x, double y, double lat0, double lon0, double *lat, double *lon)
912 {
913 const double z = WGS84_semimajor_axis_meters * mercator_k0;
914
915 *lat = lat0 + (y / (DEGREE * z));
916 // if(fabs(*lat) > 90) *lat = NAN;
917 *lon = lon0 + (x / (DEGREE * z));
918 }
919
920
921 /* --------------------------------------------------------------------------------- *
922
923 *Molodensky
924 *In the listing below, the class GeodeticPosition has three members, lon, lat, and h.
925 *They are double-precision values indicating the longitude and latitude in radians,
926 * and height in meters above the ellipsoid.
927
928 * The source code in the listing below may be copied and reused without restriction,
929 * but it is offered AS-IS with NO WARRANTY.
930
931 * Adapted for opencpn by David S. Register
932
933 * --------------------------------------------------------------------------------- */
934
MolodenskyTransform(double lat,double lon,double * to_lat,double * to_lon,int from_datum_index,int to_datum_index)935 void MolodenskyTransform (double lat, double lon, double *to_lat, double *to_lon, int from_datum_index, int to_datum_index)
936 {
937 double dlat = 0;
938 double dlon = 0;
939
940 if( from_datum_index < nDatums){
941 const double from_lat = lat * DEGREE;
942 const double from_lon = lon * DEGREE;
943 const double from_f = 1.0 / gEllipsoid[gDatum[from_datum_index].ellipsoid].invf; // flattening
944 const double from_esq = 2 * from_f - from_f * from_f; // eccentricity^2
945 const double from_a = gEllipsoid[gDatum[from_datum_index].ellipsoid].a; // semimajor axis
946 const double dx = gDatum[from_datum_index].dx;
947 const double dy = gDatum[from_datum_index].dy;
948 const double dz = gDatum[from_datum_index].dz;
949 const double to_f = 1.0 / gEllipsoid[gDatum[to_datum_index].ellipsoid].invf; // flattening
950 const double to_a = gEllipsoid[gDatum[to_datum_index].ellipsoid].a; // semimajor axis
951 const double da = to_a - from_a;
952 const double df = to_f - from_f;
953 const double from_h = 0;
954
955
956 const double slat = sin (from_lat);
957 const double clat = cos (from_lat);
958 const double slon = sin (from_lon);
959 const double clon = cos (from_lon);
960 const double ssqlat = slat * slat;
961 const double adb = 1.0 / (1.0 - from_f); // "a divided by b"
962
963 const double rn = from_a / sqrt (1.0 - from_esq * ssqlat);
964 const double rm = from_a * (1. - from_esq) / pow ((1.0 - from_esq * ssqlat), 1.5);
965
966 dlat = (((((-dx * slat * clon - dy * slat * slon) + dz * clat)
967 + (da * ((rn * from_esq * slat * clat) / from_a)))
968 + (df * (rm * adb + rn / adb) * slat * clat)))
969 / (rm + from_h);
970
971 dlon = (-dx * slon + dy * clon) / ((rn + from_h) * clat);
972
973 }
974
975 *to_lon = lon + dlon/DEGREE;
976 *to_lat = lat + dlat/DEGREE;
977 //
978 return;
979 }
980
981
982 /* --------------------------------------------------------------------------------- */
983 /*
984 Geodesic Forward and Reverse calculation functions
985 Abstracted and adapted from PROJ-4.5.0 by David S.Register
986
987 Original source code contains the following license:
988
989 Copyright (c) 2000, Frank Warmerdam
990
991 Permission is hereby granted, free of charge, to any person obtaining a
992 copy of this software and associated documentation files (the "Software"),
993 to deal in the Software without restriction, including without limitation
994 the rights to use, copy, modify, merge, publish, distribute, sublicense,
995 and/or sell copies of the Software, and to permit persons to whom the
996 Software is furnished to do so, subject to the following conditions:
997
998 The above copyright notice and this permission notice shall be included
999 in all copies or substantial portions of the Software.
1000
1001 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
1002 OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
1003 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
1004 THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
1005 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
1006 FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
1007 DEALINGS IN THE SOFTWARE.
1008 */
1009 /* --------------------------------------------------------------------------------- */
1010
1011
1012
1013
1014 #define DTOL 1e-12
1015
1016 #define HALFPI 1.5707963267948966
1017 #define SPI 3.14159265359
1018 #define TWOPI 6.2831853071795864769
1019 #define ONEPI 3.14159265358979323846
1020 #define MERI_TOL 1e-9
1021
adjlon(double lon)1022 double adjlon (double lon) {
1023 if (fabs(lon) <= SPI) return( lon );
1024 lon += ONEPI; /* adjust to 0..2pi rad */
1025 lon -= TWOPI * floor(lon / TWOPI); /* remove integral # of 'revolutions'*/
1026 lon -= ONEPI; /* adjust back to -pi..pi rad */
1027 return( lon );
1028 }
1029
1030
1031
1032 /* --------------------------------------------------------------------------------- */
1033 /*
1034 // Given the lat/long of starting point, and traveling a specified distance,
1035 // at an initial bearing, calculates the lat/long of the resulting location.
1036 // using sphere earth model.
1037 */
1038 /* --------------------------------------------------------------------------------- */
ll_gc_ll(double lat,double lon,double brg,double dist,double * dlat,double * dlon)1039 void ll_gc_ll(double lat, double lon, double brg, double dist, double *dlat, double *dlon)
1040 {
1041
1042 double th1,costh1,sinth1,sina12,cosa12,M,N,c1,c2,D,P,s1;
1043 int merid, signS;
1044
1045 /* Input/Output from geodesic functions */
1046 double al12; /* Forward azimuth */
1047 double al21; /* Back azimuth */
1048 double geod_S; /* Distance */
1049 double phi1, lam1, phi2, lam2;
1050
1051 int ellipse;
1052 double geod_f;
1053 double geod_a;
1054 double es, onef, f, f4;
1055
1056 /* Setup the static parameters */
1057 phi1 = lat * DEGREE; /* Initial Position */
1058 lam1 = lon * DEGREE;
1059 al12 = brg * DEGREE; /* Forward azimuth */
1060 geod_S = dist * 1852.0; /* Distance */
1061
1062
1063 //void geod_pre(struct georef_state *state)
1064 {
1065
1066 /* Stuff the WGS84 projection parameters as necessary
1067 * To avoid having to include <geodesic,h>
1068 */
1069 ellipse = 1;
1070 f = 1.0 / WGSinvf; /* WGS84 ellipsoid flattening parameter */
1071 geod_a = WGS84_semimajor_axis_meters;
1072
1073 es = 2 * f - f * f;
1074 onef = sqrt(1. - es);
1075 geod_f = 1 - onef;
1076 f4 = geod_f/4;
1077
1078 al12 = adjlon(al12); /* reduce to +- 0-PI */
1079 signS = fabs(al12) > HALFPI ? 1 : 0;
1080 th1 = ellipse ? atan(onef * tan(phi1)) : phi1;
1081 costh1 = cos(th1);
1082 sinth1 = sin(th1);
1083 if ((merid = fabs(sina12 = sin(al12)) < MERI_TOL)) {
1084 sina12 = 0.;
1085 cosa12 = fabs(al12) < HALFPI ? 1. : -1.;
1086 M = 0.;
1087 } else {
1088 cosa12 = cos(al12);
1089 M = costh1 * sina12;
1090 }
1091 N = costh1 * cosa12;
1092 if (ellipse) {
1093 if (merid) {
1094 c1 = 0.;
1095 c2 = f4;
1096 D = 1. - c2;
1097 D *= D;
1098 P = c2 / D;
1099 } else {
1100 c1 = geod_f * M;
1101 c2 = f4 * (1. - M * M);
1102 D = (1. - c2)*(1. - c2 - c1 * M);
1103 P = (1. + .5 * c1 * M) * c2 / D;
1104 }
1105 }
1106 if (merid) s1 = HALFPI - th1;
1107 else {
1108 s1 = (fabs(M) >= 1.) ? 0. : acos(M);
1109 s1 = sinth1 / sin(s1);
1110 s1 = (fabs(s1) >= 1.) ? 0. : acos(s1);
1111 }
1112 }
1113
1114
1115 //void geod_for(struct georef_state *state)
1116 {
1117 double d,sind,u,V,X,ds,cosds,sinds,ss,de;
1118
1119 ss = 0.;
1120
1121 if (ellipse) {
1122 d = geod_S / (D * geod_a);
1123 if (signS) d = -d;
1124 u = 2. * (s1 - d);
1125 V = cos(u + d);
1126 X = c2 * c2 * (sind = sin(d)) * cos(d) * (2. * V * V - 1.);
1127 ds = d + X - 2. * P * V * (1. - 2. * P * cos(u)) * sind;
1128 ss = s1 + s1 - ds;
1129 } else {
1130 ds = geod_S / geod_a;
1131 if (signS) ds = - ds;
1132 }
1133 cosds = cos(ds);
1134 sinds = sin(ds);
1135 if (signS) sinds = - sinds;
1136 al21 = N * cosds - sinth1 * sinds;
1137 if (merid) {
1138 phi2 = atan( tan(HALFPI + s1 - ds) / onef);
1139 if (al21 > 0.) {
1140 al21 = PI;
1141 if (signS)
1142 de = PI;
1143 else {
1144 phi2 = - phi2;
1145 de = 0.;
1146 }
1147 } else {
1148 al21 = 0.;
1149 if (signS) {
1150 phi2 = - phi2;
1151 de = 0;
1152 } else
1153 de = PI;
1154 }
1155 } else {
1156 al21 = atan(M / al21);
1157 if (al21 > 0)
1158 al21 += PI;
1159 if (al12 < 0.)
1160 al21 -= PI;
1161 al21 = adjlon(al21);
1162 phi2 = atan(-(sinth1 * cosds + N * sinds) * sin(al21) /
1163 (ellipse ? onef * M : M));
1164 de = atan2(sinds * sina12 ,
1165 (costh1 * cosds - sinth1 * sinds * cosa12));
1166 if (ellipse){
1167 if (signS)
1168 de += c1 * ((1. - c2) * ds +
1169 c2 * sinds * cos(ss));
1170 else
1171 de -= c1 * ((1. - c2) * ds -
1172 c2 * sinds * cos(ss));
1173 }
1174 }
1175 lam2 = adjlon( lam1 + de );
1176 }
1177
1178
1179 *dlat = phi2 / DEGREE;
1180 *dlon = lam2 / DEGREE;
1181 }
1182
ll_gc_ll_reverse(double lat1,double lon1,double lat2,double lon2,double * bearing,double * dist)1183 void ll_gc_ll_reverse(double lat1, double lon1, double lat2, double lon2,
1184 double *bearing, double *dist)
1185 {
1186 // For small distances do an ordinary mercator calc. (To prevent return of nan's )
1187 if ((fabs(lon2-lon1)<0.1) && (fabs(lat2-lat1)<0.1))
1188 {
1189 DistanceBearingMercator( lat2, lon2, lat1, lon1, bearing, dist);
1190 return;
1191 }
1192 else{
1193 /* Input/Output from geodesic functions */
1194 double al12; /* Forward azimuth */
1195 double al21; /* Back azimuth */
1196 double geod_S; /* Distance */
1197 double phi1, lam1, phi2, lam2;
1198
1199 int ellipse;
1200 double geod_f;
1201 double geod_a;
1202 double es, onef, f, f64, f2, f4;
1203
1204 /* Setup the static parameters */
1205 phi1 = lat1 * DEGREE; /* Initial Position */
1206 lam1 = lon1 * DEGREE;
1207 phi2 = lat2 * DEGREE;
1208 lam2 = lon2 * DEGREE;
1209
1210 //void geod_inv(struct georef_state *state)
1211 {
1212 double th1,th2,thm,dthm,dlamm,dlam,sindlamm,costhm,sinthm,cosdthm,
1213 sindthm,L,E,cosd,d,X,Y,T,sind,tandlammp,u,v,D,A,B;
1214
1215
1216 /* Stuff the WGS84 projection parameters as necessary
1217 * To avoid having to include <geodesic,h>
1218 */
1219
1220 ellipse = 1;
1221 f = 1.0 / WGSinvf; /* WGS84 ellipsoid flattening parameter */
1222 geod_a = WGS84_semimajor_axis_meters;
1223
1224 es = 2 * f - f * f;
1225 onef = sqrt(1. - es);
1226 geod_f = 1 - onef;
1227 f2 = geod_f/2;
1228 f4 = geod_f/4;
1229 f64 = geod_f*geod_f/64;
1230
1231
1232 if (ellipse) {
1233 th1 = atan(onef * tan(phi1));
1234 th2 = atan(onef * tan(phi2));
1235 } else {
1236 th1 = phi1;
1237 th2 = phi2;
1238 }
1239 thm = .5 * (th1 + th2);
1240 dthm = .5 * (th2 - th1);
1241 dlamm = .5 * ( dlam = adjlon(lam2 - lam1) );
1242 if (fabs(dlam) < DTOL && fabs(dthm) < DTOL) {
1243 al12 = al21 = geod_S = 0.;
1244 if(bearing)
1245 *bearing = 0.;
1246 if(dist)
1247 *dist = 0.;
1248 return;
1249 }
1250 sindlamm = sin(dlamm);
1251 costhm = cos(thm); sinthm = sin(thm);
1252 cosdthm = cos(dthm); sindthm = sin(dthm);
1253 L = sindthm * sindthm + (cosdthm * cosdthm - sinthm * sinthm)
1254 * sindlamm * sindlamm;
1255 d = acos(cosd = 1 - L - L);
1256 if (ellipse) {
1257 E = cosd + cosd;
1258 sind = sin( d );
1259 Y = sinthm * cosdthm;
1260 Y *= (Y + Y) / (1. - L);
1261 T = sindthm * costhm;
1262 T *= (T + T) / L;
1263 X = Y + T;
1264 Y -= T;
1265 T = d / sind;
1266 D = 4. * T * T;
1267 A = D * E;
1268 B = D + D;
1269 geod_S = geod_a * sind * (T - f4 * (T * X - Y) +
1270 f64 * (X * (A + (T - .5 * (A - E)) * X) -
1271 Y * (B + E * Y) + D * X * Y));
1272 tandlammp = tan(.5 * (dlam - .25 * (Y + Y - E * (4. - X)) *
1273 (f2 * T + f64 * (32. * T - (20. * T - A)
1274 * X - (B + 4.) * Y)) * tan(dlam)));
1275 } else {
1276 geod_S = geod_a * d;
1277 tandlammp = tan(dlamm);
1278 }
1279 u = atan2(sindthm , (tandlammp * costhm));
1280 v = atan2(cosdthm , (tandlammp * sinthm));
1281 al12 = adjlon(TWOPI + v - u);
1282 al21 = adjlon(TWOPI - v - u);
1283 if(al12 < 0)
1284 al12 += 2*PI;
1285 if(bearing)
1286 *bearing = al12 / DEGREE;
1287 if(dist)
1288 *dist = geod_S / 1852.0;
1289 }
1290 }
1291 }
1292
PositionBearingDistanceMercator(double lat,double lon,double brg,double dist,double * dlat,double * dlon)1293 void PositionBearingDistanceMercator(double lat, double lon, double brg, double dist,
1294 double *dlat, double *dlon)
1295 {
1296 ll_gc_ll(lat, lon, brg, dist, dlat, dlon);
1297 }
1298
DistLoxodrome(double slat,double slon,double dlat,double dlon)1299 double DistLoxodrome(double slat, double slon, double dlat, double dlon)
1300 {
1301 double dist = 60 * sqrt( pow(slat - dlat, 2) + pow( (slon - dlon) * cos( (slat + dlat)/2 * DEGREE), 2 ) );
1302 return dist;
1303 }
1304
1305 /* --------------------------------------------------------------------------------- */
1306 /*
1307 // Given the lat/long of starting point and ending point,
1308 // calculates the distance (in Nm) along a geodesic curve, using sphere earth model.
1309 */
1310 /* --------------------------------------------------------------------------------- */
1311
DistGreatCircle(double slat,double slon,double dlat,double dlon)1312 double DistGreatCircle(double slat, double slon, double dlat, double dlon)
1313 {
1314
1315 double d5;
1316 d5 = DistLoxodrome(slat, slon, dlat, dlon);
1317 if ( d5 < 10 ) // Miles
1318 return d5;
1319
1320 /* Input/Output from geodesic functions */
1321 //double al12; /* Forward azimuth */
1322 //double al21; /* Back azimuth */
1323 double geod_S; /* Distance */
1324 double phi1, lam1, phi2, lam2;
1325
1326 int ellipse;
1327 double geod_f;
1328 double geod_a;
1329 double es, onef, f, f64, f4;
1330
1331 phi1 = slat * DEGREE;
1332 lam1 = slon * DEGREE;
1333 phi2 = dlat * DEGREE;
1334 lam2 = dlon * DEGREE;
1335
1336 //void geod_inv(struct georef_state *state)
1337 {
1338 double th1,th2,thm,dthm,dlamm,dlam,sindlamm,costhm,sinthm,cosdthm,
1339 sindthm,L,E,cosd,d,X,Y,T,sind,D,A,B;
1340 //double tandlammp,u,v;
1341
1342
1343 /* Stuff the WGS84 projection parameters as necessary
1344 * To avoid having to include <geodesic,h>
1345 */
1346
1347 ellipse = 0;
1348 f = 1.0 / WGSinvf; /* WGS84 ellipsoid flattening parameter */
1349 geod_a = WGS84_semimajor_axis_meters;
1350
1351 es = 2 * f - f * f;
1352 onef = sqrt(1. - es);
1353 geod_f = 1 - onef;
1354 //f2 = geod_f/2;
1355 f4 = geod_f/4;
1356 f64 = geod_f*geod_f/64;
1357
1358
1359 if (ellipse) {
1360 th1 = atan(onef * tan(phi1));
1361 th2 = atan(onef * tan(phi2));
1362 } else {
1363 th1 = phi1;
1364 th2 = phi2;
1365 }
1366 thm = .5 * (th1 + th2);
1367 dthm = .5 * (th2 - th1);
1368 dlamm = .5 * ( dlam = adjlon(lam2 - lam1) );
1369 if (fabs(dlam) < DTOL && fabs(dthm) < DTOL) {
1370 return 0.0;
1371 }
1372 sindlamm = sin(dlamm);
1373 costhm = cos(thm); sinthm = sin(thm);
1374 cosdthm = cos(dthm); sindthm = sin(dthm);
1375 L = sindthm * sindthm + (cosdthm * cosdthm - sinthm * sinthm)
1376 * sindlamm * sindlamm;
1377 d = acos(cosd = 1 - L - L);
1378
1379 if (ellipse) {
1380 wxASSERT( d != 0.0 );
1381 E = cosd + cosd;
1382 sind = sin( d );
1383 Y = sinthm * cosdthm;
1384 Y *= (Y + Y) / (1. - L);
1385 T = sindthm * costhm;
1386 T *= (T + T) / L;
1387 X = Y + T;
1388 Y -= T;
1389 T = d / sind;
1390 D = 4. * T * T;
1391 A = D * E;
1392 B = D + D;
1393 geod_S = geod_a * sind * (T - f4 * (T * X - Y) +
1394 f64 * (X * (A + (T - .5 * (A - E)) * X) -
1395 Y * (B + E * Y) + D * X * Y));
1396 // tandlammp = tan(.5 * (dlam - .25 * (Y + Y - E * (4. - X)) *
1397 // (f2 * T + f64 * (32. * T - (20. * T - A)
1398 // * X - (B + 4.) * Y)) * tan(dlam)));
1399 } else {
1400 geod_S = geod_a * d;
1401 // tandlammp = tan(dlamm);
1402 }
1403 // u = atan2(sindthm , (tandlammp * costhm));
1404 // v = atan2(cosdthm , (tandlammp * sinthm));
1405 // al12 = adjlon(TWOPI + v - u);
1406 // al21 = adjlon(TWOPI - v - u);
1407 }
1408
1409 d5 = geod_S / 1852.0;
1410
1411 return d5;
1412 }
1413
1414
DistanceBearingMercator(double lat1,double lon1,double lat0,double lon0,double * brg,double * dist)1415 void DistanceBearingMercator(double lat1, double lon1, double lat0, double lon0, double *brg, double *dist)
1416 {
1417 //Calculate bearing and distance between two points
1418 double latm = (lat0 + lat1)/2 * DEGREE; //median of latitude
1419 double delta_lat = (lat1 - lat0);
1420 double delta_lon = (lon1 - lon0);
1421 double ex_lat0, ex_lat1;
1422 double bearing, distance;
1423 //make sure we calc the shortest route, even if this across the date line.
1424 if(delta_lon < -180) delta_lon += 360;
1425 if(delta_lon > 180) delta_lon -= 360;
1426
1427 if (delta_lon == 0) bearing = .0; //delta lon = 0 so course is due N or S
1428 else if ( fabs(delta_lat) != .0 )
1429 bearing = atan( delta_lon * cos(latm) / delta_lat );
1430 else
1431 bearing = PI / 2; //delta lat = 0 so course must be E or W
1432
1433 distance = sqrt( pow(delta_lat,2) + pow(delta_lon*cos(latm),2) ) ;
1434
1435 if (distance > 0.01745) // > 1 degree we use exaggerated latitude to be more exact
1436 {
1437 if ( delta_lat != 0. ){
1438 ex_lat0=10800/PI * log( tan( PI/4 + lat0*DEGREE/2 ));
1439 ex_lat1=10800/PI * log( tan( PI/4 + lat1*DEGREE/2 ));
1440 bearing = atan( delta_lon*60 / (ex_lat1-ex_lat0));
1441 distance = fabs(delta_lat / cos( bearing ));
1442 }
1443 else{
1444 bearing = PI / 2;
1445 }
1446 }
1447
1448 bearing = fabs( bearing );
1449 if ( lat1 > lat0){
1450 if ( delta_lon < 0)
1451 bearing = 2*PI - bearing;} //NW
1452 else{
1453 if ( delta_lon > 0)
1454 bearing = PI - bearing; //SE
1455 else
1456 bearing = PI + bearing;} //SW
1457
1458 if(brg)
1459 *brg = bearing * RADIAN; //in degrees
1460 if(dist)
1461 *dist = distance * 60; //in NM
1462 }
1463
1464
1465 /* --------------------------------------------------------------------------------- */
1466 /*
1467 * lmfit
1468 *
1469 * Solves or minimizes the sum of squares of m nonlinear
1470 * functions of n variables.
1471 *
1472 * From public domain Fortran version
1473 * of Argonne National Laboratories MINPACK
1474 * argonne national laboratory. minpack project. march 1980.
1475 * burton s. garbow, kenneth e. hillstrom, jorge j. more
1476 * C translation by Steve Moshier
1477 * Joachim Wuttke converted the source into C++ compatible ANSI style
1478 * and provided a simplified interface
1479 */
1480
1481
1482 //#include "lmmin.h" // all moved to georef.h
1483 #define _LMDIF
1484
1485 ///=================================================================================
1486 /// Customized section for openCPN georeferencing
1487
my_fit_function(double tx,double ty,int n_par,double * p)1488 double my_fit_function( double tx, double ty, int n_par, double* p )
1489 {
1490
1491 double ret = p[0] + p[1]*tx;
1492
1493 if(n_par > 2)
1494 ret += p[2]*ty;
1495 if(n_par > 3)
1496 {
1497 ret += p[3]*tx*tx;
1498 ret += p[4]*tx*ty;
1499 ret += p[5]*ty*ty;
1500 }
1501 if(n_par > 6)
1502 {
1503 ret += p[6]*tx*tx*tx;
1504 ret += p[7]*tx*tx*ty;
1505 ret += p[8]*tx*ty*ty;
1506 ret += p[9]*ty*ty*ty;
1507 }
1508
1509 return ret;
1510 }
1511
Georef_Calculate_Coefficients_Onedir(int n_points,int n_par,double * tx,double * ty,double * y,double * p,double hintp0,double hintp1,double hintp2)1512 int Georef_Calculate_Coefficients_Onedir(int n_points, int n_par, double *tx, double *ty, double *y, double *p,
1513 double hintp0, double hintp1, double hintp2)
1514 /*
1515 n_points : number of sample points
1516 n_par : 3, 6, or 10, 6 is probably good
1517 tx: sample data independent variable 1
1518 ty: sample data independent variable 2
1519 y: sample data dependent result
1520 p: curve fit result coefficients
1521 */
1522 {
1523
1524 int i;
1525 lm_control_type control;
1526 lm_data_type data;
1527
1528 lm_initialize_control( &control );
1529
1530
1531 for(i=0 ; i<12 ; i++)
1532 p[i] = 0.;
1533
1534 // memset(p, 0, 12 * sizeof(double));
1535
1536 // Insert hints
1537 p[0] = hintp0;
1538 p[1] = hintp1;
1539 p[2] = hintp2;
1540
1541 data.user_func = my_fit_function;
1542 data.user_tx = tx;
1543 data.user_ty = ty;
1544 data.user_y = y;
1545 data.n_par = n_par;
1546 data.print_flag = 0;
1547
1548 // perform the fit:
1549
1550 lm_minimize( n_points, n_par, p, lm_evaluate_default, lm_print_default,
1551 &data, &control );
1552
1553 // print results:
1554 // printf( "status: %s after %d evaluations\n",
1555 // lm_infmsg[control.info], control.nfev );
1556
1557 // Control.info results [1,2,3] are success, other failure
1558 return control.info;
1559 }
1560
Georef_Calculate_Coefficients(struct GeoRef * cp,int nlin_lon)1561 int Georef_Calculate_Coefficients(struct GeoRef *cp, int nlin_lon)
1562 {
1563 // Zero out the points
1564 for(int i = 0; i < 10; ++i)
1565 cp->pwx[i] = cp->pwy[i] = cp->wpx[i] = cp->wpy[i] = 0.0;
1566
1567 int mp = 3;
1568
1569 switch (cp->order)
1570 {
1571 case 1:
1572 mp = 3;
1573 break;
1574 case 2:
1575 mp = 6;
1576 break;
1577 case 3:
1578 mp = 10;
1579 break;
1580 default:
1581 mp = 3;
1582 break;
1583 }
1584
1585 const int mp_lat = mp;
1586
1587 // Force linear fit for longitude if nlin_lon > 0
1588 const int mp_lon = nlin_lon ? 2 : mp;
1589
1590 // Make a dummy double array
1591 std::vector<double> pnull(cp->count, 1.0);
1592
1593 // pixel(tx,ty) to (lat,lon)
1594 // Calculate and use a linear equation for p[0..2] to hint the solver
1595
1596 int r1 = Georef_Calculate_Coefficients_Onedir(cp->count, mp_lon, cp->tx, cp->ty, cp->lon, cp->pwx,
1597 cp->lonmin - (cp->txmin * (cp->lonmax - cp->lonmin) /(cp->txmax - cp->txmin)),
1598 (cp->lonmax - cp->lonmin) /(cp->txmax - cp->txmin),
1599 0.);
1600
1601 // if blin_lon > 0, force cross terms in latitude equation coefficients to be zero by making lat not dependent on tx,
1602 double *px = nlin_lon ? &pnull[0] : cp->tx;
1603
1604 int r2 = Georef_Calculate_Coefficients_Onedir(cp->count, mp_lat, px, cp->ty, cp->lat, cp->pwy,
1605 cp->latmin - (cp->tymin * (cp->latmax - cp->latmin) /(cp->tymax - cp->tymin)),
1606 0.,
1607 (cp->latmax - cp->latmin) /(cp->tymax - cp->tymin));
1608
1609 // (lat,lon) to pixel(tx,ty)
1610 // Calculate and use a linear equation for p[0..2] to hint the solver
1611
1612 int r3 = Georef_Calculate_Coefficients_Onedir(cp->count, mp_lon, cp->lon, cp->lat, cp->tx, cp->wpx,
1613 cp->txmin - ((cp->txmax - cp->txmin) * cp->lonmin / (cp->lonmax - cp->lonmin)),
1614 (cp->txmax - cp->txmin) / (cp->lonmax - cp->lonmin),
1615 0.0);
1616
1617 // if blin_lon > 0, force cross terms in latitude equation coefficients to be zero by making ty not dependent on lon,
1618 px = nlin_lon ? &pnull[0] : cp->lon;
1619
1620 int r4 = Georef_Calculate_Coefficients_Onedir(cp->count, mp_lat, &pnull[0]/*cp->lon*/, cp->lat, cp->ty, cp->wpy,
1621 cp->tymin - ((cp->tymax - cp->tymin) * cp->latmin / (cp->latmax - cp->latmin)),
1622 0.0,
1623 (cp->tymax - cp->tymin) / (cp->latmax - cp->latmin));
1624
1625 if((r1) && (r1 < 4) && (r2) && (r2 < 4) && (r3) && (r3 < 4) && (r4) && (r4 < 4))
1626 return 0;
1627
1628 return 1;
1629 }
1630
1631
1632
Georef_Calculate_Coefficients_Proj(struct GeoRef * cp)1633 int Georef_Calculate_Coefficients_Proj(struct GeoRef *cp)
1634 {
1635 int r1, r2, r3, r4;
1636 int mp;
1637
1638 // Zero out the points
1639 cp->pwx[6] = cp->pwy[6] = cp->wpx[6] = cp->wpy[6] = 0.;
1640 cp->pwx[7] = cp->pwy[7] = cp->wpx[7] = cp->wpy[7] = 0.;
1641 cp->pwx[8] = cp->pwy[8] = cp->wpx[8] = cp->wpy[8] = 0.;
1642 cp->pwx[9] = cp->pwy[9] = cp->wpx[9] = cp->wpy[9] = 0.;
1643 cp->pwx[3] = cp->pwy[3] = cp->wpx[3] = cp->wpy[3] = 0.;
1644 cp->pwx[4] = cp->pwy[4] = cp->wpx[4] = cp->wpy[4] = 0.;
1645 cp->pwx[5] = cp->pwy[5] = cp->wpx[5] = cp->wpy[5] = 0.;
1646 cp->pwx[0] = cp->pwy[0] = cp->wpx[0] = cp->wpy[0] = 0.;
1647 cp->pwx[1] = cp->pwy[1] = cp->wpx[1] = cp->wpy[1] = 0.;
1648 cp->pwx[2] = cp->pwy[2] = cp->wpx[2] = cp->wpy[2] = 0.;
1649
1650 mp = 3;
1651
1652
1653 // pixel(tx,ty) to (northing,easting)
1654 // Calculate and use a linear equation for p[0..2] to hint the solver
1655
1656 r1 = Georef_Calculate_Coefficients_Onedir(cp->count, mp, cp->tx, cp->ty, cp->lon, cp->pwx,
1657 cp->lonmin - (cp->txmin * (cp->lonmax - cp->lonmin) /(cp->txmax - cp->txmin)),
1658 (cp->lonmax - cp->lonmin) /(cp->txmax - cp->txmin),
1659 0.);
1660
1661
1662
1663 r2 = Georef_Calculate_Coefficients_Onedir(cp->count, mp, cp->tx, cp->ty, cp->lat, cp->pwy,
1664 cp->latmin - (cp->tymin * (cp->latmax - cp->latmin) /(cp->tymax - cp->tymin)),
1665 0.,
1666 (cp->latmax - cp->latmin) /(cp->tymax - cp->tymin));
1667
1668 // (northing/easting) to pixel(tx,ty)
1669 // Calculate and use a linear equation for p[0..2] to hint the solver
1670
1671 r3 = Georef_Calculate_Coefficients_Onedir(cp->count, mp, cp->lon, cp->lat, cp->tx, cp->wpx,
1672 cp->txmin - ((cp->txmax - cp->txmin) * cp->lonmin / (cp->lonmax - cp->lonmin)),
1673 (cp->txmax - cp->txmin) / (cp->lonmax - cp->lonmin),
1674 0.0);
1675
1676 r4 = Georef_Calculate_Coefficients_Onedir(cp->count, mp, cp->lon, cp->lat, cp->ty, cp->wpy,
1677 cp->tymin - ((cp->tymax - cp->tymin) * cp->latmin / (cp->latmax - cp->latmin)),
1678 0.0,
1679 (cp->tymax - cp->tymin) / (cp->latmax - cp->latmin));
1680
1681
1682 if((r1) && (r1 < 4) && (r2) && (r2 < 4) && (r3) && (r3 < 4) && (r4) && (r4 < 4))
1683 return 0;
1684 else
1685 return 1;
1686
1687 }
1688
1689
1690 /*
1691 * This file contains default implementation of the evaluate and printout
1692 * routines. In most cases, customization of lmfit can be done by modifying
1693 * these two routines. Either modify them here, or copy and rename them.
1694 */
1695
lm_evaluate_default(double * par,int m_dat,double * fvec,void * data,int * info)1696 void lm_evaluate_default( double* par, int m_dat, double* fvec,
1697 void *data, int *info )
1698 /*
1699 * par is an input array. At the end of the minimization, it contains
1700 * the approximate solution vector.
1701 *
1702 * m_dat is a positive integer input variable set to the number
1703 * of functions.
1704 *
1705 * fvec is an output array of length m_dat which contains the function
1706 * values the square sum of which ought to be minimized.
1707 *
1708 * data is a read-only pointer to lm_data_type, as specified by lmuse.h.
1709 *
1710 * info is an integer output variable. If set to a negative value, the
1711 * minimization procedure will stop.
1712 */
1713 {
1714 lm_data_type *mydata = (lm_data_type*)data;
1715
1716 for (int i=0; i<m_dat; i++)
1717 {
1718 fvec[i] = mydata->user_y[i] - mydata->user_func( mydata->user_tx[i], mydata->user_ty[i], mydata->n_par, par);
1719 }
1720 *info = *info; /* to prevent a 'unused variable' warning */
1721 /* if <parameters drifted away> { *info = -1; } */
1722 }
1723
lm_print_default(int n_par,double * par,int m_dat,double * fvec,void * data,int iflag,int iter,int nfev)1724 void lm_print_default( int n_par, double* par, int m_dat, double* fvec,
1725 void *data, int iflag, int iter, int nfev )
1726 /*
1727 * data : for soft control of printout behaviour, add control
1728 * variables to the data struct
1729 * iflag : 0 (init) 1 (outer loop) 2(inner loop) -1(terminated)
1730 * iter : outer loop counter
1731 * nfev : number of calls to *evaluate
1732 */
1733 {
1734 lm_data_type *mydata = (lm_data_type*)data;
1735
1736 if(mydata->print_flag)
1737 {
1738 if (iflag==2) {
1739 printf ("trying step in gradient direction\n");
1740 } else if (iflag==1) {
1741 printf ("determining gradient (iteration %d)\n", iter);
1742 } else if (iflag==0) {
1743 printf ("starting minimization\n");
1744 } else if (iflag==-1) {
1745 printf ("terminated after %d evaluations\n", nfev);
1746 }
1747
1748 printf( " par: " );
1749 for(int i=0; i<n_par; ++i )
1750 printf( " %12g", par[i] );
1751 printf ( " => norm: %12g\n", lm_enorm( m_dat, fvec ) );
1752
1753 if ( iflag == -1 ) {
1754 printf( " fitting data as follows:\n" );
1755 for(int i=0; i<m_dat; ++i ) {
1756 const double tx = (mydata->user_tx)[i];
1757 const double ty = (mydata->user_ty)[i];
1758 const double y = (mydata->user_y)[i];
1759 const double f = mydata->user_func( tx, ty, mydata->n_par, par );
1760 printf( " tx[%2d]=%8g ty[%2d]=%8g y=%12g fit=%12g residue=%12g\n",
1761 i, tx, i, ty, y, f, y-f );
1762 }
1763 }
1764 } // if print_flag
1765 }
1766
1767
1768
1769
1770
1771 ///=================================================================================
1772
1773 /* *********************** high-level interface **************************** */
1774
1775
lm_initialize_control(lm_control_type * control)1776 void lm_initialize_control( lm_control_type *control )
1777 {
1778 control->maxcall = 100;
1779 control->epsilon = 1.e-10; //1.e-14;
1780 control->stepbound = 100; //100.;
1781 control->ftol = 1.e-14;
1782 control->xtol = 1.e-14;
1783 control->gtol = 1.e-14;
1784 }
1785
lm_minimize(int m_dat,int n_par,double * par,lm_evaluate_ftype * evaluate,lm_print_ftype * printout,void * data,lm_control_type * control)1786 void lm_minimize( int m_dat, int n_par, double* par,
1787 lm_evaluate_ftype *evaluate, lm_print_ftype *printout,
1788 void *data, lm_control_type *control )
1789 {
1790 std::vector<double> fvec(m_dat);
1791 std::vector<double> diag(n_par);
1792 std::vector<double> qtf(n_par);
1793 std::vector<double> fjac(n_par*m_dat);
1794 std::vector<double> wa1(n_par);
1795 std::vector<double> wa2(n_par);
1796 std::vector<double> wa3(n_par);
1797 std::vector<double> wa4(m_dat);
1798 std::vector<int> ipvt(n_par);
1799
1800 // *** perform fit.
1801
1802 control->info = 0;
1803 control->nfev = 0;
1804
1805 // this goes through the modified legacy interface:
1806 lm_lmdif( m_dat, n_par, par, &fvec[0], control->ftol, control->xtol, control->gtol,
1807 control->maxcall*(n_par+1), control->epsilon, &diag[0], 1,
1808 control->stepbound, &(control->info),
1809 &(control->nfev), &fjac[0], &ipvt[0], &qtf[0], &wa1[0], &wa2[0], &wa3[0], &wa4[0],
1810 evaluate, printout, data );
1811
1812 (*printout)( n_par, par, m_dat, &fvec[0], data, -1, 0, control->nfev );
1813 control->fnorm = lm_enorm(m_dat, &fvec[0]);
1814 if (control->info < 0 ) control->info = 10;
1815 }
1816
1817
1818 // ***** the following messages are referenced by the variable info.
1819
1820 const char *lm_infmsg[] = {
1821 "improper input parameters",
1822 "the relative error in the sum of squares is at most tol",
1823 "the relative error between x and the solution is at most tol",
1824 "both errors are at most tol",
1825 "fvec is orthogonal to the columns of the jacobian to machine precision",
1826 "number of calls to fcn has reached or exceeded 200*(n+1)",
1827 "ftol is too small. no further reduction in the sum of squares is possible",
1828 "xtol too small. no further improvement in approximate solution x possible",
1829 "gtol too small. no further improvement in approximate solution x possible",
1830 "not enough memory",
1831 "break requested within function evaluation"
1832 };
1833
1834 const char *lm_shortmsg[] = {
1835 "invalid input",
1836 "success (f)",
1837 "success (p)",
1838 "success (f,p)",
1839 "degenerate",
1840 "call limit",
1841 "failed (f)",
1842 "failed (p)",
1843 "failed (o)",
1844 "no memory",
1845 "user break"
1846 };
1847
1848
1849 /* ************************** implementation ******************************* */
1850
1851
1852 #define BUG 0
1853 #if BUG
1854 #include <stdio.h>
1855 #endif
1856
1857 // the following values seem good for an x86:
1858 //#define LM_MACHEP .555e-16 /* resolution of arithmetic */
1859 //#define LM_DWARF 9.9e-324 /* smallest nonzero number */
1860 // the follwoing values should work on any machine:
1861 #define LM_MACHEP 1.2e-16
1862 #define LM_DWARF 1.0e-38
1863
1864 // the squares of the following constants shall not under/overflow:
1865 // these values seem good for an x86:
1866 //#define LM_SQRT_DWARF 1.e-160
1867 //#define LM_SQRT_GIANT 1.e150
1868 // the following values should work on any machine:
1869 #define LM_SQRT_DWARF 3.834e-20
1870 #define LM_SQRT_GIANT 1.304e19
1871
1872
1873 void lm_qrfac( int m, int n, double* a, int pivot, int* ipvt,
1874 double* rdiag, double* acnorm, double* wa);
1875 void lm_qrsolv(int n, double* r, int ldr, int* ipvt, double* diag,
1876 double* qtb, double* x, double* sdiag, double* wa);
1877 void lm_lmpar( int n, double* r, int ldr, int* ipvt, double* diag, double* qtb,
1878 double delta, double* par, double* x, double* sdiag,
1879 double* wa1, double* wa2);
1880
1881 #define MIN(a,b) (((a)<=(b)) ? (a) : (b))
1882 #define MAX(a,b) (((a)>=(b)) ? (a) : (b))
1883 #define SQR(x) (x)*(x)
1884
1885
1886 // ***** the low-level legacy interface for full control.
1887
lm_lmdif(int m,int n,double * x,double * fvec,double ftol,double xtol,double gtol,int maxfev,double epsfcn,double * diag,int mode,double factor,int * info,int * nfev,double * fjac,int * ipvt,double * qtf,double * wa1,double * wa2,double * wa3,double * wa4,lm_evaluate_ftype * evaluate,lm_print_ftype * printout,void * data)1888 void lm_lmdif( int m, int n, double* x, double* fvec, double ftol, double xtol,
1889 double gtol, int maxfev, double epsfcn, double* diag, int mode,
1890 double factor, int *info, int *nfev,
1891 double* fjac, int* ipvt, double* qtf,
1892 double* wa1, double* wa2, double* wa3, double* wa4,
1893 lm_evaluate_ftype *evaluate, lm_print_ftype *printout,
1894 void *data )
1895 {
1896 /*
1897 * the purpose of lmdif is to minimize the sum of the squares of
1898 * m nonlinear functions in n variables by a modification of
1899 * the levenberg-marquardt algorithm. the user must provide a
1900 * subroutine evaluate which calculates the functions. the jacobian
1901 * is then calculated by a forward-difference approximation.
1902 *
1903 * the multi-parameter interface lm_lmdif is for users who want
1904 * full control and flexibility. most users will be better off using
1905 * the simpler interface lmfit provided above.
1906 *
1907 * the parameters are the same as in the legacy FORTRAN implementation,
1908 * with the following exceptions:
1909 * the old parameter ldfjac which gave leading dimension of fjac has
1910 * been deleted because this C translation makes no use of two-
1911 * dimensional arrays;
1912 * the old parameter nprint has been deleted; printout is now controlled
1913 * by the user-supplied routine *printout;
1914 * the parameter field *data and the function parameters *evaluate and
1915 * *printout have been added; they help avoiding global variables.
1916 *
1917 * parameters:
1918 *
1919 * m is a positive integer input variable set to the number
1920 * of functions.
1921 *
1922 * n is a positive integer input variable set to the number
1923 * of variables. n must not exceed m.
1924 *
1925 * x is an array of length n. on input x must contain
1926 * an initial estimate of the solution vector. on output x
1927 * contains the final estimate of the solution vector.
1928 *
1929 * fvec is an output array of length m which contains
1930 * the functions evaluated at the output x.
1931 *
1932 * ftol is a nonnegative input variable. termination
1933 * occurs when both the actual and predicted relative
1934 * reductions in the sum of squares are at most ftol.
1935 * therefore, ftol measures the relative error desired
1936 * in the sum of squares.
1937 *
1938 * xtol is a nonnegative input variable. termination
1939 * occurs when the relative error between two consecutive
1940 * iterates is at most xtol. therefore, xtol measures the
1941 * relative error desired in the approximate solution.
1942 *
1943 * gtol is a nonnegative input variable. termination
1944 * occurs when the cosine of the angle between fvec and
1945 * any column of the jacobian is at most gtol in absolute
1946 * value. therefore, gtol measures the orthogonality
1947 * desired between the function vector and the columns
1948 * of the jacobian.
1949 *
1950 * maxfev is a positive integer input variable. termination
1951 * occurs when the number of calls to lm_fcn is at least
1952 * maxfev by the end of an iteration.
1953 *
1954 * epsfcn is an input variable used in determining a suitable
1955 * step length for the forward-difference approximation. this
1956 * approximation assumes that the relative errors in the
1957 * functions are of the order of epsfcn. if epsfcn is less
1958 * than the machine precision, it is assumed that the relative
1959 * errors in the functions are of the order of the machine
1960 * precision.
1961 *
1962 * diag is an array of length n. if mode = 1 (see below), diag is
1963 * internally set. if mode = 2, diag must contain positive entries
1964 * that serve as multiplicative scale factors for the variables.
1965 *
1966 * mode is an integer input variable. if mode = 1, the
1967 * variables will be scaled internally. if mode = 2,
1968 * the scaling is specified by the input diag. other
1969 * values of mode are equivalent to mode = 1.
1970 *
1971 * factor is a positive input variable used in determining the
1972 * initial step bound. this bound is set to the product of
1973 * factor and the euclidean norm of diag*x if nonzero, or else
1974 * to factor itself. in most cases factor should lie in the
1975 * interval (.1,100.). 100. is a generally recommended value.
1976 *
1977 * info is an integer output variable that indicates the termination
1978 * status of lm_lmdif as follows:
1979 *
1980 * info < 0 termination requested by user-supplied routine *evaluate;
1981 *
1982 * info = 0 improper input parameters;
1983 *
1984 * info = 1 both actual and predicted relative reductions
1985 * in the sum of squares are at most ftol;
1986 *
1987 * info = 2 relative error between two consecutive iterates
1988 * is at most xtol;
1989 *
1990 * info = 3 conditions for info = 1 and info = 2 both hold;
1991 *
1992 * info = 4 the cosine of the angle between fvec and any
1993 * column of the jacobian is at most gtol in
1994 * absolute value;
1995 *
1996 * info = 5 number of calls to lm_fcn has reached or
1997 * exceeded maxfev;
1998 *
1999 * info = 6 ftol is too small. no further reduction in
2000 * the sum of squares is possible;
2001 *
2002 * info = 7 xtol is too small. no further improvement in
2003 * the approximate solution x is possible;
2004 *
2005 * info = 8 gtol is too small. fvec is orthogonal to the
2006 * columns of the jacobian to machine precision;
2007 *
2008 * nfev is an output variable set to the number of calls to the
2009 * user-supplied routine *evaluate.
2010 *
2011 * fjac is an output m by n array. the upper n by n submatrix
2012 * of fjac contains an upper triangular matrix r with
2013 * diagonal elements of nonincreasing magnitude such that
2014 *
2015 * t t t
2016 * p *(jac *jac)*p = r *r,
2017 *
2018 * where p is a permutation matrix and jac is the final
2019 * calculated jacobian. column j of p is column ipvt(j)
2020 * (see below) of the identity matrix. the lower trapezoidal
2021 * part of fjac contains information generated during
2022 * the computation of r.
2023 *
2024 * ipvt is an integer output array of length n. ipvt
2025 * defines a permutation matrix p such that jac*p = q*r,
2026 * where jac is the final calculated jacobian, q is
2027 * orthogonal (not stored), and r is upper triangular
2028 * with diagonal elements of nonincreasing magnitude.
2029 * column j of p is column ipvt(j) of the identity matrix.
2030 *
2031 * qtf is an output array of length n which contains
2032 * the first n elements of the vector (q transpose)*fvec.
2033 *
2034 * wa1, wa2, and wa3 are work arrays of length n.
2035 *
2036 * wa4 is a work array of length m.
2037 *
2038 * the following parameters are newly introduced in this C translation:
2039 *
2040 * evaluate is the name of the subroutine which calculates the functions.
2041 * a default implementation lm_evaluate_default is provided in lm_eval.c;
2042 * alternatively, evaluate can be provided by a user calling program.
2043 * it should be written as follows:
2044 *
2045 * void evaluate ( double* par, int m_dat, double* fvec,
2046 * void *data, int *info )
2047 * {
2048 * // for ( i=0; i<m_dat; ++i )
2049 * // calculate fvec[i] for given parameters par;
2050 * // to stop the minimization,
2051 * // set *info to a negative integer.
2052 * }
2053 *
2054 * printout is the name of the subroutine which nforms about fit progress.
2055 * a default implementation lm_print_default is provided in lm_eval.c;
2056 * alternatively, printout can be provided by a user calling program.
2057 * it should be written as follows:
2058 *
2059 * void printout ( int n_par, double* par, int m_dat, double* fvec,
2060 * void *data, int iflag, int iter, int nfev )
2061 * {
2062 * // iflag : 0 (init) 1 (outer loop) 2(inner loop) -1(terminated)
2063 * // iter : outer loop counter
2064 * // nfev : number of calls to *evaluate
2065 * }
2066 *
2067 * data is an input pointer to an arbitrary structure that is passed to
2068 * evaluate. typically, it contains experimental data to be fitted.
2069 *
2070 */
2071
2072 *nfev = 0; // function evaluation counter
2073
2074 // *** check the input parameters for errors.
2075
2076 if ( (n <= 0) || (m < n) || (ftol < 0.)
2077 || (xtol < 0.) || (gtol < 0.) || (maxfev <= 0) || (factor <= 0.) )
2078 {
2079 *info = 0; // invalid parameter
2080 return;
2081 }
2082
2083 if ( mode == 2 ) /* scaling by diag[] */
2084 {
2085 for ( int j=0; j<n; j++ ) /* check for non positive elements */
2086 {
2087 if ( diag[j] <= 0.0 )
2088 {
2089 *info = 0; // invalid parameter
2090 return;
2091 }
2092 }
2093 }
2094 #if BUG
2095 printf( "lmdif\n" );
2096 #endif
2097
2098 // *** evaluate the function at the starting point and calculate its norm.
2099
2100 *info = 0;
2101 (*evaluate)( x, m, fvec, data, info );
2102 (*printout)( n, x, m, fvec, data, 0, 0, ++(*nfev) );
2103 if ( *info < 0 ) return;
2104
2105 // *** the outer loop.
2106 int iter = 1; // outer loop counter
2107 double delta = 0.0; // just to prevent a warning (initialization within if-clause)
2108 double xnorm = 0.0;
2109 double fnorm = lm_enorm(m,fvec);
2110 const double eps = sqrt(MAX(epsfcn,LM_MACHEP)); // used in calculating the Jacobian by forward differences
2111
2112 do {
2113 #if BUG
2114 printf( "lmdif/ outer loop iter=%d nfev=%d fnorm=%.10e\n",
2115 iter, *nfev, fnorm );
2116 #endif
2117
2118 // O** calculate the jacobian matrix.
2119
2120 for ( int j=0; j<n; j++ )
2121 {
2122 const double temp = x[j];
2123 const double step = eps * fabs(temp) == 0.0 ? eps : eps * fabs(temp);
2124
2125 x[j] = temp + step;
2126 *info = 0;
2127 (*evaluate)( x, m, wa4, data, info );
2128 (*printout)( n, x, m, wa4, data, 1, iter, ++(*nfev) );
2129 if ( *info < 0 ) return; // user requested break
2130 x[j] = temp;
2131 for (int i=0; i<m; i++ )
2132 fjac[j*m+i] = (wa4[i] - fvec[i]) / step;
2133 }
2134 #if BUG>1
2135 // DEBUG: print the entire matrix
2136 for ( i=0; i<m; i++ )
2137 {
2138 for ( j=0; j<n; j++ )
2139 printf( "%.5e ", y[j*m+i] );
2140 printf( "\n" );
2141 }
2142 #endif
2143
2144 // O** compute the qr factorization of the jacobian.
2145
2146 lm_qrfac( m, n, fjac, 1, ipvt, wa1, wa2, wa3);
2147
2148 // O** on the first iteration ...
2149
2150 if (iter == 1)
2151 {
2152 if (mode != 2)
2153 // ... scale according to the norms of the columns of the initial jacobian.
2154 {
2155 for (int j=0; j<n; j++ )
2156 {
2157 diag[j] = wa2[j];
2158 if ( wa2[j] == 0. )
2159 diag[j] = 1.;
2160 }
2161 }
2162
2163 // ... calculate the norm of the scaled x and
2164 // initialize the step bound delta.
2165
2166 for (int j=0; j<n; j++ )
2167 wa3[j] = diag[j] * x[j];
2168
2169 xnorm = lm_enorm( n, wa3 );
2170 delta = factor*xnorm;
2171 if (delta == 0.)
2172 delta = factor;
2173 }
2174
2175 // O** form (q transpose)*fvec and store the first n components in qtf.
2176
2177 for (int i=0; i<m; i++ )
2178 wa4[i] = fvec[i];
2179
2180 for (int j=0; j<n; j++ )
2181 {
2182 double temp3 = fjac[j*m+j];
2183 if (temp3 != 0.)
2184 {
2185 double sum = 0.0;
2186 for (int i=j; i<m; i++ )
2187 sum += fjac[j*m+i] * wa4[i];
2188 double temp = -sum / temp3;
2189 for (int i=j; i<m; i++ )
2190 wa4[i] += fjac[j*m+i] * temp;
2191 }
2192 fjac[j*m+j] = wa1[j];
2193 qtf[j] = wa4[j];
2194 }
2195
2196 // O** compute the norm of the scaled gradient and test for convergence.
2197
2198 double gnorm = 0;
2199 if ( fnorm != 0 )
2200 {
2201 for (int j=0; j<n; j++ )
2202 {
2203 if ( wa2[ ipvt[j] ] == 0 ) continue;
2204
2205 double sum = 0.0;
2206 for (int i=0; i<=j; i++ )
2207 sum += fjac[j*m+i] * qtf[i] / fnorm;
2208 gnorm = MAX( gnorm, fabs(sum/wa2[ ipvt[j] ]) );
2209 }
2210 }
2211
2212 if ( gnorm <= gtol )
2213 {
2214 *info = 4;
2215 return;
2216 }
2217
2218 // O** rescale if necessary.
2219
2220 if ( mode != 2 )
2221 {
2222 for (int j=0; j<n; j++ )
2223 diag[j] = MAX(diag[j],wa2[j]);
2224 }
2225
2226 // O** the inner loop.
2227 const double kP0001 = 1.0e-4;
2228 double ratio = 0.0;
2229 do {
2230 #if BUG
2231 printf( "lmdif/ inner loop iter=%d nfev=%d\n", iter, *nfev );
2232 #endif
2233
2234 // OI* determine the levenberg-marquardt parameter.
2235 double par = 0; // levenberg-marquardt parameter
2236 lm_lmpar( n,fjac,m,ipvt,diag,qtf,delta,&par,wa1,wa2,wa3,wa4 );
2237
2238 // OI* store the direction p and x + p. calculate the norm of p.
2239
2240 for (int j=0; j<n; j++ )
2241 {
2242 wa1[j] = -wa1[j];
2243 wa2[j] = x[j] + wa1[j];
2244 wa3[j] = diag[j]*wa1[j];
2245 }
2246 double pnorm = lm_enorm(n,wa3);
2247
2248 // OI* on the first iteration, adjust the initial step bound.
2249
2250 if ( *nfev<= 1+n ) // bug corrected by J. Wuttke in 2004
2251 delta = MIN(delta,pnorm);
2252
2253 // OI* evaluate the function at x + p and calculate its norm.
2254
2255 *info = 0;
2256 (*evaluate)( wa2, m, wa4, data, info );
2257 (*printout)( n, x, m, wa4, data, 2, iter, ++(*nfev) );
2258 if ( *info < 0 ) return; // user requested break
2259
2260 double fnorm1 = lm_enorm(m,wa4);
2261 #if BUG
2262 printf( "lmdif/ pnorm %.10e fnorm1 %.10e fnorm %.10e"
2263 " delta=%.10e par=%.10e\n",
2264 pnorm, fnorm1, fnorm, delta, par );
2265 #endif
2266
2267 // OI* compute the scaled actual reduction.
2268 const double kP1 = 0.1;
2269 const double actred = kP1 * fnorm1 < fnorm ? 1 - SQR( fnorm1/fnorm ) : -1;
2270
2271 // OI* compute the scaled predicted reduction and
2272 // the scaled directional derivative.
2273
2274 for (int j=0; j<n; j++ )
2275 {
2276 wa3[j] = 0;
2277 for (int i=0; i<=j; i++ )
2278 wa3[i] += fjac[j*m+i]*wa1[ ipvt[j] ];
2279 }
2280 const double temp1 = lm_enorm(n,wa3) / fnorm;
2281 const double temp2 = sqrt(par) * pnorm / fnorm;
2282 const double prered = SQR(temp1) + 2 * SQR(temp2);
2283 const double dirder = - ( SQR(temp1) + SQR(temp2) );
2284
2285 // OI* compute the ratio of the actual to the predicted reduction.
2286
2287 ratio = prered !=0 ? actred / prered : 0.0;
2288 #if BUG
2289 printf( "lmdif/ actred=%.10e prered=%.10e ratio=%.10e"
2290 " sq(1)=%.10e sq(2)=%.10e dd=%.10e\n",
2291 actred, prered, prered!=0 ? ratio : 0.,
2292 SQR(temp1), SQR(temp2), dirder );
2293 #endif
2294
2295 // OI* update the step bound.
2296 const double kP5 = 0.5;
2297 const double kP25 = 0.25;
2298 const double kP75 = 0.75;
2299
2300 if (ratio <= kP25)
2301 {
2302 double temp = actred >= 0.0 ? kP5 : kP5*dirder/(dirder + kP5*actred);
2303 if ( kP1*fnorm1 >= fnorm || temp < kP1 )
2304 temp = kP1;
2305 delta = temp * MIN(delta,pnorm/kP1);
2306 par /= temp;
2307 }
2308 else if ( par == 0. || ratio >= kP75 )
2309 {
2310 delta = pnorm/kP5;
2311 par *= kP5;
2312 }
2313
2314 // OI* test for successful iteration...
2315 if (ratio >= kP0001)
2316 {
2317
2318 // ... successful iteration. update x, fvec, and their norms.
2319
2320 for (int j=0; j<n; j++ )
2321 {
2322 x[j] = wa2[j];
2323 wa2[j] = diag[j]*x[j];
2324 }
2325 for (int i=0; i<m; i++ )
2326 fvec[i] = wa4[i];
2327 xnorm = lm_enorm(n,wa2);
2328 fnorm = fnorm1;
2329 iter++;
2330 }
2331 #if BUG
2332 else {
2333 printf( "ATTN: iteration considered unsuccessful\n" );
2334 }
2335 #endif
2336
2337 // OI* tests for convergence ( otherwise *info = 1, 2, or 3 )
2338
2339 *info = 0; // do not terminate (unless overwritten by nonzero value)
2340 if ( fabs(actred) <= ftol && prered <= ftol && kP5*ratio <= 1 )
2341 *info = 1;
2342 if (delta <= xtol*xnorm)
2343 *info += 2;
2344 if ( *info != 0)
2345 return;
2346
2347 // OI* tests for termination and stringent tolerances.
2348
2349 if ( *nfev >= maxfev)
2350 *info = 5;
2351 if ( fabs(actred) <= LM_MACHEP &&
2352 prered <= LM_MACHEP && kP5*ratio <= 1 )
2353 *info = 6;
2354 if (delta <= LM_MACHEP*xnorm)
2355 *info = 7;
2356 if (gnorm <= LM_MACHEP)
2357 *info = 8;
2358 if ( *info != 0)
2359 return;
2360
2361 // OI* end of the inner loop. repeat if iteration unsuccessful.
2362
2363 } while (ratio < kP0001);
2364
2365 // O** end of the outer loop.
2366
2367 } while (1);
2368
2369 }
2370
2371
2372
lm_lmpar(int n,double * r,int ldr,int * ipvt,double * diag,double * qtb,double delta,double * par,double * x,double * sdiag,double * wa1,double * wa2)2373 void lm_lmpar(int n, double* r, int ldr, int* ipvt, double* diag, double* qtb,
2374 double delta, double* par, double* x, double* sdiag,
2375 double* wa1, double* wa2)
2376 {
2377 /* given an m by n matrix a, an n by n nonsingular diagonal
2378 * matrix d, an m-vector b, and a positive number delta,
2379 * the problem is to determine a value for the parameter
2380 * par such that if x solves the system
2381 *
2382 * a*x = b , sqrt(par)*d*x = 0 ,
2383 *
2384 * in the least squares sense, and dxnorm is the euclidean
2385 * norm of d*x, then either par is 0. and
2386 *
2387 * (dxnorm-delta) .le. 0.1*delta ,
2388 *
2389 * or par is positive and
2390 *
2391 * abs(dxnorm-delta) .le. 0.1*delta .
2392 *
2393 * this subroutine completes the solution of the problem
2394 * if it is provided with the necessary information from the
2395 * qr factorization, with column pivoting, of a. that is, if
2396 * a*p = q*r, where p is a permutation matrix, q has orthogonal
2397 * columns, and r is an upper triangular matrix with diagonal
2398 * elements of nonincreasing magnitude, then lmpar expects
2399 * the full upper triangle of r, the permutation matrix p,
2400 * and the first n components of (q transpose)*b. on output
2401 * lmpar also provides an upper triangular matrix s such that
2402 *
2403 * t t t
2404 * p *(a *a + par*d*d)*p = s *s .
2405 *
2406 * s is employed within lmpar and may be of separate interest.
2407 *
2408 * only a few iterations are generally needed for convergence
2409 * of the algorithm. if, however, the limit of 10 iterations
2410 * is reached, then the output par will contain the best
2411 * value obtained so far.
2412 *
2413 * parameters:
2414 *
2415 * n is a positive integer input variable set to the order of r.
2416 *
2417 * r is an n by n array. on input the full upper triangle
2418 * must contain the full upper triangle of the matrix r.
2419 * on output the full upper triangle is unaltered, and the
2420 * strict lower triangle contains the strict upper triangle
2421 * (transposed) of the upper triangular matrix s.
2422 *
2423 * ldr is a positive integer input variable not less than n
2424 * which specifies the leading dimension of the array r.
2425 *
2426 * ipvt is an integer input array of length n which defines the
2427 * permutation matrix p such that a*p = q*r. column j of p
2428 * is column ipvt(j) of the identity matrix.
2429 *
2430 * diag is an input array of length n which must contain the
2431 * diagonal elements of the matrix d.
2432 *
2433 * qtb is an input array of length n which must contain the first
2434 * n elements of the vector (q transpose)*b.
2435 *
2436 * delta is a positive input variable which specifies an upper
2437 * bound on the euclidean norm of d*x.
2438 *
2439 * par is a nonnegative variable. on input par contains an
2440 * initial estimate of the levenberg-marquardt parameter.
2441 * on output par contains the final estimate.
2442 *
2443 * x is an output array of length n which contains the least
2444 * squares solution of the system a*x = b, sqrt(par)*d*x = 0,
2445 * for the output par.
2446 *
2447 * sdiag is an output array of length n which contains the
2448 * diagonal elements of the upper triangular matrix s.
2449 *
2450 * wa1 and wa2 are work arrays of length n.
2451 *
2452 */
2453
2454 #if BUG
2455 printf( "lmpar\n" );
2456 #endif
2457
2458 // *** compute and store in x the gauss-newton direction. if the
2459 // jacobian is rank-deficient, obtain a least squares solution.
2460
2461 int nsing = n;
2462 for (int j=0; j<n; j++ )
2463 {
2464 wa1[j] = qtb[j];
2465 if ( r[j*ldr+j] == 0 && nsing == n )
2466 nsing = j;
2467 if (nsing < n)
2468 wa1[j] = 0;
2469 }
2470 #if BUG
2471 printf( "nsing %d ", nsing );
2472 #endif
2473 for (int j=nsing-1; j>=0; j-- )
2474 {
2475 wa1[j] = wa1[j]/r[j+ldr*j];
2476 const double temp = wa1[j];
2477 for (int i=0; i<j; i++ )
2478 wa1[i] -= r[j*ldr+i]*temp;
2479 }
2480
2481 for (int j=0; j<n; j++ )
2482 x[ ipvt[j] ] = wa1[j];
2483
2484 // *** initialize the iteration counter.
2485 // evaluate the function at the origin, and test
2486 // for acceptance of the gauss-newton direction.
2487
2488 int iter = 0;
2489 for (int j=0; j<n; j++ )
2490 wa2[j] = diag[j]*x[j];
2491
2492 double dxnorm = lm_enorm(n,wa2);
2493 double fp = dxnorm - delta;
2494 const double kP1 = 0.1;
2495 if (fp <= kP1*delta)
2496 {
2497 #if BUG
2498 printf( "lmpar/ terminate (fp<delta/10\n" );
2499 #endif
2500 *par = 0;
2501 return;
2502 }
2503
2504 // *** if the jacobian is not rank deficient, the newton
2505 // step provides a lower bound, parl, for the 0. of
2506 // the function. otherwise set this bound to 0..
2507
2508 double parl = 0.0;
2509 if (nsing >= n)
2510 {
2511 for (int j=0; j<n; j++ )
2512 wa1[j] = diag[ ipvt[j] ] * wa2[ ipvt[j] ] / dxnorm;
2513
2514 for (int j=0; j<n; j++ )
2515 {
2516 double sum = 0.0;
2517 for (int i=0; i<j; i++ )
2518 sum += r[j*ldr+i]*wa1[i];
2519 wa1[j] = (wa1[j] - sum)/r[j+ldr*j];
2520 }
2521 const double temp = lm_enorm(n,wa1);
2522 parl = fp/delta/temp/temp;
2523 }
2524
2525 // *** calculate an upper bound, paru, for the 0. of the function.
2526
2527 for (int j=0; j<n; j++ )
2528 {
2529 double sum = 0;
2530 for (int i=0; i<=j; i++ )
2531 sum += r[j*ldr+i]*qtb[i];
2532 wa1[j] = sum/diag[ ipvt[j] ];
2533 }
2534 const double gnorm = lm_enorm(n,wa1);
2535 double paru = gnorm/delta == 0.0 ? LM_DWARF/MIN(delta,kP1) : gnorm / delta;
2536
2537 // *** if the input par lies outside of the interval (parl,paru),
2538 // set par to the closer endpoint.
2539
2540 *par = MAX( *par,parl);
2541 *par = MIN( *par,paru);
2542 if ( *par == 0.)
2543 *par = gnorm/dxnorm;
2544 #if BUG
2545 printf( "lmpar/ parl %.4e par %.4e paru %.4e\n", parl, *par, paru );
2546 #endif
2547
2548 // *** iterate.
2549
2550 for ( ; ; iter++ ) {
2551
2552 // *** evaluate the function at the current value of par.
2553 const double kP001 = 0.001;
2554 if ( *par == 0.)
2555 *par = MAX(LM_DWARF,kP001*paru);
2556 double temp = sqrt( *par );
2557 for (int j=0; j<n; j++ )
2558 wa1[j] = temp*diag[j];
2559 lm_qrsolv( n, r, ldr, ipvt, wa1, qtb, x, sdiag, wa2);
2560 for (int j=0; j<n; j++ )
2561 wa2[j] = diag[j]*x[j];
2562 dxnorm = lm_enorm(n,wa2);
2563 const double fp_old = fp;
2564 fp = dxnorm - delta;
2565
2566 // *** if the function is small enough, accept the current value
2567 // of par. also test for the exceptional cases where parl
2568 // is 0. or the number of iterations has reached 10.
2569
2570 if ( fabs(fp) <= kP1*delta
2571 || (parl == 0. && fp <= fp_old && fp_old < 0.)
2572 || iter == 10 )
2573 break; // the only exit from this loop
2574
2575 // *** compute the Newton correction.
2576
2577 for (int j=0; j<n; j++ )
2578 wa1[j] = diag[ ipvt[j] ] * wa2[ ipvt[j] ] / dxnorm;
2579
2580 for (int j=0; j<n; j++ )
2581 {
2582 wa1[j] = wa1[j]/sdiag[j];
2583 for (int i=j+1; i<n; i++ )
2584 wa1[i] -= r[j*ldr+i]*wa1[j];
2585 }
2586 temp = lm_enorm( n, wa1);
2587 double parc = fp/delta/temp/temp;
2588
2589 // *** depending on the sign of the function, update parl or paru.
2590
2591 if (fp > 0)
2592 parl = MAX(parl, *par);
2593 else if (fp < 0)
2594 paru = MIN(paru, *par);
2595 // the case fp==0 is precluded by the break condition
2596
2597 // *** compute an improved estimate for par.
2598
2599 *par = MAX(parl, *par + parc);
2600
2601 }
2602
2603 }
2604
2605
2606
lm_qrfac(int m,int n,double * a,int pivot,int * ipvt,double * rdiag,double * acnorm,double * wa)2607 void lm_qrfac(int m, int n, double* a, int pivot, int* ipvt,
2608 double* rdiag, double* acnorm, double* wa)
2609 {
2610 /*
2611 * this subroutine uses householder transformations with column
2612 * pivoting (optional) to compute a qr factorization of the
2613 * m by n matrix a. that is, qrfac determines an orthogonal
2614 * matrix q, a permutation matrix p, and an upper trapezoidal
2615 * matrix r with diagonal elements of nonincreasing magnitude,
2616 * such that a*p = q*r. the householder transformation for
2617 * column k, k = 1,2,...,min(m,n), is of the form
2618 *
2619 * t
2620 * i - (1/u(k))*u*u
2621 *
2622 * where u has 0.s in the first k-1 positions. the form of
2623 * this transformation and the method of pivoting first
2624 * appeared in the corresponding linpack subroutine.
2625 *
2626 * parameters:
2627 *
2628 * m is a positive integer input variable set to the number
2629 * of rows of a.
2630 *
2631 * n is a positive integer input variable set to the number
2632 * of columns of a.
2633 *
2634 * a is an m by n array. on input a contains the matrix for
2635 * which the qr factorization is to be computed. on output
2636 * the strict upper trapezoidal part of a contains the strict
2637 * upper trapezoidal part of r, and the lower trapezoidal
2638 * part of a contains a factored form of q (the non-trivial
2639 * elements of the u vectors described above).
2640 *
2641 * pivot is a logical input variable. if pivot is set true,
2642 * then column pivoting is enforced. if pivot is set false,
2643 * then no column pivoting is done.
2644 *
2645 * ipvt is an integer output array of length lipvt. ipvt
2646 * defines the permutation matrix p such that a*p = q*r.
2647 * column j of p is column ipvt(j) of the identity matrix.
2648 * if pivot is false, ipvt is not referenced.
2649 *
2650 * rdiag is an output array of length n which contains the
2651 * diagonal elements of r.
2652 *
2653 * acnorm is an output array of length n which contains the
2654 * norms of the corresponding columns of the input matrix a.
2655 * if this information is not needed, then acnorm can coincide
2656 * with rdiag.
2657 *
2658 * wa is a work array of length n. if pivot is false, then wa
2659 * can coincide with rdiag.
2660 *
2661 */
2662
2663 // *** compute the initial column norms and initialize several arrays.
2664
2665 for (int j=0; j<n; j++ )
2666 {
2667 acnorm[j] = lm_enorm(m, &a[j*m]);
2668 rdiag[j] = acnorm[j];
2669 wa[j] = rdiag[j];
2670 if ( pivot )
2671 ipvt[j] = j;
2672 }
2673 #if BUG
2674 printf( "qrfac\n" );
2675 #endif
2676
2677 // *** reduce a to r with householder transformations.
2678
2679 const int minmn = MIN(m,n);
2680 for (int j=0; j<minmn; j++ )
2681 {
2682 int kmax = j, k;
2683 if ( !pivot ) goto pivot_ok;
2684
2685 // *** bring the column of largest norm into the pivot position.
2686
2687 for (k=j+1; k<n; k++ )
2688 if (rdiag[k] > rdiag[kmax])
2689 kmax = k;
2690 if (kmax == j) goto pivot_ok; // bug fixed in rel 2.1
2691
2692 for (int i=0; i<m; i++ )
2693 {
2694 std::swap(a[j*m+i], a[kmax*m+i]);
2695 //const double temp = a[j*m+i];
2696 //a[j*m+i] = a[kmax*m+i];
2697 //a[kmax*m+i] = temp;
2698 }
2699 rdiag[kmax] = rdiag[j];
2700 wa[kmax] = wa[j];
2701 std::swap(ipvt[j], ipvt[kmax]);
2702 //k = ipvt[j];
2703 //ipvt[j] = ipvt[kmax];
2704 //ipvt[kmax] = k;
2705
2706 pivot_ok:
2707
2708 // *** compute the Householder transformation to reduce the
2709 // j-th column of a to a multiple of the j-th unit vector.
2710
2711 double ajnorm = lm_enorm( m-j, &a[j*m+j] );
2712 if (ajnorm == 0.)
2713 {
2714 rdiag[j] = 0;
2715 continue;
2716 }
2717
2718 if (a[j*m+j] < 0.)
2719 ajnorm = -ajnorm;
2720 for (int i=j; i<m; i++ )
2721 a[j*m+i] /= ajnorm;
2722 a[j*m+j] += 1;
2723
2724 // *** apply the transformation to the remaining columns
2725 // and update the norms.
2726
2727 for ( k=j+1; k<n; k++ )
2728 {
2729 double sum = 0;
2730
2731 for (int i=j; i<m; i++ )
2732 sum += a[j*m+i]*a[k*m+i];
2733
2734 double temp = sum/a[j+m*j];
2735
2736 for (int i=j; i<m; i++ )
2737 a[k*m+i] -= temp * a[j*m+i];
2738
2739 if ( pivot && rdiag[k] != 0. )
2740 {
2741 temp = a[m*k+j]/rdiag[k];
2742 temp = MAX( 0., 1-temp*temp );
2743 rdiag[k] *= sqrt(temp);
2744 temp = rdiag[k]/wa[k];
2745 const double kP05 = 0.05;
2746 if ( kP05*SQR(temp) <= LM_MACHEP )
2747 {
2748 rdiag[k] = lm_enorm( m-j-1, &a[m*k+j+1]);
2749 wa[k] = rdiag[k];
2750 }
2751 }
2752 }
2753
2754 rdiag[j] = -ajnorm;
2755 }
2756 }
2757
2758
2759
lm_qrsolv(int n,double * r,int ldr,int * ipvt,double * diag,double * qtb,double * x,double * sdiag,double * wa)2760 void lm_qrsolv(int n, double* r, int ldr, int* ipvt, double* diag,
2761 double* qtb, double* x, double* sdiag, double* wa)
2762 {
2763 /*
2764 * given an m by n matrix a, an n by n diagonal matrix d,
2765 * and an m-vector b, the problem is to determine an x which
2766 * solves the system
2767 *
2768 * a*x = b , d*x = 0 ,
2769 *
2770 * in the least squares sense.
2771 *
2772 * this subroutine completes the solution of the problem
2773 * if it is provided with the necessary information from the
2774 * qr factorization, with column pivoting, of a. that is, if
2775 * a*p = q*r, where p is a permutation matrix, q has orthogonal
2776 * columns, and r is an upper triangular matrix with diagonal
2777 * elements of nonincreasing magnitude, then qrsolv expects
2778 * the full upper triangle of r, the permutation matrix p,
2779 * and the first n components of (q transpose)*b. the system
2780 * a*x = b, d*x = 0, is then equivalent to
2781 *
2782 * t t
2783 * r*z = q *b , p *d*p*z = 0 ,
2784 *
2785 * where x = p*z. if this system does not have full rank,
2786 * then a least squares solution is obtained. on output qrsolv
2787 * also provides an upper triangular matrix s such that
2788 *
2789 * t t t
2790 * p *(a *a + d*d)*p = s *s .
2791 *
2792 * s is computed within qrsolv and may be of separate interest.
2793 *
2794 * parameters
2795 *
2796 * n is a positive integer input variable set to the order of r.
2797 *
2798 * r is an n by n array. on input the full upper triangle
2799 * must contain the full upper triangle of the matrix r.
2800 * on output the full upper triangle is unaltered, and the
2801 * strict lower triangle contains the strict upper triangle
2802 * (transposed) of the upper triangular matrix s.
2803 *
2804 * ldr is a positive integer input variable not less than n
2805 * which specifies the leading dimension of the array r.
2806 *
2807 * ipvt is an integer input array of length n which defines the
2808 * permutation matrix p such that a*p = q*r. column j of p
2809 * is column ipvt(j) of the identity matrix.
2810 *
2811 * diag is an input array of length n which must contain the
2812 * diagonal elements of the matrix d.
2813 *
2814 * qtb is an input array of length n which must contain the first
2815 * n elements of the vector (q transpose)*b.
2816 *
2817 * x is an output array of length n which contains the least
2818 * squares solution of the system a*x = b, d*x = 0.
2819 *
2820 * sdiag is an output array of length n which contains the
2821 * diagonal elements of the upper triangular matrix s.
2822 *
2823 * wa is a work array of length n.
2824 *
2825 */
2826
2827 // *** copy r and (q transpose)*b to preserve input and initialize s.
2828 // in particular, save the diagonal elements of r in x.
2829
2830 for (int j=0; j<n; j++ )
2831 {
2832 for (int i=j; i<n; i++ )
2833 r[j*ldr+i] = r[i*ldr+j];
2834 x[j] = r[j*ldr+j];
2835 wa[j] = qtb[j];
2836 }
2837 #if BUG
2838 printf( "qrsolv\n" );
2839 #endif
2840
2841 // *** eliminate the diagonal matrix d using a givens rotation.
2842
2843 for (int j=0; j<n; j++ )
2844 {
2845
2846 // *** prepare the row of d to be eliminated, locating the
2847 // diagonal element using p from the qr factorization.
2848
2849 double qtbpj = 0.0;
2850
2851 if (diag[ ipvt[j] ] == 0.)
2852 goto L90;
2853 for (int k=j; k<n; k++ )
2854 sdiag[k] = 0.;
2855 sdiag[j] = diag[ ipvt[j] ];
2856
2857 // *** the transformations to eliminate the row of d
2858 // modify only a single element of (q transpose)*b
2859 // beyond the first n, which is initially 0..
2860
2861 for (int k=j; k<n; k++ )
2862 {
2863 const double p25 = 0.25;
2864 const double p5 = 0.5;
2865
2866 // determine a givens rotation which eliminates the
2867 // appropriate element in the current row of d.
2868
2869 if (sdiag[k] == 0.)
2870 continue;
2871 const int kk = k + ldr * k; // <! keep this shorthand !>
2872 double sin, cos; // these are local variables, not functions
2873
2874 if ( fabs(r[kk]) < fabs(sdiag[k]) )
2875 {
2876 const double cotan = r[kk]/sdiag[k];
2877 sin = p5/sqrt(p25+p25*SQR(cotan));
2878 cos = sin*cotan;
2879 }
2880 else
2881 {
2882 const double tan = sdiag[k]/r[kk];
2883 cos = p5/sqrt(p25+p25*SQR(tan));
2884 sin = cos*tan;
2885 }
2886
2887 // *** compute the modified diagonal element of r and
2888 // the modified element of ((q transpose)*b,0).
2889
2890 r[kk] = cos*r[kk] + sin*sdiag[k];
2891 double temp = cos*wa[k] + sin*qtbpj;
2892 qtbpj = -sin*wa[k] + cos*qtbpj;
2893 wa[k] = temp;
2894
2895 // *** accumulate the transformation in the row of s.
2896
2897 for (int i=k+1; i<n; i++ )
2898 {
2899 temp = cos*r[k*ldr+i] + sin*sdiag[i];
2900 sdiag[i] = -sin*r[k*ldr+i] + cos*sdiag[i];
2901 r[k*ldr+i] = temp;
2902 }
2903 }
2904 L90:
2905
2906 // *** store the diagonal element of s and restore
2907 // the corresponding diagonal element of r.
2908
2909 sdiag[j] = r[j*ldr+j];
2910 r[j*ldr+j] = x[j];
2911 }
2912
2913 // *** solve the triangular system for z. if the system is
2914 // singular, then obtain a least squares solution.
2915
2916 int nsing = n;
2917 for (int j=0; j<n; j++ )
2918 {
2919 if ( sdiag[j] == 0. && nsing == n )
2920 nsing = j;
2921 if (nsing < n)
2922 wa[j] = 0;
2923 }
2924
2925 for (int j=nsing-1; j>=0; j-- )
2926 {
2927 double sum = 0.0;
2928 for (int i=j+1; i<nsing; i++ )
2929 sum += r[j*ldr+i]*wa[i];
2930 wa[j] = (wa[j] - sum)/sdiag[j];
2931 }
2932
2933 // *** permute the components of z back to components of x.
2934
2935 for (int j=0; j<n; j++ )
2936 x[ ipvt[j] ] = wa[j];
2937 }
2938
2939
2940
lm_enorm(int n,double * x)2941 double lm_enorm( int n, double *x )
2942 {
2943 /* given an n-vector x, this function calculates the
2944 * euclidean norm of x.
2945 *
2946 * the euclidean norm is computed by accumulating the sum of
2947 * squares in three different sums. the sums of squares for the
2948 * small and large components are scaled so that no overflows
2949 * occur. non-destructive underflows are permitted. underflows
2950 * and overflows do not occur in the computation of the unscaled
2951 * sum of squares for the intermediate components.
2952 * the definitions of small, intermediate and large components
2953 * depend on two constants, LM_SQRT_DWARF and LM_SQRT_GIANT. the main
2954 * restrictions on these constants are that LM_SQRT_DWARF**2 not
2955 * underflow and LM_SQRT_GIANT**2 not overflow.
2956 *
2957 * parameters
2958 *
2959 * n is a positive integer input variable.
2960 *
2961 * x is an input array of length n.
2962 */
2963
2964 double s1 = 0.0, s2 = 0.0, s3 = 0.0;
2965 double x1max = 0.0, x3max = 0.0;
2966 const double agiant = LM_SQRT_GIANT/( (double) n);
2967
2968 for (int i=0; i<n; i++ )
2969 {
2970 double xabs = fabs(x[i]);
2971 if ( xabs > LM_SQRT_DWARF && xabs < agiant )
2972 {
2973 // ** sum for intermediate components.
2974 s2 += xabs*xabs;
2975 continue;
2976 }
2977
2978 if ( xabs > LM_SQRT_DWARF )
2979 {
2980 // ** sum for large components.
2981 if (xabs > x1max)
2982 {
2983 s1 = 1 + s1*SQR(x1max/xabs);
2984 x1max = xabs;
2985 }
2986 else
2987 {
2988 s1 += SQR(xabs/x1max);
2989 }
2990 continue;
2991 }
2992 // ** sum for small components.
2993 if (xabs > x3max)
2994 {
2995 s3 = 1 + s3*SQR(x3max/xabs);
2996 x3max = xabs;
2997 }
2998 else
2999 {
3000 if (xabs != 0.)
3001 {
3002 s3 += SQR(xabs/x3max);
3003 }
3004 }
3005 }
3006
3007 // *** calculation of norm.
3008
3009 if (s1 != 0)
3010 return x1max*sqrt(s1 + (s2/x1max)/x1max);
3011 if (s2 != 0)
3012 {
3013 if (s2 >= x3max)
3014 return sqrt( s2*(1+(x3max/s2)*(x3max*s3)) );
3015 return sqrt( x3max*((s2/x3max)+(x3max*s3)) );
3016 }
3017
3018 return x3max*sqrt(s3);
3019 }
3020
3021
lat_gc_crosses_meridian(double lat1,double lon1,double lat2,double lon2,double lon)3022 double lat_gc_crosses_meridian( double lat1, double lon1, double lat2, double lon2, double lon )
3023 {
3024 /*
3025 Calculates a latitude at which a GC route between two points crosses a given meridian
3026 */
3027
3028 double dlon = lon * DEGREE;
3029 double dlat1 = lat1 * DEGREE;
3030 double dlat2 = lat2 * DEGREE;
3031 double dlon1 = lon1 * DEGREE;
3032 double dlon2 = lon2 * DEGREE;
3033
3034 return RADIAN * atan((sin(dlat1) * cos(dlat2) * sin(dlon-dlon2)
3035 - sin(dlat2) * cos(dlat1) * sin(dlon-dlon1)) / (cos(dlat1) * cos(dlat2) * sin(dlon1-dlon2)));
3036 }
3037
lat_rl_crosses_meridian(double lat1,double lon1,double lat2,double lon2,double lon)3038 double lat_rl_crosses_meridian( double lat1, double lon1, double lat2, double lon2, double lon )
3039 {
3040 /*
3041 Calculates a latitude at which a loxodromic route between two points crosses a given meridian
3042 */
3043
3044 double brg;
3045
3046 DistanceBearingMercator( lat2, lon2, lat1, lon1, &brg, NULL );
3047
3048 double x1, y1, x;
3049 toSM( lat1, lon1, 0., lon, &x1, &y1 );
3050 toSM( lat1, lon, 0., lon, &x, &y1 );
3051
3052 double dir = 1.0;
3053 if ( brg >= 270.0 )
3054 {
3055 brg -= 270.0;
3056 }
3057 else if ( brg >= 180. )
3058 {
3059 brg = 270.0 - brg;
3060 dir = -1.0;
3061 }
3062 else if ( brg >= 90. )
3063 {
3064 brg -= 90.0;
3065 dir = -1.0;
3066 }
3067 else
3068 {
3069 brg = 90.0 - brg;
3070 }
3071
3072 double ydelta = fabs( x1 ) * tan( brg * DEGREE );
3073
3074 double crosslat, crosslon;
3075 fromSM(x, y1 + dir * ydelta, 0., lon, &crosslat, &crosslon);
3076
3077 return crosslat;
3078 }
3079