Поиск минимального расстояния от точки до ломанной на сфере. Язык Си

[silent_1991]

Здравствуйте!
Задача заключается в следующем: дано количество точек n, по которым будет строиться ломанная на сфере. Далее пары чисел. Первая пара - точка M (в сферических координатах, углы пси и фи), от которой будет искаться расстояние, далее n пар, соответственно сферические координаты точек, по которым строится ломанная.

Моя идея решения. Первым делом, конечно, переводим всё в декартовы координаты. Рассматриваем каждый раз отрезок, заключённый между i-й и (i + 1)-й точками. Строим плоскость, проходящую через начало координат (сфера имеет центр в начале прямоугольной системы координат) и точки A и B, между которыми заключён рассматриваемый отрезок. Далее проверяем, попадает ли проекция точки M на плоскость AOB в круговой сектор (т.е. в область AOB), и в зависимости от результата выполняем одно из следующих действий:
1) Если проекция точки M попадает в область AOB - находим расстояние от точки M до плоскости AOB, а затем, зная радиус сферы (он дан) находим угол между прямыми OM и плоскостью AOB, и, умножив его на радиус сферы, находим длину дуги MK (K - точка, лежащая на отрезке AB, OK соответственно - кратчайшее расстояние от точки до данного отрезка);
2) Если же проекция точки M не попадает в нужную область - ищем дуги MA и MB (проводим прямые OA, OB, OM, ищем углы между OA и OM, OB и OM, умножаем их на радиус - получаем нужные дуги). Выбираем минимальную из них - это и есть расстояние от M до данного отрезка AB.
Таким образом просматриваем все отрезки и ищем все расстояния. Затем берём минимальное из них - оно и будет минимальным расстоянием от точки до ломанной на сфере.
Как проверяем, попадает ли проекция точки M в область AOB:
Ищем координаты точки M' - проекции точки M на плоскость AOB. Проводим прямую OM', ищем точку K её пересечения с отрезком AB, переводим декартовы координаты этой точки в сферические и если хотя бы одна из координат точки K не попадает в промежуток между соответствующими сферическими координатами точек A и B - то точка M' не попадает в область AOB. Если же обе координаты K лежат между соответствующими координатами точек A и B - точка M' попадает в область AOB.

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

[silent_1991]

Вот мой вариант реализации на чистом Си:

#include <stdio.h>
#include <math.h>

#define N 100
#define R 1

double min(double mas[N][3], int n)
{
    int i, j;
    double min = mas[0][0];
    
    for (i = 0; i < n; i++)
    {
        for (j = 0; j < 3; j++)
        {
            if (mas[i][j] < min)
            {
                min = mas[i][j];
            }
        }
    }
    
    return min;
}

int charact(double M[3], double A, double B, double C, double psi1, double phi1, double psi2, double phi2)
{
    double Mx, My, Mz;
    double Kx, Ky, Kz;
    double t;
    double psiK, phiK;
    double x, y;
    
    t = -(A * M[0] + B * M[1] + C * M[2]) / (pow(A, 2) + pow(B, 2) + pow(C, 2));
    
    Mx = A * t + M[0];
    My = B * t + M[1];
    Mz = C * t + M[2];
    
    Kx = (R * Mx) / sqrt(pow(Mx, 2) + pow(My, 2) + pow(Mz, 2));
    Ky = (R * My) / sqrt(pow(Mx, 2) + pow(My, 2) + pow(Mz, 2));
    Kz = (R * Mz) / sqrt(pow(Mx, 2) + pow(My, 2) + pow(Mz, 2));

    psiK = asin(Kz / sqrt(pow(Kx, 2) + pow(Ky, 2) + pow(Kz, 2)));
    
    x = Kx / sqrt(pow(Kx, 2) + pow(Ky, 2));
    y = Ky / sqrt(pow(Kx, 2) + pow(Ky, 2));
    
    if ((x > 0) && (y >= 0))
    {
        phiK = atan(y / x);
    }
    if ((x > 0) && (y < 0))
    {
        phiK = atan(y / x) + 2 * M_PI;
    }
    if (x < 0)
    {
        phiK = atan(y / x) + M_PI;
    }
    if ((x == 0) && (y > 0))
    {
        phiK = M_PI / 2;
    }
    if ((x == 0) && (y < 0))
    {
        phiK = 3 * M_PI / 2;
    }
    
    if (((psiK > psi1) && (psiK < psi2)) && ((phiK > phi1) && (phiK < phi2)))
    {
        return 1;
    }
    else
    {
        return 0;
    }
}

int main()
{
    int n;
    double psi, phi;
    double crd[N][3];
    double M[3];
    double S[N][2];
    int i;
    double l[N][3];
    double A, B, C;
    double d, lA, lB, lM;
    int f;
    
    FILE *fin;
    
    if ((fin = fopen("sphere.in", "r")) == NULL)
    {
        return 1;
    }
    else
    {
        fscanf(fin, "%d", &n);

        fscanf(fin, "%lf", &psi);
        fscanf(fin, "%lf", &phi);
        
        M[0] = R * cos(psi) * cos(phi);
        M[1] = R * cos(psi) * sin(phi);
        M[2] = R * sin(psi);
        
        for (i = 0; i < n; i++)
        {
            fscanf(fin, "%lf", &psi);
            fscanf(fin, "%lf", &phi);
            
            S[i][0] = psi;
            S[i][1] = phi;
            
            crd[i][0] = R * cos(psi) * cos(phi);
            crd[i][1] = R * cos(psi) * sin(phi);
            crd[i][2] = R * sin(psi);
        }
    }
    
    fclose(fin);

    for (i = 0; i < n; i++)
    {
        if (i + 1 < n)
        {
            A = crd[i][1] * crd[i + 1][2] - crd[i][2] * crd[i + 1][1];
            B = crd[i][2] * crd[i + 1][0] - crd[i][0] * crd[i + 1][2];
            C = crd[i][0] * crd[i + 1][1] - crd[i][1] * crd[i + 1][0];
            f = charact(M, A, B, C, S[i][0], S[i][1], S[i + 1][0], S[i + 1][1]);
        }
        
        if (i + 1 == n)
        {
            A = crd[i][1] * crd[0][2] - crd[i][2] * crd[0][1];
            B = crd[i][2] * crd[0][0] - crd[i][0] * crd[0][2];
            C = crd[i][0] * crd[0][1] - crd[i][1] * crd[0][0];
            f = charact(M, A, B, C, S[i][0], S[i][1], S[0][0], S[0][1]);
        }
        
        if (f)
        {
            d = fabs(A * M[0] + B * M[1] + C * M[2]) / sqrt(pow(A, 2) + pow(B, 2) + pow(C, 2));
            lM = R * asin(d / R);
            lA = 2 * lM;
            lB = 2 * lM;
        }
        else
        {
            if (i + 1 < n)
            {
                lA = R * acos((crd[i][0] * M[0] + crd[i][1] * M[1] + crd[i][2] * M[2]) / (sqrt(pow(crd[i][0], 2) + pow(crd[i][1], 2) + pow(crd[i][2], 2)) * sqrt(pow(M[0], 2) + pow(M[1], 2) + pow(M[2], 2))));
                lB = R * acos((crd[i + 1][0] * M[0] + crd[i + 1][1] * M[1] + crd[i + 1][2] * M[2]) / (sqrt(pow(crd[i + 1][0], 2) + pow(crd[i + 1][1], 2) + pow(crd[i + 1][2], 2)) * sqrt(pow(M[0], 2) + pow(M[1], 2) + pow(M[2], 2))));
            }
            
            if (i + 1 == n)
            {
                lA = R * acos((crd[i][0] * M[0] + crd[i][1] * M[1] + crd[i][2] * M[2]) / (sqrt(pow(crd[i][0], 2) + pow(crd[i][1], 2) + pow(crd[i][2], 2)) * sqrt(pow(M[0], 2) + pow(M[1], 2) + pow(M[2], 2))));
                lB = R * acos((crd[0][0] * M[0] + crd[0][1] * M[1] + crd[0][2] * M[2]) / (sqrt(pow(crd[0][0], 2) + pow(crd[0][1], 2) + pow(crd[0][2], 2)) * sqrt(pow(M[0], 2) + pow(M[1], 2) + pow(M[2], 2))));
            }
            
            lM = lA + lB;
        }
        
        l[i][0] = lM;
        l[i][1] = lA;
        l[i][2] = lB;
    }
    
    FILE *fout;
    
    if ((fout = fopen("sphere.out", "w")) == NULL)
    {
        return 1;
    }
    else
    {
        fprintf(fout, "%lf", min(l, n));
    }
    
	fclose(fout);
	
    return 0;
}

[silent_1991]

Суть в том, что значение вроде бы считается, и вроде оно даже похоже на правду, но, например, если во входных данных я записываю координаты точки M как пару (90, 0) - считается одно значение, а если как пару (90, 45) или (90, 90) (при тех же координатах остальных точек) - другие, хотя при повороте точки по углу пси на 90 градусов поворот по углу фи не должен играть абсолютно никакой роли... Или я ошибаюсь?
Далее, если я запишу пару (88, 0) - посчитается настолько отличное от предыдущего (90, 0) значения, что такого просто быть не может.

Я думаю, что напортачил с проверкой попадания точки M' в область AOB, хотя вроде все условия соблюдены, с проверкой, в какой четверти находятся координаты, тоже вроде всё как надо...
Кто заинтересовался, пожалуйста, помогите разобраться... Я конечно тоже не буду сиднем сидеть, попробую поискать ошибку, но коллективный разум - он решает... Заранее огромное спасибо за помощь!
P.S. Прошу прощения за мультипост, но в 5000 символов, как можно заметить, не впихнуть полное описание... Уж извините меня, не могу я коротко рассказывать)))

[silent_1991]

Сегодня понял, что для точки, лежащей на полюсе необходимо дополнительное условие, т.к. её проекция на плоскость OAB совпадает с точкой O, таким образом мы не можем провети прямую, проходящую через 2 точки - O и M', т.к. они совпадают. Пока не придумал, каким должно быть это условие, но тем не менее не понятно, почеми результяты для 89 и 88 градусов ТАК разнятся...
P.S. Прилагаю файл кода с комментариями, может это продвинет дело...