Написал я алгоритм, стал его проверять с новой мишенью. Сначала по тангажу:
Не с первого раза, кое-чего пришлось подрихтовать, но довольно быстро оно зафурычило как надо. Цифры расставляет алгоритм, потому как у каждого уголка есть свой номер. Собственно, идентификация точек и есть самая хитрая часть. Дальше уже всё готово (помножить матрицы, получив матрицу и вектор аффинного преобразования, потом из них вычленить дальность, крен и активные тангаж с курсом), разве что ещё по отклонению вынесенных вперёд уголков относительно всего остального можно довольно легко ещё и пассивные тангаж с курсом найти, это уже всё дело техники. Тем более, сейчас три уголка, сидящие вокруг каждого центрального, образуют правильный треугольник, и если взять от них среднее арифметическое - получим позицию центрального уголка при взгляде "по нормали". В общем, халява.
А вот когда стал проверять крен, что-то пошло не так:
Наблюдаем за одним из крайних. Они имеют номера 5 и 6, и эти номера должны стоять как вкопанные, а у нас там проскакивает 1 и 2 время от времени.
Ошибка оказалась в самом начале. Я нашёл индексы i и j двух точек, максимально отдалённых друг от друга, в массиве Points[], где индексы идут от 0 до 7, и дальше хотел эти две точки запихать в самый конец массива, для чего написал:
Swap(Points[i],Points[6]); Swap(Points[j],Points[7]);
То есть поменял местами точку под индексом i и под индексом 6, а затем - с индексами j и 7 соответственно. Что могло пойти не так??
На отладке я нашёл, что проблемы начались, когда i=1, j=6. Жирным отмечены самые отдалённые точки:
0, 1, 2, 3, 4, 5, 6, 7
Сначала мы меняем точки с индексами 1 и 6:
0, 6, 2, 3, 4, 5, 1, 7
А потом точки с индексами 6 и 7, вот только под индексом 6 уже закопалась другая точка, а исходная убежала в начало массива, поэтому получается что-то не то:
0, 6, 2, 3, 4, 5, 7, 1
Лишь одна из точек переместилась на ожидаемую позицию, и дальше всё наперекосяк.
Проблема понятна, но как же эту примитивную операцию выполнить самым простым способом?
Так-то ясно, можно было бы в явном виде проверить, вдруг одна из точек уже сидит где надо, или даже обе, но как-то оно некрасиво.
И мог бы ещё далеко зайти в размышлениях, но вспомнил, что на i и j у меня есть ограничения:
1) i принимает значения от 0 до 6,
2) j принимает значения от 1 до 7,
3) i < j
Так получается, потому что при поиске пары наиболее отдалённых друг от друга точек мы используем два вложенных цикла, первый как раз по i от 0 до 6, второй - по j от i+1 до 7.
Но решение всё равно чего-то не приходило, время было уже позднее, а у меня закончился сахар, без сахара мозги не работают. Написал брату эту задачку. И пока я возвращался домой, брат нашёл правильное решение:
[Узнать ответ][А может не надо?]надо тупо две мои строки поменять местами:
Swap(Points[j],Points[7]); Swap(Points[i],Points[6]);
Вот теперь никаких проблем не будет. Если было j=7, то точка "поменяется сама с собой" - ничего страшного, останется на том же месте. Если было j=6, то эта точка переместится на позицию 7. Но после этого мы заведомо знаем, что i может равняться от 0 до 5, так что и вторая точка попадёт куда надо :)
Попробуем ещё раз:
УРА!
Только хотел рассказать эту историю, но сдуру зашёл на ютуб, увидел там видюшку про жульнические машины для голосования - и пришла мысль, как они могли работать :)