Геопространственные координаты и расстояние в километрах

Это продолжение этот вопрос.

Кажется, я зациклился на этом. По сути, мне нужно иметь возможность конвертировать туда и обратно для ссылки на координаты либо в стандартной системе градусов, либо путем измерения расстояния к северу от южного полюса вдоль международной линии дат, а затем расстояния на восток, начиная с этой точки на дату. линия. Для этого (а также для некоторых более общих задач измерения расстояния) у меня есть один метод определения расстояния между двумя точками широты / долготы и другой метод, который берет точку широты / долготы, направление и расстояние и возвращает точка широты / долготы в конце этого курса.

Вот два статических метода, которые я определил:

/* Takes two lon/lat pairs and returns the distance between them in kilometers.
*/
public static double distance (double lat1, double lon1, double lat2, double lon2) {
    double theta = toRadians(lon1-lon2);
    lat1 = toRadians(lat1);
    lon1 = toRadians(lon1);
    lat2 = toRadians(lat2);
    lon2 = toRadians(lon2);

    double dist = sin(lat1)*sin(lat2) + cos(lat1)*cos(lat2)*cos(theta);
    dist = toDegrees(acos(dist)) * 60 * 1.1515 * 1.609344 * 1000;

    return dist;
}

/* endOfCourse takes a lat/lon pair, a heading (in degrees clockwise from north), and a distance (in kilometers), and returns
 * the lat/lon pair that would be reached by traveling that distance in that direction from the given point.
 */
public static double[] endOfCourse (double lat1, double lon1, double tc, double dist) {
    double pi = Math.PI;
    lat1 = toRadians(lat1);
    lon1 = toRadians(lon1);
    tc = toRadians(tc);
    double dist_radians = toRadians(dist / (60 * 1.1515 * 1.609344 * 1000));
    double lat = asin(sin(lat1) * cos(dist_radians) + cos(lat1) * sin(dist_radians) * cos(tc));
    double dlon = atan2(sin(tc) * sin(dist_radians) * cos(lat1), cos(dist_radians) - sin(lat1) * sin(lat));
    double lon = ((lon1-dlon + pi) % (2*pi)) - pi;
    double[] endPoint = new double[2];
    endPoint[0] = lat; endPoint[1] = lon;
    return endPoint;
}

А вот функция, которую я использую для проверки:

public static void main(String args[]) throws java.io.IOException, java.io.FileNotFoundException {
    double distNorth = distance(0.0, 0.0, 72.0, 0.0);
    double distEast = distance(72.0, 0.0, 72.0, 31.5);
    double lat1 = endOfCourse(0.0, 0.0, 0.0, distNorth)[0];
    double lon1 = endOfCourse(lat1, 0.0, 90.0, distEast)[1];
    System.out.println("end at: " + lat1 + " / " + lon1);
    return;
}

Значения «конца в» должны быть прибл. 72,0 / 31,5. Но вместо этого я получаю примерно 1,25 / 0,021.

Я предполагаю, что мне не хватает чего-то глупого, я забыл где-то преобразовать единицы или что-то еще ... Любая помощь будет принята с благодарностью!

ОБНОВЛЕНИЕ 1:

Я (правильно) написал функцию расстояния для возврата метров, но по ошибке написал километры в комментариях ... что, конечно, смутило меня, когда я вернулся к этому сегодня. В любом случае, теперь это исправлено, и я исправил ошибку факторинга в методе endOfCourse, и я также понял, что забыл преобразовать обратно в градусы из радианов в этом методе. В любом случае: хотя кажется, что теперь я получаю правильное число широты (71,99 ...), число долготы неверно (я получаю 3,54 вместо 11,5).

ОБНОВЛЕНИЕ 2: У меня была опечатка в тесте, как указано ниже. Теперь это исправлено в коде. Однако число долготы по-прежнему неверное: теперь я получаю -11,34 вместо 11,5. Я думаю, что с этими строками что-то не так:

double dlon = atan2(sin(tc) * sin(dist_radians) * cos(lat1), cos(dist_radians) - sin(lat1) * sin(lat));
double lon = ((lon1-dlon + pi) % (2*pi)) - pi;