nabbla (nabbla1) wrote,
nabbla
nabbla1

Categories:

Аффинный алгоритм ч.2 - сортировка против часовой стрелки

Часть 1 - нахождение наиболее отдалённой точки
Часть 2 - сортировка против часовой стрелки
Комментарий об операторе упорядочивания
Часть 3 - нахождение матрицы преобразования
Часть 4 - нахождение крена
Математическая интерлюдия
Часть 5 - нахождение масштаба
Финал!

Эти посты логически должны являться продолжением серии о кватернионах и серии о ПЛИС 5576ХС4Т - здесь мы реализуем алгоритмы ориентации в пространстве, используя математику из первого и железо из второго. К сожалению пока они все "висят в воздухе" - ни одну, ни другую серию я не довёл до этого места.

Математическое описание "аффинного алгоритма" почти целиком есть здесь.
Будущий процессор QuatCore - здесь сколько-нибудь корректная принципиальная схема, общее описание его ассемблера и эмулятора, первые примеры программ (уже изрядно устаревшие!).

Но я спешу объяснить этот код, пока сам помню, как он работает! Увы, ассемблерные программы зачастую являются Write-only: написать их можно, а спустя неделю-другую разобраться даже в своём коде весьма затруднительно, особенно если при его написании посетила муза :) Хорошо бы в глаза посмотреть этой музе, и спросить - за что!?

Мы остановились на том, что из 4 точек нашли одну, наиболее отдалённую (МДД3), и поставили её первой в списке. Теперь мы хотим расставить остальные точки "против часовой стрелки", если за начало координат принять МДД3, которую мы нашли в прошлый раз:


Этим занимается "процедура" SortCCW (Sort Counter-ClockWise), занимающая 20 слов, но она также 2 раза вызывает процедуру ShiftOrigin, которая больше нигде не задействована, и занимает ещё 8 слов. В общей сложности - 28 слов, или 56 байт.


Для начала покажем листинг процедуры ShiftOrigin:

;по адресу [Y], [Y+1] лежат коорд X,Y на которые надо передвинуть 3 точки,
;лежащие по адресу [X], [X+1], и так далее
;нужно ли их прибавить или вычесть - определяется регистром Inv
ShiftOrigin proc
                i           2
@@i_loop:       k           1   ;как всегда, чтобы Y и X
@@k_loop:       Acc         [X+2i+k]
                PM          [Y+k]
                [X+2i+k]    Acc
                kLOOP       @@k_loop
                iLOOP       @@i_loop
                JMP         [--SP]
ShiftOrigin endp


Снова мы видим 2 вложенных цикла (мы вообще обожаем циклы, когда они так легко организуются!), первый - по i от 0 до 2, т.е для 3 точек, которым мы хотим "переместить начало координат". Второй - по k от 0 до 1, т.е чтобы выполнить одинаковые действия для координат X и Y.

Внутри циклов мы загружаем в аккумулятор очередную координату очередной точки, а затем делаем операцию PM, т.е Plus-Minus. Идея была немножко описана здесь. Если регистр Inv=0, делаем сложение, а если Inv=1 - вычитание. Поэтому одной процедурой мы можем как переместить начало координат в точку МДД3, так и по окончании сортировки "вернуть всё как было".

Теперь рассмотрим саму процедуру сортировки:
        ;состояние регистров:
        ;X = Y = Points2D,
        ;Z неизвестен
        ;j=k=0
        ;i от 0 до 3 (точка, которая оказалась МДД3)
        ;Inv неизвестен
        
        SortCCW     proc
                    X           Fx1 ;чтобы индекс от 0 до 2 соотв. точкам (Fx1,Fy1) ... (Fx3, Fy3)
                    Z           Fx2 ;чтобы иметь сдвинутую на 1 адресацию

                    Inv         1
                    CALL        ShiftOrigin
        ;за счёт введения этой процедуры сэкономили аж 2 слова, т.е 4 байта, мы молодцы :)
        ;правда, ввели новый Dest: PM (Plus-Minus), но скорее всего пригодится и позже!
        ;вот теперь пора сортировать!
                    j           1
        ;ищем точку, которую поставить в позицию j+1
        ;это получается сортировка простым выбором
        @@j_loop:   i           j
        ;теперь надо сравнить точку (j+1) и i, кто из них больше? это по сути умножение комплексных чисел
        ;здесь заведомо k=0, так что спец. команды [X+2i] или [X+2j] не нужны!
        @@i_loop:   C           [Z+2j+k]
                    MUL         [X+2i+1]
                    C           [Z+2j+1]
                    FMS         [X+2i+k]    ;нахождение "векторного произведения"
                    JL          @@skip
        ;меняем местами, как обычно
                    k           1
        @@swap:     Acc         [Z+2j+k]
                    [Z+2j+k]    [X+2i+k]
                    [X+2i+k]    Acc
                    kLOOP       @@swap
        @@skip:     iLOOP       @@i_loop
                    jLOOP       @@j_loop
        
        ;возвращаем на место, как ни в чём не бывало
                    Inv         0
                    CALL        ShiftOrigin
        SortCCW     endp


В начале мы весьма хитро "расставляем" регистры X,Y,Z, ровно "через 2": [Y], как и прежде, ссылается на нулевую точку Fx0,Fy0 (которая МДД3), [X] - на первую точку Fx1, Fy1, а [Z] - на вторую точку Fx2,Fy2.

Выставляем наш регистр Inv, чтобы при первом вызове процедуры ShiftOrigin мы вычитали координаты МДД3. Впрочем, можно ещё немножко сэкономить. У нас есть ещё один интересный адрес: ijk, который применим как в поле Dest, так и в поле Src. Наши индексные регистры на данный момент 5-битные беззнаковые (0..31), хотя даже этого может быть многовато, но мы не жадные. Так вот, чтобы заменить "простыню"
[SP++]  i    ;PUSH i
[SP++]  j    ;PUSH j
[SP++]  k    ;PUSH k
;используем их по своему усмотрению

i       [--SP]    ;POP i
j       [--SP]    ;POP j
k       [--SP]    ;POP k

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

мы и ввели адрес ijk, когда все эти регистры, и Inv впридачу "упаковываются" в 16-битное значение:
[SP++]     ijk
;код
ijk        [--SP]


Так вот, есть определённый смысл разместить эти регистры в 16 битах следующим образом:
Inv  k4 k3 k2 k1 k0 i4 i3 i2 i1 i0 j4 j3 j2 j1 j0


Дело в том, что в большинстве наших циклов j будет самой "внешней" (например, по строкам матрицы), затем i (по столбцам матрицы) и самый внутренний цикл будет по k. И тогда мы можем задать j не через обычный адрес, а через ijk. Тех 128 значений (Imm, они же литералы), которые мы можем задать напрямую, достаточно, чтобы присвоить любое значение j, а за счёт знака задать и Inv. Тогда в предыдущей процедуре FindMDD3, вместо строки
j      3

мы пишем:
ijk    -61

и вуаля - мы заодно определили Inv=1. Также у нас определилось i=30, k=31, но это не страшно - они будут переопределены на следующих строчках!

(значение -61 представляется в бинарной форме как:
1 0 0 0 0 1 1

т.е 7-битное число в дополнительном коде. Но при выходе на шину данных, старший бит дублируется во все остальные, от 15-го до 8-го:
1 | 1 1 1 1 1 | 1 1 1   1 0 | 0 0 0 1 1

что и задаёт значения Inv=1, k=31, i=30, j=3)

Поехали дальше.
Наконец, мы вызвали процедуру ShiftOrigin, и она передвинула нам начало координат. Чем хороша целочисленная реализация - так это тем, что мы заранее знаем, что тождество
(a - b) + b = a 


выполняется неукоснительно!

В случае плавающей точки нам не хотелось бы "по живому" двигать начало координат туда-сюда, т.к могут накапливаться ошибки!

Пора начать сортировку. Мы решили применить сортировку "методом простого выбора": находим "самое большое значение" и помещаем его в конец массива. А в оставшемся массиве снова ищем "самое большое значение" - и помещаем его на предпоследнее место - и так далее.

Немножко смешно, когда такого рода алгоритм реализуется для сортировки 3 точек, но он весьма компактен, почему бы и нет :) Поначалу я думал, что нужно "ручками" закодировать что-то вроде такого:
if (a<b) {
  if (b<c) {  //a < b < c - всё хорошо!
    return;
  }
  else {  //a<b, c<b, отношение между a и с неизвестно
    if (a<c) { //a < c < b - меняем местами последние 2
      Swap(&b,&c);
      return;
    }
    else {  //c < a < b - нужна циклическая перестановка "вправо"
      t = c;
      c = b;
      b = a;
      a = t;
      return;
    }
  }
else {  //b < a
  if (a<c) { //b < a < c - нужно поменять местами a,b
    Swap(&b,&a);
    return;
  }
  else {  //b<a, c<a, отношение между b,c неизвестно
    if (b<c) { //b < c < a - нужна циклическая перестановка "влево"
      t = a;
      a = b;
      b = c;
      c = t;
      return;
    }
    else { //c < b < a - нужно поменять местами a,c
      Swap(&a, &c);
      return;
    }
}

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

Итак, внешний цикл идёт по j, от 1 до 0. Но в действительности, используя "сдвинутый на 2 позиции" регистр Z, мы будем обращаться к элементам F[j+1], т.е на первой итерации мы ищем кандидата на позицию j+1. Понятно, почему итерации всего 2 - когда остаётся последний элемент, он уже заведомо на своём месте!

А внутренний цикл идёт по i, от j до 0. То есть на первом проходе будет 2 итерации, на втором - всего одна. Здесь мы на каждой итерации сравниваем элемент F[j+1] с элементом F[i], и если последний "больше" - меняем их местами.

Вот код "внутри цикла":
@@i_loop:   C           [Z+2j+k]
            MUL         [X+2i+1]
            C           [Z+2j+1]
            FMS         [X+2i+k]    ;нахождение "векторного произведения"
            JL          @@skip
        ;меняем местами, как обычно
            k           1
@@swap:     Acc         [Z+2j+k]
            [Z+2j+k]    [X+2i+k]
            [X+2i+k]    Acc
            kLOOP       @@swap
@@skip:     iLOOP       @@i_loop


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

F[j+1].x * F[i].y - F[j+1].y * F[i].x


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

Команды АЛУ довольно понятные:
C - занести в регистр "С" (он используется только для умножений),
MUL - занести текущее значение в регистр "B", и запустить умножение со знаком: Acc = B * C,
FMS - занести текущее значение в регистр "B" и запустить умножение с накоплением (Fused Multiply-Subtract): Acc = Acc - B*C

Затем остаётся посмотреть на знак результата, и если это "-" - пропустить код, меняющий точки местами.

И потом мы возвращаем начало координат в (0;0) - и второй этап завершён.

Чтобы не использовать целую команду
Inv 0


мы можем снова "схитрить" - вместо команды
j   1

в начале сортировки, написать:
ijk 1


Тем самым, будут присвоены значения Inv=0, k=0, i=0, j=1 - всё как надо :)

После таких ухищрений весь код данной процедуры (с учетом процедуры ShiftOrigin) составит уже не 28, а 26 слов, или 52 байта. Пока неплохо.

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


Продолжение следует...

PS. Когда первый раз опубликовал, мне козёл Фрэнк сжевал весь сишный код, а вот ассемблерный остался в полной неприкосновенности :) Ну да, знак "меньше" в HTML-редакторе надо писать как &lt;. Вот ещё одно достоинство ассемблера - он HTML-friendly!
Tags: ПЛИС, Программки, кватернионы-это просто (том 1), математика, работа, странные девайсы
Subscribe

  • Ремонт лыжных мостиков

    Вернулся с сегодняшнего субботника. Очень продуктивно: отремонтировали все ТРИ мостика! Правда, для этого надо было разделиться, благо народу…

  • Гетто-байк

    В субботу во время Великой Октябрьской резни бензопилой умудрился петуха сломать в велосипеде. По счастью, уже на следующий день удалось купить…

  • А всё-таки есть польза от ковариаций

    Вчера опробовал "сценарий", когда варьируем дальность от 1 метра до 11 метров. Получилось, что грамотное усреднение - это взять с огромными весами…

  • Post a new comment

    Error

    Anonymous comments are disabled in this journal

    default userpic

    Your IP address will be recorded 

  • 0 comments