260 lines
7.1 KiB
C++
260 lines
7.1 KiB
C++
/*
|
||
* geomath.cpp
|
||
*
|
||
*/
|
||
|
||
|
||
#include "geomath.h"
|
||
|
||
#include <math.h>
|
||
|
||
#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;
|
||
//#]
|
||
}
|
||
|
||
|
||
|