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;
}
int pj_geodetic_to_geocentric( 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;
if( Convert_Geodetic_To_Geocentric( y[io], x[io], z[io],
x+io, y+io, z+io ) != 0 )
{
pj_errno = PJD_ERR_GEOCENTRIC;
return PJD_ERR_GEOCENTRIC;
}
}
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 */
