/** \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 #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; }