mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-09-28 15:08:51 +00:00
Code Issues Releases Wiki Activity

Avoid passing Armadillo vectors by value, code cleaning

Browse Source
This commit is contained in:
Carles Fernandez 2018-10-15 02:04:15 +02:00
parent 9710fa9430
commit 21eb9517fe
No known key found for this signature in database
GPG Key ID: 4C583C52B0C3877D
2 changed files with 158 additions and 197 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

338
src/tests/system-tests/libs/geofunctions.cc
View File

@ -28,11 +28,10 @@
* *
* ------------------------------------------------------------------------- * -------------------------------------------------------------------------
*/ */
#include "geofunctions.h"
#include <iomanip>
const double STRP_G_SI = 9.80665; #include "geofunctions.h"
const double STRP_PI = 3.1415926535898; //!< Pi as defined in IS-GPS-200E
const double STRP_PI = 3.1415926535898; // Pi as defined in IS-GPS-200E
arma::mat Skew_symmetric(const arma::vec &a) arma::mat Skew_symmetric(const arma::vec &a)
{ {
@ -206,17 +205,17 @@ int togeod(double *dphi, double *dlambda, double *h, double a, double finv, doub
cosphi = cos(*dphi); cosphi = cos(*dphi);
// compute radius of curvature in prime vertical direction // compute radius of curvature in prime vertical direction
N_phi = a / sqrt(1 - esq * sinphi * sinphi); N_phi = a / sqrt(1.0 - esq * sinphi * sinphi);
// compute residuals in P and Z // compute residuals in P and Z
dP = P - (N_phi + (*h)) * cosphi; dP = P - (N_phi + (*h)) * cosphi;
dZ = Z - (N_phi * oneesq + (*h)) * sinphi; dZ = Z - (N_phi * oneesq + (*h)) * sinphi;
// update height and latitude // update height and latitude
*h = *h + (sinphi * dZ + cosphi * dP); *h = *h + (sinphi * dZ + cosphi * dP);
*dphi = *dphi + (cosphi * dZ - sinphi * dP) / (N_phi + (*h)); *dphi = *dphi + (cosphi * dZ - sinphi * dP) / (N_phi + (*h));
// test for convergence // test for convergence
if ((dP * dP + dZ * dZ) < tolsq) if ((dP * dP + dZ * dZ) < tolsq)
{ {
break; break;
@ -234,7 +233,7 @@ int togeod(double *dphi, double *dlambda, double *h, double a, double finv, doub
arma::mat Gravity_ECEF(const arma::vec &r_eb_e) arma::mat Gravity_ECEF(const arma::vec &r_eb_e)
{ {
// Parameters // Parameters
const double R_0 = 6378137; // WGS84 Equatorial radius in meters const double R_0 = 6378137.0; // WGS84 Equatorial radius in meters
const double mu = 3.986004418E14; // WGS84 Earth gravitational constant (m^3 s^-2) const double mu = 3.986004418E14; // WGS84 Earth gravitational constant (m^3 s^-2)
const double J_2 = 1.082627E-3; // WGS84 Earth's second gravitational constant const double J_2 = 1.082627E-3; // WGS84 Earth's second gravitational constant
const double omega_ie = 7.292115E-5; // Earth rotation rate (rad/s) const double omega_ie = 7.292115E-5; // Earth rotation rate (rad/s)
@ -260,12 +259,14 @@ arma::mat Gravity_ECEF(const arma::vec &r_eb_e)
} }
arma::vec LLH_to_deg(arma::vec &LLH) arma::vec LLH_to_deg(const arma::vec &LLH)
{ {
const double rtd = 180.0 / STRP_PI; const double rtd = 180.0 / STRP_PI;
LLH(0) = LLH(0) * rtd; arma::vec deg = arma::zeros(3, 1);
LLH(1) = LLH(1) * rtd; deg(0) = LLH(0) * rtd;
return LLH; deg(1) = LLH(1) * rtd;
deg(2) = LLH(2);
return deg;
} }
@ -297,15 +298,16 @@ double mstokph(double MetersPerSeconds)
} }
arma::vec CTM_to_Euler(arma::mat &C) arma::vec CTM_to_Euler(const arma::mat &C)
{ {
// Calculate Euler angles using (2.23) // Calculate Euler angles using (2.23)
arma::mat CTM = C;
arma::vec eul = arma::zeros(3, 1); arma::vec eul = arma::zeros(3, 1);
eul(0) = atan2(C(1, 2), C(2, 2)); // roll eul(0) = atan2(CTM(1, 2), CTM(2, 2)); // roll
if (C(0, 2) < -1.0) C(0, 2) = -1.0; if (CTM(0, 2) < -1.0) CTM(0, 2) = -1.0;
if (C(0, 2) > 1.0) C(0, 2) = 1.0; if (CTM(0, 2) > 1.0) CTM(0, 2) = 1.0;
eul(1) = -asin(C(0, 2)); // pitch eul(1) = -asin(CTM(0, 2)); // pitch
eul(2) = atan2(C(0, 1), C(0, 0)); // yaw eul(2) = atan2(CTM(0, 1), CTM(0, 0)); // yaw
return eul; return eul;
} }
@ -354,19 +356,19 @@ arma::vec cart2geo(const arma::vec &XYZ, int elipsoid_selection)
do do
{ {
oldh = h; oldh = h;
N = c / sqrt(1 + ex2 * (cos(phi) * cos(phi))); N = c / sqrt(1.0 + ex2 * (cos(phi) * cos(phi)));
phi = atan(XYZ[2] / ((sqrt(XYZ[0] * XYZ[0] + XYZ[1] * XYZ[1]) * (1.0 - (2.0 - f[elipsoid_selection]) * f[elipsoid_selection] * N / (N + h))))); phi = atan(XYZ[2] / ((sqrt(XYZ[0] * XYZ[0] + XYZ[1] * XYZ[1]) * (1.0 - (2.0 - f[elipsoid_selection]) * f[elipsoid_selection] * N / (N + h)))));
h = sqrt(XYZ[0] * XYZ[0] + XYZ[1] * XYZ[1]) / cos(phi) - N; h = sqrt(XYZ[0] * XYZ[0] + XYZ[1] * XYZ[1]) / cos(phi) - N;
iterations = iterations + 1; iterations = iterations + 1;
if (iterations > 100) if (iterations > 100)
{ {
//std::cout << "Failed to approximate h with desired precision. h-oldh= " << h - oldh; // std::cout << "Failed to approximate h with desired precision. h-oldh= " << h - oldh;
break; break;
} }
} }
while (std::fabs(h - oldh) > 1.0e-12); while (std::fabs(h - oldh) > 1.0e-12);
arma::vec LLH = {{phi, lambda, h}}; //radians arma::vec LLH = {{phi, lambda, h}}; // radians
return LLH; return LLH;
} }
@ -399,11 +401,11 @@ void ECEF_to_Geo(const arma::vec &r_eb_e, const arma::vec &v_eb_e, const arma::m
void Geo_to_ECEF(const arma::vec &LLH, const arma::vec &v_eb_n, const arma::mat &C_b_n, arma::vec &r_eb_e, arma::vec &v_eb_e, arma::mat &C_b_e) void Geo_to_ECEF(const arma::vec &LLH, const arma::vec &v_eb_n, const arma::mat &C_b_n, arma::vec &r_eb_e, arma::vec &v_eb_e, arma::mat &C_b_e)
{ {
// Parameters // Parameters
double R_0 = 6378137; // WGS84 Equatorial radius in meters double R_0 = 6378137.0; // WGS84 Equatorial radius in meters
double e = 0.0818191908425; // WGS84 eccentricity double e = 0.0818191908425; // WGS84 eccentricity
// Calculate transverse radius of curvature using (2.105) // Calculate transverse radius of curvature using (2.105)
double R_E = R_0 / sqrt(1 - (e * sin(LLH(0))) * (e * sin(LLH(0)))); double R_E = R_0 / sqrt(1.0 - (e * sin(LLH(0))) * (e * sin(LLH(0))));
// Convert position using (2.112) // Convert position using (2.112)
double cos_lat = cos(LLH(0)); double cos_lat = cos(LLH(0));
@ -435,7 +437,7 @@ void Geo_to_ECEF(const arma::vec &LLH, const arma::vec &v_eb_n, const arma::mat
void pv_Geo_to_ECEF(double L_b, double lambda_b, double h_b, const arma::vec &v_eb_n, arma::vec &r_eb_e, arma::vec &v_eb_e) void pv_Geo_to_ECEF(double L_b, double lambda_b, double h_b, const arma::vec &v_eb_n, arma::vec &r_eb_e, arma::vec &v_eb_e)
{ {
// Parameters // Parameters
const double R_0 = 6378137; // WGS84 Equatorial radius in meters const double R_0 = 6378137.0; // WGS84 Equatorial radius in meters
const double e = 0.0818191908425; // WGS84 eccentricity const double e = 0.0818191908425; // WGS84 eccentricity
// Calculate transverse radius of curvature using (2.105) // Calculate transverse radius of curvature using (2.105)
@ -460,9 +462,10 @@ void pv_Geo_to_ECEF(double L_b, double lambda_b, double h_b, const arma::vec &v_
v_eb_e = C_e_n.t() * v_eb_n; v_eb_e = C_e_n.t() * v_eb_n;
} }
double great_circle_distance(double lat1, double lon1, double lat2, double lon2) double great_circle_distance(double lat1, double lon1, double lat2, double lon2)
{ {
//The Haversine formula determines the great-circle distance between two points on a sphere given their longitudes and latitudes. // The Haversine formula determines the great-circle distance between two points on a sphere given their longitudes and latitudes.
// generally used geo measurement function // generally used geo measurement function
double R = 6378.137; // Radius of earth in KM double R = 6378.137; // Radius of earth in KM
double dLat = lat2 * STRP_PI / 180.0 - lat1 * STRP_PI / 180.0; double dLat = lat2 * STRP_PI / 180.0 - lat1 * STRP_PI / 180.0;
@ -475,53 +478,49 @@ double great_circle_distance(double lat1, double lon1, double lat2, double lon2)
return d * 1000.0; // meters return d * 1000.0; // meters
} }
void cart2utm(arma::vec r_eb_e, int zone, arma::vec &r_enu)
void cart2utm(const arma::vec &r_eb_e, int zone, arma::vec &r_enu)
{ {
//%CART2UTM Transformation of (X,Y,Z) to (E,N,U) in UTM, zone 'zone'. // Transformation of (X,Y,Z) to (E,N,U) in UTM, zone 'zone'
//%
//%[E, N, U] = cart2utm(X, Y, Z, zone);
//%
//% Inputs:
//% X,Y,Z - Cartesian coordinates. Coordinates are referenced
//% with respect to the International Terrestrial Reference
//% Frame 1996 (ITRF96)
//% zone - UTM zone of the given position
//%
//% Outputs:
//% E, N, U - UTM coordinates (Easting, Northing, Uping)
// //
//%Kai Borre -11-1994 // Inputs:
//%Copyright (c) by Kai Borre // r_eb_e - Cartesian coordinates. Coordinates are referenced
//% // with respect to the International Terrestrial Reference
//% CVS record: // Frame 1996 (ITRF96)
//% $Id: cart2utm.m,v 1.1.1.1.2.6 2007/01/30 09:45:12 dpl Exp $ // zone - UTM zone of the given position
// //
//%This implementation is based upon // Outputs:
//%O. Andersson & K. Poder (1981) Koordinattransformationer // r_enu - UTM coordinates (Easting, Northing, Uping)
//% ved Geod\ae{}tisk Institut. Landinspekt\oe{}ren
//% Vol. 30: 552--571 and Vol. 31: 76
//%
//%An excellent, general reference (KW) is
//%R. Koenig & K.H. Weise (1951) Mathematische Grundlagen der
//% h\"oheren Geod\"asie und Kartographie.
//% Erster Band, Springer Verlag
// //
//% Explanation of variables used: // Originally written in Matlab by Kai Borre, Nov. 1994
//% f flattening of ellipsoid // Implemented in C++ by J.Arribas
//% a semi major axis in m
//% m0 1 - scale at central meridian; for UTM 0.0004
//% Q_n normalized meridian quadrant
//% E0 Easting of central meridian
//% L0 Longitude of central meridian
//% bg constants for ellipsoidal geogr. to spherical geogr.
//% gb constants for spherical geogr. to ellipsoidal geogr.
//% gtu constants for ellipsoidal N, E to spherical N, E
//% utg constants for spherical N, E to ellipoidal N, E
//% tolutm tolerance for utm, 1.2E-10*meridian quadrant
//% tolgeo tolerance for geographical, 0.00040 second of arc
// //
//% B, L refer to latitude and longitude. Southern latitude is negative // This implementation is based upon
//% International ellipsoid of 1924, valid for ED50 // O. Andersson & K. Poder (1981) Koordinattransformationer
// ved Geod\ae{}tisk Institut. Landinspekt\oe{}ren
// Vol. 30: 552--571 and Vol. 31: 76
//
// An excellent, general reference (KW) is
// R. Koenig & K.H. Weise (1951) Mathematische Grundlagen der
// h\"oheren Geod\"asie und Kartographie.
// Erster Band, Springer Verlag
//
// Explanation of variables used:
// f flattening of ellipsoid
// a semi major axis in m
// m0 1 - scale at central meridian; for UTM 0.0004
// Q_n normalized meridian quadrant
// E0 Easting of central meridian
// L0 Longitude of central meridian
// bg constants for ellipsoidal geogr. to spherical geogr.
// gb constants for spherical geogr. to ellipsoidal geogr.
// gtu constants for ellipsoidal N, E to spherical N, E
// utg constants for spherical N, E to ellipoidal N, E
// tolutm tolerance for utm, 1.2E-10*meridian quadrant
// tolgeo tolerance for geographical, 0.00040 second of arc
//
// B, L refer to latitude and longitude. Southern latitude is negative
// International ellipsoid of 1924, valid for ED50
double a = 6378388.0; double a = 6378388.0;
double f = 1.0 / 297.0; double f = 1.0 / 297.0;
@ -535,10 +534,10 @@ void cart2utm(arma::vec r_eb_e, int zone, arma::vec &r_enu)
double scale = 0.9999988; double scale = 0.9999988;
arma::vec v = scale * R * vec + trans; // coordinate vector in ED50 arma::vec v = scale * R * vec + trans; // coordinate vector in ED50
double L = atan2(v(1), v(0)); double L = atan2(v(1), v(0));
double N1 = 6395000; // preliminary value double N1 = 6395000.0; // preliminary value
double B = atan2(v(2) / ((1 - f) * (1 - f) * N1), arma::norm(v.subvec(0, 1)) / N1); // preliminary value double B = atan2(v(2) / ((1.0 - f) * (1.0 - f) * N1), arma::norm(v.subvec(0, 1)) / N1); // preliminary value
double U = 0.1; double U = 0.1;
double oldU = 0; double oldU = 0.0;
int iterations = 0; int iterations = 0;
while (fabs(U - oldU) > 1.0E-4) while (fabs(U - oldU) > 1.0E-4)
{ {
@ -553,44 +552,44 @@ void cart2utm(arma::vec r_eb_e, int zone, arma::vec &r_enu)
break; break;
} }
} }
//%Normalized meridian quadrant, KW p. 50 (96), p. 19 (38b), p. 5 (21) // Normalized meridian quadrant, KW p. 50 (96), p. 19 (38b), p. 5 (21)
double m0 = 0.0004; double m0 = 0.0004;
double n = f / (2.0 - f); double n = f / (2.0 - f);
double m = n * n * (1 / 4 + n * n / 64); double m = n * n * (1.0 / 4.0 + n * n / 64.0);
double w = (a * (-n - m0 + m * (1 - m0))) / (1 + n); double w = (a * (-n - m0 + m * (1.0 - m0))) / (1.0 + n);
double Q_n = a + w; double Q_n = a + w;
//%Easting and longitude of central meridian // Easting and longitude of central meridian
double E0 = 500000; double E0 = 500000.0;
double L0 = (zone - 30.0) * 6.0 - 3.0; double L0 = (zone - 30) * 6.0 - 3.0;
//%Check tolerance for reverse transformation // Check tolerance for reverse transformation
//double tolutm = STRP_PI / 2.0 * 1.2e-10 * Q_n; // double tolutm = STRP_PI / 2.0 * 1.2e-10 * Q_n;
//double tolgeo = 0.000040; // double tolgeo = 0.000040;
// % Coefficients of trigonometric series // Coefficients of trigonometric series
// % ellipsoidal to spherical geographical, KW p .186 --187, (51) - (52)
// % bg[1] = n * (-2 + n * (2 / 3 + n * (4 / 3 + n * (-82 / 45))));
// % bg[2] = n ^ 2 * (5 / 3 + n * (-16 / 15 + n * (-13 / 9)));
// % bg[3] = n ^ 3 * (-26 / 15 + n * 34 / 21);
// % bg[4] = n ^ 4 * 1237 / 630;
// //
// % spherical to ellipsoidal geographical, KW p.190 --191, (61) - (62) % gb[1] = n * (2 + n * (-2 / 3 + n * (-2 + n * 116 / 45))); // ellipsoidal to spherical geographical, KW p .186 --187, (51) - (52)
// % gb[2] = n ^ 2 * (7 / 3 + n * (-8 / 5 + n * (-227 / 45))); // bg[1] = n * (-2 + n * (2 / 3 + n * (4 / 3 + n * (-82 / 45))));
// % gb[3] = n ^ 3 * (56 / 15 + n * (-136 / 35)); // bg[2] = n ^ 2 * (5 / 3 + n * (-16 / 15 + n * (-13 / 9)));
// % gb[4] = n ^ 4 * 4279 / 630; // bg[3] = n ^ 3 * (-26 / 15 + n * 34 / 21);
// bg[4] = n ^ 4 * 1237 / 630;
// //
// % spherical to ellipsoidal N, E, KW p.196, (69) % gtu[1] = n * (1 / 2 + n * (-2 / 3 + n * (5 / 16 + n * 41 / 180))); // spherical to ellipsoidal geographical, KW p.190 --191, (61) - (62) % gb[1] = n * (2 + n * (-2 / 3 + n * (-2 + n * 116 / 45)));
// % gtu[2] = n ^ 2 * (13 / 48 + n * (-3 / 5 + n * 557 / 1440)); // gb[2] = n ^ 2 * (7 / 3 + n * (-8 / 5 + n * (-227 / 45)));
// % gtu[3] = n ^ 3 * (61 / 240 + n * (-103 / 140)); // gb[3] = n ^ 3 * (56 / 15 + n * (-136 / 35));
// % gtu[4] = n ^ 4 * 49561 / 161280; // gb[4] = n ^ 4 * 4279 / 630;
// //
// % ellipsoidal to spherical N, E, KW p.194, (65) % utg[1] = n * (-1 / 2 + n * (2 / 3 + n * (-37 / 96 + n * 1 / 360))); // spherical to ellipsoidal N, E, KW p.196, (69) % gtu[1] = n * (1 / 2 + n * (-2 / 3 + n * (5 / 16 + n * 41 / 180)));
// % utg[2] = n ^ 2 * (-1 / 48 + n * (-1 / 15 + n * 437 / 1440)); // gtu[2] = n ^ 2 * (13 / 48 + n * (-3 / 5 + n * 557 / 1440));
// % utg[3] = n ^ 3 * (-17 / 480 + n * 37 / 840); // gtu[3] = n ^ 3 * (61 / 240 + n * (-103 / 140));
// % utg[4] = n ^ 4 * (-4397 / 161280); // gtu[4] = n ^ 4 * 49561 / 161280;
//
// % With f = 1 / 297 we get // ellipsoidal to spherical N, E, KW p.194, (65) % utg[1] = n * (-1 / 2 + n * (2 / 3 + n * (-37 / 96 + n * 1 / 360)));
// utg[2] = n ^ 2 * (-1 / 48 + n * (-1 / 15 + n * 437 / 1440));
// utg[3] = n ^ 3 * (-17 / 480 + n * 37 / 840);
// utg[4] = n ^ 4 * (-4397 / 161280);
//
// With f = 1 / 297 we get
arma::colvec bg = {-3.37077907e-3, arma::colvec bg = {-3.37077907e-3,
4.73444769e-6, 4.73444769e-6,
@ -612,23 +611,23 @@ void cart2utm(arma::vec r_eb_e, int zone, arma::vec &r_enu)
-1.69485209e-10, -1.69485209e-10,
-2.20473896e-13}; -2.20473896e-13};
// % Ellipsoidal latitude, longitude to spherical latitude, longitude // Ellipsoidal latitude, longitude to spherical latitude, longitude
bool neg_geo = false; bool neg_geo = false;
if (B < 0) neg_geo = true; if (B < 0.0) neg_geo = true;
double Bg_r = fabs(B); double Bg_r = fabs(B);
double res_clensin = clsin(bg, 4, 2 * Bg_r); double res_clensin = clsin(bg, 4, 2.0 * Bg_r);
Bg_r = Bg_r + res_clensin; Bg_r = Bg_r + res_clensin;
L0 = L0 * STRP_PI / 180.0; L0 = L0 * STRP_PI / 180.0;
double Lg_r = L - L0; double Lg_r = L - L0;
// % Spherical latitude, longitude to complementary spherical latitude % i.e.spherical N, E // Spherical latitude, longitude to complementary spherical latitude % i.e.spherical N, E
double cos_BN = cos(Bg_r); double cos_BN = cos(Bg_r);
double Np = atan2(sin(Bg_r), cos(Lg_r) * cos_BN); double Np = atan2(sin(Bg_r), cos(Lg_r) * cos_BN);
double Ep = atanh(sin(Lg_r) * cos_BN); double Ep = atanh(sin(Lg_r) * cos_BN);
// % Spherical normalized N, E to ellipsoidal N, E // Spherical normalized N, E to ellipsoidal N, E
Np = 2.0 * Np; Np = 2.0 * Np;
Ep = 2.0 * Ep; Ep = 2.0 * Ep;
@ -651,24 +650,18 @@ void cart2utm(arma::vec r_eb_e, int zone, arma::vec &r_enu)
} }
double clsin(arma::colvec ar, int degree, double argument) double clsin(const arma::colvec &ar, int degree, double argument)
{ {
//%Clenshaw summation of sinus of argument. // Clenshaw summation of sinus of argument.
//%
//%result = clsin(ar, degree, argument);
// //
//% Written by Kai Borre // result = clsin(ar, degree, argument);
//% December 20, 1995 //
//% // Originally written in Matlab by Kai Borre
//% See also WGS2UTM or CART2UTM // Implemented in C++ by J.Arribas
//%
//% CVS record:
//% $Id: clsin.m,v 1.1.1.1.2.4 2006/08/22 13:45:59 dpl Exp $
//%==========================================================================
double cos_arg = 2.0 * cos(argument); double cos_arg = 2.0 * cos(argument);
double hr1 = 0; double hr1 = 0.0;
double hr = 0; double hr = 0.0;
double hr2; double hr2;
for (int t = degree; t > 0; t--) for (int t = degree; t > 0; t--)
{ {
@ -681,32 +674,26 @@ double clsin(arma::colvec ar, int degree, double argument)
} }
void clksin(arma::colvec ar, int degree, double arg_real, double arg_imag, double *re, double *im) void clksin(const arma::colvec &ar, int degree, double arg_real, double arg_imag, double *re, double *im)
{ {
//%Clenshaw summation of sinus with complex argument // Clenshaw summation of sinus with complex argument
//%[re, im] = clksin(ar, degree, arg_real, arg_imag); // [re, im] = clksin(ar, degree, arg_real, arg_imag);
// //
//% Written by Kai Borre // Originally written in Matlab by Kai Borre
//% December 20, 1995 // Implemented in C++ by J.Arribas
//%
//% See also WGS2UTM or CART2UTM
//%
//% CVS record:
//% $Id: clksin.m,v 1.1.1.1.2.4 2006/08/22 13:45:59 dpl Exp $
//%==========================================================================
double sin_arg_r = sin(arg_real); double sin_arg_r = sin(arg_real);
double cos_arg_r = cos(arg_real); double cos_arg_r = cos(arg_real);
double sinh_arg_i = sinh(arg_imag); double sinh_arg_i = sinh(arg_imag);
double cosh_arg_i = cosh(arg_imag); double cosh_arg_i = cosh(arg_imag);
double r = 2 * cos_arg_r * cosh_arg_i; double r = 2.0 * cos_arg_r * cosh_arg_i;
double i = -2 * sin_arg_r * sinh_arg_i; double i = -2.0 * sin_arg_r * sinh_arg_i;
double hr1 = 0; double hr1 = 0.0;
double hr = 0; double hr = 0.0;
double hi1 = 0; double hi1 = 0.0;
double hi = 0; double hi = 0.0;
double hi2; double hi2;
double hr2; double hr2;
for (int t = degree; t > 0; t--) for (int t = degree; t > 0; t--)
@ -727,86 +714,61 @@ void clksin(arma::colvec ar, int degree, double arg_real, double arg_imag, doubl
*im = r * hi + i * hr; *im = r * hi + i * hr;
} }
int findUtmZone(double latitude_deg, double longitude_deg) int findUtmZone(double latitude_deg, double longitude_deg)
{ {
//%Function finds the UTM zone number for given longitude and latitude. // Function finds the UTM zone number for given longitude and latitude.
//%The longitude value must be between -180 (180 degree West) and 180 (180 // The longitude value must be between -180 (180 degree West) and 180 (180
//%degree East) degree. The latitude must be within -80 (80 degree South) and // degree East) degree. The latitude must be within -80 (80 degree South) and
//%84 (84 degree North). // 84 (84 degree North).
//%
//%utmZone = findUtmZone(latitude, longitude);
//%
//%Latitude and longitude must be in decimal degrees (e.g. 15.5 degrees not
//%15 deg 30 min).
// //
//%-------------------------------------------------------------------------- // utmZone = findUtmZone(latitude, longitude);
//% SoftGNSS v3.0
//%
//% Copyright (C) Darius Plausinaitis
//% Written by Darius Plausinaitis
//%--------------------------------------------------------------------------
//%This program is free software; you can redistribute it and/or
//%modify it under the terms of the GNU General Public License
//%as published by the Free Software Foundation; either version 2
//%of the License, or (at your option) any later version.
//%
//%This program is distributed in the hope that it will be useful,
//%but WITHOUT ANY WARRANTY; without even the implied warranty of
//%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
//%GNU General Public License for more details.
//%
//%You should have received a copy of the GNU General Public License
//%along with this program; if not, write to the Free Software
//%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301,
//%USA.
//%==========================================================================
// //
//%CVS record: // Latitude and longitude must be in decimal degrees (e.g. 15.5 degrees not
//%$Id: findUtmZone.m,v 1.1.2.2 2006/08/22 13:45:59 dpl Exp $ // 15 deg 30 min).
// //
// % % Check value bounds == == == == == == == == == == == == == == == == == == == == == == == == == == = // Originally written in Matlab by Darius Plausinaitis
// Implemented in C++ by J.Arribas
if ((longitude_deg > 180) || (longitude_deg < -180)) // Check value bounds
if ((longitude_deg > 180.0) || (longitude_deg < -180.0))
std::cout << "Longitude value exceeds limits (-180:180).\n"; std::cout << "Longitude value exceeds limits (-180:180).\n";
if ((latitude_deg > 84.0) || (latitude_deg < -80.0))
if ((latitude_deg > 84) || (latitude_deg < -80))
std::cout << "Latitude value exceeds limits (-80:84).\n"; std::cout << "Latitude value exceeds limits (-80:84).\n";
// //
// % % Find zone == // Find zone
// == == == == == == == == == == == == == == == == == == == == == == == == == == == == == == //
// % Start at 180 deg west = -180 deg
// Start at 180 deg west = -180 deg
int utmZone = floor((180 + longitude_deg) / 6) + 1; int utmZone = floor((180 + longitude_deg) / 6) + 1;
//%% Correct zone numbers for particular areas ============================== // Correct zone numbers for particular areas
if (latitude_deg > 72.0)
if (latitude_deg > 72)
{ {
// % Corrections for zones 31 33 35 37 // Corrections for zones 31 33 35 37
if ((longitude_deg >= 0) && (longitude_deg < 9)) if ((longitude_deg >= 0.0) && (longitude_deg < 9.0))
{ {
utmZone = 31; utmZone = 31;
} }
else if ((longitude_deg >= 9) && (longitude_deg < 21)) else if ((longitude_deg >= 9.0) && (longitude_deg < 21.0))
{ {
utmZone = 33; utmZone = 33;
} }
else if ((longitude_deg >= 21) && (longitude_deg < 33)) else if ((longitude_deg >= 21.0) && (longitude_deg < 33.0))
{ {
utmZone = 35; utmZone = 35;
} }
else if ((longitude_deg >= 33) && (longitude_deg < 42)) else if ((longitude_deg >= 33.0) && (longitude_deg < 42.0))
{ {
utmZone = 37; utmZone = 37;
} }
} }
else if ((latitude_deg >= 56) && (latitude_deg < 64)) else if ((latitude_deg >= 56.0) && (latitude_deg < 64.0))
{ {
// % Correction for zone 32 // Correction for zone 32
if ((longitude_deg >= 3) && (longitude_deg < 12)) if ((longitude_deg >= 3.0) && (longitude_deg < 12.0))
utmZone = 32; utmZone = 32;
} }
return utmZone; return utmZone;

17
src/tests/system-tests/libs/geofunctions.h
View File

@ -94,7 +94,7 @@ arma::mat Gravity_ECEF(const arma::vec &r_eb_e); //!< Calculates acceleration d
*/ */
arma::vec cart2geo(const arma::vec &XYZ, int elipsoid_selection); arma::vec cart2geo(const arma::vec &XYZ, int elipsoid_selection);
arma::vec LLH_to_deg(arma::vec &LLH); arma::vec LLH_to_deg(const arma::vec &LLH);
double degtorad(double angleInDegrees); double degtorad(double angleInDegrees);
@ -104,7 +104,7 @@ double mstoknotsh(double MetersPerSeconds);
double mstokph(double Kph); double mstokph(double Kph);
arma::vec CTM_to_Euler(arma::mat &C); arma::vec CTM_to_Euler(const arma::mat &C);
arma::mat Euler_to_CTM(const arma::vec &eul); arma::mat Euler_to_CTM(const arma::vec &eul);
@ -151,35 +151,34 @@ void Geo_to_ECEF(const arma::vec &LLH, const arma::vec &v_eb_n, const arma::mat
*/ */
void pv_Geo_to_ECEF(double L_b, double lambda_b, double h_b, const arma::vec &v_eb_n, arma::vec &r_eb_e, arma::vec &v_eb_e); void pv_Geo_to_ECEF(double L_b, double lambda_b, double h_b, const arma::vec &v_eb_n, arma::vec &r_eb_e, arma::vec &v_eb_e);
/*! /*!
* \brief The Haversine formula determines the great-circle distance between two points on a sphere given their longitudes and latitudes. * \brief The Haversine formula determines the great-circle distance between two points on a sphere given their longitudes and latitudes.
*/ */
double great_circle_distance(double lat1, double lon1, double lat2, double lon2); double great_circle_distance(double lat1, double lon1, double lat2, double lon2);
/*! /*!
* \brief Transformation of ECEF (X,Y,Z) to (E,N,U) in UTM, zone 'zone'. * \brief Transformation of ECEF (X,Y,Z) to (E,N,U) in UTM, zone 'zone'.
*/ */
void cart2utm(const arma::vec &r_eb_e, int zone, arma::vec &r_enu);
void cart2utm(arma::vec r_eb_e, int zone, arma::vec &r_enu);
/*! /*!
* \brief Function finds the UTM zone number for given longitude and latitude. * \brief Function finds the UTM zone number for given longitude and latitude.
*/ */
int findUtmZone(double latitude_deg, double longitude_deg); int findUtmZone(double latitude_deg, double longitude_deg);
/*! /*!
* \brief Clenshaw summation of sinus of argument. * \brief Clenshaw summation of sinus of argument.
*/ */
double clsin(const arma::colvec &ar, int degree, double argument);
double clsin(arma::colvec ar, int degree, double argument);
/*! /*!
* \brief Clenshaw summation of sinus with complex argument. * \brief Clenshaw summation of sinus with complex argument.
*/ */
void clksin(const arma::colvec &ar, int degree, double arg_real, double arg_imag, double *re, double *im);
void clksin(arma::colvec ar, int degree, double arg_real, double arg_imag, double *re, double *im);
#endif #endif