/* * geomath.cpp * */ #include "geomath.h" #include #define WGS84_K_SEMI_MAJOR_AXIS_A 6378137.0 /*raggio terrestre equatoriale in m*/ #define WGS84_K_SEMI_MINOR_AXIS_B 6356752.314245 /*raggio terrestre polare in m*/ #define WGS84_K_INVERSE_FLATTENING 298.257223563 #define WGS84_K_FLATTENING 0.00335281066474748072 /* 1/f -> 1/WGS84_K_INVERSE_FLATTENING*/ #define WGS84_K_R_METERS WGS84_K_SEMI_MINOR_AXIS_B #define WGS84_K_F_METERS WGS84_K_SEMI_MINOR_AXIS_B #define WGS84_K_ECCENTRITIC 0.08181919084296430236 //=SQRT(1 - b^2 / a^2) #define WGS84_K_ECCENTRITIC2 0.08209443795004196372 //=SQRT(a^2/b^2 - 1) #define WGS84_K_E2 (WGS84_K_ECCENTRITIC*WGS84_K_ECCENTRITIC) /* PRO calc_tgt_lat_lon, $ ;Input az_tgt_loc_geo, $ Range, $ Lat_own , $ Lon_own , $ z_own_m , $ el_tgt_loc_geo,$ ;elevation tgt in local geodetic. Nota: uguale a quella in nav ; output Lat_tgt,$ Lon_tgt ;--Opzioni di compilazione-- COMPILE_OPT STRICTARR COMMON cmp_signal_data_rec_c , cmp_signal_data_rec ; Definizione delle costanti a = double(6378137) ;raggio terrestre equatoriale in m b = double(6356752.3142) ;raggio terrestre polare in m e = SQRT(1 - b^2 / a^2) ;eccentricit e1 = SQRT(a^2/b^2 - 1) ;seconda eccentricit ;componenti del vettore antenna-target nel sist di rif.local geodetic) LOS_nav_x = Range*COS(el_tgt_loc_geo)*COS(az_tgt_loc_geo) LOS_nav_y = Range*COS(el_tgt_loc_geo)*SIN(az_tgt_loc_geo) LOS_nav_z = Range*SIN(el_tgt_loc_geo) ;componenti del vettore antenna-target nel sist di rif. NED LOS_NED_x = LOS_nav_x LOS_NED_y = - LOS_nav_y LOS_NED_z = - LOS_nav_z ;calcolo delle componenti del vettore antenna target in ECEF. ;Per calcolarle si costruisce la matrice di rotazione mat_ned_ecef= [ [COS(Lat_own), 0, -SIN(Lat_own)], $ [SIN(Lon_own)*SIN(Lat_own), -COS(Lon_own), SIN(Lon_own)*COS(Lat_own)], $ [-COS(Lon_own)*SIN(Lat_own), -SIN(Lon_own), -COS(Lon_own)*COS(Lat_own)] ] LOS_ECEF= mmul(mat_ned_ecef,[[LOS_NED_x],[LOS_NED_y],[LOS_NED_z]]) N = a / SQRT(1 - e^2 * SIN(Lat_own)^2) cc= 1 - e^2 ac_pos_ECEF = [ (N*cc+z_own_m)*SIN(Lat_own), $ -(N+z_own_m)*COS(Lat_own)*SIN(Lon_own), $ (N+z_own_m)*COS(Lat_own)*COS(Lon_own)] ;calcolo del vettore posizione del target in ECEF tgt_ECEF = LOS_ECEF + ac_pos_ECEF tgt_ECEF_x = tgt_ECEF[0] tgt_ECEF_y = tgt_ECEF[1] tgt_ECEF_z = tgt_ECEF[2] p=SQRT(tgt_ECEF_z^2 + tgt_ECEF_y^2) ;con questo ottengo la P teta=ATAN( tgt_ECEF_x * a , b * p) Lat_tgt = ATAN( tgt_ECEF_x + e1^2 * b * SIN(teta)^3 , p - e^2 * a * COS(teta)^3 ) Lon_tgt = ATAN(-tgt_ECEF_y,tgt_ECEF_z) N = a / SQRT(1 - e^2 * SIN(Lat_tgt)^2) v=p/COS(Lat_tgt) h=v-N RETURN END */ typedef double real_t; namespace geomath { struct m3x3_t { real_t m[3][3]; }; struct v3_t { real_t m[3]; }; static v3_t mmul3v3(const m3x3_t& x, const v3_t& v) { v3_t r= {{ x.m[0][0]*v.m[0]+x.m[0][1]*v.m[1]+x.m[0][2]*v.m[2], x.m[1][0]*v.m[0]+x.m[1][1]*v.m[1]+x.m[1][2]*v.m[2], x.m[2][0]*v.m[0]+x.m[2][1]*v.m[1]+x.m[2][2]*v.m[2], }}; return r; } static v3_t vadd(const v3_t& v1, const v3_t& v2) //tgt_ECEF = LOS_ECEF + ac_pos_ECEF; { v3_t r={{ v1.m[0]+v2.m[0], v1.m[1]+v2.m[1], v1.m[2]+v2.m[2]}}; return r; } static real_t pow2(real_t x) { return x*x;} static real_t pow3(real_t x) { return x*x*x;} static real_t ATAN(real_t y, real_t x) { return atan2(y, x);} }; static inline real_t SIN(real_t x) { return sin(x);} static inline real_t COS(real_t x) { return cos(x);} static inline real_t SQRT(real_t x) { return sqrt(x);} void geomath::wgs84_rngaz_to_geo(float& tgt_latitude, float& tgt_longitue, float tgt_rng, float tgt_az, float tgt_el, float ref_lat, float ref_lon) { const real_t a = WGS84_K_SEMI_MAJOR_AXIS_A; //raggio terrestre equatoriale in m const real_t b = WGS84_K_SEMI_MINOR_AXIS_B; //raggio terrestre polare in m const real_t e = WGS84_K_ECCENTRITIC; const real_t e2 = e*e; const real_t e1 = WGS84_K_ECCENTRITIC2; const real_t el_tgt_loc_geo=tgt_el; const real_t az_tgt_loc_geo=tgt_az; const real_t Range=tgt_rng; const real_t LOS_nav_x = Range*COS(el_tgt_loc_geo)*COS(az_tgt_loc_geo); const real_t LOS_nav_y = Range*COS(el_tgt_loc_geo)*SIN(az_tgt_loc_geo); const real_t LOS_nav_z = Range*SIN(el_tgt_loc_geo); const real_t LOS_NED_x = LOS_nav_x; const real_t LOS_NED_y = - LOS_nav_y; const real_t LOS_NED_z = - LOS_nav_z; const real_t Lat_own=ref_lat; const real_t Lon_own=ref_lon; const real_t z_own_m=0; m3x3_t mat_ned_ecef= {{ {COS(Lat_own), 0, -SIN(Lat_own)}, {SIN(Lon_own)*SIN(Lat_own), -COS(Lon_own), SIN(Lon_own)*COS(Lat_own)}, {-COS(Lon_own)*SIN(Lat_own), -SIN(Lon_own), -COS(Lon_own)*COS(Lat_own)} }}; v3_t tmp={{LOS_NED_x,LOS_NED_y,LOS_NED_z}}; v3_t LOS_ECEF=mmul3v3(mat_ned_ecef, tmp); const real_t sin_Lat_own=SIN(Lat_own); const real_t N = a / SQRT(1 - e2 * (sin_Lat_own*sin_Lat_own)); const real_t cc= 1 - e2; const v3_t ac_pos_ECEF = {{ (N*cc+z_own_m)*SIN(Lat_own), -(N+z_own_m)*COS(Lat_own)*SIN(Lon_own), (N+z_own_m)*COS(Lat_own)*COS(Lon_own) }}; const v3_t tgt_ECEF = vadd(LOS_ECEF, ac_pos_ECEF); const real_t tgt_ECEF_x = tgt_ECEF.m[0]; const real_t tgt_ECEF_y = tgt_ECEF.m[1]; const real_t tgt_ECEF_z = tgt_ECEF.m[2]; const real_t p=SQRT(pow2(tgt_ECEF_z) + pow2(tgt_ECEF_y)); // ;con questo ottengo la P const real_t teta=ATAN( tgt_ECEF_x * a , b * p); const real_t Lat_tgt = ATAN( tgt_ECEF_x + pow2(e1) * b * pow3(SIN(teta)) , p - e2 * a * pow3(COS(teta)) ); const real_t Lon_tgt = ATAN(-tgt_ECEF_y,tgt_ECEF_z); #if 0 N = a / SQRT(1 - e2 * pow2(SIN(Lat_tgt))); const real_t v=p/COS(Lat_tgt); const real_t h=v-N; #endif tgt_latitude=Lat_tgt; tgt_longitue=Lon_tgt; } void geomath::th_rngaz_to_geo(float& p_phi, float& p_lambda, const float n_phi, const float n_lambda, float th/*true heading*/, float psi/*ptaz*/, float p_rng, float p_az) { const float R_WGS84 = 6378131.6341; const float F_WGS84 = 0.0033528; //constexpr float R_WGS84 = 6378137.0; //constexpr float F_WGS84 = 0.00335281066474748072; //constexpr float F_WGS84 = (3.35281068231769422166749745962564e-03)*1852; //*GC_K_NM_TO_METERS; //#[ operation rngAzToGeo(ncPosType,float,float,ncPosType&) //- LV 4.3.4 NAV_TO_LAT(?,?,?,?,?)–NAV_TO_LON(?,?,?,?,?) – Range/Azimuth to Geo transformation, //pg 94 Rev1.5 SRS for cursor management for grifo-f_br.doc -} float sinPhi = sinf( n_phi ); float rEast = R_WGS84 * ( 1 + (F_WGS84 * sinPhi * sinPhi) ); float rNorth = R_WGS84 * ( 1 - (F_WGS84 * ( 2 - (3 * sinPhi * sinPhi ))) ); float az = th - psi - p_az; //in_app.true_heading float sinA = sinf( az ); float cosA = cosf( az ); float cosN = cosf( n_phi ); float dPhi = (p_rng * cosA) / rNorth; float dLambda = (p_rng * sinA) /( rEast * cosN ); p_phi = n_phi + dPhi; p_lambda = n_lambda + dLambda; //#] }