double GeoAlgorithms::getPythagoreanOrVincentyDistance(double aLat1, double aLon1, double aLat2, double aLon2, double elevation1, double elevation2) { double a_84; double b_84; double old_a; double old_b; double ax; double ay; double az; double bx; double by; double bz; double returnValue; Initialize_Ellipsoids(); WGS84_Axes(&a_84, &b_84); Get_Geocentric_Parameters(&old_a, &old_b); Set_Geocentric_Parameters(a_84, b_84); if (Convert_Geodetic_To_Geocentric(aLat1 * PI / 180, aLon1 * PI / 180, elevation1, &ax, &ay, &az) == 0 && Convert_Geodetic_To_Geocentric(aLat2 * PI / 180, aLon2 * PI / 180, elevation2, &bx, &by, &bz) == 0) { double pythagoreanDistance = getPythagoreanDistance(ax, ay, az, bx, by, bz); double vincentyDistance = getVincentyDistance(aLat1, aLon1, aLat2, aLon2); //Note: if getVincentyDistance fails and returns < 0, pythagoreanDistance will be returned. returnValue = pythagoreanDistance > vincentyDistance ? pythagoreanDistance : vincentyDistance; } else { returnValue = getVincentyDistance(aLat1, aLon1, aLat2, aLon2); } Set_Geocentric_Parameters(old_a, old_b); return returnValue; }
void Convert_Local_Cartesian_To_Geodetic (double X, double Y, double Z, double *Latitude, double *Longitude, double *Height) { /* BEGIN Convert_Local_Cartesian_To_Geodetic */ /* * The function Convert_Local_Cartesian_To_Geodetic converts local cartesian * coordinates (X, Y, Z) to geodetic coordinates (latitude, longitude, * and height), according to the current ellipsoid and local origin parameters. * * X : Local cartesian X coordinate, in meters (input) * Y : Local cartesian Y coordinate, in meters (input) * Z : Local cartesian Z coordinate, in meters (input) * Latitude : Calculated latitude value, in radians (output) * Longitude : Calculated longitude value, in radians (output) * Height : Calculated height value, in meters (output) */ double U, V, W; Convert_Local_Cartesian_To_Geocentric(X, Y, Z, &U, &V, &W); Set_Geocentric_Parameters(LocalCart_a, LocalCart_f); Convert_Geocentric_To_Geodetic(U, V, W, Latitude, Longitude, Height); if (*Longitude > PI) *Longitude -= TWO_PI; if (*Longitude < -PI) *Longitude += TWO_PI; } /* END OF Convert_Local_Cartesian_To_Geodetic */
int pj_geocentric_to_geodetic( double a, double es, long point_count, int point_offset, double *x, double *y, double *z ) { double b; int i; if( es == 0.0 ) b = a; else b = a * sqrt(1-es); if( Set_Geocentric_Parameters( a, b ) != 0 ) { pj_errno = PJD_ERR_GEOCENTRIC; return pj_errno; } for( i = 0; i < point_count; i++ ) { long io = i * point_offset; Convert_Geocentric_To_Geodetic( x[io], y[io], z[io], y+io, x+io, z+io ); } return 0; }
long Convert_Geodetic_To_Local_Cartesian (double Latitude, double Longitude, double Height, double *X, double *Y, double *Z) { /* BEGIN Convert_Geodetic_TO_Local_Cartesian /* * The function Convert_Geodetic_To_Local_Cartesian converts geodetic coordinates * (latitude, longitude, and height) to local cartesian coordinates (X, Y, Z), * according to the current ellipsoid and local origin parameters. * * Latitude : Geodetic latitude, in radians (input) * Longitude : Geodetic longitude, in radians (input) * Height : Geodetic height, in meters (input) * X : Calculated local cartesian X coordinate, in meters (output) * Y : Calculated local cartesian Y coordinate, in meters (output) * Z : Calculated local cartesian Z coordinate, in meters (output) * */ double U, V, W; long Error_Code = LOCCART_NO_ERROR; if ((Latitude < -PI_OVER_2) || (Latitude > PI_OVER_2)) { /* geodetic latitude out of range */ Error_Code |= LOCCART_LAT_ERROR; } if ((Longitude < -PI) || (Longitude > TWO_PI)) { /* geodetic longitude out of range */ Error_Code |= LOCCART_LON_ERROR; } if (!Error_Code) { Set_Geocentric_Parameters(LocalCart_a, LocalCart_f); Convert_Geodetic_To_Geocentric(Latitude, Longitude, Height, &U, &V, &W); Convert_Geocentric_To_Local_Cartesian(U, V, W, X, Y, Z); } /* END OF if(!Error_Code) */ return (Error_Code); } /* END OF Convert_Geodetic_To_Local_Cartesian */