I implemented a WGS84 distance function using the average of the start and end altitude as the constant altitude. If you are certain that there will be relatively little altitude variation along your path this works acceptably well (error is relative to the altitude difference of your two LLA points).
Here's my code (C#):
/// <summary>
/// Gets the geodesic distance between two pathpoints in the current mode's coordinate system
/// </summary>
/// <param name="point1">First point</param>
/// <param name="point2">Second point</param>
/// <param name="mode">Coordinate mode that both points are in</param>
/// <returns>Distance between the two points in the current coordinate mode</returns>
public static double GetGeodesicDistance(PathPoint point1, PathPoint point2, CoordMode mode) {
// calculate proper geodesics for LLA paths
if (mode == CoordMode.LLA) {
// meeus approximation
double f = (point1.Y + point2.Y) / 2 * LatLonAltTransformer.DEGTORAD;
double g = (point1.Y - point2.Y) / 2 * LatLonAltTransformer.DEGTORAD;
double l = (point1.X - point2.X) / 2 * LatLonAltTransformer.DEGTORAD;
double sinG = Math.Sin(g);
double sinL = Math.Sin(l);
double sinF = Math.Sin(f);
double s, c, w, r, d, h1, h2;
// not perfect but use the average altitude
double a = (LatLonAltTransformer.A + point1.Z + LatLonAltTransformer.A + point2.Z) / 2.0;
sinG *= sinG;
sinL *= sinL;
sinF *= sinF;
s = sinG * (1 - sinL) + (1 - sinF) * sinL;
c = (1 - sinG) * (1 - sinL) + sinF * sinL;
w = Math.Atan(Math.Sqrt(s / c));
r = Math.Sqrt(s * c) / w;
d = 2 * w * a;
h1 = (3 * r - 1) / 2 / c;
h2 = (3 * r + 1) / 2 / s;
return d * (1 + (1 / LatLonAltTransformer.RF) * (h1 * sinF * (1 - sinG) - h2 * (1 - sinF) * sinG));
}
PathPoint diff = new PathPoint(point2.X - point1.X, point2.Y - point1.Y, point2.Z - point1.Z, 0);
return Math.Sqrt(diff.X * diff.X + diff.Y * diff.Y + diff.Z * diff.Z);
}
In practice we've found that the altitude difference rarely makes a large difference, our paths are typically 1-2km long with altitude varying on the order of 100m and we see about ~5m change on average versus using the WGS84 ellipsoid unmodified.
Edit:
To add to this, if you do expect large altitude changes, you can convert your WGS84 coordinates to ECEF (earth centered earth fixed) and evaluate straight-line paths as shown at the bottom of my function. Converting a point to ECEF is simple to do:
/// <summary>
/// Converts a point in the format (Lon, Lat, Alt) to ECEF
/// </summary>
/// <param name="point">Point as (Lon, Lat, Alt)</param>
/// <returns>Point in ECEF</returns>
public static PathPoint WGS84ToECEF(PathPoint point) {
PathPoint outPoint = new PathPoint(0);
double lat = point.Y * DEGTORAD;
double lon = point.X * DEGTORAD;
double e2 = 1.0 / RF * (2.0 - 1.0 / RF);
double sinLat = Math.Sin(lat), cosLat = Math.Cos(lat);
double chi = A / Math.Sqrt(1 - e2 * sinLat * sinLat);
outPoint.X = (chi + point.Z) * cosLat * Math.Cos(lon);
outPoint.Y = (chi + point.Z) * cosLat * Math.Sin(lon);
outPoint.Z = (chi * (1 - e2) + point.Z) * sinLat;
return outPoint;
}