nabbla (nabbla1) wrote,
nabbla
nabbla1

Category:

Алг. ближней дистанции, вычисление крена

Всех с прошедшими первомаем, днём радио и днём Победы!

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

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


Посмотрим её исходный "заголовок":
;состояние регистров к этому моменту
;X = AffineMat (константная матрица 3х4, чтобы посчитать преобр)
;Y = Points2D
;Z = Matrix (матрица 2х2 и вектор 1х2, пока не AfTransf)
;i=j=k=0
;Inv неизвестен
;C, Acc - пофиг наверное
FindRoll	proc


С тех пор немного всё поменялось, надо привести в соответствие:
;состояние регистров к этому моменту
;X = Points2D 
;Y = Matrix = a11 (временное хранилище, где лежит матрица аф преобразования)
;Z = ShortRangeAffine либо LongRangeAffine (в зависимости от алгоритма)
;i=j=k=Inv=0
;C, Acc - пофиг наверное
FindRoll	proc


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

	;нам понадобятся [Z]+[Z+3] и [Z+1]-[Z+2]			
		i		1
		j		3
	@@sico:	Acc		[Z+i]
		Inv		i
		PM		[Z+i^j]
		Y		QuatY 	;в этом кватернионе построим,используя Y/Z значения как временные
					;(потом они занулятся)				
		[Y+i]		Acc
		iLOOP		@@sico


По сути, меняем регистры Y и Z местами: раньше в Z были входные данные, в Y выходные, теперь наоборот. И ещё вспоминаем замечательную команду ijk (инициализировать регистры i,j,k, Inv одним махом), которая сэкономит аж одно слово кода. Выйдет так:
	;нам понадобятся [Y]+[Y+3] и [Y+1]-[Y+2]			
		ijk	0x0023	;Inv=k=0, i=1, j=3
	@@sico:	Acc	[Y+i]
		Inv	i
		PM	[Y+i^j]
		Z	QuatY 	;в этом кватернионе построим,используя Y/Z значения как временные
				;(потом они занулятся)				
		[Z+i]	Acc
		iLOOP	@@sico				


Далее вызываем процедуру NormSiCo, бессмысленную и беспощадную:
	;теперь наших друзей надо отнормировать, причём динамический диапазон очень велик
	;максимум 13 итераций, давайте столько и делать...
	;здесь у нас появляется беззнаковое число, без него получалось бы очень грустно	
	CALL NormSiCo
	;здесь у нас i=j=k=Inv=0


И сама процедура:
;по адресу Y лежит два числа, двухмерный вектор
;нужно его отнормировать.
;необходимо, чтобы i=0. 
;после завершения процедуры, i=j=0, значения в Acc и C меняются
NormSiCo 	proc
		j	12 ;если "передавать" j как аргумент, то можно регулировать число итераций
;при первом вызове нужно 13 итераций, при втором хватило бы и 6 итераций,
;но мы пока пытаемся оптимизировать по памяти кода, а не по быстродействию
;по быстродействию запас в 300 раз пока что...
@@norm:		ZAcc	ThreeHalf	;выставить 3/2
		SQRSD2	[Y+1]
		SQRSD2	[Y+i]		;вычесть половину от квадрата 
		;теперь на содержимое аккумулятора надо домножить Si и Co
		i	1
		C	UAC
@@inmult:	MULSU	[Y+i]		;уможение знакового B на беззнак. C
		[Y+i]	Acc		;вернули на место!
		iLOOP	@@inmult		
		jLOOP	@@norm
		JMP	[--SP]
NormSiCo	endp


Очень дурацкое использование метода Ньютона: в подавляющем большинстве случаев значения si и co будут очень маленькими и будут медленно и печально умножаться на 3/2 от итерации к итерации (квадратичная сходимость - нет, не слышали), и лишь на нескольких последних итерациях все значащие разряды вдруг все будут найдены. Правильнее было бы сначала смасштабировать числа более простыми методами - да хоть последовательными сдвигами влево. Может, так и сделаем потом, если окажется, что памяти ещё вагон, а пока жаба душит.

Надо поменять Y на Z, а больше ничего толком не сделаешь, ужимал эту процедуру как мог, и ужал-таки до 11 слов, меньше не получается:
;по адресу Z лежит два числа, двухмерный вектор
;нужно его отнормировать.
;необходимо, чтобы i=0. 
;после завершения процедуры, i=j=0, значения в Acc и C меняются
NormSiCo 	proc
		j	12 ;если "передавать" j как аргумент, то можно регулировать число итераций
;при первом вызове нужно 13 итераций, при втором хватило бы и 6 итераций,
;но мы пока пытаемся оптимизировать по памяти кода, а не по быстродействию
;по быстродействию запас в 300 раз пока что...
@@norm:		ZAcc	ThreeHalf		;выставить 3/2
		SQRSD2	[Z+1]
		SQRSD2	[Z+i]	;вычесть половину от квадрата 
		;теперь на содержимое аккумулятора надо домножить Si и Co
		i	1
		C	UAC
@@inmult:	MULSU	[Z+i]	;уможение знакового B на беззнак. C
		[Z+i]	Acc		;вернули на место!
		iLOOP	@@inmult		
		jLOOP	@@norm
		JMP	[--SP]
NormSiCo	endp


Далее идёт "превращение в кватернион", см "кватернион из синуса и косинуса" из ликбеза по кватернионам:
	;теперь синус-косинус превращаем в кватернион, то есть в синус-косинус половинного угла
	;если co>0, то
	;QuatX = (1+abs(co))/2,
	;QuatY = si/2
	;в противном случае
	;QuatX = si/2,
	;QuatY = (1+abs(co))/2
	
	;сейчас X = Points2D
	;Y = Matrix (матрица 2х2 и вектор 1х2, к ним ещё вернёмся)
	;Z = QuatY (компоненты co / si),	
	;i=j=k=Inv=0
					
		ABS	[Z+k]			
		DIV2	Acc
		ADD	16384
;флаг S (sign) сейчас показывает знак co, что для нас очень полезно
		JGE	@@skip
		i	1		;выходит i=S
	@@skip:	X	QuatA
		[X+i]	Acc
		j	1
		DIV2	[Z+1]
		Z	QuatA
		[X+i^j]	Acc			;по сути, Y+(~S)
		CALL	NormSiCo	;подготовили первые 2 компонента кватерниона


Регистры я поменял местами уже, тут проблем нет. Но код чрезвычайно укуренный... Мы оставляем нетронутыми исходные значения si/co, а на новое место в правильном порядке запихиваем (1+abs(co))/2 и si/2, которые после нормировки и станут синусом и косинусом половинного угла. Но чтобы отнормировать их, приходится "затирать" регистр Z, а потом "возвращать его на место", чтобы повернуть матрицу. А что мешает немножко подождать с этой нормировкой - значения от нас уже никуда не убегут! Так что две строки пока убираем:

		ABS	[Z+k]			
		DIV2	Acc
		ADD	16384
;флаг S (sign) сейчас показывает знак co, что для нас очень полезно
		JGE	@@skip
		i	1		;выходит i=S
	@@skip:	X	QuatA
		[X+i]	Acc
		j	1
		DIV2	[Z+1]
		[X+i^j]	Acc			;по сути, Y+(~S)


И наконец, "поворот" матрицы (умножение на матрицу поворота, "на лету" сформированную из si/co), старый код:
;сейчас X=Y=QuatA (2 компоненты кватерниона),
;Z = Matrix
;теперь помножаем две матрицы, одна в явном виде - Matrix (Z)
;вторая - задана неявно, в виде co / si (в QuatY)
;результат идет в AfTransf
;здесь i=1, k неизвестен, j=0, Inv=0
			Y		QuatY
			i		1	;номер строки результата, и строки co/si
	@@i_loop:	k		1	;номер столбца результата, и столбца AfTransf
	@@k_loop:	j		1	;номер столбца co/si и строки AfTransf
			ZAcc		RoundZero	;обнулить до 1/2 мл. разр
	@@j_loop:	C		[Y+i^j]	
			FMPM		[Z+2j+k]
			jLOOP		@@j_loop
			X		AfTransf	;сюда результат складируем				
			[X+2i+k]	Acc
			kLOOP		@@k_loop
			iLOOP		@@i_loop


Регистры надо поменять местами, это понятно. Строку "Y QuatY" вообще убрать - у нас уже Z=QuatY, там лежат эти si/co.

Для начала надо переписать комментарий, описывающий состояние всех регистров:
;сейчас X=QuatA (2 пока ненормированные компоненты кватерниона),
;Y = Matrix,
;Z = QuatY (на данный момент здесь co и si, потом надо будет занулить)
;теперь помножаем две матрицы, одна в явном виде - Matrix (Y)
;вторая - задана неявно, в виде co / si (в Z)
;результат идет в AfTransf
;здесь i неизвестен, j=1, k=Inv=0


И ещё хочется воспользоваться фактом, что уже на данный момент j=1. Если цикл по j вытащить "наверх", то не придётся его инициализировать. В принципе, i и j в данном цикле получаются взаимозаменяемыми:
		;внешний цикл по j, номер строки результата, и строки co/si
@@j_loop:	k		1	;номер столбца результата, и столбца AfTransf
@@k_loop:	i		1	;номер столбца co/si и строки AfTransf
		ZAcc		RoundZero	;обнулить до 1/2 мл. разр
@@i_loop:	C		[Z+i^j]	
		FMPM		[Y+2i+k]
		iLOOP		@@i_loop
		X		AfTransf	;сюда результат складируем				
		[X+2j+k]	Acc
		kLOOP		@@k_loop
		jLOOP		@@j_loop


Напрягает только, что может знак поменяться у FMPM (Fused Multiply - Plus Minus), т.к он определяется как раз регистрами i,j и Inv по хитрому закону. Когда мы i и j поменяли местами, то сдаётся мне, "синус" и "минус синус" тоже поменялись местами, и мы, вместо того, чтобы убрать составляющую крена из матрицы, лишь усилим его!

Один выход здесь есть - компоненту "si" посчитать с обратным знаком, это легко, вместо
	ijk	0x0023	;Inv=k=0, i=1, j=3
@@sico:	Acc	[Y+i]
	Inv	i
	PM	[Y+i^j]
	Z	QuatY 	;в этом кватернионе построим,используя Y/Z значения как временные
			;(потом они занулятся)				
	[Z+i]	Acc
	iLOOP	@@sico				


поставим:
;нам понадобятся [Y]+[Y+3] и [Y+1]-[Y+2]			
;скорее,         [Y+3]+[Y] и [Y+2]-[Y+1]
	ijk	0x0023	;Inv=k=0, i=1, j=3
@@sico:	Acc	[Y+i^j]
	Inv	i
	PM	[Y+i]
	Z	QuatY 	;в этом кватернионе построим,используя Y/Z значения как временные
			;(потом они занулятся)				
	[Z+i]	Acc
	iLOOP	@@sico				


Вообще, до сих пор чётко не определили, "в какую сторону" кватернион надо выдавать, "ковариантный" или "контрвариантный" (см. порядок перемножения кватернионов из ликбеза), так что пока имею право знак выбрать как хочу...

Если же всё-таки захочется поменять знак поворота, то можно будет вычисление si/co вернуть "как было", а перед умножением матриц где-нибудь установить Inv=1, тогда он по сути умножит на матрицу, выражающую поворот в противоположную сторону. Ну или тоже вернуть всё как было...

Ну и напоследок надо всё-таки отнормировать кватернион:
		Z	QuatA
		CALL	NormSiCo	;подготовили первые 2 компонента кватерниона

	;здесь у нас i=j=k=0
	;теперь у нас должна получиться матрица, включающая в себя масштаб и ракурс (ужатие вдоль некоторой оси), но не крен...
	;ещё кватернион подчистим - по Y,Z может лежать всякий мусор
	;нет, сделаем это внутри цикла в FindScale

	FindRoll	endp


Весь код этой процедуры в сборе:

;состояние регистров к этому моменту
;X = Points2D 
;Y = Matrix = a11 (временное хранилище, где лежит матрица аф преобразования)
;Z = ShortRangeAffine либо LongRangeAffine (в зависимости от алгоритма)
;i=j=k=Inv=0
;C, Acc - пофиг наверное
FindRoll	proc
		;нам понадобятся [Y]+[Y+3] и [Y+1]-[Y+2]			
		;скорее,         [Y+3]+[Y] и [Y+2]-[Y+1]
			ijk		0x0023	;Inv=k=0, i=1, j=3
		@@sico:	Acc		[Y+i^j]
			Inv		i
			PM		[Y+i]
			Z		QuatY 	;в этом кватернионе построим,используя Y/Z значения как временные
						;(потом они занулятся)				
			[Z+i]		Acc
			iLOOP		@@sico				
		
	;теперь наших друзей надо отнормировать, причём динамический диапазон очень велик
	;максимум 13 итераций, давайте столько и делать...
	;здесь у нас появляется беззнаковое число, без него получалось бы очень грустно	
			CALL NormSiCo
	;здесь у нас i=j=k=Inv=0
	
	;теперь синус-косинус превращаем в кватернион, то есть в синус-косинус половинного угла
	;если co>0, то
	;QuatX = (1+abs(co))/2,
	;QuatY = si/2
	;в противном случае
	;QuatX = si/2,
	;QuatY = (1+abs(co))/2
	
	;сейчас X = Points2D
	;Y = Matrix (матрица 2х2 и вектор 1х2, к ним ещё вернёмся)
	;Z = QuatY (компоненты co / si),	
	;i=j=k=Inv=0
				
			ABS		[Z+k]			
			DIV2		Acc
			ADD		16384
	;флаг S (sign) сейчас показывает знак co, что для нас очень полезно
			JGE		@@skip
			i		1		;выходит i=S
		@@skip:	X		QuatA
			[X+i]		Acc
			j		1
			DIV2		[Z+1]
			[X+i^j]		Acc			;по сути, Y+(~S)
								
	;сейчас X=QuatA (2 пока ненормированные компоненты кватерниона),
	;Y = Matrix,
	;Z = QuatY (на данный момент здесь co и si, потом надо будет занулить)
	;теперь помножаем две матрицы, одна в явном виде - Matrix (Y)
	;вторая - задана неявно, в виде co / si (в Z)
	;результат идет в AfTransf
	;здесь i неизвестен, j=1, k=Inv=0
			;внешний цикл по j, номер строки результата, и строки co/si
	@@j_loop:	k		1	;номер столбца результата, и столбца AfTransf
	@@k_loop:	i		1	;номер столбца co/si и строки AfTransf
			ZAcc		RoundZero	;обнулить до 1/2 мл. разр
	@@i_loop:	C		[Z+i^j]	
			FMPM		[Y+2i+k]
			iLOOP		@@i_loop
			X		AfTransf	;сюда результат складируем				
			[X+2j+k]	Acc
			kLOOP		@@k_loop
			jLOOP		@@j_loop


			Z		QuatA
			CALL		NormSiCo	;подготовили первые 2 компонента кватерниона

	;здесь у нас i=j=k=0
	;теперь у нас должна получиться матрица, включающая в себя масштаб и ракурс (ужатие вдоль некоторой оси), но не крен...
	;ещё кватернион подчистим - по Y,Z может лежать всякий мусор
	;нет, сделаем это внутри цикла в FindScale

	FindRoll	endp


Всего 30 слов кода (60 байт), но ещё процедура NormSiCo, занимающая 11 слов (22 байта).


Теперь программа "ближней дистанции" компилится в 157 слов кода (314 байт), вышли-таки на 8-битную адресацию ПЗУ, ну ничего не поделаешь, сложный алгоритм всё-таки. И сейчас посмотрим на симуляции, правильно ли всё работает.
Tags: ПЛИС, программки, работа, странные девайсы
Subscribe

  • Тестируем atan1 на QuatCore

    Пора уже перебираться на "железо" потихоньку. Решил начать с самого первого алгоритма, поскольку он уже был написан на ассемблере. В программу внёс…

  • Формулы приведения, что б их... (и atan на ТРЁХ умножениях)

    Формулу арктангенса на 4 умножениях ещё немножко оптимизировал с помощью алгоритма Ремеза: Ошибка уменьшилась с 4,9 до 4,65 угловой секунды, и…

  • Алгоритм Ремеза в экселе

    Вот и до него руки дошли, причина станет ясна в следующем посте. Изучать чужие библиотеки было лениво (в том же BOOSTе сам чёрт ногу сломит), писать…

  • atan на ЧЕТЫРЁХ умножениях

    Мишка такой человек — ему обязательно надо, чтоб от всего была польза. Когда у него бывают лишние деньги, он идёт в магазин и покупает какую-нибудь…

  • Ай да Пафнутий Львович!

    Решил ещё немного поковыряться со своим арктангенсом. Хотел применить алгоритм Ремеза, но начал с узлов Чебышёва. И для начала со своего "линейного…

  • atan(y/x) на двух умножениях!

    Чего-то никак меня не отпустит эта тема, всё кажется, что есть очень простой и эффективный метод, надо только его найти! Сейчас вот такое…

  • Post a new comment

    Error

    Anonymous comments are disabled in this journal

    default userpic

    Your IP address will be recorded 

  • 0 comments