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 */
