40 #ifndef ROBOT_LOCALIZATION_NAVSAT_CONVERSIONS_H 41 #define ROBOT_LOCALIZATION_NAVSAT_CONVERSIONS_H 59 namespace NavsatConversions
69 #define WGS84_A 6378137.0 // major axis 70 #define WGS84_B 6356752.31424518 // minor axis 71 #define WGS84_F 0.0033528107 // ellipsoid flattening 72 #define WGS84_E 0.0818191908 // first eccentricity 73 #define WGS84_EP 0.0820944379 // second eccentricity 76 #define UTM_K0 0.9996 // scale factor 77 #define UTM_FE 500000.0 // false easting 78 #define UTM_FN_N 0.0 // false northing, northern hemisphere 79 #define UTM_FN_S 10000000.0 // false northing, southern hemisphere 80 #define UTM_E2 (WGS84_E*WGS84_E) // e^2 81 #define UTM_E4 (UTM_E2*UTM_E2) // e^4 82 #define UTM_E6 (UTM_E4*UTM_E2) // e^6 83 #define UTM_EP2 (UTM_E2/(1-UTM_E2)) // e'^2 94 static inline void UTM(
double lat,
double lon,
double *x,
double *y)
99 static const double m2 = (15*
UTM_E4/256 + 45*
UTM_E6/1024);
100 static const double m3 = -(35*
UTM_E6/3072);
103 int cm = ((lon >= 0.0)
104 ? (
static_cast<int>(lon) - (static_cast<int>(lon)) % 6 + 3)
105 : (
static_cast<int>(lon) - (static_cast<int>(lon)) % 6 - 3));
113 double slat =
sin(rlat);
114 double clat =
cos(rlat);
115 double tlat =
tan(rlat);
120 double T = tlat * tlat;
121 double C =
UTM_EP2 * clat * clat;
122 double A = (rlon - rlon0) * clat;
123 double M =
WGS84_A * (m0*rlat + m1*
sin(2*rlat)
124 + m2*
sin(4*rlat) + m3*
sin(6*rlat));
129 + (5-18*T+T*T+72*C-58*
UTM_EP2)*
pow(A, 5)/120);
130 *y = fn +
UTM_K0 * (M + V * tlat * (A*A/2
131 + (5-T+9*C+4*C*C)*
pow(A, 4)/24
132 + ((61-58*T+T*T+600*C-330*
UTM_EP2)
149 char LetterDesignator;
151 if ((84 >= Lat) && (Lat >= 72)) LetterDesignator =
'X';
152 else if ((72 > Lat) && (Lat >= 64)) LetterDesignator =
'W';
153 else if ((64 > Lat) && (Lat >= 56)) LetterDesignator =
'V';
154 else if ((56 > Lat) && (Lat >= 48)) LetterDesignator =
'U';
155 else if ((48 > Lat) && (Lat >= 40)) LetterDesignator =
'T';
156 else if ((40 > Lat) && (Lat >= 32)) LetterDesignator =
'S';
157 else if ((32 > Lat) && (Lat >= 24)) LetterDesignator =
'R';
158 else if ((24 > Lat) && (Lat >= 16)) LetterDesignator =
'Q';
159 else if ((16 > Lat) && (Lat >= 8)) LetterDesignator =
'P';
160 else if (( 8 > Lat) && (Lat >= 0)) LetterDesignator =
'N';
161 else if (( 0 > Lat) && (Lat >= -8)) LetterDesignator =
'M';
162 else if ((-8 > Lat) && (Lat >= -16)) LetterDesignator =
'L';
163 else if ((-16 > Lat) && (Lat >= -24)) LetterDesignator =
'K';
164 else if ((-24 > Lat) && (Lat >= -32)) LetterDesignator =
'J';
165 else if ((-32 > Lat) && (Lat >= -40)) LetterDesignator =
'H';
166 else if ((-40 > Lat) && (Lat >= -48)) LetterDesignator =
'G';
167 else if ((-48 > Lat) && (Lat >= -56)) LetterDesignator =
'F';
168 else if ((-56 > Lat) && (Lat >= -64)) LetterDesignator =
'E';
169 else if ((-64 > Lat) && (Lat >= -72)) LetterDesignator =
'D';
170 else if ((-72 > Lat) && (Lat >= -80)) LetterDesignator =
'C';
172 else LetterDesignator =
'Z';
173 return LetterDesignator;
189 static inline void LLtoUTM(
const double Lat,
const double Long,
190 double &UTMNorthing,
double &UTMEasting,
191 std::string &UTMZone,
double &gamma)
194 double eccSquared =
UTM_E2;
198 double eccPrimeSquared;
199 double N, T, C, A, M;
202 double LongTemp = (Long+180)-static_cast<int>((Long+180)/360)*360-180;
206 double LongOriginRad;
209 ZoneNumber =
static_cast<int>((LongTemp + 180)/6) + 1;
211 if ( Lat >= 56.0 && Lat < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0 )
215 if ( Lat >= 72.0 && Lat < 84.0 )
217 if ( LongTemp >= 0.0 && LongTemp < 9.0 ) ZoneNumber = 31;
218 else if ( LongTemp >= 9.0 && LongTemp < 21.0 ) ZoneNumber = 33;
219 else if ( LongTemp >= 21.0 && LongTemp < 33.0 ) ZoneNumber = 35;
220 else if ( LongTemp >= 33.0 && LongTemp < 42.0 ) ZoneNumber = 37;
223 LongOrigin = (ZoneNumber - 1)*6 - 180 + 3;
227 char zone_buf[] = {0, 0, 0, 0};
229 UTMZone = std::string(zone_buf);
231 eccPrimeSquared = (eccSquared)/(1-eccSquared);
233 N = a/
sqrt(1-eccSquared*
sin(LatRad)*
sin(LatRad));
234 T =
tan(LatRad)*
tan(LatRad);
235 C = eccPrimeSquared*
cos(LatRad)*
cos(LatRad);
236 A = cos(LatRad)*(LongRad-LongOriginRad);
238 M = a*((1 - eccSquared/4 - 3*eccSquared*eccSquared/64
239 - 5*eccSquared*eccSquared*eccSquared/256) * LatRad
240 - (3*eccSquared/8 + 3*eccSquared*eccSquared/32
241 + 45*eccSquared*eccSquared*eccSquared/1024)*
sin(2*LatRad)
242 + (15*eccSquared*eccSquared/256
243 + 45*eccSquared*eccSquared*eccSquared/1024)*
sin(4*LatRad)
244 - (35*eccSquared*eccSquared*eccSquared/3072)*
sin(6*LatRad));
246 UTMEasting =
static_cast<double> 247 (k0*N*(A+(1-T+C)*A*A*A/6
248 + (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120)
251 UTMNorthing =
static_cast<double> 253 *(A*A/2+(5-T+9*C+4*C*C)*A*A*A*A/24
254 + (61-58*T+T*T+600*C-330*eccPrimeSquared)*A*A*A*A*A*A/720)));
261 UTMNorthing += 10000000.0;
274 static inline void LLtoUTM(
const double Lat,
const double Long,
275 double &UTMNorthing,
double &UTMEasting,
276 std::string &UTMZone)
279 LLtoUTM(Lat, Long, UTMNorthing, UTMEasting, UTMZone, gamma);
295 static inline void UTMtoLL(
const double UTMNorthing,
const double UTMEasting,
296 const std::string &UTMZone,
double& Lat,
double& Long,
double &gamma)
300 double eccSquared =
UTM_E2;
301 double eccPrimeSquared;
302 double e1 = (1-
sqrt(1-eccSquared))/(1+
sqrt(1-eccSquared));
303 double N1, T1, C1, R1, D, M;
310 x = UTMEasting - 500000.0;
313 ZoneNumber = strtoul(UTMZone.c_str(), &ZoneLetter, 10);
314 if ((*ZoneLetter -
'N') < 0)
321 LongOrigin = (ZoneNumber - 1)*6 - 180 + 3;
322 eccPrimeSquared = (eccSquared)/(1-eccSquared);
325 mu = M/(a*(1-eccSquared/4-3*eccSquared*eccSquared/64
326 -5*eccSquared*eccSquared*eccSquared/256));
328 phi1Rad = mu + ((3*e1/2-27*e1*e1*e1/32)*
sin(2*mu)
329 + (21*e1*e1/16-55*e1*e1*e1*e1/32)*
sin(4*mu)
330 + (151*e1*e1*e1/96)*
sin(6*mu));
332 N1 = a/
sqrt(1-eccSquared*
sin(phi1Rad)*
sin(phi1Rad));
333 T1 =
tan(phi1Rad)*
tan(phi1Rad);
334 C1 = eccPrimeSquared*
cos(phi1Rad)*
cos(phi1Rad);
335 R1 = a*(1-eccSquared)/
pow(1-eccSquared*
sin(phi1Rad)*
sin(phi1Rad), 1.5);
338 Lat = phi1Rad - ((N1*
tan(phi1Rad)/R1)
340 -(5+3*T1+10*C1-4*C1*C1-9*eccPrimeSquared)*D*D*D*D/24
341 +(61+90*T1+298*C1+45*T1*T1-252*eccPrimeSquared
342 -3*C1*C1)*D*D*D*D*D*D/720));
346 Long = ((D-(1+2*T1+C1)*D*D*D/6
347 +(5-2*C1+28*T1-3*C1*C1+8*eccPrimeSquared+24*T1*T1)
364 static inline void UTMtoLL(
const double UTMNorthing,
const double UTMEasting,
365 const std::string &UTMZone,
double& Lat,
double& Long)
368 UTMtoLL(UTMNorthing, UTMEasting, UTMZone, Lat, Long, gamma);
374 #endif // ROBOT_LOCALIZATION_NAVSAT_CONVERSIONS_H static void UTM(double lat, double lon, double *x, double *y)
INLINE Rall1d< T, V, S > tanh(const Rall1d< T, V, S > &arg)
INLINE Rall1d< T, V, S > atan(const Rall1d< T, V, S > &x)
const double RADIANS_PER_DEGREE
static char UTMLetterDesignator(double Lat)
static void LLtoUTM(const double Lat, const double Long, double &UTMNorthing, double &UTMEasting, std::string &UTMZone, double &gamma)
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
INLINE Rall1d< T, V, S > tan(const Rall1d< T, V, S > &arg)
static void UTMtoLL(const double UTMNorthing, const double UTMEasting, const std::string &UTMZone, double &Lat, double &Long, double &gamma)
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
const double DEGREES_PER_RADIAN