Rev 1212 Rev 1269
1 /************************************************************************ 1 /************************************************************************
2 * 2 *
3 * File: Utm.cpp 3 * File: Utm.cpp
4 * RCS: $Header: /cvsroot/stelvio/stelvio/NavStar/Utm.cpp,v 1.1 2001/03/18 20:07:03 steve_l Exp $ 4 * RCS: $Header: /cvsroot/stelvio/stelvio/NavStar/Utm.cpp,v 1.1 2001/03/18 20:07:03 steve_l Exp $
5 * Author: Steve Loughran 5 * Author: Steve Loughran
6 * Created: 2001 6 * Created: 2001
7 * Language: C++ 7 * Language: C++
8 * Package: 8 * Package:
9 * Status: Experimental 9 * Status: Experimental
10 * @doc 10 * @doc
11 * 11 *
12 ************************************************************************/ 12 ************************************************************************/
13   13  
14 /* 14 /*
15 This is code to do UTM conversion. 15 This is code to do UTM conversion.
16   16  
17 I took this code from Jason Bevins' GPS thing which blagged the VB algorithms 17 I took this code from Jason Bevins' GPS thing which blagged the VB algorithms
18 from the Mapping Datum Transformation Software (MADTRAN) program, 18 from the Mapping Datum Transformation Software (MADTRAN) program,
19 written in PowerBasic. To get the source code for MADTRAN, go to: 19 written in PowerBasic. To get the source code for MADTRAN, go to:
20   20  
21 http://164.214.2.59/publications/guides/MADTRAN/index.html 21 http://164.214.2.59/publications/guides/MADTRAN/index.html
22 22
23 this version retains the core algorithms as static functions 23 this version retains the core algorithms as static functions
24   24  
25 */ 25 */
26   26  
27 #include "stdafx.h" 27 //#include "stdafx.h"
28 #include "Common.h" 28 //#include "Common.h"
29 #include "osgb.h" 29 //#include "osgb.h"
30 #include "utm.h" 30 #include "utm.h"
31 #include <math.h> 31 #include <math.h>
32   32  
33 // Some constants used by these functions. 33 // Some constants used by these functions.
34 static const double fe = 500000.0; 34 static const double fe = 500000.0;
35 static const double ok = 0.9996; 35 static const double ok = 0.9996;
36   36  
37 // An array containing each vertical UTM zone. 37 // An array containing each vertical UTM zone.
38 static char cArray[] = "CDEFGHJKLMNPQRSTUVWX"; 38 static char cArray[] = "CDEFGHJKLMNPQRSTUVWX";
39   39  
40   40  
41 ///////////////////////////////////////////////////////////////////////////// 41 /////////////////////////////////////////////////////////////////////////////
42 // Miscellaneous functions for these UTM conversion formulas. 42 // Miscellaneous functions for these UTM conversion formulas.
43   43  
44 double CalculateESquared (double a, double b) 44 double CalculateESquared (double a, double b)
45 { 45 {
46 return ((a * a) - (b * b)) / (a * a); 46 return ((a * a) - (b * b)) / (a * a);
47 } 47 }
48   48  
49   49  
50 double CalculateE2Squared (double a, double b) 50 double CalculateE2Squared (double a, double b)
51 { 51 {
52 return ((a * a) - (b * b)) / (b * b); 52 return ((a * a) - (b * b)) / (b * b);
53 } 53 }
54   54  
55   55  
56 double denom (double es, double sphi) 56 double denom (double es, double sphi)
57 { 57 {
58 double sinSphi = sin (sphi); 58 double sinSphi = sin (sphi);
59 return sqrt (1.0 - es * (sinSphi * sinSphi)); 59 return sqrt (1.0 - es * (sinSphi * sinSphi));
60 } 60 }
61   61  
62   62  
63 double sphsr (double a, double es, double sphi) 63 double sphsr (double a, double es, double sphi)
64 { 64 {
65 double dn = denom (es, sphi); 65 double dn = denom (es, sphi);
66 return a * (1.0 - es) / (dn * dn * dn); 66 return a * (1.0 - es) / (dn * dn * dn);
67 } 67 }
68   68  
69   69  
70 double sphsn (double a, double es, double sphi) 70 double sphsn (double a, double es, double sphi)
71 { 71 {
72 double sinSphi = sin (sphi); 72 double sinSphi = sin (sphi);
73 return a / sqrt (1.0 - es * (sinSphi * sinSphi)); 73 return a / sqrt (1.0 - es * (sinSphi * sinSphi));
74 } 74 }
75   75  
76   76  
77 double sphtmd (double ap, double bp, double cp, double dp, double ep, 77 double sphtmd (double ap, double bp, double cp, double dp, double ep,
78 double sphi) 78 double sphi)
79 { 79 {
80 return (ap * sphi) - (bp * sin (2.0 * sphi)) + (cp * sin (4.0 * sphi)) 80 return (ap * sphi) - (bp * sin (2.0 * sphi)) + (cp * sin (4.0 * sphi))
81 - (dp * sin (6.0 * sphi)) + (ep * sin (8.0 * sphi)); 81 - (dp * sin (6.0 * sphi)) + (ep * sin (8.0 * sphi));
82 } 82 }
83   83  
84   84  
85 //======================================================================= 85 //=======================================================================
86 // Purpose: 86 // Purpose:
87 // This function converts the specified lat/lon coordinate to a UTM 87 // This function converts the specified lat/lon coordinate to a UTM
88 // coordinate. 88 // coordinate.
89 // Parameters: 89 // Parameters:
90 // double a: 90 // double a:
91 // Ellipsoid semi-major axis, in meters. (For WGS84 datum, use 6378137.0) 91 // Ellipsoid semi-major axis, in meters. (For WGS84 datum, use 6378137.0)
92 // double f: 92 // double f:
93 // Ellipsoid flattening. (For WGS84 datum, use 1 / 298.257223563) 93 // Ellipsoid flattening. (For WGS84 datum, use 1 / 298.257223563)
94 // int& utmXZone: 94 // int& utmXZone:
95 // Upon exit, this parameter will contain the hotizontal zone number of 95 // Upon exit, this parameter will contain the hotizontal zone number of
96 // the UTM coordinate. The returned value for this parameter is a number 96 // the UTM coordinate. The returned value for this parameter is a number
97 // within the range 1 to 60, inclusive. 97 // within the range 1 to 60, inclusive.
98 // char& utmYZone: 98 // char& utmYZone:
99 // Upon exit, this parameter will contain the zone letter of the UTM 99 // Upon exit, this parameter will contain the zone letter of the UTM
100 // coordinate. The returned value for this parameter will be one of: 100 // coordinate. The returned value for this parameter will be one of:
101 // CDEFGHJKLMNPQRSTUVWX. 101 // CDEFGHJKLMNPQRSTUVWX.
102 // double& easting: 102 // double& easting:
103 // Upon exit, this parameter will contain the UTM easting, in meters. 103 // Upon exit, this parameter will contain the UTM easting, in meters.
104 // double& northing: 104 // double& northing:
105 // Upon exit, this parameter will contain the UTM northing, in meters. 105 // Upon exit, this parameter will contain the UTM northing, in meters.
106 // double lat, double lon: 106 // double lat, double lon:
107 // The lat/lon coordinate to convert. 107 // The lat/lon coordinate to convert.
108 // Notes: 108 // Notes:
109 // - The code in this function is a C conversion of some of the source code 109 // - The code in this function is a C conversion of some of the source code
110 // from the Mapping Datum Transformation Software (MADTRAN) program, 110 // from the Mapping Datum Transformation Software (MADTRAN) program,
111 // written in PowerBasic. To get the source code for MADTRAN, go to: 111 // written in PowerBasic. To get the source code for MADTRAN, go to:
112 // 112 //
113 // http://164.214.2.59/publications/guides/MADTRAN/index.html 113 // http://164.214.2.59/publications/guides/MADTRAN/index.html
114 // 114 //
115 // and download MADTRAN.ZIP 115 // and download MADTRAN.ZIP
116 // - If the UTM zone is out of range, the y-zone character is set to the 116 // - If the UTM zone is out of range, the y-zone character is set to the
117 // asterisk character ('*'). 117 // asterisk character ('*').
118 //======================================================================= 118 //=======================================================================
119   119  
120 void LatLonToUtm (double a, double f, int& utmXZone, char& utmYZone, 120 void LatLonToUtm (double a, double f, int* utmXZone, char* utmYZone,
121 double& easting, double& northing, double lat, double lon) 121 double* easting, double* northing, double lat, double lon)
122 { 122 {
123 double recf; 123 double recf;
124 double b; 124 double b;
125 double eSquared; 125 double eSquared;
126 double e2Squared; 126 double e2Squared;
127 double tn; 127 double tn;
128 double ap; 128 double ap;
129 double bp; 129 double bp;
130 double cp; 130 double cp;
131 double dp; 131 double dp;
132 double ep; 132 double ep;
133 double olam; 133 double olam;
134 double dlam; 134 double dlam;
135 double s; 135 double s;
136 double c; 136 double c;
137 double t; 137 double t;
138 double eta; 138 double eta;
139 double sn; 139 double sn;
140 double tmd; 140 double tmd;
141 double t1, t2, t3, t6, t7; 141 double t1, t2, t3, t6, t7;
142 double nfn; 142 double nfn;
143   143  
144 if (lon <= 0.0) { 144 if (lon <= 0.0) {
145 utmXZone = 30 + (int)(lon / 6.0); 145 *utmXZone = 30 + (int)(lon / 6.0);
146 } else { 146 } else {
147 utmXZone = 31 + (int)(lon / 6.0); 147 *utmXZone = 31 + (int)(lon / 6.0);
148 } 148 }
149 if (lat < 84.0 && lat >= 72.0) { 149 if (lat < 84.0 && lat >= 72.0) {
150 // Special case: zone X is 12 degrees from north to south, not 8. 150 // Special case: zone X is 12 degrees from north to south, not 8.
151 utmYZone = cArray[19]; 151 *utmYZone = cArray[19];
152 } else { 152 } else {
153 utmYZone = cArray[(int)((lat + 80.0) / 8.0)]; 153 *utmYZone = cArray[(int)((lat + 80.0) / 8.0)];
154 } 154 }
155 if (lat >= 84.0 || lat < -80.0) { 155 if (lat >= 84.0 || lat < -80.0) {
156 // Invalid coordinate; the vertical zone is set to the invalid 156 // Invalid coordinate; the vertical zone is set to the invalid
157 // character. 157 // character.
158 utmYZone = '*'; 158 *utmYZone = '*';
159 } 159 }
160   160  
-   161 double latRad = lat * (M_PI/180);
-   162 double lonRad = lon * (M_PI/180);
161 double latRad = lat * deg2rad; 163 // double latRad = lat * deg2rad;
162 double lonRad = lon * deg2rad; 164 // double lonRad = lon * deg2rad;
163 recf = 1.0 / f; 165 recf = 1.0 / f;
164 b = a * (recf - 1.0) / recf; 166 b = a * (recf - 1.0) / recf;
165 eSquared = CalculateESquared (a, b); 167 eSquared = CalculateESquared (a, b);
166 e2Squared = CalculateE2Squared (a, b); 168 e2Squared = CalculateE2Squared (a, b);
167 tn = (a - b) / (a + b); 169 tn = (a - b) / (a + b);
168 ap = a * (1.0 - tn + 5.0 * ((tn * tn) - (tn * tn * tn)) / 4.0 + 81.0 * 170 ap = a * (1.0 - tn + 5.0 * ((tn * tn) - (tn * tn * tn)) / 4.0 + 81.0 *
169 ((tn * tn * tn * tn) - (tn * tn * tn * tn * tn)) / 64.0); 171 ((tn * tn * tn * tn) - (tn * tn * tn * tn * tn)) / 64.0);
170 bp = 3.0 * a * (tn - (tn * tn) + 7.0 * ((tn * tn * tn) 172 bp = 3.0 * a * (tn - (tn * tn) + 7.0 * ((tn * tn * tn)
171 - (tn * tn * tn * tn)) / 8.0 + 55.0 * (tn * tn * tn * tn * tn) / 64.0) 173 - (tn * tn * tn * tn)) / 8.0 + 55.0 * (tn * tn * tn * tn * tn) / 64.0)
172 / 2.0; 174 / 2.0;
173 cp = 15.0 * a * ((tn * tn) - (tn * tn * tn) + 3.0 * ((tn * tn * tn * tn) 175 cp = 15.0 * a * ((tn * tn) - (tn * tn * tn) + 3.0 * ((tn * tn * tn * tn)
174 - (tn * tn * tn * tn * tn)) / 4.0) / 16.0; 176 - (tn * tn * tn * tn * tn)) / 4.0) / 16.0;
175 dp = 35.0 * a * ((tn * tn * tn) - (tn * tn * tn * tn) + 11.0 177 dp = 35.0 * a * ((tn * tn * tn) - (tn * tn * tn * tn) + 11.0
176 * (tn * tn * tn * tn * tn) / 16.0) / 48.0; 178 * (tn * tn * tn * tn * tn) / 16.0) / 48.0;
177 ep = 315.0 * a * ((tn * tn * tn * tn) - (tn * tn * tn * tn * tn)) / 512.0; 179 ep = 315.0 * a * ((tn * tn * tn * tn) - (tn * tn * tn * tn * tn)) / 512.0;
178 olam = (utmXZone * 6 - 183) * deg2rad; 180 olam = (*utmXZone * 6 - 183) * (M_PI/180);
179 dlam = lonRad - olam; 181 dlam = lonRad - olam;
180 s = sin (latRad); 182 s = sin (latRad);
181 c = cos (latRad); 183 c = cos (latRad);
182 t = s / c; 184 t = s / c;
183 eta = e2Squared * (c * c); 185 eta = e2Squared * (c * c);
184 sn = sphsn (a, eSquared, latRad); 186 sn = sphsn (a, eSquared, latRad);
185 tmd = sphtmd (ap, bp, cp, dp, ep, latRad); 187 tmd = sphtmd (ap, bp, cp, dp, ep, latRad);
186 t1 = tmd * ok; 188 t1 = tmd * ok;
187 t2 = sn * s * c * ok / 2.0; 189 t2 = sn * s * c * ok / 2.0;
188 t3 = sn * s * (c * c * c) * ok * (5.0 - (t * t) + 9.0 * eta + 4.0 190 t3 = sn * s * (c * c * c) * ok * (5.0 - (t * t) + 9.0 * eta + 4.0
189 * (eta * eta)) / 24.0; 191 * (eta * eta)) / 24.0;
190 if (latRad < 0.0) nfn = 10000000.0; else nfn = 0; 192 if (latRad < 0.0) nfn = 10000000.0; else nfn = 0;
191 northing = nfn + t1 + (dlam * dlam) * t2 + (dlam * dlam * dlam 193 *northing = nfn + t1 + (dlam * dlam) * t2 + (dlam * dlam * dlam
192 * dlam) * t3 + (dlam * dlam * dlam * dlam * dlam * dlam) + 0.5; 194 * dlam) * t3 + (dlam * dlam * dlam * dlam * dlam * dlam) + 0.5;
193 t6 = sn * c * ok; 195 t6 = sn * c * ok;
194 t7 = sn * (c * c * c) * (1.0 - (t * t) + eta) / 6.0; 196 t7 = sn * (c * c * c) * (1.0 - (t * t) + eta) / 6.0;
195 easting = fe + dlam * t6 + (dlam * dlam * dlam) * t7 + 0.5; 197 *easting = fe + dlam * t6 + (dlam * dlam * dlam) * t7 + 0.5;
196 if (northing >= 9999999.0) northing = 9999999.0; 198 if (*northing >= 9999999.0) *northing = 9999999.0;
197 } 199 }
198   200  
199 //======================================================================= 201 //=======================================================================
200 // Purpose: 202 // Purpose:
201 // This function converts the specified lat/lon coordinate to a UTM 203 // This function converts the specified lat/lon coordinate to a UTM
202 // coordinate in the WGS84 datum. (See the comment block for the 204 // coordinate in the WGS84 datum. (See the comment block for the
203 // LatLonToUtm() member function.) 205 // LatLonToUtm() member function.)
204 //======================================================================= 206 //=======================================================================
205   207  
206 void LatLonToUtmWGS84 (int& utmXZone, char& utmYZone, 208 void LatLonToUtmWGS84 (int* utmXZone, char* utmYZone,
207 double& easting, double& northing, double lat, double lon) 209 double* easting, double* northing, double lat, double lon)
208 { 210 {
209 LatLonToUtm (6378137.0, 1 / 298.257223563, utmXZone, utmYZone, 211 LatLonToUtm (6378137.0, 1 / 298.257223563, utmXZone, utmYZone,
210 easting, northing, lat, lon); 212 easting, northing, lat, lon);
211 } 213 }
212   -  
213   -  
214   -  
215   -  
216 //======================================================================= -  
217 // Purpose: -  
218 // This function converts the specified UTM coordinate to a lat/lon -  
219 // coordinate. -  
220 // Pre: -  
221 // - utmXZone must be between 1 and 60, inclusive. -  
222 // - utmYZone must be one of: CDEFGHJKLMNPQRSTUVWX -  
223 // Parameters: -  
224 // double a: -  
225 // Ellipsoid semi-major axis, in meters. (For WGS84 datum, use 6378137.0) -  
226 // double f: -  
227 // Ellipsoid flattening. (For WGS84 datum, use 1 / 298.257223563) -  
228 // int utmXZone: -  
229 // The horizontal zone number of the UTM coordinate. -  
230 // char utmYZone: -  
231 // The vertical zone letter of the UTM coordinate. -  
232 // double easting, double northing: -  
233 // The UTM coordinate to convert. -  
234 // double& lat: -  
235 // Upon exit, lat contains the latitude. -  
236 // double& lon: -  
237 // Upon exit, lon contains the longitude. -  
238 // Notes: -  
239 // The code in this function is a C conversion of some of the source code -  
240 // from the Mapping Datum Transformation Software (MADTRAN) program, written -  
241 // in PowerBasic. To get the source code for MADTRAN, go to: -  
242 // -  
243 // http://164.214.2.59/publications/guides/MADTRAN/index.html -  
244 // -  
245 // and download MADTRAN.ZIP -  
246 //======================================================================= -  
247   -  
248 void UtmToLatLon (double a, double f, int utmXZone, char utmYZone, -  
249 double easting, double northing, double& lat, double& lon) -  
250 { -  
251 double recf; -  
252 double b; -  
253 double eSquared; -  
254 double e2Squared; -  
255 double tn; -  
256 double ap; -  
257 double bp; -  
258 double cp; -  
259 double dp; -  
260 double ep; -  
261 double nfn; -  
262 double tmd; -  
263 double sr; -  
264 double sn; -  
265 double ftphi; -  
266 double s; -  
267 double c; -  
268 double t; -  
269 double eta; -  
270 double de; -  
271 double dlam; -  
272 double olam; -  
273   -  
274 recf = 1.0 / f; -  
275 b = a * (recf - 1) / recf; -  
276 eSquared = CalculateESquared (a, b); -  
277 e2Squared = CalculateE2Squared (a, b); -  
278 tn = (a - b) / (a + b); -  
279 ap = a * (1.0 - tn + 5.0 * ((tn * tn) - (tn * tn * tn)) / 4.0 + 81.0 * -  
280 ((tn * tn * tn * tn) - (tn * tn * tn * tn * tn)) / 64.0); -  
281 bp = 3.0 * a * (tn - (tn * tn) + 7.0 * ((tn * tn * tn) -  
282 - (tn * tn * tn * tn)) / 8.0 + 55.0 * (tn * tn * tn * tn * tn) / 64.0) -  
283 / 2.0; -  
284 cp = 15.0 * a * ((tn * tn) - (tn * tn * tn) + 3.0 * ((tn * tn * tn * tn) -  
285 - (tn * tn * tn * tn * tn)) / 4.0) / 16.0; -  
286 dp = 35.0 * a * ((tn * tn * tn) - (tn * tn * tn * tn) + 11.0 -  
287 * (tn * tn * tn * tn * tn) / 16.0) / 48.0; -  
288 ep = 315.0 * a * ((tn * tn * tn * tn) - (tn * tn * tn * tn * tn)) / 512.0; -  
289 if ((utmYZone <= 'M' && utmYZone >= 'C') -  
290 || (utmYZone <= 'm' && utmYZone >= 'c')) { -  
291 nfn = 10000000.0; -  
292 } else { -  
293 nfn = 0; -  
294 } -  
295 tmd = (northing - nfn) / ok; -  
296 sr = sphsr (a, eSquared, 0.0); -  
297 ftphi = tmd / sr; -  
298 double t10, t11, t14, t15; -  
299 for (int i = 0; i < 5; i++) { -  
300 t10 = sphtmd (ap, bp, cp, dp, ep, ftphi); -  
301 sr = sphsr (a, eSquared, ftphi); -  
302 ftphi = ftphi + (tmd - t10) / sr; -  
303 } -  
304 sr = sphsr (a, eSquared, ftphi); -  
305 sn = sphsn (a, eSquared, ftphi); -  
306 s = sin (ftphi); -  
307 c = cos (ftphi); -  
308 t = s / c; -  
309 eta = e2Squared * (c * c); -  
310 de = easting - fe; -  
311 t10 = t / (2.0 * sr * sn * (ok * ok)); -  
312 t11 = t * (5.0 + 3.0 * (t * t) + eta - 4.0 * (eta * eta) - 9.0 * (t * t) -  
313 * eta) / (24.0 * sr * (sn * sn * sn) * (ok * ok * ok * ok)); -  
314 lat = ftphi - (de * de) * t10 + (de * de * de * de) * t11; -  
315 t14 = 1.0 / (sn * c * ok); -  
316 t15 = (1.0 + 2.0 * (t * t) + eta) / (6 * (sn * sn * sn) * c -  
317 * (ok * ok * ok)); -  
318 dlam = de * t14 - (de * de * de) * t15; -  
319 olam = (utmXZone * 6 - 183.0) * deg2rad; -  
320 lon = olam + dlam; -  
321 lon *= rad2deg; -  
322 lat *= rad2deg; -  
323 } -  
324   -  
325 //======================================================================= -  
326 // Purpose: -  
327 // This function converts the specified UTM coordinate to a lat/lon -  
328 // coordinate in the WGS84 datum. (See the comment block for the -  
329 // UtmToLatLon() member function. -  
330 //======================================================================= -  
331   -  
332 void UtmToLatLonWGS84 (int utmXZone, char utmYZone, double easting, -  
333 double northing, double& lat, double& lon) -  
334 { -  
335 UtmToLatLon (6378137.0, 1 / 298.257223563, utmXZone, utmYZone, -  
336 easting, northing, lat, lon); -  
337 } -  
338   -  
339 //======================================================================= -  
340 /** -  
341 @func Build a position string -  
342 @parm target. must be 30 characters or longer. -  
343 */ -  
344 //======================================================================= -  
345   -  
346 void CUtmPoint::GetString(TCHAR *position) const -  
347 { -  
348 _stprintf(position, -  
349 _T("%02d%c %06d %07d"), -  
350 m_xzone, m_yzone, -  
351 (int)m_easting, -  
352 (int)m_northing); -  
353 } -  
354   -  
355 //======================================================================= -  
356 /** -  
357 @func get the position of a UTM point -  
358 @parm point out -  
359 */ -  
360 //======================================================================= -  
361   -  
362 void CUtmPoint::ToPosition(CPosition &pos) const -  
363 { -  
364 double lat,lon; -  
365 UtmToLatLonWGS84(m_xzone,m_yzone,m_easting,m_northing, -  
366 lat,lon); -  
367 pos.Clear(); -  
368 pos.SetLatitude(lat); -  
369 pos.SetLongitude(lon); -  
370 } -  
371   -  
372 //======================================================================= -  
373 /** -  
374 @func turn a position into a UTM point -  
375 @parm position -  
376 @rdesc true if it was in range -  
377 */ -  
378 //======================================================================= -  
379   -  
380 bool CUtmPoint::FromPosition(const CPosition &pos) -  
381 { -  
382 Clear(); -  
383 if(!IsPositionInUtmSpace(pos)) -  
384 return false; -  
385 LatLonToUtmWGS84(m_xzone,m_yzone,m_easting,m_northing, -  
386 pos.GetLatitude(), -  
387 pos.GetLongitude()); -  
388 return true; -  
389 } -