nabbla (nabbla1) wrote,
nabbla
nabbla1

Category:

Алг. ближ. дист, вычисление масштаба

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

Старый код:
	;Состояние регистров к этому моменту:
	;X=AfTransf,
	;Y=QuatY,
	;Z=Matrix,
	;i=j=k=Inv=0,
	;C=Acc=Txx
	FindScale	proc
	;1. Вычисляем величины |X-Z| (т.е |Txx - Tyy| ) и |2Y| (например, |Txy+Tyx|, хотя можно 2|Txy| или 2 |Tyx| )
	;важно, что в данный момент Inv=0
				[SP]		0	;манхэттенская метрика, сумма модулей (метрика городских кварталов)
				[SP+1]		0	;чебышевская, максимум от модулей 
				;текущее значение будем хранить в регистре C - дешево и сердито!
				i		1
				j		3					
		@@loop:		Acc		[X+i]	;+Tyx (i=1), -Tyy (i=0)
				PM		[X+i^j]	;Txy (i=1), Txx (i=0)   ;поменяли местами чтобы проверить модуль
				Inv		1		;уже можно! Так что на следующей итерации будет "-"
				[Y+i]		0		;очистим две компоненты кватерниона
				ABS		Acc		;|Txy+Tyx| (i=1), |Txx-Tyy| (i=0)
				C		Acc		;сохранили это значение
				ADD		[SP]	
				[SP]		Acc	;обновили манхэттенскую
				Acc		C
				SUB		[SP+1]
				JL		@@skipCheb ;лучше подошел бы JGE и поменять местами C и [SP+1]. Разница при нуле-на 1 больше операций.
				[SP+1]		C	;обновили Чебышевскую					
		@@skipCheb:	iLOOP		@@loop
	;теперь в [SP] лежит манхэттенская метрика, а в [SP+1] - чебышевская
	;осталось посчитать выражение целиком - сумму Txx+Tyy с весом 1/2, а эти - с весами 0,332 и 0,168
	;или сначала всё сложить - а потом поделить пополам...
	;можно, если ввести команду UDIV2 - беззнаковое деление
	;по сути, SHR1 
	
	;кстати, у нас до сих пор j=3 :)
	;i=k=0
				Acc		[X+i]	;добавили Txx
				ADD		[X+i^j]	;и Tyy
	;теперь наши метрики берём с весами	
				C		[SP]
				FMA		11010	;ManhW
				C		[SP+1]
				FMA		21758	;ChebW
				
	;ага, сделали
	;продемонстрируем результат
				JO		@@endLoop
		@@shl1:		ADD		Acc
				j++		0
				JNO		@@shl1		
	;ага, сделали
		@@endLoop:	DIV2		UAC ;получается отрицательное значение от -16384 до -1 (число -1 при "делении на 2" так и останется -1)
				SUB		32767 ;вычитает 32767, что даст отрицательное значение от -49151 до -32766. Но в UAC отобразится от 16385 до 32768.
				;если теперь найти обратную величину, это должно получиться в диапазоне 32768..65532
				;а если мы вычтем ещё половинку, может выйти от 16384 до 32768, и диапазон 32768..65536.
				;так что ну её нахрен, эту половинку?
				;или можем уже на этапе Ньютона этот случай проверить.
				DIV2S		1 ;это для нужд округления. 					
				[SP]		UAC							
	FindScale	endp


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

И ещё одна вещь: в [SP+1] у нас до сих пор лежит число, позволяющее отличить "ближнюю дистанцию" от "дальней". В первом случае там лежит пятёрка (т.е точки от 0 до 5), во втором - тройка (т.е от 0 до 3). Не затереть бы его ненароком!


Пожалуй, мы снова повременим с нормировкой кватерниона, хотели сделать это в конце FindRoll, но неудобно, пусть лучше рег. Z всё ещё указывает на QuatY, где два элемента, которые неплохо бы установить нулями. Так что две последние строки уберём, и тогда можем описать состояние регистров:

	;Состояние регистров к этому моменту:
	;X=AfTransf,
	;Y=Matrix (первые 4 значения уже можно "мусорить", дальше не лезть!)
	;Z=QuatY (последние 2 компонента кватерниона, которые надо бы занулить),
	;i=j=k=Inv=0,
	;C, Acc - пофиг
	;[SP] - пофиг (какой-то "протухший" адрес возврата)
	;в [SP+1] лежит либо 3 (работа по дальней дистанции), либо 5 (по ближней)
	FindScale	proc


И далее, вместо [SP] и [SP+1] в качестве "локальных переменных", можем взять [Y+k] и [Y+1], там лежит "старая" (с нескомпенсированным креном) матрица преобразования, её первые 4 значения уже не нужны. А вот дальше 2 значения - это "вектор", выражающий параллельный перенос, он ещё пригодится. На всём протяжении этой процедуры будет k=0, так что [Y+k] - это просто [Y].

Заменим отдельные команды i,j на одну, ijk (экономит 1 слово), но только её нужно отдалить от [X+i] в начале цикла, поэтому ставим её повыше, а потом зануляем текущие значения метрик. В конечном итоге, первая часть процедуры выходит такой:

	;1. Вычисляем величины |X-Z| (т.е |Txx - Tyy| ) и |2Y| (например, |Txy+Tyx|, хотя можно 2|Txy| или 2 |Tyx| )
			;текущее значение будем хранить в регистре C - дешево и сердито!
			ijk		0x0023	;Inv=k=0, i=1, j=3. 
			[Y+1]		0	;чебышевская, максимум от модулей 				
			[Y+k]		0	;манхэттенская метрика, сумма модулей (метрика городских кварталов)				
	@@loop:		Acc		[X+i]	;+Tyx (i=1), -Tyy (i=0)
			PM		[X+i^j]	;Txy (i=1), Txx (i=0)   ;поменяли местами чтобы проверить модуль
			Inv		1		;уже можно! Так что на следующей итерации будет "-"
			[Z+i]		0		;очистим две компоненты кватерниона
			ABS		Acc		;|Txy+Tyx| (i=1), |Txx-Tyy| (i=0)
			C		Acc		;сохранили это значение
			ADD		[Y+k]	
			[Y+k]		Acc	;обновили манхэттенскую
			Acc		C
			SUB		[Y+1]
			JL		@@skipCheb ;лучше подошел бы JGE и поменять местами C и [SP+1]. Разница при нуле-на 1 больше операций.
			[Y+1]		C	;обновили Чебышевскую					
	@@skipCheb:	iLOOP		@@loop
	;теперь в [Y]=[Y+k] лежит манхэттенская метрика, а в [Y+1] - чебышевская
	;осталось посчитать выражение целиком - сумму Txx+Tyy с весом 1/2, а эти - с весами 0,332 и 0,168
	;или сначала всё сложить - а потом поделить пополам...


Эта часть вычисляет две метрики - Чебышевскую и Манхэттенскую, и вся эта возня для того, чтобы оценить корень из суммы квадратов, когда числа могут меняться в широких пределах. См. как оценить длину вектора на плоскости.

Здесь я не ожидаю переполнений, т.к корень, что мы ищем, равен s(1-a), где s - масштаб,
- "ракурс".

Максимальный масштаб будет как раз при 0,5 метрах, и составит единицу, т.е максимальное представимое число в нашем формате Q1.15.

Следовательно, максимальное значение s(1-a) равно 0,134, до переполнения очень далеко.

Так что поехали дальше, где будем находить непосредственно масштаб по формуле


Этот корень мы как раз и оценим сейчас как взвешенную сумму двух "метрик", потом прибавим ещё два элемента матрицы.

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

И сейчас оно заработать должно, т.к мы видели, что масштаб как раз может достигнуть единички, а его удвоенное значение - двойки, а на самом деле и того не будет. Реально дальность до этой плоскости 0,6 метра, а то и чуть больше, учитывая, что линза несколько "утоплена" вглубь прибора, перед ней ещё светодиоды стоят.

Так что этот кусок оставляем практически "как есть", ну только меняем [SP] и [SP+1] на [Y+k] и [Y+1]:

;кстати, у нас до сих пор j=3 :)
;i=k=0
		Acc	[X+i]	;добавили Txx
		ADD	[X+i^j]	;и Tyy
;теперь наши метрики берём с весами	
		C	[Y+k]
		FMA	11010	;ManhW
		C	[Y+1]
		FMA	21758	;ChebW


А дальше мы уже начинаем преобразовывать этот масштаб в дальность в метрах. Сначала выставляем "экспоненту", т.е степень двойки, начиная от "-1", т.е от 0,5 до 1 метра.

Вот тут нам и понадобится информация, мы вообще по какой мишени работаем, по "ближней" или по "дальней"? В первом случае у нас [SP+1]=5, нужно установить j=0, во втором [SP+1]=3 и нужно оставить j=3, как и было. Но сделать это стоило перед тем, как "забивать" аккумулятор масштабом. С другой стороны, нам было удобно, чтобы j=3, чтобы обратиться к [X+3]. Поэтому данная проверка "сращивается" с нахождением масштаба:

	;если работаем в ближней дистанции, нужно установить j=0, иначе оставить j=3
		Acc	[SP+1]	;3 для дальней дистанции, 5 для ближней
		SUB	4		;получили флаг
	;кстати, у нас до сих пор j=3 :)
	;i=k=0
		Acc	[X+i]	;добавили Txx
		ADD	[X+i^j]	;и Tyy
		;теперь j=3 уже не обязателен, а флаг после SUB не поменялся
		JL	@@skip
		j	0				
	;теперь наши метрики берём с весами	
	@@skip:	C	[Y+k]
		FMA	11010	;ManhW
		C	[Y+1]
		FMA	21758	;ChebW


Далее, наконец-то ищем "экспоненту", тут ничего менять не придётся:

		JO	@@endLoop
	@@shl1:	ADD	Acc
		j++	0
		JNO	@@shl1		


И осталось "подготовить" мантиссу к обращению по методу Ньютона. Сейчас она в диапазоне от 1 до 2, слишком много, надо поделить на 2, но деление у нас только знаковое, и так уж всё устроено по-дурацки, что ответ выйдет отрицательным. Поэтому нужно ещё вычесть 32767. А потом, из перфекционизма, можно вычесть ещё половинку, хотя не уверен, что оно надо, это и дальше осложняет код, может результат 16385 округлиться "вниз" до 16384, который после обращения по методу Ньютона даст 65536, что никуда не влезет, и надо будет его "сбросить" до 32768 и ещё к экспоненте прибавить единичку.

Как-то так:

	@@endLoop:	DIV2	UAC ;получается отрицательное значение от -16384 до -1 (число -1 при "делении на 2" так и останется -1)
			SUB	32767 ;вычитает 32767, что даст отрицательное значение от -49151 до -32768. Но в UAC отобразится от 16385 до 32768.
				;если теперь найти обратную величину, это должно получиться в диапазоне 32768..65532
				;а если мы вычтем ещё половинку, может выйти от 16384 до 32768, и диапазон 32768..65536.
				;так что ну её нахрен, эту половинку?
				;или можем уже на этапе Ньютона этот случай проверить.
			DIV2S	1 ;это для нужд округления. 					
			[SP]	UAC							
FindScale	endp


В конечном итоге получается вот так:

	;Состояние регистров к этому моменту:
	;X=AfTransf,
	;Y=Matrix (первые 4 значения уже можно "мусорить", дальше не лезть!)
	;Z=QuatY (последние 2 компонента кватерниона, которые надо бы занулить),
	;i=j=k=Inv=0,
	;C, Acc - пофиг
	;[SP] - пофиг (какой-то "протухший" адрес возврата)
	;в [SP+1] лежит либо 3 (работа по дальней дистанции), либо 5 (по ближней)
	FindScale	proc
	;1. Вычисляем величины |X-Z| (т.е |Txx - Tyy| ) и |2Y| (например, |Txy+Tyx|, хотя можно 2|Txy| или 2 |Tyx| )
			;текущее значение будем хранить в регистре C - дешево и сердито!
			ijk	0x0023	;Inv=k=0, i=1, j=3. 
			[Y+1]	0	;чебышевская, максимум от модулей 				
			[Y+k]	0	;манхэттенская метрика, сумма модулей (метрика городских кварталов)				
	@@loop:		Acc	[X+i]	;+Tyx (i=1), -Tyy (i=0)
			PM	[X+i^j]	;Txy (i=1), Txx (i=0)   ;поменяли местами чтобы проверить модуль
			Inv	1		;уже можно! Так что на следующей итерации будет "-"
			[Z+i]	0		;очистим две компоненты кватерниона
			ABS	Acc		;|Txy+Tyx| (i=1), |Txx-Tyy| (i=0)
			C	Acc		;сохранили это значение
			ADD	[Y+k]	
			[Y+k]	Acc	;обновили манхэттенскую
			Acc	C
			SUB	[Y+1]
			JL	@@skipCheb ;лучше подошел бы JGE и поменять местами C и [SP+1]. Разница при нуле-на 1 больше операций.
			[Y+1]	C	;обновили Чебышевскую					
	@@skipCheb:	iLOOP	@@loop
	;теперь в [Y]=[Y+k] лежит манхэттенская метрика, а в [Y+1] - чебышевская
	;осталось посчитать выражение целиком - сумму Txx+Tyy с весом 1/2, а эти - с весами 0,332 и 0,168
	;или сначала всё сложить - а потом поделить пополам...

	;если работаем в ближней дистанции, нужно установить j=0, иначе оставить j=3
			Acc	[SP+1]	;3 для дальней дистанции, 5 для ближней
			SUB	4		;получили флаг
	;кстати, у нас до сих пор j=3 :)
	;i=k=0
			Acc	[X+i]	;добавили Txx
			ADD	[X+i^j]	;и Tyy
			;теперь j=3 уже не обязателен, а флаг после SUB не поменялся
			JL	@@skip
			j	0				
	;теперь наши метрики берём с весами	
	@@skip:		C	[Y+k]
			FMA	11010	;ManhW
			C	[Y+1]
			FMA	21758	;ChebW
				
			JO	@@endLoop
	@@shl1:		ADD	Acc
			j++	0
			JNO	@@shl1		
	;ага, сделали
	@@endLoop:	DIV2	UAC ;получается отрицательное значение от -16384 до -1 (число -1 при "делении на 2" так и останется -1)
			SUB	32767 ;вычитает 32767, что даст отрицательное значение от -49151 до -32768. Но в UAC отобразится от 16385 до 32768.
				;если теперь найти обратную величину, это должно получиться в диапазоне 32768..65532
				;а если мы вычтем ещё половинку, может выйти от 16384 до 32768, и диапазон 32768..65536.
				;так что ну её нахрен, эту половинку?
				;или можем уже на этапе Ньютона этот случай проверить.
			DIV2S	1 ;это для нужд округления. 					
			[SP]	UAC							
	FindScale	endp




Компилируется всё это дело (алг. ближ дистанции) в 191 слово кода (382 байта). Возможно, здесь я чуть перемудрил с нахождением масштаба, можно львиную долю этого кода выполнить с незначительным снижением точности. Это когда появится алгоритм сопровождения, надо будет основательную отработку устроить, смотреть, удаётся ли "втянуться" в сопровождение с различных ракурсов и различных масштабов, или происходит срыв.

А сначала хоть этот кусок надо потестировать...
Tags: ПЛИС, математика, программки, работа, странные девайсы
Subscribe

Recent Posts from This Journal

  • Тестируем 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