conversions.h
Go to the documentation of this file.
1 /* Taken from utexas-art-ros-pkg:art_vehicle/applanix */
2 
3 /*
4  * Conversions between coordinate systems.
5  *
6  * Includes LatLong<->UTM.
7  */
8 
9 #ifndef _UTM_H
10 #define _UTM_H
11 
23 #include <cmath>
24 #include <cstdio>
25 #include <cstdlib>
26 
27 namespace gps_common
28 {
29 
30 const double RADIANS_PER_DEGREE = M_PI/180.0;
31 const double DEGREES_PER_RADIAN = 180.0/M_PI;
32 
33 // WGS84 Parameters
34 const double WGS84_A = 6378137.0; // major axis
35 const double WGS84_B = 6356752.31424518; // minor axis
36 const double WGS84_F = 0.0033528107; // ellipsoid flattening
37 const double WGS84_E = 0.0818191908; // first eccentricity
38 const double WGS84_EP = 0.0820944379; // second eccentricity
39 
40 // UTM Parameters
41 const double UTM_K0 = 0.9996; // scale factor
42 const double UTM_FE = 500000.0; // false easting
43 const double UTM_FN_N = 0.0; // false northing on north hemisphere
44 const double UTM_FN_S = 10000000.0; // false northing on south hemisphere
45 const double UTM_E2 = (WGS84_E*WGS84_E); // e^2
46 const double UTM_E4 = (UTM_E2*UTM_E2); // e^4
47 const double UTM_E6 = (UTM_E4*UTM_E2); // e^6
48 const double UTM_EP2 = (UTM_E2/(1-UTM_E2)); // e'^2
49 
57 static inline void UTM(double lat, double lon, double *x, double *y)
58 {
59  // constants
60  const static double m0 = (1 - UTM_E2/4 - 3*UTM_E4/64 - 5*UTM_E6/256);
61  const static double m1 = -(3*UTM_E2/8 + 3*UTM_E4/32 + 45*UTM_E6/1024);
62  const static double m2 = (15*UTM_E4/256 + 45*UTM_E6/1024);
63  const static double m3 = -(35*UTM_E6/3072);
64 
65  // compute the central meridian
66  int cm = ((lon >= 0.0)
67  ? ((int)lon - ((int)lon)%6 + 3)
68  : ((int)lon - ((int)lon)%6 - 3));
69 
70  // convert degrees into radians
71  double rlat = lat * RADIANS_PER_DEGREE;
72  double rlon = lon * RADIANS_PER_DEGREE;
73  double rlon0 = cm * RADIANS_PER_DEGREE;
74 
75  // compute trigonometric functions
76  double slat = sin(rlat);
77  double clat = cos(rlat);
78  double tlat = tan(rlat);
79 
80  // decide the false northing at origin
81  double fn = (lat > 0) ? UTM_FN_N : UTM_FN_S;
82 
83  double T = tlat * tlat;
84  double C = UTM_EP2 * clat * clat;
85  double A = (rlon - rlon0) * clat;
86  double M = WGS84_A * (m0*rlat + m1*sin(2*rlat)
87  + m2*sin(4*rlat) + m3*sin(6*rlat));
88  double V = WGS84_A / sqrt(1 - UTM_E2*slat*slat);
89 
90  // compute the easting-northing coordinates
91  *x = UTM_FE + UTM_K0 * V * (A + (1-T+C)*pow(A,3)/6
92  + (5-18*T+T*T+72*C-58*UTM_EP2)*pow(A,5)/120);
93  *y = fn + UTM_K0 * (M + V * tlat * (A*A/2
94  + (5-T+9*C+4*C*C)*pow(A,4)/24
95  + ((61-58*T+T*T+600*C-330*UTM_EP2)
96  * pow(A,6)/720)));
97 
98  return;
99 }
100 
101 
110 static inline char UTMLetterDesignator(double Lat)
111 {
112  char LetterDesignator;
113 
114  if ((84 >= Lat) && (Lat >= 72)) LetterDesignator = 'X';
115  else if ((72 > Lat) && (Lat >= 64)) LetterDesignator = 'W';
116  else if ((64 > Lat) && (Lat >= 56)) LetterDesignator = 'V';
117  else if ((56 > Lat) && (Lat >= 48)) LetterDesignator = 'U';
118  else if ((48 > Lat) && (Lat >= 40)) LetterDesignator = 'T';
119  else if ((40 > Lat) && (Lat >= 32)) LetterDesignator = 'S';
120  else if ((32 > Lat) && (Lat >= 24)) LetterDesignator = 'R';
121  else if ((24 > Lat) && (Lat >= 16)) LetterDesignator = 'Q';
122  else if ((16 > Lat) && (Lat >= 8)) LetterDesignator = 'P';
123  else if (( 8 > Lat) && (Lat >= 0)) LetterDesignator = 'N';
124  else if (( 0 > Lat) && (Lat >= -8)) LetterDesignator = 'M';
125  else if ((-8 > Lat) && (Lat >= -16)) LetterDesignator = 'L';
126  else if((-16 > Lat) && (Lat >= -24)) LetterDesignator = 'K';
127  else if((-24 > Lat) && (Lat >= -32)) LetterDesignator = 'J';
128  else if((-32 > Lat) && (Lat >= -40)) LetterDesignator = 'H';
129  else if((-40 > Lat) && (Lat >= -48)) LetterDesignator = 'G';
130  else if((-48 > Lat) && (Lat >= -56)) LetterDesignator = 'F';
131  else if((-56 > Lat) && (Lat >= -64)) LetterDesignator = 'E';
132  else if((-64 > Lat) && (Lat >= -72)) LetterDesignator = 'D';
133  else if((-72 > Lat) && (Lat >= -80)) LetterDesignator = 'C';
134  // 'Z' is an error flag, the Latitude is outside the UTM limits
135  else LetterDesignator = 'Z';
136  return LetterDesignator;
137 }
138 
148 static inline void LLtoUTM(const double Lat, const double Long,
149  double &UTMNorthing, double &UTMEasting,
150  char* UTMZone)
151 {
152  double a = WGS84_A;
153  double eccSquared = UTM_E2;
154  double k0 = UTM_K0;
155 
156  double LongOrigin;
157  double eccPrimeSquared;
158  double N, T, C, A, M;
159 
160  //Make sure the longitude is between -180.00 .. 179.9
161  double LongTemp = (Long+180)-int((Long+180)/360)*360-180;
162 
163  double LatRad = Lat*RADIANS_PER_DEGREE;
164  double LongRad = LongTemp*RADIANS_PER_DEGREE;
165  double LongOriginRad;
166  int ZoneNumber;
167 
168  ZoneNumber = int((LongTemp + 180)/6) + 1;
169 
170  if( Lat >= 56.0 && Lat < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0 )
171  ZoneNumber = 32;
172 
173  // Special zones for Svalbard
174  if( Lat >= 72.0 && Lat < 84.0 )
175  {
176  if( LongTemp >= 0.0 && LongTemp < 9.0 ) ZoneNumber = 31;
177  else if( LongTemp >= 9.0 && LongTemp < 21.0 ) ZoneNumber = 33;
178  else if( LongTemp >= 21.0 && LongTemp < 33.0 ) ZoneNumber = 35;
179  else if( LongTemp >= 33.0 && LongTemp < 42.0 ) ZoneNumber = 37;
180  }
181  // +3 puts origin in middle of zone
182  LongOrigin = (ZoneNumber - 1)*6 - 180 + 3;
183  LongOriginRad = LongOrigin * RADIANS_PER_DEGREE;
184 
185  //compute the UTM Zone from the latitude and longitude
186  snprintf(UTMZone, 13, "%d%c", ZoneNumber, UTMLetterDesignator(Lat));
187 
188  eccPrimeSquared = (eccSquared)/(1-eccSquared);
189 
190  N = a/sqrt(1-eccSquared*sin(LatRad)*sin(LatRad));
191  T = tan(LatRad)*tan(LatRad);
192  C = eccPrimeSquared*cos(LatRad)*cos(LatRad);
193  A = cos(LatRad)*(LongRad-LongOriginRad);
194 
195  M = a*((1 - eccSquared/4 - 3*eccSquared*eccSquared/64 - 5*eccSquared*eccSquared*eccSquared/256)*LatRad
196  - (3*eccSquared/8 + 3*eccSquared*eccSquared/32 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(2*LatRad)
197  + (15*eccSquared*eccSquared/256 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(4*LatRad)
198  - (35*eccSquared*eccSquared*eccSquared/3072)*sin(6*LatRad));
199 
200  UTMEasting = (double)(k0*N*(A+(1-T+C)*A*A*A/6
201  + (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120)
202  + 500000.0);
203 
204  UTMNorthing = (double)(k0*(M+N*tan(LatRad)*(A*A/2+(5-T+9*C+4*C*C)*A*A*A*A/24
205  + (61-58*T+T*T+600*C-330*eccPrimeSquared)*A*A*A*A*A*A/720)));
206  if(Lat < 0)
207  UTMNorthing += 10000000.0; //10000000 meter offset for southern hemisphere
208 }
209 
210 static inline void LLtoUTM(const double Lat, const double Long,
211  double &UTMNorthing, double &UTMEasting,
212  std::string &UTMZone) {
213  char zone_buf[13] = {0};
214 
215  LLtoUTM(Lat, Long, UTMNorthing, UTMEasting, zone_buf);
216 
217  UTMZone = zone_buf;
218 }
219 
220 
230 static inline void UTMtoLL(const double UTMNorthing, const double UTMEasting,
231  const char* UTMZone, double& Lat, double& Long )
232 {
233  double k0 = UTM_K0;
234  double a = WGS84_A;
235  double eccSquared = UTM_E2;
236  double eccPrimeSquared;
237  double e1 = (1-sqrt(1-eccSquared))/(1+sqrt(1-eccSquared));
238  double N1, T1, C1, R1, D, M;
239  double LongOrigin;
240  double mu, phi1Rad;
241  double x, y;
242  int ZoneNumber;
243  char* ZoneLetter;
244 
245  x = UTMEasting - 500000.0; //remove 500,000 meter offset for longitude
246  y = UTMNorthing;
247 
248  ZoneNumber = strtoul(UTMZone, &ZoneLetter, 10);
249  if((*ZoneLetter - 'N') < 0)
250  {
251  y -= 10000000.0;//remove 10,000,000 meter offset used for southern hemisphere
252  }
253 
254  LongOrigin = (ZoneNumber - 1)*6 - 180 + 3; //+3 puts origin in middle of zone
255 
256  eccPrimeSquared = (eccSquared)/(1-eccSquared);
257 
258  M = y / k0;
259  mu = M/(a*(1-eccSquared/4-3*eccSquared*eccSquared/64-5*eccSquared*eccSquared*eccSquared/256));
260 
261  phi1Rad = mu + (3*e1/2-27*e1*e1*e1/32)*sin(2*mu)
262  + (21*e1*e1/16-55*e1*e1*e1*e1/32)*sin(4*mu)
263  +(151*e1*e1*e1/96)*sin(6*mu);
264 
265  N1 = a/sqrt(1-eccSquared*sin(phi1Rad)*sin(phi1Rad));
266  T1 = tan(phi1Rad)*tan(phi1Rad);
267  C1 = eccPrimeSquared*cos(phi1Rad)*cos(phi1Rad);
268  R1 = a*(1-eccSquared)/pow(1-eccSquared*sin(phi1Rad)*sin(phi1Rad), 1.5);
269  D = x/(N1*k0);
270 
271  Lat = phi1Rad - (N1*tan(phi1Rad)/R1)*(D*D/2-(5+3*T1+10*C1-4*C1*C1-9*eccPrimeSquared)*D*D*D*D/24
272  +(61+90*T1+298*C1+45*T1*T1-252*eccPrimeSquared-3*C1*C1)*D*D*D*D*D*D/720);
273  Lat = Lat * DEGREES_PER_RADIAN;
274 
275  Long = (D-(1+2*T1+C1)*D*D*D/6+(5-2*C1+28*T1-3*C1*C1+8*eccPrimeSquared+24*T1*T1)
276  *D*D*D*D*D/120)/cos(phi1Rad);
277  Long = LongOrigin + Long * DEGREES_PER_RADIAN;
278 
279 }
280 
281 static inline void UTMtoLL(const double UTMNorthing, const double UTMEasting,
282  std::string UTMZone, double& Lat, double& Long) {
283  UTMtoLL(UTMNorthing, UTMEasting, UTMZone.c_str(), Lat, Long);
284 }
285 
286 } // end namespace UTM
287 
288 #endif // _UTM_H
gps_common::UTMLetterDesignator
static char UTMLetterDesignator(double Lat)
Definition: conversions.h:110
gps_common::WGS84_A
const double WGS84_A
Definition: conversions.h:34
gps_common::UTM_FN_S
const double UTM_FN_S
Definition: conversions.h:44
gps_common::UTM_K0
const double UTM_K0
Definition: conversions.h:41
gps_common::UTM_E2
const double UTM_E2
Definition: conversions.h:45
gps_common::RADIANS_PER_DEGREE
const double RADIANS_PER_DEGREE
Definition: conversions.h:30
gps_common::WGS84_EP
const double WGS84_EP
Definition: conversions.h:38
gps_common::UTM_E4
const double UTM_E4
Definition: conversions.h:46
gps_common::WGS84_E
const double WGS84_E
Definition: conversions.h:37
gps_common::UTM_E6
const double UTM_E6
Definition: conversions.h:47
gps_common::UTM
static void UTM(double lat, double lon, double *x, double *y)
Definition: conversions.h:57
gps_common::UTM_FN_N
const double UTM_FN_N
Definition: conversions.h:43
gps_common::WGS84_B
const double WGS84_B
Definition: conversions.h:35
gps_common::UTMtoLL
static void UTMtoLL(const double UTMNorthing, const double UTMEasting, const char *UTMZone, double &Lat, double &Long)
Definition: conversions.h:230
gps_common::WGS84_F
const double WGS84_F
Definition: conversions.h:36
gps_common::DEGREES_PER_RADIAN
const double DEGREES_PER_RADIAN
Definition: conversions.h:31
gps_common
Definition: conversions.h:27
gps_common::UTM_FE
const double UTM_FE
Definition: conversions.h:42
gps_common::LLtoUTM
static void LLtoUTM(const double Lat, const double Long, double &UTMNorthing, double &UTMEasting, char *UTMZone)
Definition: conversions.h:148
gps_common::UTM_EP2
const double UTM_EP2
Definition: conversions.h:48


gps_common
Author(s): Timo Roehling , P. J. Reed
autogenerated on Sat Sep 23 2023 02:09:18