kopia lustrzana https://github.com/sp9skp/spdxl
849 wiersze
28 KiB
C
849 wiersze
28 KiB
C
/**
|
|
\file geodesy.c
|
|
\brief GNSS core 'c' function library: geodesy related functions.
|
|
\author Glenn D. MacGougan (GDM)
|
|
\date 2007-11-28
|
|
\since 2005-08-14
|
|
|
|
\b "LICENSE INFORMATION" \n
|
|
Copyright (c) 2007, refer to 'author' doxygen tags \n
|
|
All rights reserved. \n
|
|
|
|
Redistribution and use in source and binary forms, with or without
|
|
modification, are permitted provided the following conditions are met: \n
|
|
|
|
- Redistributions of source code must retain the above copyright
|
|
notice, this list of conditions and the following disclaimer. \n
|
|
- Redistributions in binary form must reproduce the above copyright
|
|
notice, this list of conditions and the following disclaimer in the
|
|
documentation and/or other materials provided with the distribution. \n
|
|
- The name(s) of the contributor(s) may not be used to endorse or promote
|
|
products derived from this software without specific prior written
|
|
permission. \n
|
|
|
|
THIS SOFTWARE IS PROVIDED BY THE CONTRIBUTORS ``AS IS'' AND ANY EXPRESS
|
|
OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
|
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
|
DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
|
|
INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
|
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
|
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
|
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
|
|
OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
|
|
SUCH DAMAGE.
|
|
*/
|
|
|
|
#include <math.h>
|
|
#include "gnss_error.h"
|
|
#include "geodesy.h"
|
|
#include "constants.h"
|
|
|
|
|
|
/*************************************************************************************************/
|
|
// static function definitions (for functions used in this file only)
|
|
// static functions are functions that are only visable to other functions in the same file.
|
|
|
|
// Many functions contained herein only need a, and e2.
|
|
static BOOL GEODESY_GetReferenceEllipseParameters_A_E2(
|
|
const GEODESY_enumReferenceEllipse ellipse,
|
|
double* a, // semi-major axis of the reference ellipse [m]
|
|
double* e2 // eccentricity of the reference ellipse (e2 = (a*a-b*b)/(a*a)) []
|
|
);
|
|
|
|
// Many functions contained herein only need a, b and e2.
|
|
static BOOL GEODESY_GetReferenceEllipseParameters_A_B_E2(
|
|
const GEODESY_enumReferenceEllipse ellipse,
|
|
double* a, // semi-major axis of the reference ellipse [m]
|
|
double* b, // semi-minor axis of the reference ellipse [m]
|
|
double* e2 // eccentricity of the reference ellipse (e2 = (a*a-b*b)/(a*a)) []
|
|
);
|
|
|
|
// return TRUE(1) if valid, FALSE(0) otherwise.
|
|
static BOOL GEODESY_IsLatitudeValid(
|
|
const double latitude //!< expecting a value -pi/2 to pi/2 [rad]
|
|
);
|
|
/*************************************************************************************************/
|
|
|
|
|
|
|
|
|
|
BOOL GEODESY_GetReferenceEllipseParameters(
|
|
const GEODESY_enumReferenceEllipse ellipse, //!< reference ellipse enumerated []
|
|
double* a, //!< semi-major axis of the reference ellipse [m]
|
|
double* b, //!< semi-minor axis of the reference ellipse (b = a - a*f_inv) [m]
|
|
double* f_inv, //!< inverse of the flattening of the reference ellipse []
|
|
double* e2 //!< eccentricity of the reference ellipse (e2 = (a*a-b*b)/(a*a)) []
|
|
)
|
|
{
|
|
switch( ellipse )
|
|
{
|
|
case GEODESY_REFERENCE_ELLIPSE_AIRY_1830:
|
|
*a = GEODESY_REFERENCE_ELLIPSE_AIRY_1830_A;
|
|
*f_inv = GEODESY_REFERENCE_ELLIPSE_AIRY_1830_F_INV;
|
|
*b = GEODESY_REFERENCE_ELLIPSE_AIRY_1830_B;
|
|
*e2 = GEODESY_REFERENCE_ELLIPSE_AIRY_1830_E2;
|
|
break;
|
|
|
|
case GEODESY_REFERENCE_ELLIPSE_MODIFED_AIRY:
|
|
*a = GEODESY_REFERENCE_ELLIPSE_MODIFED_AIRY_A;
|
|
*f_inv = GEODESY_REFERENCE_ELLIPSE_MODIFED_AIRY_F_INV;
|
|
*b = GEODESY_REFERENCE_ELLIPSE_MODIFED_AIRY_B;
|
|
*e2 = GEODESY_REFERENCE_ELLIPSE_MODIFED_AIRY_E2;
|
|
break;
|
|
|
|
case GEODESY_REFERENCE_ELLIPSE_AUSTRALIAN_NATIONAL:
|
|
*a = GEODESY_REFERENCE_ELLIPSE_AUSTRALIAN_NATIONAL_A;
|
|
*f_inv = GEODESY_REFERENCE_ELLIPSE_AUSTRALIAN_NATIONAL_F_INV;
|
|
*b = GEODESY_REFERENCE_ELLIPSE_AUSTRALIAN_NATIONAL_B;
|
|
*e2 = GEODESY_REFERENCE_ELLIPSE_AUSTRALIAN_NATIONAL_E2;
|
|
break;
|
|
|
|
case GEODESY_REFERENCE_ELLIPSE_BESSEL_1841:
|
|
*a = GEODESY_REFERENCE_ELLIPSE_BESSEL_1841_A;
|
|
*f_inv = GEODESY_REFERENCE_ELLIPSE_BESSEL_1841_F_INV;
|
|
*b = GEODESY_REFERENCE_ELLIPSE_BESSEL_1841_B;
|
|
*e2 = GEODESY_REFERENCE_ELLIPSE_BESSEL_1841_E2;
|
|
break;
|
|
|
|
case GEODESY_REFERENCE_ELLIPSE_CLARKE_1866:
|
|
*a = GEODESY_REFERENCE_ELLIPSE_CLARKE_1866_A;
|
|
*f_inv = GEODESY_REFERENCE_ELLIPSE_CLARKE_1866_F_INV;
|
|
*b = GEODESY_REFERENCE_ELLIPSE_CLARKE_1866_B;
|
|
*e2 = GEODESY_REFERENCE_ELLIPSE_CLARKE_1866_E2;
|
|
break;
|
|
|
|
case GEODESY_REFERENCE_ELLIPSE_CLARKE_1880:
|
|
*a = GEODESY_REFERENCE_ELLIPSE_CLARKE_1880_A;
|
|
*f_inv = GEODESY_REFERENCE_ELLIPSE_CLARKE_1880_F_INV;
|
|
*b = GEODESY_REFERENCE_ELLIPSE_CLARKE_1880_B;
|
|
*e2 = GEODESY_REFERENCE_ELLIPSE_CLARKE_1880_E2;
|
|
break;
|
|
|
|
case GEODESY_REFERENCE_ELLIPSE_EVEREST_INDIA_1830:
|
|
*a = GEODESY_REFERENCE_ELLIPSE_EVEREST_INDIA_1830_A;
|
|
*f_inv = GEODESY_REFERENCE_ELLIPSE_EVEREST_INDIA_1830_F_INV;
|
|
*b = GEODESY_REFERENCE_ELLIPSE_EVEREST_INDIA_1830_B;
|
|
*e2 = GEODESY_REFERENCE_ELLIPSE_EVEREST_INDIA_1830_E2;
|
|
break;
|
|
|
|
case GEODESY_REFERENCE_ELLIPSE_EVEREST_BRUNEI_E_MALAYSIA:
|
|
*a = GEODESY_REFERENCE_ELLIPSE_EVEREST_BRUNEI_E_MALAYSIA_A;
|
|
*f_inv = GEODESY_REFERENCE_ELLIPSE_EVEREST_BRUNEI_E_MALAYSIA_F_INV;
|
|
*b = GEODESY_REFERENCE_ELLIPSE_EVEREST_BRUNEI_E_MALAYSIA_B;
|
|
*e2 = GEODESY_REFERENCE_ELLIPSE_EVEREST_BRUNEI_E_MALAYSIA_E2;
|
|
break;
|
|
|
|
case GEODESY_REFERENCE_ELLIPSE_EVEREST_W_MALAYSIA_SINGAPORE:
|
|
*a = GEODESY_REFERENCE_ELLIPSE_EVEREST_W_MALAYSIA_SINGAPORE_A;
|
|
*f_inv = GEODESY_REFERENCE_ELLIPSE_EVEREST_W_MALAYSIA_SINGAPORE_F_INV;
|
|
*b = GEODESY_REFERENCE_ELLIPSE_EVEREST_W_MALAYSIA_SINGAPORE_B;
|
|
*e2 = GEODESY_REFERENCE_ELLIPSE_EVEREST_W_MALAYSIA_SINGAPORE_E2;
|
|
break;
|
|
|
|
case GEODESY_REFERENCE_ELLIPSE_GRS_1980:
|
|
*a = GEODESY_REFERENCE_ELLIPSE_GRS_1980_A;
|
|
*f_inv = GEODESY_REFERENCE_ELLIPSE_GRS_1980_F_INV;
|
|
*b = GEODESY_REFERENCE_ELLIPSE_GRS_1980_B;
|
|
*e2 = GEODESY_REFERENCE_ELLIPSE_GRS_1980_E2;
|
|
break;
|
|
|
|
case GEODESY_REFERENCE_ELLIPSE_HELMERT_1906:
|
|
*a = GEODESY_REFERENCE_ELLIPSE_HELMERT_1906_A;
|
|
*f_inv = GEODESY_REFERENCE_ELLIPSE_HELMERT_1906_F_INV;
|
|
*b = GEODESY_REFERENCE_ELLIPSE_HELMERT_1906_B;
|
|
*e2 = GEODESY_REFERENCE_ELLIPSE_HELMERT_1906_E2;
|
|
break;
|
|
|
|
case GEODESY_REFERENCE_ELLIPSE_HOUGH_1960:
|
|
*a = GEODESY_REFERENCE_ELLIPSE_HOUGH_1960_A;
|
|
*f_inv = GEODESY_REFERENCE_ELLIPSE_HOUGH_1960_F_INV;
|
|
*b = GEODESY_REFERENCE_ELLIPSE_HOUGH_1960_B;
|
|
*e2 = GEODESY_REFERENCE_ELLIPSE_HOUGH_1960_E2;
|
|
break;
|
|
|
|
case GEODESY_REFERENCE_ELLIPSE_INTERNATIONAL_1924:
|
|
*a = GEODESY_REFERENCE_ELLIPSE_INTERNATIONAL_1924_A;
|
|
*f_inv = GEODESY_REFERENCE_ELLIPSE_INTERNATIONAL_1924_F_INV;
|
|
*b = GEODESY_REFERENCE_ELLIPSE_INTERNATIONAL_1924_B;
|
|
*e2 = GEODESY_REFERENCE_ELLIPSE_INTERNATIONAL_1924_E2;
|
|
break;
|
|
|
|
case GEODESY_REFERENCE_ELLIPSE_SOUTH_AMERICAN_1969:
|
|
*a = GEODESY_REFERENCE_ELLIPSE_SOUTH_AMERICAN_1969_A;
|
|
*f_inv = GEODESY_REFERENCE_ELLIPSE_SOUTH_AMERICAN_1969_F_INV;
|
|
*b = GEODESY_REFERENCE_ELLIPSE_SOUTH_AMERICAN_1969_B;
|
|
*e2 = GEODESY_REFERENCE_ELLIPSE_SOUTH_AMERICAN_1969_E2;
|
|
break;
|
|
|
|
case GEODESY_REFERENCE_ELLIPSE_WGS72:
|
|
*a = GEODESY_REFERENCE_ELLIPSE_WGS72_A;
|
|
*f_inv = GEODESY_REFERENCE_ELLIPSE_WGS72_F_INV;
|
|
*b = GEODESY_REFERENCE_ELLIPSE_WGS72_B;
|
|
*e2 = GEODESY_REFERENCE_ELLIPSE_WGS72_E2;
|
|
break;
|
|
|
|
case GEODESY_REFERENCE_ELLIPSE_WGS84:
|
|
*a = GEODESY_REFERENCE_ELLIPSE_WGS84_A;
|
|
*f_inv = GEODESY_REFERENCE_ELLIPSE_WGS84_F_INV;
|
|
*b = GEODESY_REFERENCE_ELLIPSE_WGS84_B;
|
|
*e2 = GEODESY_REFERENCE_ELLIPSE_WGS84_E2;
|
|
break;
|
|
|
|
default:
|
|
GNSS_ERROR_MSG( "Unexpected default case." );
|
|
return FALSE;
|
|
break;
|
|
}
|
|
return TRUE;
|
|
}
|
|
|
|
|
|
// static
|
|
BOOL GEODESY_GetReferenceEllipseParameters_A_E2(
|
|
const GEODESY_enumReferenceEllipse ellipse,
|
|
double* a, // semi-major axis of the reference ellipse [m]
|
|
double* e2 // eccentricity of the reference ellipse (e2 = (a*a-b*b)/(a*a)) []
|
|
)
|
|
{
|
|
double b;
|
|
double f_inv;
|
|
BOOL result;
|
|
|
|
result = GEODESY_GetReferenceEllipseParameters(
|
|
ellipse,
|
|
a,
|
|
&b,
|
|
&f_inv,
|
|
e2 );
|
|
|
|
return result;
|
|
}
|
|
|
|
|
|
// static
|
|
BOOL GEODESY_GetReferenceEllipseParameters_A_B_E2(
|
|
const GEODESY_enumReferenceEllipse ellipse,
|
|
double* a, // semi-major axis of the reference ellipse [m]
|
|
double* b, // semi-minor axis of the reference ellipse [m]
|
|
double* e2 // eccentricity of the reference ellipse (e2 = (a*a-b*b)/(a*a)) []
|
|
)
|
|
{
|
|
double f_inv;
|
|
BOOL result;
|
|
|
|
result = GEODESY_GetReferenceEllipseParameters(
|
|
ellipse,
|
|
a,
|
|
b,
|
|
&f_inv,
|
|
e2 );
|
|
|
|
return result;
|
|
}
|
|
|
|
// static
|
|
BOOL GEODESY_IsLatitudeValid(
|
|
const double latitude //!< expecting a value -pi/2 to pi/2 [rad]
|
|
)
|
|
{
|
|
// check for valid latitude out of range
|
|
if( latitude > HALFPI || latitude < -HALFPI )
|
|
{
|
|
GNSS_ERROR_MSG( "if( latitude > HALFPI || latitude < -HALFPI )" );
|
|
return FALSE;
|
|
}
|
|
else
|
|
{
|
|
return TRUE;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
BOOL GEODESY_ConvertGeodeticCurvilinearToEarthFixedCartesianCoordinates(
|
|
const GEODESY_enumReferenceEllipse referenceEllipse, //!< reference ellipse enumerated []
|
|
const double latitude, //!< geodetic latitude [rad]
|
|
const double longitude, //!< geodetic longitude [rad]
|
|
const double height, //!< geodetic height [m]
|
|
double *x, //!< earth fixed cartesian coordinate [m]
|
|
double *y, //!< earth fixed cartesian coordinate [m]
|
|
double *z //!< earth fixed cartesian coordinate [m]
|
|
)
|
|
{
|
|
double a; // semi-major axis of reference ellipse [m]
|
|
double e2; // first eccentricity of reference ellipse []
|
|
double N; // prime vertical radius of curvature [m]
|
|
double sinlat; // sin of the latitude
|
|
double dtmp; // temp
|
|
BOOL result;
|
|
|
|
// get necessary reference ellipse parameters
|
|
result = GEODESY_GetReferenceEllipseParameters_A_E2( referenceEllipse, &a, &e2 );
|
|
if( result == FALSE )
|
|
{
|
|
*x = 0.0;
|
|
*y = 0.0;
|
|
*z = 0.0;
|
|
GNSS_ERROR_MSG( "Reference ellipse invalid." );
|
|
return FALSE;
|
|
}
|
|
|
|
// check for valid latitude out of range
|
|
result = GEODESY_IsLatitudeValid( latitude );
|
|
if( result == FALSE )
|
|
{
|
|
*x = 0.0;
|
|
*y = 0.0;
|
|
*z = 0.0;
|
|
GNSS_ERROR_MSG( "Input latitude is invalid." );
|
|
return FALSE;
|
|
}
|
|
|
|
sinlat = sin( latitude );
|
|
N = a / sqrt( 1.0 - e2 * sinlat*sinlat );
|
|
dtmp = (N + height) * cos(latitude);
|
|
|
|
*x = dtmp * cos(longitude);
|
|
*y = dtmp * sin(longitude);
|
|
*z = ( (1.0 - e2)*N + height ) * sinlat;
|
|
|
|
return TRUE;
|
|
}
|
|
|
|
|
|
|
|
|
|
BOOL GEODESY_ConvertEarthFixedCartesianToGeodeticCurvilinearCoordinates(
|
|
const GEODESY_enumReferenceEllipse referenceEllipse, //!< reference ellipse enumerated []
|
|
const double x, // earth fixed cartesian coordinate [m]
|
|
const double y, // earth fixed cartesian coordinate [m]
|
|
const double z, // earth fixed cartesian coordinate [m]
|
|
double *latitude, // geodetic latitude [rad]
|
|
double *longitude, // geodetic longitude [rad]
|
|
double *height // geodetic height [m]
|
|
)
|
|
{
|
|
double a; // semi-major axis of reference ellipse [m]
|
|
double b; // semi-minor axis of reference ellipse [m]
|
|
double e2; // first eccentricity of reference ellipse []
|
|
double N; // prime vertical radius of curvature [m]
|
|
double p; // sqrt( x^2 + y^2 ) [m]
|
|
double dtmp; // temp
|
|
double sinlat; // sin(lat)
|
|
double lat; // temp geodetic latitude [rad]
|
|
double lon; // temp geodetic longitude [rad]
|
|
double hgt; // temp geodetic height [m]
|
|
BOOL result;
|
|
|
|
// get necessary reference ellipse parameters
|
|
result = GEODESY_GetReferenceEllipseParameters_A_B_E2( referenceEllipse, &a, &b, &e2 );
|
|
if( result == FALSE )
|
|
{
|
|
*latitude = 0;
|
|
*longitude = 0;
|
|
*height = 0;
|
|
GNSS_ERROR_MSG( "Reference ellipse invalid." );
|
|
return FALSE;
|
|
}
|
|
|
|
if( x == 0.0 && y == 0.0 )
|
|
{
|
|
// at a pole
|
|
// most likely to happen while using a simulator
|
|
|
|
// longitude is really unknown
|
|
lon = 0.0;
|
|
|
|
if( z < 0 )
|
|
{
|
|
hgt = -z - b;
|
|
lat = -HALFPI;
|
|
}
|
|
else
|
|
{
|
|
hgt = z - b;
|
|
lat = HALFPI;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
p = sqrt( x*x + y*y );
|
|
|
|
// unique solution for longitude
|
|
// best formula for any longitude and applies well near the poles
|
|
// pp. 178 reference [2]
|
|
lon = 2.0 * atan2( y , ( x + p ) );
|
|
|
|
// set approximate initial latitude assuming a height of 0.0
|
|
lat = atan( z / (p * (1.0 - e2)) );
|
|
hgt = 0.0;
|
|
do
|
|
{
|
|
dtmp = hgt;
|
|
sinlat = sin(lat);
|
|
N = a / sqrt( 1.0 - e2*sinlat*sinlat );
|
|
hgt = p / cos(lat) - N;
|
|
lat = atan( z / (p * ( 1.0 - e2*N/(N + hgt) )) );
|
|
|
|
} while( fabs( hgt - dtmp ) > 0.0001 ); // 0.1 mm convergence for height
|
|
}
|
|
|
|
*latitude = lat;
|
|
*longitude = lon;
|
|
*height = hgt;
|
|
return TRUE;
|
|
}
|
|
|
|
|
|
BOOL GEODESY_ComputeNorthingEastingVertical(
|
|
const GEODESY_enumReferenceEllipse referenceEllipse, //!< reference ellipse enumerated []
|
|
const double referenceLatitude, //!< datum geodetic latitude [rad]
|
|
const double referenceLongitude, //!< datum geodetic longitude [rad]
|
|
const double referenceHeight, //!< datum geodetic height [m]
|
|
const double latitude, //!< geodetic latitude [rad]
|
|
const double longitude, //!< geodetic longitude [rad]
|
|
const double height, //!< geodetic height [m]
|
|
double *northing, //!< local geodetic northing [m]
|
|
double *easting, //!< local geodetic easting [m]
|
|
double *vertical //!< local geodetic vertical [m]
|
|
)
|
|
{
|
|
double x_ref;
|
|
double y_ref;
|
|
double z_ref;
|
|
double x;
|
|
double y;
|
|
double z;
|
|
double dx;
|
|
double dy;
|
|
double dz;
|
|
double A; // rotation angle [rad]
|
|
double B; // rotation angle [rad]
|
|
double cosA;
|
|
double sinA;
|
|
double cosB;
|
|
double sinB;
|
|
BOOL result;
|
|
|
|
*northing = 0;
|
|
*easting = 0;
|
|
*vertical = 0;
|
|
|
|
result = GEODESY_IsLatitudeValid( referenceLatitude );
|
|
if( result == FALSE )
|
|
{
|
|
GNSS_ERROR_MSG( "Input reference latitude is invalid" );
|
|
return FALSE;
|
|
}
|
|
result = GEODESY_IsLatitudeValid( latitude );
|
|
if( result == FALSE )
|
|
{
|
|
GNSS_ERROR_MSG( "Input latitude is invalid." );
|
|
return FALSE;
|
|
}
|
|
|
|
result = GEODESY_ConvertGeodeticCurvilinearToEarthFixedCartesianCoordinates(
|
|
referenceEllipse,
|
|
referenceLatitude,
|
|
referenceLongitude,
|
|
referenceHeight,
|
|
&x_ref,
|
|
&y_ref,
|
|
&z_ref );
|
|
if( result == FALSE )
|
|
{
|
|
GNSS_ERROR_MSG( "GEODESY_ConvertGeodeticCurvilinearToEarthFixedCartesianCoordinates return FALSE." );
|
|
return FALSE;
|
|
}
|
|
|
|
result = GEODESY_ConvertGeodeticCurvilinearToEarthFixedCartesianCoordinates(
|
|
referenceEllipse,
|
|
latitude,
|
|
longitude,
|
|
height,
|
|
&x,
|
|
&y,
|
|
&z );
|
|
if( result == FALSE )
|
|
{
|
|
GNSS_ERROR_MSG( "GEODESY_ConvertGeodeticCurvilinearToEarthFixedCartesianCoordinates return FALSE." );
|
|
return FALSE;
|
|
}
|
|
|
|
// A and B are rotation angles
|
|
A = referenceLatitude - HALFPI;
|
|
B = referenceLongitude - PI;
|
|
|
|
cosA = cos(A);
|
|
sinA = sin(A);
|
|
cosB = cos(B);
|
|
sinB = sin(B);
|
|
|
|
// the cartesian vector between the two points in the geodetic
|
|
// frame is rotated to the local geodetic frame
|
|
dx = x - x_ref;
|
|
dy = y - y_ref;
|
|
dz = z - z_ref;
|
|
|
|
*northing = cosA*cosB * dx + cosA*sinB * dy - sinA*dz;
|
|
*easting = sinB * dx - cosB * dy;
|
|
*vertical = sinA*cosB * dx + sinA*sinB * dy + cosA*dz;
|
|
return TRUE;
|
|
}
|
|
|
|
|
|
BOOL GEODESY_ComputePositionDifference(
|
|
const GEODESY_enumReferenceEllipse referenceEllipse, //!< reference ellipse enumerated []
|
|
const double referenceLatitude, //!< reference point geodetic latitude [rad]
|
|
const double referenceLongitude, //!< reference point geodetic longitude [rad]
|
|
const double referenceHeight, //!< reference point geodetic height [m]
|
|
const double latitude, //!< geodetic latitude [rad]
|
|
const double longitude, //!< geodetic longitude [rad]
|
|
const double height, //!< geodetic height [m]
|
|
double *difference_northing, //!< difference in northing [m] (+2 m, means 2 m North of the reference)
|
|
double *difference_easting, //!< difference in easting [m] (+2 m, means 2 m East of the reference)
|
|
double *difference_vertical //!< difference in vertical [m] (+2 m, means 2 m above of the reference)
|
|
)
|
|
{
|
|
BOOL result;
|
|
result = GEODESY_ComputeNorthingEastingVertical(
|
|
referenceEllipse,
|
|
referenceLatitude,
|
|
referenceLongitude,
|
|
referenceHeight,
|
|
latitude,
|
|
longitude,
|
|
height,
|
|
difference_northing,
|
|
difference_easting,
|
|
difference_vertical );
|
|
return result;
|
|
}
|
|
|
|
|
|
|
|
|
|
BOOL GEODESY_ComputeMeridianRadiusOfCurvature(
|
|
const GEODESY_enumReferenceEllipse referenceEllipse, //!< reference ellipse enumerated []
|
|
const double latitude, //!< geodetic latitude [rad]
|
|
double* M //!< computed meridian radius of curvature [m]
|
|
)
|
|
{
|
|
double a; // semi-major axis of reference ellipse [m]
|
|
double e2; // first eccentricity of reference ellipse []
|
|
double dtmp;
|
|
BOOL result;
|
|
|
|
// get necessary reference ellipse parameters
|
|
result = GEODESY_GetReferenceEllipseParameters_A_E2( referenceEllipse, &a, &e2 );
|
|
if( result == FALSE )
|
|
{
|
|
*M = 0;
|
|
GNSS_ERROR_MSG( "Reference ellipse invalid." );
|
|
return result;
|
|
}
|
|
|
|
dtmp = sin(latitude);
|
|
dtmp = sqrt( 1.0 - e2 * dtmp * dtmp ); // W
|
|
dtmp = dtmp*dtmp*dtmp; // W^3
|
|
|
|
*M = a * ( 1.0 - e2 ) / dtmp;
|
|
return TRUE;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
BOOL GEODESY_ComputePrimeVerticalRadiusOfCurvature(
|
|
const GEODESY_enumReferenceEllipse referenceEllipse, //!< reference ellipse enumerated []
|
|
const double latitude, //!< geodetic latitude [rad]
|
|
double* N //!< computed prime vertical radius of curvature [m]
|
|
)
|
|
{
|
|
double a; // semi-major axis of reference ellipse [m]
|
|
double e2; // first eccentricity of reference ellipse []
|
|
double W;
|
|
BOOL result;
|
|
|
|
// get necessary reference ellipse parameters
|
|
result = GEODESY_GetReferenceEllipseParameters_A_E2( referenceEllipse, &a, &e2 );
|
|
if( result == FALSE )
|
|
{
|
|
*N = 0;
|
|
GNSS_ERROR_MSG( "Reference ellipse invalid." );
|
|
return result;
|
|
}
|
|
|
|
W = sin(latitude);
|
|
W = sqrt( 1.0 - e2 * W * W );
|
|
|
|
*N = a / W;
|
|
return TRUE;
|
|
}
|
|
|
|
|
|
BOOL GEODESY_ComputeMeridianArcBetweenTwoLatitudes(
|
|
const GEODESY_enumReferenceEllipse referenceEllipse, //!< reference ellipse enumerated []
|
|
const double referenceLatitude, //!< datum geodetic latitude [rad]
|
|
const double latitude, //!< geodetic latitude [rad]
|
|
double* arc //!< computed meridian arc, North +ve, South -ve [m]
|
|
)
|
|
{
|
|
double a; // semi-major axis of reference ellipse [m]
|
|
double e2; // first eccentricity of reference ellipse []
|
|
double e4;
|
|
double e6;
|
|
double e8;
|
|
double dtmp;
|
|
double A;
|
|
double B;
|
|
double C;
|
|
double D;
|
|
double E;
|
|
double arc_ref; // arc from equator for the reference lat [m]
|
|
double arc_p; // arc from eqautor for point 'P' [m]
|
|
BOOL result;
|
|
|
|
*arc = 0;
|
|
|
|
result = GEODESY_IsLatitudeValid( referenceLatitude );
|
|
if( result == FALSE )
|
|
{
|
|
GNSS_ERROR_MSG( "Reference latitude is invalid." );
|
|
return result;
|
|
}
|
|
|
|
// get necessary reference ellipse parameters
|
|
result = GEODESY_GetReferenceEllipseParameters_A_E2( referenceEllipse, &a, &e2 );
|
|
if( result == FALSE )
|
|
{
|
|
GNSS_ERROR_MSG( "Reference ellipse invalid." );
|
|
return result;
|
|
}
|
|
|
|
e4 = e2*e2;
|
|
e6 = e4*e2;
|
|
e8 = e6*e2;
|
|
dtmp = a*(1.0-e2);
|
|
|
|
A = dtmp * ( 1.0 + 0.75 * e2 + 0.703125 * e4 + 0.68359375 * e6 + 0.67291259765625 * e8 ); // dtmp * (1.0 + 3.0/4.0*e2 + 45.0/64.0*e4 + 175.0/256.0*e6 + 11025.0/16384.0*e8 );
|
|
B = -dtmp * ( 0.375 * e2 + 0.46875 * e4 + 0.5126953125 * e6 + 0.538330078125 * e8 ); // -dtmp * ( 3.0/8.0*e2 + 15.0/32.0*e4 + 525.0/1024.0*e6 + 2205.0/4096.0*e8 );
|
|
C = dtmp * ( 0.05859375 * e4 + 0.1025390625 * e6 + 0.13458251953125 * e8 ); // -dtmp * ( 15.0/256.0*e4 + 105.0/1024.0*e6 + 2205.0/16384.0*e8 );
|
|
D = -dtmp * ( 0.011393229167 * e6 + 0.025634765625 * e8 ); // -dtmp * ( 35.0/3072.0*e6 + 105.0/4096.0*e8 );
|
|
E = dtmp * ( 2.4032593e-03 * e8 );
|
|
|
|
arc_ref = A*referenceLatitude + B*sin(2.0*referenceLatitude) + C*sin(4.0*referenceLatitude) + D*sin(6.0*referenceLatitude) + E*sin(8.0*referenceLatitude);
|
|
arc_p = A*latitude + B*sin(2.0*latitude) + C*sin(4.0*latitude) + D*sin(6.0*latitude) + E*sin(8.0*latitude);
|
|
|
|
*arc = arc_p - arc_ref;
|
|
return TRUE;
|
|
}
|
|
|
|
|
|
|
|
|
|
BOOL GEODESY_ComputeParallelArcBetweenTwoLongitudes(
|
|
const GEODESY_enumReferenceEllipse referenceEllipse, //!< reference ellipse enumerated []
|
|
const double referenceLatitude, //!< reference geodetic latitude [rad]
|
|
const double referenceLongitude, //!< reference geodetic longitude [rad]
|
|
const double longitude, //!< geodetic longitude [rad]
|
|
double* arc //!< computed parallel arc, East +ve, West -ve [m]
|
|
)
|
|
{
|
|
double a; // semi-major axis of reference ellipse [m]
|
|
double e2; // first eccentricity of reference ellipse []
|
|
double N; // computed prime vertical radius of curvature [m]
|
|
BOOL result;
|
|
|
|
*arc = 0;
|
|
|
|
// get necessary reference ellipse parameters
|
|
result = GEODESY_GetReferenceEllipseParameters_A_E2( referenceEllipse, &a, &e2 );
|
|
if( result == FALSE )
|
|
{
|
|
GNSS_ERROR_MSG( "Reference ellipse invalid." );
|
|
return result;
|
|
}
|
|
|
|
result = GEODESY_IsLatitudeValid( referenceLatitude );
|
|
if( result == FALSE )
|
|
{
|
|
GNSS_ERROR_MSG( "Reference latitude is invalid." );
|
|
return result;
|
|
}
|
|
|
|
N = sin(referenceLatitude);
|
|
N = a / sqrt( 1.0 - e2 * N * N );
|
|
|
|
*arc = N * cos(referenceLatitude) * (longitude - referenceLongitude);
|
|
return TRUE;
|
|
}
|
|
|
|
|
|
|
|
|
|
BOOL GEODESY_RotateVectorFromLocalGeodeticFrameToEarthFixedFrame(
|
|
const double referenceLatitude, //!< reference geodetic latitude [rad]
|
|
const double referenceLongitude, //!< reference geodetic longitude [rad]
|
|
const double dN, //!< local geodetic northing vector component [m]
|
|
const double dE, //!< local geodetic easting vector component [m]
|
|
const double dUp, //!< local geodetic vertical vector component [m]
|
|
double* dX, //!< earth centered earth fixed vector component [m]
|
|
double* dY, //!< earth centered earth fixed vector component [m]
|
|
double* dZ //!< earth centered earth fixed vector component [m]
|
|
)
|
|
{
|
|
double sinlat;
|
|
double coslat;
|
|
double sinlon;
|
|
double coslon;
|
|
BOOL result;
|
|
|
|
result = GEODESY_IsLatitudeValid( referenceLatitude );
|
|
if( result == FALSE )
|
|
{
|
|
*dX = 0;
|
|
*dY = 0;
|
|
*dZ = 0;
|
|
GNSS_ERROR_MSG( "Reference latitude is invalid." );
|
|
return result;
|
|
}
|
|
|
|
sinlat = sin(referenceLatitude);
|
|
coslat = cos(referenceLatitude);
|
|
sinlon = sin(referenceLongitude);
|
|
coslon = cos(referenceLongitude);
|
|
|
|
*dX = -sinlat*coslon * dN - sinlon * dE + coslat*coslon * dUp;
|
|
*dY = -sinlat*sinlon * dN + coslon * dE + coslat*sinlon * dUp;
|
|
*dZ = coslat * dN + sinlat * dUp;
|
|
return TRUE;
|
|
}
|
|
|
|
|
|
|
|
|
|
BOOL GEODESY_RotateVectorFromEarthFixedFrameToLocalGeodeticFrame(
|
|
const double referenceLatitude, //!< reference geodetic latitude [rad]
|
|
const double referenceLongitude, //!< reference geodetic longitude [rad]
|
|
const double dX, //!< earth centered earth fixed vector component [m]
|
|
const double dY, //!< earth centered earth fixed vector component [m]
|
|
const double dZ, //!< earth centered earth fixed vector component [m]
|
|
double* dN, //!< local geodetic northing vector component [m]
|
|
double* dE, //!< local geodetic easting vector component [m]
|
|
double* dUp //!< local geodetic vertical vector component [m]
|
|
)
|
|
{
|
|
double sinlat;
|
|
double coslat;
|
|
double sinlon;
|
|
double coslon;
|
|
BOOL result;
|
|
|
|
result = GEODESY_IsLatitudeValid( referenceLatitude );
|
|
if( result == FALSE )
|
|
{
|
|
*dN = 0;
|
|
*dE = 0;
|
|
*dUp = 0;
|
|
GNSS_ERROR_MSG( "Reference latitude is invalid." );
|
|
return result;
|
|
}
|
|
|
|
sinlat = sin(referenceLatitude);
|
|
coslat = cos(referenceLatitude);
|
|
sinlon = sin(referenceLongitude);
|
|
coslon = cos(referenceLongitude);
|
|
|
|
*dN = -sinlat*coslon * dX - sinlat*sinlon * dY + coslat * dZ;
|
|
*dE = -sinlon * dX + coslon * dY;
|
|
*dUp = coslat*coslon * dX + coslat*sinlon * dY + sinlat * dZ;
|
|
|
|
return TRUE;
|
|
}
|
|
|
|
|
|
|
|
BOOL GEODESY_ComputeAzimuthAndElevationAnglesBetweenToPointsInTheEarthFixedFrame(
|
|
const GEODESY_enumReferenceEllipse referenceEllipse, //!< reference ellipse enumerated []
|
|
const double fromX, //!< earth centered earth fixed vector from point X component [m]
|
|
const double fromY, //!< earth centered earth fixed vector from point Y component [m]
|
|
const double fromZ, //!< earth centered earth fixed vector from point Z component [m]
|
|
const double toX, //!< earth centered earth fixed vector to point X component [m]
|
|
const double toY, //!< earth centered earth fixed vector to point Y component [m]
|
|
const double toZ, //!< earth centered earth fixed vector to point Z component [m]
|
|
double* elevation, //!< elevation angle [rad]
|
|
double* azimuth //!< azimuth angle [rad]
|
|
)
|
|
{
|
|
double lat; // reference geodetic latitude ('from' point) [rad]
|
|
double lon; // reference geodetic longitude ('from' point) [rad]
|
|
double dX; // ECEF X vector component between 'from' and 'to' point (m)
|
|
double dY; // ECEF Y vector component between 'from' and 'to' point (m)
|
|
double dZ; // ECEF Z vector component between 'from' and 'to' point (m)
|
|
double dN; // LG northing vector component between 'from' and 'to' point (m)
|
|
double dE; // LG easting vector component between 'from' and 'to' point (m)
|
|
double dUp; // LG vertical vector component between 'from' and 'to' point (m)
|
|
double tmp; // temp value
|
|
BOOL result;
|
|
|
|
*elevation = 0;
|
|
*azimuth = 0;
|
|
|
|
// get the reference geodetic curvilinear coordinates from the 'from' point
|
|
result = GEODESY_ConvertEarthFixedCartesianToGeodeticCurvilinearCoordinates(
|
|
referenceEllipse,
|
|
fromX,
|
|
fromY,
|
|
fromZ,
|
|
&lat,
|
|
&lon,
|
|
&tmp );
|
|
if( result == FALSE )
|
|
{
|
|
GNSS_ERROR_MSG( "GEODESY_ConvertEarthFixedCartesianToGeodeticCurvilinearCoordinates returned FALSE." );
|
|
return result;
|
|
}
|
|
|
|
// vector between the two points in the earth fixed frame
|
|
dX = toX - fromX;
|
|
dY = toY - fromY;
|
|
dZ = toZ - fromZ;
|
|
|
|
// rotate the vector to the local geodetic frame
|
|
result = GEODESY_RotateVectorFromEarthFixedFrameToLocalGeodeticFrame(
|
|
lat,
|
|
lon,
|
|
dX,
|
|
dY,
|
|
dZ,
|
|
&dN,
|
|
&dE,
|
|
&dUp );
|
|
if( result == FALSE )
|
|
{
|
|
GNSS_ERROR_MSG( "GEODESY_RotateVectorFromEarthFixedFrameToLocalGeodeticFrame returned FALSE." );
|
|
return result;
|
|
}
|
|
|
|
// compute the elevation
|
|
tmp = sqrt( dN*dN + dE*dE );
|
|
*elevation = atan( dUp / tmp );
|
|
|
|
// compute the azimuth
|
|
*azimuth = atan2(dE, dN);
|
|
|
|
// by convention, azimuth will be between 0 to 2 PI
|
|
if( *azimuth < 0.0 )
|
|
*azimuth += TWOPI;
|
|
|
|
return TRUE;
|
|
}
|
|
|