Начну с громкого заявления: я придумал способ найти точку пересечения двух отрезков, заданных координатами концов. Придумал давно, лет 7 назад, в 2017 году, примерно, да, путь к этой публикации был долог, в основном из‑за лени.

И да, я его придумал потому что не смог нагуглить, может он где‑то и без меня описан был, может за эти 7 лет кто‑то написал что‑то похожее, а может я придумал какую‑то фигню, которую умные люди изобретать не станут...

Вы, возможно, скажете, что нет тут ничего сложного, открываешь Википедию: пересечение прямых и смотришь. Хм, даже на Хабр об этом уже писали: Нахождение точки пересечения двух прямых (и отрезков). Но не всё так однозначно... недаром на КДПВ изображена Земля. Интересует меня именно пересечение прямых не на плоскости, а на поверхности Земли. Если уж пошла такая пьянка, то прямые вовсе не прямые, а ортодромия. Об этом тоже писали на Хабре и не раз, например вот: Занимательная геодезия.

В общем, Земля у нас не плоская! Была бы плоская, уже бы закончили статью и не мучились, и не начинали бы.

Поскольку Земля у нас в лучшем случае круглая, а может даже сферическая в вакууме, то применим Сферическую Тригонометрию. Здесь я ничего нового не придумывал — здесь уже всё придумано до нас, просто опишу, то что удалось нагуглить. Такое длительное вступление поможет лучше проникнуться проблемой.

Вообще уже на этом шаге будет уже много мороки, количество формул и тригонометрии в публикации зашкаливает, даже я заколебался писать их в TeX, спасибо@NewTechAuditи публикации Как красиво писать формулы c LaTeX, — без этой статьи я бы не справился.

Но на сферической тригонометрии мы не остановимся, она конечно позволит найти аналитическое решение для ситуации, когда полярный и экваториальный радиусы Земли равны (возможно для Ваших задач такое допущение приемлемо, — я не настаиваю), но мы то знаем, что Земля не только не плоская, и имеет форму шара, так она ещё и сплюснута на полюсах — к‑к-к‑комбо.

Усложнили себе жизнь на ровном месте, простите за плоскоземельный каламбур.

В общем, аналитического решения с формулами, для Сплюснутой Земли — нет. С этим вопросом я вышел в интернет... и ничего не нашёл, кроме решения главных геодезических задач — это близко, но не оно. Я вышел из интернета. Однако придумал способ применить главные геодезические задачи для решения своей проблемы, — вот его и опишу, помимо прочего.

На начнём всё же со Сферической Земли, без сплющивания. Как я уже писал, возможно, для Вашей задачи этого будет достаточно.

В процессе чтения этой статьи могут возникнуть МатАн'овские или ВышМат'овские флешбеки, — мужайтесь, но примеры кода приложу, на C#. Вероятно код будет проще воспринимать, чем формулы, кстати, вот он, на GitHub: Geodesic.

А теперь, приступим.

• Расчёты

  • Сфероид: Вывод формулы долготы промежуточной точки как функции широты

  • Сфероид: Поиск координат точки пересечения линий

  • Эллипсоид: Определение широты или долготы промежуточной точки

  • Эллипсоид: Поиск координат точки пересечения линий

  • Сфероид: Решение геодезических задач

    • Прямая геодезическая задача

    • Обратная геодезическая задача

  • Эллипсоид: Решение геодезических задач

    • Обратная геодезическая задача

    • Прямая геодезическая задача

• Итоги

Расчёты

И так, нам нужно найти координаты точки пересечения. Решаем задачу на сфероиде, у которого полярный и экваториальный радиусы равны.

Начнём с того, что на Википедии есть формула широты промежуточной точки как функция долготы:

Здесь:

и — широта и долгота начальной точки отрезка

и — широта и долгота конечной точки отрезка

и — широта и долгота промежуточной точки

Т.е. формула позволяет найти широту промежуточной точки, в интересующей нас долготе . Начальные и конечные точки отрезка считаем известными.

private double GetLatitudeSpheroid(double longitude, Point coord1, Point coord2)
{
    return Math.Atan(Math.Tan(coord1.LatR) / Math.Sin(coord2.LonR - coord1.LonR) *
                         Math.Sin(coord2.LonR - longitude) +
                         Math.Tan(coord2.LatR) / Math.Sin(coord2.LonR - coord1.LonR) *
                         Math.Sin(longitude - coord1.LonR)) * 180 / Math.PI;
}

Наоборот — найти долготу по широте, не нашёл формулу, но если не лень, можем её вывести сами.

Вывод формулы долготы промежуточной точки как функции широты

Для начала введём обозначения для удобства:

Представление на C#:

double a1 = Math.Tan(coord1.LatR) / Math.Sin(coord2.LonR - coord1.LonR);
double a2 = Math.Tan(coord2.LatR) / Math.Sin(coord2.LonR - coord1.LonR);

Здесь все известные нам величины. Формулу широты промежуточной точки теперь можно переписать вот так (будем искать тангенс широты, чтобы избавиться от арктангенса):

Теперь мы можем раскрыть синус разности, по формуле:

Получим:

Раскроем скобки:

Приведём подобные и снова занесём всё в скобки:

Для вычисления той части выражения, что оказалась внутри скобок, у нас есть всё необходимое — все величины известны, поэтому можем ввести для них буквенные обозначения:

Оно же в виде программного кода:

double b = a1 * Math.Sin(coord2.LonR) - a2 * Math.Sin(coord1.LonR);
double c = a2 * Math.Cos(coord1.LonR) - a1 * Math.Cos(coord2.LonR);

Подставим эти обозначения в формулу, получим:

Существует такое представление:

где угол находится из соотношения:

Её тоже можно посмотреть на Википедии, в разделе суммы, только А и B, заменил на X и Y, что бы не было путаницы с ведёнными нами обозначениями.

С принятыми нами обозначениями получим:

или

Соответствующая строка в программе:

double lonfet = Math.Asin(Math.Tan(latitude) / Math.Sqrt(b * b + c * c));

где — долгота промежуточной точки, а получается из соотношения:

Теперь программная реализация.

private double GetLongitudeSpheroid(double latitude, Point coord1, Point coord2)
{
    double a1 = Math.Tan(coord1.LatR) / Math.Sin(coord2.LonR - coord1.LonR);
    double a2 = Math.Tan(coord2.LatR) / Math.Sin(coord2.LonR - coord1.LonR);
    double b = a1 * Math.Sin(coord2.LonR) - a2 * Math.Sin(coord1.LonR);
    double c = a2 * Math.Cos(coord1.LonR) - a1 * Math.Cos(coord2.LonR);

    double lonfet = Math.Asin(Math.Tan(latitude) / Math.Sqrt(b * b + c * c));

    // В зависимости от значений c, b и угла - разные преобразования ответа

    var angle = Math.Atan((coord2.LatR - coord1.LatR) / (coord2.LonR - coord1.LonR)) * 180 / Math.PI;

    if (c < 0 && b < 0 && angle > 0)
        return (lonfet + Math.Atan(c / b)) * 180 / Math.PI + 90;
    if (c > 0 && b > 0 && angle > 0)
        return (lonfet + Math.Atan(c / b)) * 180 / Math.PI - 90;
    if (c < 0 && b < 0 && angle < 0)
        return -90 - (lonfet - Math.Atan(c / b)) * 180 / Math.PI;
    if (c > 0 && b > 0 && angle < 0)
        return 90 - (lonfet - Math.Atan(c / b)) * 180 / Math.PI;

    if (Math.Abs(b) < 0.00000001 && angle > 0)
        return -90 + (lonfet + Math.Atan(c / b)) * 180 / Math.PI;
    if (Math.Abs(b) < 0.00000001 && angle < 0)
        return 90 - (lonfet - Math.Atan(c / b)) * 180 / Math.PI;

    if (c * angle > 0)
    {
        if (c > 0)
            return (lonfet + Math.Atan(c / b)) * 180 / Math.PI + 90; 
        return 90 - (lonfet - Math.Atan(c / b)) * 180 / Math.PI;
    }
    else
    {
        if (c > 0)
            return -90 - (lonfet - Math.Atan(c / b)) * 180 / Math.PI;
        return -90 + (lonfet + Math.Atan(c / b)) * 180 / Math.PI;
    }
}

Но вернёмся к поиску пересечения.

Поиск координат точки пересечения линий на сфероиде

Нам же нужно найти точку пересечения двух линий, логично, что в точке пересечения значения широт двух линий должны совпасть, как и значения долготы, иначе это не пересечение вовсе. Вооружившись сим размышлением и формулой расчёта широты, можем вывести равенство:

Здесь:

и — широты и долготы граничных точек первой линии.

и — широты и долготы граничных точек второй линии.

— долгота точки пересечения

В этом уравнении у нас одна неизвестная — долгота точки пересечения , остальные должны быть известны изначально. Если мы её сможем вывести, то подставив в формулу широты, мы получим долготу и широту точки пересечения.

Выведем из получившегося равенства долготу.

Для удобства введём следующие обозначения:

Т.е. введём буквенные обозначения для известной части уравнения, которые мы можем вычислить просто зная координаты границ отрезков (погодите, пока рано это делать), теперь равенство можно записать следующим образом:

Или перенесём всё в левую часть:

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

Получим:

Мы можем вновь раскрыть скобки, например (для части выражения):

И ввести буквенные обозначения для тех частей уравнения, в которых у нас изначально есть всё для вычислений т.е. нет неизвестной, как например для .

Всё это в виде программного кода:

double dl1 = Math.Sin(coord12.LonR - coord11.LonR);
double dl2 = Math.Sin(coord22.LonR - coord21.LonR);

// Первая ортодрома
double a1S = Math.Tan(coord11.LatR) / dl1 * Math.Sin(coord12.LonR);
double a2S = Math.Tan(coord12.LatR) / dl1 * Math.Cos(coord11.LonR);
double a1Ss = Math.Tan(coord11.LatR) / dl1 * Math.Cos(coord12.LonR);
double a2Ss = Math.Tan(coord12.LatR) / dl1 * Math.Sin(coord11.LonR);

// Вторая ортодрома
double a3S = Math.Tan(coord21.LatR) / dl2 * Math.Sin(coord22.LonR);
double a4S = Math.Tan(coord22.LatR) / dl2 * Math.Cos(coord21.LonR);
double a3Ss = Math.Tan(coord21.LatR) / dl2 * Math.Cos(coord22.LonR);
double a4Ss = Math.Tan(coord22.LatR) / dl2 * Math.Sin(coord21.LonR);

Выражение приобретёт следующий вид:

поделим на :

или

Соответствует коду:

double b1 = a1S - a2Ss - a3S + a4Ss;
double b2 = a2S - a1Ss + a3Ss - a4S;

var answer = Math.Atan(-b1 / b2) * 180 / Math.PI;

Код метода целиком:

private double SpheroidLongitude(Point coord11, Point coord12, Point coord21, Point coord22)
{
    double dl1 = Math.Sin(coord12.LonR - coord11.LonR);
    double dl2 = Math.Sin(coord22.LonR - coord21.LonR);

    // Первая ортодрома
    double a1S = Math.Tan(coord11.LatR) / dl1 * Math.Sin(coord12.LonR);
    double a2S = Math.Tan(coord12.LatR) / dl1 * Math.Cos(coord11.LonR);
    double a1Ss = Math.Tan(coord11.LatR) / dl1 * Math.Cos(coord12.LonR);
    double a2Ss = Math.Tan(coord12.LatR) / dl1 * Math.Sin(coord11.LonR);

    // Вторая ортодрома
    double a3S = Math.Tan(coord21.LatR) / dl2 * Math.Sin(coord22.LonR);
    double a4S = Math.Tan(coord22.LatR) / dl2 * Math.Cos(coord21.LonR);
    double a3Ss = Math.Tan(coord21.LatR) / dl2 * Math.Cos(coord22.LonR);
    double a4Ss = Math.Tan(coord22.LatR) / dl2 * Math.Sin(coord21.LonR);

    double b1 = a1S - a2Ss - a3S + a4Ss;
    double b2 = a2S - a1Ss + a3Ss - a4S;

    var answer = Math.Atan(-b1 / b2) * 180 / Math.PI;
    if (answer < 0 && coord11.Longitude > 0 && coord12.Longitude > 0)
        return 180 + answer;
    if (answer > 0 && coord11.Longitude < 0 && coord12.Longitude < 0)
        return -180 + answer;

    return answer;
}

Таким образом мы можем найти долготу точки пересечения, найти широту можно подставив значение в функцию широты. У нас есть две линии, в функции вычисления широты по долготе, используется одна, поэтому в качестве , и , можно использовать координаты любой из двух пересекающихся линий - значение совпадёт. А можно использовать вторую пару точек для контрольной проверки.

var inLat1 = _interPoint.GetLatitude(inLon, coord11, coord12);
var inLat2 = _interPoint.GetLatitude(inLon, coord21, coord22);

Вот так мы вычислили долготу и широту точки пересечения двух линий на сфероиде - сферическая точка пересечения найдена.

Со сфероидом всё хорошо, есть аналитическое решение, осталось только определиться с эллипсоидом вращения. И вот тут аналитического решения не будет, - придётся изобретать алгоритм.

Решая задачу для сфероида, мы начали с того, что у нас была формула широты промежуточной точки по известному значению долготы, да эллипсоида так же начнём с этой задачи.

Определение широты или долготы промежуточной точки на эллипсоиде

Для этого предлагаю использовать прямую и обратную геодезические задачи:

1. Прямая геодезическая задача: по известной координате начала, направлению и расстоянию, позволяет найти координаты конца отрезка.

2. Обратная геодезическая задача: по известным координатам двух точек вычисляется расстояние и угол.

В нашем случае как раз известны координаты начала и координаты конца отрезка . И нам нужно либо найти на этой линии долготу по известной широте , либо найти широту , по известной долготе . Давайте, для примера, по известной долготе, найдём значение широты, алгоритм таков:

1. Используя обратную геодезическую задачу, определим расстояние между двумя известными точками и угол.

2. Полученное расстояние разделим пополам, и, применив прямую геодезическую задачу, по найденному ранее углу, и полученному расстоянию, найдём координаты средней точки.

3. ЕСЛИ долгота этой точки равна целевой долготе, пусть даже с некоторой погрешностью, ТО и её широта — наш ответ, алгоритм ЗАВЕРШАЕТСЯ.

4. ИНАЧЕ мы проверяем: целевая долгота ближе в началу отрезка, или ближе к концу:

   1. ЕСЛИ ближе к началу, ТО алгоритм рекурсивно продолжается с ШАГА 1, где в качестве начальной точки используется начальная точка, а в качестве конечной — средняя.

   2. ЕСЛИ ближе к концу, ТО алгоритм рекурсивно продолжается с ШАГА 1, где в качестве начальной точки используется средняя точка, а в качестве конечной — конечная.

Этот алгоритм будет рекурсивно продолжаться пока мы не достигнем на ШАГЕ 3 заданной точности.

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

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

Поиск координат точки пересечения линий на эллипсоиде

И так нам известны координаты первой точки и координаты второй точки. Требуется найти точку пересечения.

1. Отсортируем полученные 4 точки, например по долготе, и возьмём две средних, т.е. те, которые в отсортированном списке оказались на втором и третьем местах. Значения их долготы обозначим и

2. По и найдём среднюю арифметическую и обозначим .

3. У нас есть две линии, и выше мы уже описали алгоритм нахождения значения широты, по известному значению долготы, поэтому вычислим для обеих линий значения широты соответствующих значениям долготы , , , получим такой набор точек для первой и для второй линии соответственно:

4. ЕСЛИ какая-либо пара координат первой ортодромии совпала (опять же с некоторой погрешностью) с какой-либо парой координат второй, ТО это и есть точка пересечения. И алгоритм ЗАВЕРШАЕТСЯ. ИНАЧЕ переходим на следующий шаг.

5. Возьмём попарно широты из двух разных линий отложенные на одной долготе: и , и , и , вычитаем их попарно друг из друга:

6. Теперь попарно сравним с и с

   1. ЕСЛИ у и разные знаки, ТО

   2. ЕСЛИ у и разные знаки, ТО

      Различие знаков говорит о том, что пересечение произошло на этом участке, между соответствующими значениями долготы. Алгоритм рекурсивно продолжается, переходим на ШАГ 2.

Алгоритм так же на каждой итерации приближается к ответу, рано или поздно, с некоторой точностью он найдёт решение, и это будут координаты точки пересечения двух линий.

Ура, мы нашли решение, которое позволяет найти точку пересечения линий для эллипсоида. Это собственно и было изначальной задачей.

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

Решение геодезических задач на сфероиде

Решение геодезической задачи на сфероиде вероятно можно загуглить, так я и сделал когда‑то, и нашёл решение на сайте geogr.msu.ru в виде pdf файла, однако со временем ссылка стала битой, а веб‑архив ничего не сохранил, аналогичного решения сходу найти не получилось, поэтому опишу вычисления так, как я их помню и могу воспроизвести.

Прямая геодезическая задача

Как выше уже говорилось, нам известны координаты начальной точки , азимут , т.е. направление к конечной точке, и расстояние до конечной точки. Необходимо найти эту конечную точку , и обратный азимут .

Примем за радиус сферы.

Широту определим используя теоремы сферической тригонометрии (теорему косинусов).

где — длинна , выраженная в долях радиуса сферы

var sigma = s / _ellipsoid.PolarRadius;

Теперь определим долготу.

Из теоремы котангенсов:

Выполним некоторые преобразования:

Домножим всё на чтобы избавиться от всяких котангенсов в знаменателе:

В коде:

var lambda = Math.Atan(Math.Sin(sigma) * Math.Sin(a1) /
                                   (Math.Cos(sigma) * Math.Cos(lat1) - Math.Sin(sigma) * Math.Sin(lat1) * Math.Cos(a1)));

Здесь это разность долгот, второй и первой точек, зная долготу первой, мы можем теперь вычислить долготу второй.

var lon2 = -lambda + coord.LonR;

Осталось вычислить обратный азимут , по теореме котангенсов:

Выполним всё те же некоторые преобразования, аналогичные тем что были выше, я уже не буду их расписывать, получим:

Соответствующий код:

 var a2 = -Math.Atan(Math.Cos(lat1) * Math.Sin(a1) /
                                (Math.Cos(lat1) * Math.Cos(sigma) * Math.Cos(a1) - Math.Sin(lat1) * Math.Sin(sigma))) *
                     180 / Math.PI;

Ну и метод полностью:

private DirectProblemAnswer DirectProblemSpheroid(Point coord, double a1, double s)
{
    var aDegree = a1;
    a1 = a1 * Math.PI / 180;
    var lat1 = coord.LatR;

    var sigma = s / _ellipsoid.PolarRadius;
    var lat2 = Math.Asin(Math.Sin(lat1) * Math.Cos(sigma) + Math.Cos(lat1) * Math.Sin(sigma) * Math.Cos(a1));

    var lambda = Math.Atan(Math.Sin(sigma) * Math.Sin(a1) /
                           (Math.Cos(sigma) * Math.Cos(lat1) - Math.Sin(sigma) * Math.Sin(lat1) * Math.Cos(a1)));
    var lon2 = -lambda + coord.LonR;

    var a2 = -Math.Atan(Math.Cos(lat1) * Math.Sin(a1) /
                        (Math.Cos(lat1) * Math.Cos(sigma) * Math.Cos(a1) - Math.Sin(lat1) * Math.Sin(sigma))) *
             180 / Math.PI;
    

    lon2 = lon2 * 180 / Math.PI;
    lat2 = lat2 * 180 / Math.PI;

    var point1 = IsPolisIntersect(coord, aDegree, s)
        ? new Point(lon2 + (lon2 < 0 ? 180 : -180), lat2)
        : new Point(lon2, lat2);

    a2 = Azimuth.AzimuthRecovery(point1, new Point(coord.Longitude, lat1 * 180 / Math.PI), a2);

    return  new DirectProblemAnswer(point1, a2);
}

Мы привели все формулы, необходимые для решения прямой геодезической задачи на сфере, применяя сферическую тригонометрию.

Обратная геодезическая задача

Обратная геодезическая задача всё ещё заключается в том, чтобы по координатам двух точек, определить длину и азимутов, как прямого , так и обратного .

Для сферы радиуса , решение следующее, опять же по теореме косинусов:

где , — расстояние, выраженное в долях радиуса сферы. Само расстояние определяется как:

Немного кода:

var ω = coord2.LonR - coord1.LonR;

var cosSigma = Math.Sin(coord1.LatR) * Math.Sin(coord2.LatR) + Math.Cos(coord1.LatR) * Math.Cos(coord2.LatR) * Math.Cos(ω);

double s;
if (cosSigma < 0)
    s = _ellipsoid.PolarRadius * (Math.PI - Math.Abs(Math.Acos(cosSigma)));
else if (Math.Abs(cosSigma - 1) < TOLERANCE)
    s = 0;
else
    s = _ellipsoid.PolarRadius * Math.Acos(cosSigma);

Прямой и обратный азимуты, можно выразить применяя формулы котангенсов:

Ещё немного кода:

var a1 =
    Math.Atan(Math.Cos(coord2.LatR) * Math.Sin(ω) /
              (Math.Cos(coord1.LatR) * Math.Sin(coord2.LatR) - Math.Sin(coord1.LatR) * Math.Cos(coord2.LatR) * Math.Cos(ω))) * 180 / Math.PI;
var a2 =
    Math.Atan(Math.Cos(coord1.LatR) * Math.Sin(ω) /
              (Math.Cos(coord1.LatR) * Math.Sin(coord2.LatR) * Math.Cos(ω) - Math.Sin(coord1.LatR) * Math.Cos(coord2.LatR))) * 180 / Math.PI;

Ну и метод полностью:

private InverseProblemAnswer OrthodromicSpheroidDistance(Point coord1, Point coord2)
{
    var ω = coord2.LonR - coord1.LonR;

    var cosSigma = Math.Sin(coord1.LatR) * Math.Sin(coord2.LatR) + Math.Cos(coord1.LatR) * Math.Cos(coord2.LatR) * Math.Cos(ω);

    double s;
    if (cosSigma < 0)
        s = _ellipsoid.PolarRadius * (Math.PI - Math.Abs(Math.Acos(cosSigma)));
    else if (Math.Abs(cosSigma - 1) < TOLERANCE)
        s = 0;
    else
        s = _ellipsoid.PolarRadius * Math.Acos(cosSigma);

    var a1 =
        Math.Atan(Math.Cos(coord2.LatR) * Math.Sin(ω) /
                  (Math.Cos(coord1.LatR) * Math.Sin(coord2.LatR) - Math.Sin(coord1.LatR) * Math.Cos(coord2.LatR) * Math.Cos(ω))) * 180 /
        Math.PI;
    var a2 =
        Math.Atan(Math.Cos(coord1.LatR) * Math.Sin(ω) /
                  (Math.Cos(coord1.LatR) * Math.Sin(coord2.LatR) * Math.Cos(ω) - Math.Sin(coord1.LatR) * Math.Cos(coord2.LatR))) * 180 /
        Math.PI;

    a1 = Azimuth.AzimuthRecovery(coord1, coord2, a1);
    a2 = Azimuth.AzimuthRecovery(coord2, coord1, a2);

    return new InverseProblemAnswer(a1, a2, s);
}

По обратной геодезической задаче на сфере — всё, нашли азимуты и расстояние. Теперь опишу их решение для эллипсоида.

Решение геодезических задач на эллипсоиде

Помните как мы искали широту промежуточной точки или координаты пересечения двух линий на эллипсоиде? Так у нас ещё были какие‑то итерационные алгоритмы. То есть ситуация следующая: есть эллипсоид — есть итерационный алгоритм, нет аналитического решения.

И здесь нам так же понадобится итерационный алгоритм. К счастью нам его не придётся выдумывать, его уже придумал Thaddeus Vincenty (Тадеуш Винсенти?)

На Вики, опять же, есть описание его решения: en:Vincenty's formulae. Пишут что имеет очень высокую точность, аж до 0,5 мм. Ну что же, начнём с обратной геодезической задачи.

Обратная геодезическая задача

Всё ещё пытаемся вычислить расстояние по известным координатам:

и — широта и долгота начальной точки отрезка

и — широта и долгота конечной точки отрезка

Начинается решение с пересчёта геодезических широт в приведённую широту, следующим образом:

Здесь — коэффициент полярного сжатия.

В коде мы сразу рассчитаем для них синусы и косинусы — пригодится.

double l = coord2.LonR - coord1.LonR; // Разность геодезических долгот

// Приведённая широта
double u1 = Math.Atan((1 - _ellipsoid.F) * Math.Tan(coord1.LatR));
double u2 = Math.Atan((1 - _ellipsoid.F) * Math.Tan(coord2.LatR));
double sinU1 = Math.Sin(u1), cosU1 = Math.Cos(u1);
double sinU2 = Math.Sin(u2), cosU2 = Math.Cos(u2);

Последовательным приближением рассчитывается разность долгот на эллипсоиде и угловое расстояние .

В первом приближении принимаем , где и проводим итеративные вычисления, пока не сойдётся.

Код:

var sinLambda = Math.Sin(lambda);
var cosLambda = Math.Cos(lambda);

sinSigma =
    Math.Sqrt(Math.Pow(cosU2 * sinLambda, 2) + Math.Pow(cosU1 * sinU2 - sinU1 * cosU2 * cosLambda, 2));

...

cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda;

Из этих двух формул можем вывести :

Далее:

В коде:

sigma = Math.Atan(sinSigma / cosSigma);
var sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma;

По основному тригонометрическому тождеству :

Вычисления в программном коде:

cosSqAlpha = 1 - sinAlpha * sinAlpha;

if (Math.Abs(cosSqAlpha) > TOLERANCE)
    cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha;
else
    cos2SigmaM = 0;

double c = _ellipsoid.F / 16 * cosSqAlpha * (4 + _ellipsoid.F * (4 - 3 * cosSqAlpha));
lambdaP = lambda;
lambda = l +
         (1 - c) * _ellipsoid.F * sinAlpha *
         (sigma + c * sinSigma * (cos2SigmaM + c * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)));

Приближение продолжается, пока изменения в сравнении с предыдущей итераций не станут допустимо малы, например , что соответствует примерно 0,06 мм.

do
{
  ...
} while (Math.Abs(lambda - lambdaP) > 1e-12 && --iterLimit > 0);

Теперь начинаем расчёт дистанции:

где — экваториальный радиус, — полярный радиус.

Дальше идут магические числа и :

Код:

double uSq = cosSqAlpha * (Math.Pow(_ellipsoid.EquatorialRadius, 2) - Math.Pow(_ellipsoid.PolarRadius, 2)) /
             Math.Pow(_ellipsoid.PolarRadius, 2);
double a = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)));
double b = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)));

Кстати, для вычисления и предложена модификация с более простыми формулами, давайте их тоже приведём, на всякий случай:

где вводится коэффициент:

В общем, выбирайте что больше нравится из этих двух вариантов, я выбрал первый.

Далее жуткая формула, надеюсь я ни кого не запутал своей расстановкой отступов:

Наконец, вычисляется расстояние между точками:

Снова немного кода:

double deltaSigma = b * sinSigma *
                    (cos2SigmaM +
                     b / 4 *
                     (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM) -
                      b / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) *
                      (-3 + 4 * cos2SigmaM * cos2SigmaM)));
double s = _ellipsoid.PolarRadius * a * (sigma - deltaSigma);

Ну и какая обратная геодезическая задача без вычисления азимутов:

Код для азимутов:

var a1 = Math.Atan(cosU2 * Math.Sin(lambda) / (cosU1 * sinU2 - sinU1 * cosU2 * Math.Cos(lambda))) * 180 /
         Math.PI;
var a2 = Math.Atan(cosU1 * Math.Sin(lambda) / (-sinU1 * cosU2 + cosU1 * sinU2 * Math.Cos(lambda))) * 180 /
         Math.PI;
a1 = Azimuth.AzimuthRecovery(coord1, coord2, a1);
a2 = Azimuth.AzimuthRecovery(coord2, coord1, a2);

Весь код вместе:

private InverseProblemAnswer OrthodromicEllipsoidDistance(Point coord1, Point coord2)
{
    double l = coord2.LonR - coord1.LonR; // Разность геодезических долгот

    double u1 = Math.Atan((1 - _ellipsoid.F) * Math.Tan(coord1.LatR)); // Приведённая широта
    double u2 = Math.Atan((1 - _ellipsoid.F) * Math.Tan(coord2.LatR));
    double sinU1 = Math.Sin(u1), cosU1 = Math.Cos(u1);
    double sinU2 = Math.Sin(u2), cosU2 = Math.Cos(u2);

    double lambda = l, lambdaP;
    double cosSqAlpha, sinSigma, cosSigma, cos2SigmaM, sigma;
    int iterLimit = 100;

    do
    {
        var sinLambda = Math.Sin(lambda);
        var cosLambda = Math.Cos(lambda);

        sinSigma =
            Math.Sqrt(Math.Pow(cosU2 * sinLambda, 2) + Math.Pow(cosU1 * sinU2 - sinU1 * cosU2 * cosLambda, 2));

        if (Math.Abs(sinSigma) < TOLERANCE)
            return new InverseProblemAnswer(0, 0, 0);

        cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda;
        sigma = Math.Atan(sinSigma / cosSigma);
        var sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma;
        cosSqAlpha = 1 - sinAlpha * sinAlpha;

        if (Math.Abs(cosSqAlpha) > TOLERANCE)
            cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha;
        else
            cos2SigmaM = 0;

        double c = _ellipsoid.F / 16 * cosSqAlpha * (4 + _ellipsoid.F * (4 - 3 * cosSqAlpha));
        lambdaP = lambda;
        lambda = l +
                 (1 - c) * _ellipsoid.F * sinAlpha *
                 (sigma + c * sinSigma * (cos2SigmaM + c * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)));
    } while (Math.Abs(lambda - lambdaP) > 1e-12 && --iterLimit > 0);

    double uSq = cosSqAlpha * (Math.Pow(_ellipsoid.EquatorialRadius, 2) - Math.Pow(_ellipsoid.PolarRadius, 2)) /
                 Math.Pow(_ellipsoid.PolarRadius, 2);
    double a = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)));
    double b = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)));
    double deltaSigma = b * sinSigma *
                        (cos2SigmaM +
                         b / 4 *
                         (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM) -
                          b / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) *
                          (-3 + 4 * cos2SigmaM * cos2SigmaM)));
    double s = _ellipsoid.PolarRadius * a * (sigma - deltaSigma);

    var a1 = Math.Atan(cosU2 * Math.Sin(lambda) / (cosU1 * sinU2 - sinU1 * cosU2 * Math.Cos(lambda))) * 180 /
             Math.PI;
    var a2 = Math.Atan(cosU1 * Math.Sin(lambda) / (-sinU1 * cosU2 + cosU1 * sinU2 * Math.Cos(lambda))) * 180 /
             Math.PI;
    a1 = Azimuth.AzimuthRecovery(coord1, coord2, a1);
    a2 = Azimuth.AzimuthRecovery(coord2, coord1, a2);

    return new InverseProblemAnswer(a1, a2, s);
}

Ну, вроде всё, идём дальше.

Прямая геодезическая задача

Напоминаю, что задача состоит в том, чтобы по известной начальной координате , расстоянию , и азимуту , найти вторую координату .

Начнём с пересчёта геодезической широты в приведённую широту:

где, - всё ещё коэффициент сжатия.

Угловое расстояние между точкой и экватором:

Прямой азимут геодезической линии на экваторе, если бы она была продолжена до него:

Код:

double u1 = Math.Atan((1 - _ellipsoid.F) * Math.Tan(coord.LatR));
double sigma1 = Math.Atan(Math.Tan(u1) / Math.Cos(a1));
var sinAlpha = Math.Cos(u1) * Math.Sin(a1);

Вычисляем вспомогательные величины:

, здесь - экваториальный радиус, - полярный радиус:

var cosSqAlpha = 1 - sinAlpha * sinAlpha;

double uSq = cosSqAlpha * (Math.Pow(_ellipsoid.EquatorialRadius, 2) - Math.Pow(_ellipsoid.PolarRadius, 2)) /
             Math.Pow(_ellipsoid.PolarRadius, 2);

И вновь магические числа, о которых мы уже писали, повторим ещё раз:

Зададим начальное значение :

Код:

var si = s / _ellipsoid.PolarRadius / a;
double sigma = si;

Будем повторять следующие вычисления, пока изменения (угловое расстояние между точками), не станут незначительными:

– угловое расстояние от экватора до середины линии.

Опять страшная формула:

Вся эта жуть в коде:

var sinSigma = Math.Sin(sigma);
var cosSigma = Math.Cos(sigma);

cos2SigmaM = Math.Cos(2 * sigma1 + sigma);
double deltaSigma = b * sinSigma *
                    (cos2SigmaM +
                     b / 4 *
                     (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM) -
                      b / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) *
                      (-3 + 4 * cos2SigmaM * cos2SigmaM)));
sigmaP = sigma;
sigma = si + deltaSigma;

После того как получена с достаточной точностью, вычислить широту второй точки:

Можно упростить эту формулу, введя :

Тогда формула будет выглядеть:

Код:

var sinU2 = Math.Sin(u1) * Math.Cos(sigma) + Math.Cos(u1) * Math.Sin(sigma) * Math.Cos(a1);
var lat2 = sinU2 /
           ((1 - _ellipsoid.F) *
            Math.Sqrt(sinAlpha * sinAlpha +
                      Math.Pow(
                          Math.Sin(u1) * Math.Sin(sigma) - Math.Cos(u1) * Math.Cos(sigma) * Math.Cos(a1), 2)));
lat2 = Math.Atan(lat2);

Теперь вычисляем долготу. Для этого рассчитывается разность долгот:

Код:

// Разность долгот
var lamda = Math.Atan(Math.Sin(sigma) * Math.Sin(a1) /
                      (Math.Cos(u1) * Math.Cos(sigma) - Math.Sin(u1) * Math.Sin(sigma) * Math.Cos(a1)));

double c = _ellipsoid.F / 16 * cosSqAlpha * (4 + _ellipsoid.F * (4 - 3 * cosSqAlpha));

var l = lamda - (1 - c) * _ellipsoid.F * sinAlpha *
        (sigma +
         c * Math.Sin(sigma) * (cos2SigmaM + c * Math.Cos(sigma) * (-1 + 2 * cos2SigmaM * cos2SigmaM)));
var lon2 = -l + coord.LonR;

Ну и обратный азимут:

В программе:

var a2 = -Math.Atan(sinAlpha / (-Math.Sin(u1) * Math.Sin(sigma) + Math.Cos(u1) * Math.Cos(sigma) * Math.Cos(a1))) * 180 / Math.PI;
a2 = Azimuth.AzimuthRecovery(point1, coord, a2);

Под обратным азимутом, можно понимать, как азимут в направлении от первой точки, ко второй, так и наоборот, они отличаются на 180°, формула тангенса не фиксирует эти различия.

Снова код метода целиком:

private DirectProblemAnswer DirectProblemEllipsoid(Point coord, double a1, double s)
{
    var aDegree = a1;
    a1 = a1 * Math.PI / 180;

    double u1 = Math.Atan((1 - _ellipsoid.F) * Math.Tan(coord.LatR));
    double sigma1 = Math.Atan(Math.Tan(u1) / Math.Cos(a1));
    var sinAlpha = Math.Cos(u1) * Math.Sin(a1);

    var cosSqAlpha = 1 - sinAlpha * sinAlpha;

    double uSq = cosSqAlpha * (Math.Pow(_ellipsoid.EquatorialRadius, 2) - Math.Pow(_ellipsoid.PolarRadius, 2)) /
                 Math.Pow(_ellipsoid.PolarRadius, 2);
    double a = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)));
    double b = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)));

    var si = s / _ellipsoid.PolarRadius / a;
    double sigma = si;

    double sigmaP, cos2SigmaM;
    do
    {
        var sinSigma = Math.Sin(sigma);
        var cosSigma = Math.Cos(sigma);

        cos2SigmaM = Math.Cos(2 * sigma1 + sigma);
        double deltaSigma = b * sinSigma *
                            (cos2SigmaM +
                             b / 4 *
                             (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM) -
                              b / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) *
                              (-3 + 4 * cos2SigmaM * cos2SigmaM)));
        sigmaP = sigma;
        sigma = si + deltaSigma;
    } while (Math.Abs(sigma - sigmaP) > 1e-12);

    var sinU2 = Math.Sin(u1) * Math.Cos(sigma) + Math.Cos(u1) * Math.Sin(sigma) * Math.Cos(a1);
    var lat2 = sinU2 /
               ((1 - _ellipsoid.F) *
                Math.Sqrt(sinAlpha * sinAlpha +
                          Math.Pow(
                              Math.Sin(u1) * Math.Sin(sigma) - Math.Cos(u1) * Math.Cos(sigma) * Math.Cos(a1), 2)));
    lat2 = Math.Atan(lat2);

    // Разность долгот
    var lamda = Math.Atan(Math.Sin(sigma) * Math.Sin(a1) /
                          (Math.Cos(u1) * Math.Cos(sigma) - Math.Sin(u1) * Math.Sin(sigma) * Math.Cos(a1)));

    double c = _ellipsoid.F / 16 * cosSqAlpha * (4 + _ellipsoid.F * (4 - 3 * cosSqAlpha));

    var l = lamda - (1 - c) * _ellipsoid.F * sinAlpha *
            (sigma +
             c * Math.Sin(sigma) * (cos2SigmaM + c * Math.Cos(sigma) * (-1 + 2 * cos2SigmaM * cos2SigmaM)));
    var lon2 = -l + coord.LonR;

    lon2 = lon2 * 180 / Math.PI;
    lat2 = lat2 * 180 / Math.PI;

    var point1 = IsPolisIntersect(coord, aDegree, s)
        ? new Point(lon2 + (lon2 < 0 ? 180 : -180), lat2)
        : new Point(lon2, lat2);

    var a2 = -Math.Atan(sinAlpha / (-Math.Sin(u1) * Math.Sin(sigma) + Math.Cos(u1) * Math.Cos(sigma) * Math.Cos(a1))) * 180 / Math.PI;
    a2 = Azimuth.AzimuthRecovery(point1, coord, a2);

    return new DirectProblemAnswer(point1, a2);
}

Ну вот и всё, наконец-то, переходим к итогам.

Итоги

Голова идёт кругом от всей этой сферической геометрии. Давайте ещё раз сделаем краткий обзор того, что мы преодолели.

Для начала мы привели формулу вычисления широты по долготе, для сфероида. Потом мы вывели из неё формулу вычисления долготы по широте, опять же для сфероида, так, по фану.

Потом мы уже перешли непосредственно к задаче поиска точки пересечения, и воспользовавшись, всё теми же формулами вычисления широты по долготе, и долготы по широте, научились находить точку пересечения ортодромий на сфероиде.

Но нам то нужен эллипсоид, ибо Земля сплюснута. И мы проделали всё то же самое для эллипсоида: научились вычислять широту по долготе и долготу по широте. И снова вернулись к изначальной задаче - поиска точки пересечения двух линий, - определили алгоритм для эллипсоида.

Работа на эллипсоиде - это именно та часть, которую я и ставлю себе в заслугу. Magnum opus, так сказать. Остальное можно нагуглить, хоть и не без проблем, всё же вещь специфическая, либо, если не гуглится, можно аналитически вывести используя известные тригонометрические формулы.

Так или иначе при работе на эллипсоиде активно применяются главные геодезические задачи, их тоже нельзя было обойти стороной, поэтому описал и их, как для сфероида: прямая и обратная, так и для эллипсоида: обратная и прямая.

Решение для эллипсоида было предложено Тадеушем Винсенти и характеризуется высокой точностью. Однако высокая точность на эллипсоиде достигается за счёт итеративного приближения к ответу, таким образом, по сравнению с аналитическим решением, страдает производительность. Поэтому Вам остаётся только ответить на вопрос: а стоит ли игра свеч?