nabbla (nabbla1) wrote,
nabbla
nabbla1

Categories:

Мучительно пишем atan1

Думаю, будет правильно данную функцию назвать atan1. А то что такое: обычный atan(x) есть, atan2(y,x) есть, а вот atan1(y,x) куда-то делся :) Вот теперь будет! Напомню, это по сути atan(y/x), вот только мы позволяем x принимать нулевые и околонулевые значения и находим правильный ответ, работая в 16-битной целочисленной арифметике.

Мне казалось, что получился очень простой и компактный алгоритм, который прямо "просится" в QuatCore:

Значения x,y воспринимаем как действительную и мнимую части числа V = x + iy
Ответ A (от слова Angle, угол) инициализируем нулём
Для n=1 .. 5
    смотрим знак x
    домножаем V на exp(±i*pi/2n)
    если n>1, то A = A ± pi/2n
    (вместо ± подставляется знак числа x) 

A = A + 1,0012x. 



Значений экспоненты нужно всего 5, их можно посчитать заранее и поместить в память. Избавиться от условия внутри цикла также можно, если числа 0, pi/2, pi/4, ... , pi/32 также поместить в таблицу. Аж целых 5 слов на это дело истратим, зато "единообразно".

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

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


Допустим, в регистр X перед вызовом процедуры мы запишем адрес, по которому лежат значения x,y. Т.е значение x в [X], а значение y в [X+1].

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

Значит, на первой итерации результат умножения на exp(±i*pi) я должен положить куда-то ещё, например, на место результата. Я же хочу ещё дополнительную функциональность: получить длину исходного вектора, хотя б приблизительно - тогда можно будет выкинуть веселье с метриками. Так что и в результате значений должно быть два, угол и длина. Как раз значение y превратится в длину и попадёт аккурат куда надо!

Но вот пошла вторая итерация - и теперь мы должны сообразить взять число V с нового места, а не с адресов [X], [X+1]. И аккуратно произвести умножение "на месте", а с этим тоже не всё так просто...

В общем-то, это классическая подлянка, что мы посчитаем один компонент комплексного числа - и ЗАТРЁМ предыдущее значение, которое по-прежнему требуется для нахождения второй компоненты.

Весёлая матрица поворота
Давным-давно вынашивал хитрый план, применить вот такую матрицу:


(косеканс, cosec, это 1/cos(α))

В том плане, что мы сначала вычисляем новое значение y, используя нижнюю строку:



А затем вычисляем новое значение x, используя уже НОВОЕ ЗНАЧЕНИЕ y:


Если подставить сюда выражение для y1, получим:





То есть, посчитали всё правильно, зато не пришлось сохранять старое значение y, просто использовали чуть другие коэффициенты!

У такого подхода "в общем случае" есть недостаток: когда cos(α) близок к нулю и тем более равен нулю, "всё очень плохо".

У нас на самой первой итерации как раз был поворот на 90 градусов, где значения x,y, по сути, меняются местами, и там, очевидно, такой метод не может работать. Но на первой итерации нам и не нужно работать "на месте", поэтому там мы с чистой совестью используем самую обыкновенную матрицу поворота и получаем то, что нужно.

То есть, мы вполне могли бы такое применить. Теперь уже вместо комплексных чисел придётся засунуть в память 5 матриц 2х2, и это само по себе вроде не страшно. Но с индексацией начинается полнейший геморрой. Обратиться к i-му элементу k-й строки матрицы под номером j - это нужна адресация наподобие [X+4j+2k+i], ВСЕ ТРИ ИНДЕКСА задействовать. А если нет, то нужно к X прибавлять по 4 на каждой итерации, а у нас такие вещи только через АЛУ проходят, т.е переписать X в аккумулятор, прибавить 4, записать результат назад в X, тоже чего-то не нравится...

Использование регистра C
В АЛУ QuatCore имеется регистр Acc (аккумулятор) и "входные" регистры B,C. Регистр B "в явном виде" в командах не виден, поскольку любая запись в него сопровождается началом какой-либо арифметической операции. Кроме того, он сдвиговый, и после выполнения команды его содержимое "самоуничтожается". А вот регистр C с виду работает как самый обычный регистр - в него можно записать, из него можно считать. Он используется в качестве второго операнда при умножении, и на самом деле в нём идёт циклический сдвиг, но по завершении умножения его содержимое остаётся таким же, как было.

Так что если подгадать - и запихнуть в него конкретно значение y, то затем, когда значение y в памяти уже будет перезаписано, мы сможем применить его ещё разок для вычисления нового значения x.

Итак, регистр X указывает на исходный вектор, регистр Y - на начало таблицы комплексных чисел. Идёт итерация j, и нам нужно обращаться к [Y+3j] за действительной частью и к [Y+3j+1] за мнимой. Куда помещать результат - указывает регистр Z. Но имеем в виду, он может совпадать с X, т.е операция проходит "на месте". Тогда умножение комплексного числа выглядит так:

;умножение комплексного числа
;[X] - действ часть, [X+1] - мнимая,
;[Y+3j] - дейст. часть "поворота", [Y+3j+1] - мнимая часть "поворота"
;кладём результат в [Z] и [Z+1]
;k=0, поэтому [X+k] = [X]
C	[X+k]
MUL	[Y+3j+1]
C	[X+1]		;сохранили сюда значение y
FMA	[Y+3j+k]
[Z+1]	Acc		;новое значение y нашли и записали
ZAcc	RoundZero
FMS	[Y+3j+1]
C	[X+k]
FMA	[Y+3j+k]
[Z+k]	Acc	


Напомню, FMA - это Fused Multiply - Add (результат умножения прибавить к значению в аккумуляторе), а FMS - Fused Multiply-Subtract (результат умножения вычесть из значения в акк.).

10 строк (20 байт), не так уж и плохо. Но этот код в таком виде не удастся заставить умножать либо на число из таблицы, либо на сопряжённое к нему, в зависимости от значения Inv. Потому как у нас есть команда FMPM (Fused Multiply Plus-Minus), но нет команды FMMP (Fused Multiply Minus-Plus). Вместо этого выбор знака при умножениях определяется регистром Inv и индексными регистрами i,j:



Когда Inv=0, на месте ± берём плюс, на ∓ берём минус, и наоборот.

Опять терзают смутные сомнения - те ли я регистры применил для этого, потому как и при умножении кватернионов какая-то катавасия вышла...

Попробуем всё-таки эту хреновину уложить в цикл. Пока для упрощения скажем, что j=1, так что мы попадаем на правильную строку таблицы, где рядышком, при i=0 и i=1 лежат "плюс-минус" и "минус-плюс". Первую инициализацию регистра C выносим из цикла:

i	1
C	[X+k]


Далее должен начаться цикл. Обнуляем аккумулятор. Регистр C оба раза будет содержать правильное значение (первый раз x, второй раз y, который только в этом регистре и сохранится), с ним всё хорошо. А умножать его оба раза надо на "синус", причём первый раз со знаком "+", второй раз - со знаком "-". Вот так:

@@i_loop:	ZAcc	RoundZero
		FMPM	[Y+3j+1]


Затем на первой итерации мы должны прибавить y*cos, а на второй итерации прибавить x*cos. Халява, сэр:
C	[X+i]
FMA	[Y+3j+k]


И, наконец, поместить результат в память и завершить цикл. Всё вместе получается так:
		i	1
		C	[X+k]
@@i_loop:	ZAcc	RoundZero
		FMPM	[Y+3j+1]
		C	[X+i]
		FMA	[Y+3j+k]
		[Z+i]	Acc
		iLOOP	@@i_loop


Так даже поменьше, 8 строк (16 байт), причём теперь мы "реагируем" на значение Inv, поворачиваясь либо в одну, либо в другую сторону. Плохо только, что знак зависит ещё от значения j, по которому мы собирались вести внешний цикл.

И ещё чуть подумаем: а может быть, ДВА вложенных цикла будут ещё лучше? Нет, в случае кватернионов в них определённо был смысл, всё-таки 4 слагаемых, и для каждого нужно 2 множителя задать. А тут всего 2 слагаемых, и один из множителей мы задаём "заранее" (первый раз перед циклом, второй раз он не меняется с предыдущей операции, как и надо), в общем, введя 2 новых строки, мы избавимся от одной, ну его нафиг!

Ещё одна приятность: последняя проводимая операция FMA устанавливает флаг знака в соответствии с результатом, а это и есть знак числа x, который понадобится для следующей итерации.

А ещё не забудем переписать регистр X, чтобы в следующий раз мы брали не исходный вектор, а уже замученный:
X	Z	;возможно, X сохраним в стек, чтобы процедура минимально меняла регистры


А ещё мы можем обновить угол: прибавить или вычесть значение из таблицы, используя команду PM (Plus-Minus). Она не меняет флаг знака, что очень удачно. Пущай угол пока хранится на стеке, в [SP]:

i	2
Acc	[SP]
PM	[Y+3j+i]
[SP]	Acc


Да, иногда QuatCore удивляет компактностью записи в сравнении со всякими ARMами и даже x86, а иногда простейшие вещи выполняются так, что хочешь стой, хоть падай. Вот тут пришлось задать i=2, чтобы обратиться к [Y+3j+2], где лежит угол, потом загрузить значение из памяти в аккумулятор, обновить его - и загрузить назад. Ну зато PM (Plus-Minus) здорово упрощает жизнь, а то пришлось бы ветвление делать, и потом уже DEC либо INC. "То на то и выходит".

Пожалуй, и всё, цикл пора завершать:
jLOOP	@@j_loop


А теперь пора бы цикл и НАЧАТЬ - здесь удобнее было с середины начинать, чтобы потом сообразить, где сделать "склейку", то бишь прыжок в начало.

Теперь ясно: в начале цикла у нас во флаге знака (S) должен лежать знак числа x, и мы должны его записать в регистр Inv.

А сделаем-ка вот так:
@@j_loop:	Inv	S


То бишь добавим новую команду для АЛУ, на стороне SrcAddr. Там их было всего 3 штуки: Acc (значение аккумулятора "с насыщением"), UAC (Unsigned Acc, значение аккумулятора "как есть", обычно для работы с беззнаковыми числами, чтобы какой-нибудь 65535 не "насытился" до 32767) и C (прочитать регистр C). А выделено адресов аж 32 штуки! Так что вывести ещё и флаг знака НАПРЯМУЮ на шину данных - почему бы и нет. Я знаю ещё одно место, где это упростит код.

На этом цикл фактически завершён, но нужно ещё провести инициализацию в начале цикла:
; [SP++]	X	;будет изменён в процессе
; [SP++]	Y	;будет указатель на таблицу углов
; [SP++]	ijk	;все регистры в хвост и в гриву
; [SP++]	C
; ;а вот Z оставим нетронутым! Ну и Acc считаем "временным", равно как и флаги
			
ijk	4		;Inv=0, k=0, i=0, j=4
ZAcc	RoundZero
SUB	[X+k]
[SP]	0		;угол инициализируем нулём
Y	AtanTable


Занесение практически всех регистров в стек пока закомментировали - пока не знаю, насколько это необходимо, посмотрю "по ходу дела".

Обнуление аккумулятора и вычитание из него x - чтобы первый раз определить знак. Удобнее было так, а не "Acc [X+k] SUB 0", потому что иначе возникнет либо RAW Hazard на использование k в [X+k] и в этот же самый момент его обнуление предыдущей командой. Либо мы попытаемся между ними запихать [SP] 0, но тогда выйдет Mem Hazard - одновременное чтение и запись памяти, с одним-единственным формирователем эффективного адреса. Разве что "Y AtanTable" перед "SUB 0" нас устраивает - тогда никаких Hazard'ов. Но как сейчас - самую чуточку быстрее, на пару тактов. Но из-за такой перестановки надо будет первой записью в таблицу сделать не exp(i*pi/2), а exp(-i*pi/2). Почему бы и нет...

И наконец, завершение работы. Нужно к углу, лежащему в [SP], прибавить x, лежащий в [Z]. По счастью, угол лежит не только в [SP], но и в Acc в этот самый момент, так что одной строчкой меньше. Ну и потом ещё восстанавливаем старые значения регистров, если необходимо:

ADD	[Z+k]
[Z+k]	Acc
		
; C	[--SP]
; ijk	[--SP]
; Y	[--SP]
; X	[--SP]

JMP	[--SP]


Вот, пожалуй, и всё.

Приведём весь код целиком:

;atan1(y,x)
;берёт atan(y/x), результат от -pi/2 до pi/2, в формате Q2.12 (т.е представимые углы от -2 до +2)
;также находит длину вектора (x,y). 

;в [X], [X+1] лежат значения x,y, их трогать не будем
;в [Z] будет лежать длина вектора, а в [Z+1]: угол. 

;точность нахождения угла: все 16 бит, если (x,y) нормированы (т.е x^2+y^2=1), в противном случае не хуже 5,625 градусов (5..6 бит значащих) 
;точность нахождения длины вектора: 0,36% (8..9 значащих бит)

atan1 proc

		; [SP++]	X	;будет изменён в процессе
		; [SP++]	Y	;будет указатель на таблицу углов
		; [SP++]	ijk	;все регистры в хвост и в гриву
		; [SP++]	C
					; ;а вот Z оставим нетронутым! Ну и Acc считаем "временным", равно как и флаги
			
;assign ijk = {invreg, kreg, ireg, jreg};
		ijk		4		;Inv=0, k=0, i=0, j=4
		ZAcc		RoundZero
		SUB		[X+k]
		[SP]		0		;угол инициализируем нулём
		Y		AtanTable
@@j_loop:	Inv		S
		i		1
		C		[X+k]
@@i_loop:	ZAcc		RoundZero
		FMPM		[Y+3j+1]
		C		[X+i]
		FMA		[Y+3j+k]
		[Z+i]		Acc
		iLOOP		@@i_loop	
;вышло 8 строчек, но пока фигня с j (когда не равно единице, со знаками песец выходит)
		X		Z	;возможно, X сохраним в стек, чтобы процедура минимально меняла регистры
		i		2
		Acc		[SP]
		PM		[Y+3j+i]
		[SP]		Acc		
		jLOOP		@@j_loop
		ADD		[Z+k]
		[Z+k]		Acc
		
		; C		[--SP]
		; ijk		[--SP]
		; Y		[--SP]
		; X		[--SP]

		JMP		[--SP]
atan1 endp



Без сохранения значений регистров в стеке, получается 22 строки кода, или 44 байта :) С сохранениями - 30 строк, или 60 байт. И ещё 15 слов (30 байт) будут зарезервированы в оперативной памяти, для таблицы углов.

Вот только чтобы эту программу можно было откомпилировать и запустить, нужно добавить ещё одну команду в АЛУ, "S", и навести наконец-то порядок со знаками для FMPM! Этим сейчас и займёмся.
Tags: ПЛИС, математика, программки, работа, странные девайсы
Subscribe

  • Лестница для самых жадных

    В эти выходные побывал на даче, после 3-недельной "самоизоляции". Забавно, как будто зима началась! Особенно грязные галоши остались на улице, в…

  • Возвращаемся к макету

    Очень давно макетом видеоизмерителя параметров сближения не занимался: сначала "громко думал" по поводу измерения его положения на аппарате, а потом…

  • Минутка живописи

    В процессе разгребания содержимого квартиры (после нескольких ремонтов) дошёл, наконец, и до картин. В кои-то веки их повесил. Куда их вешать -…

  • Post a new comment

    Error

    Anonymous comments are disabled in this journal

    default userpic

    Your IP address will be recorded 

  • 0 comments