nabbla (nabbla1) wrote,
nabbla
nabbla1

Categories:

QuatCore: финал аффинного алгоритма!

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

На это уйдёт ещё 19 слов кода, львиная доля - на обращение масштаба методом Ньютона, и 3 слова (матерных) - на обработку ровно одного нехорошего случая, когда ответ выходит за диапазон. Ещё пришлось 2 строчки перенести из прошлой процедуру в эту.




Вот он, наш код (уже в виде листинга, т.е с добавленными адресами):
    FindVector  proc
85  A203                      k           3
86  CD64                      X           ZeroInv
87  8AC4                      C           [X+i]   ;наше нулевое приближение к обр. величине
88  8000          @@Newton:   Acc         0       ;занести двойку
89  9BFC                      UFMS        [SP]    ;Acc = 2 - a * x
8A  9882                      MULU        UAC     ;Acc = Acc * x = x * (2- a*x), т.е итерация метода Ньютона
8B  8A82                      C           UAC
8C  AA7C                      kLOOP       @@Newton            
8D  B103                      JGE         @@SkipOflo
8E  A500                      j++         0           ;раз не вмещается в мантиссу, значит добавим экспоненту, а в мантиссу нужно 32768 теперь.Ща найдём...
8F  8801                      ZAcc        MinusOne    ;вместо бесхозной минус двойки
90  CD75          @@SkipOflo: X           Exp
91  C4A1                      [X+i]       j       
92  DD76                      Y           Tx
93  D882                      [Y+k]       UAC
94  DD72                      Y           Rx
95  ED77                      Z           Ty
96  A001                      i           1
97  94D4          @@Vec:      MULSU       [Y+i]
98  E480                      [Z+i]       Acc
99  A87E                      iLOOP       @@Vec
    FindVector  endp


Первое отличие от старой версии: вместо "нулевого приближения" 32767, мы теперь загружаем значение ZeroInv:
ZeroInv		dw		43648 

Одно из лучших значений, при которых после 4 итераций мы выйдем на точные 16 бит результата. Добавилась одна лишняя строка кода и 2 байта в памяти, но оно того стоит, наверное. Хотя если бы мы следовали принципу "оптимизации памяти", то добавили бы ещё 5-ю итерацию. Если очень припрёт - запомним это :)

Далее, мы сообразили, что текущий результат можно хранить не на [SP+1], а в регистре "C", тем более что именно там в конечном итоге нам и нужна эта величина, обратная к масштабу. Тем самым мы отыграли назад одну строчку кода.

И наконец, тут у нас по завершении итерации проверяется знак числа. Если он отрицательный - значит у нас вышло 65536. У нас аккумулятор имеет один дополнительный старший бит, который всегда считается знаковым. И значение 65536 по сути превратилось в -65536, так что мы его смогли отследить.

Если такое случилось, мы увеличиваем "экспоненту" (она общая для всего вектора параллельного переноса) на единицу, а в мантиссу вместо 65536 помещаем 32768. Мы используем "литерал" MinusOne:
MinusOne		EQU 1


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

Вспомним, что там должно происходить:
000: DDDD......D... (данные с выхода сумматора, поступающие на вход D),
001: xxxxxxxxxxx... (что угодно)
010: QQQQ......Q... (сохраняются данные, лежащие в регистре),
011: 0000......0... (сброс),
100: 1010......1... (-3/2),
101: x100......x... (±1),
110: 0110......1... (3/2),
111: 0000......1... (1/2lsb)

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

Q[AccWidth-2] <= (mode[1:0]==2'b10)? 1'b1 : (mode[2:1]==2'b00)? D[AccWidth-2] : 1'b0;


и был единственным, требующим 2 ЛЭ, поскольку на его вход должны поступать 3 бита mode, 1 бит D, и ещё вход ENA. Похоже, так оно и останется, но хоть толще не станет:
Q[AccWidth-2] <= (mode[1]^mode[0])? 1'b1 : (mode[2:1]==2'b00)? D[AccWidth-2] : 1'b0;


Да, по-прежнему модуль аккумулятора занимает 34 ЛЭ (в случае 32 бит).


Далее у нас идут строки
          @@SkipOflo: X           Exp
                      [X+i]       j 

это их мы перенесли из предыдущей процедуры, поскольку оказалось, что значение экспоненты ещё может измениться.

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

Глянем, как это всё выполняется. Напомним: по адресу [SP] у нас лежит значение масштаба, от 16384 до 32768, которое нам нужно обратить.

Инициализация цикла проходит нормально. Нулевое приближение загружается какое надо. Значение масштаба: 27979 (посчитали ранее). Начинается первая длинная команда UFMS...


Результатом вычисления выражения

2 - (43648/32768)(27979/32768)

становится 28267/32768, и это правильный результат.

Далее результат умножается на "нулевое приближение", т.е вычисляется полное выражение:

(2 - (43648/32768)(27979/32768)) (43648/32768)

и получаем ответ 37653/32768. Всё верно!

Повторяем итерации - и получаем последовательно 33385 (значение скобки), 38363 (всего выражения) на 2-й итерации. Также всё верно.
На третьей итерации выходит 32779 и затем 38377, причём при сверке мы должны первое значение брать с отсечением дробной части, второе - с округлением. Всё как надо.

Наконец, на четвёртой итерации находим 32767 и 38377. И это правильный ответ, что проверяется перемножением

(38377/32768)(27979/32768) = 1,00000769179314, и это самое близкое к единице значение, которое мы можем получить.

Кстати, правильный ответ был уже на третьей итерации - и для большинства значений так и происходит - нулевое приближение уж больно хорошее :)



Начинается "перекладывание бумажек"- завершаем цикл, убеждаемся, что значение у нас положительное (переполнения не произошло) и перепрыгиваем через "три матерных слова". Запихиваем экспоненту на своё место, затем X-компоненту на своё место, потом готовимся домножить двухмерный вектор параллельного переноса (он по сути представлен в радианах) на содержимое аккумулятора. Помножаем первое значение, 2...


Ага, так двойка и осталась, немудрено, когда умножаешь её на (38377/32768). Затем умножаем (-22) на то же значение, и получаем -26, всё верно. Умножение знаковых на беззнаковые работает безотказно :)

Ну а финал всего этого дела - в первой картинке поста. Там "задом наперёд" выдаются все измеренные параметры: сначала кватернион,
32767 - 1i + 0j + 0k,
затем вектор параллельного переноса (38377; -26; 2), и наконец экспонента для этого вектора, 9.

Если мы умножим 38377/32768 на 29-1, то получим 299,8203125 - это дальность в метрах.


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

И залить это безобразие на ПЛИС, не забыв сделать какой-никакой I/O...
Tags: ПЛИС, математика, программки, работа, странные девайсы
Subscribe

  • Post a new comment

    Error

    Anonymous comments are disabled in this journal

    default userpic

    Your IP address will be recorded 

  • 0 comments