nabbla (nabbla1) wrote,
nabbla
nabbla1

Categories:

Оба алгоритма захвата - в загрузочный сектор! (512 байт)

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

Всего они заняли 254 слова кода, или 508 байт, то есть по-прежнему влезли в один блок внутренней памяти ПЛИС (БВП, он же EAB, Embedded Array Block). Ровно в той ПЛИС, которую я мучаю, 5576ХС4Т, таких блоков 24 штуки, а в той, куда хотелось бы вписаться, 5576ХС6Т (дофига радстойкая, в т.ч к тяжёлым заряженным частицам) - их всего 10.

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

Итого уже 40% доступной памяти для ХС6Т мы заняли, а ещё алгоритм сопровождения нужен! Он довольно-таки "мерзопакостный"...


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

Я его пока выполнял только на персональном компьютере, сначала "в плавающей точке", а потом попробовал и в 16-битной целочисленной арифметике. В общем-то, это вполне реально, но требует внимания к деталям. Необходимая численная устойчивость была получена только при использовани LLT-разложения, и только если при этом промежуточные операции выполнялись с 32-битной точностью. Вот поэтому в QuatCore и появился "байпас", цепь, соединяющая младшие биты аккумулятора с младшими битами входного регистра B, без него эта задача не решается! По крайней мере, в той постановке, что у меня. И боюсь, это особенность задачи, там действительно динамический диапазон большой: "поперечное" движение отслеживается очень хорошо, а "продольное" на такое же количество даёт лишь ничтожное изменение масштаба картинки.

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

Ну да ладно, алгоритмом сопровождения займёмся чуть позже. Сначала надо "захват" довести до ума, и к "обнаружению" вернуться. Вот какая получилась программа обнаружения. Не делаю цветового оформления, т.к козёл Фрэнк не может столько HTML-кода пережевать:

.code
		SP		Stack
		JMP		FindMDD3	;сейчас проверяем алгоритм дальней дистанции. Вообще прыжок туда или сюда должен происходить по окончании алгоритма обнаружения точек. Он их из списка перенесёт в массив, сосчитает, сколько получилось - и вызовет либо одно, либо другое. 
		
;точка входа, когда в ближней дистанции
Find2Points proc
;		X		Points2D	;используется с индексом i, чтобы выбрать точки 0..6
;X инициализируется нулём, а Points2D также имеют нулевой адрес				
		;в Y будем хранить индексы, сразу оба (для этого нужно хотя бы 8 бит).
		;в [SP] будем хранить текущую сумму,
		;в [SP+1] максимальную отдалённость на данный момент			
		j		6		;в дальнейшем будет выбор между 5 и 6 (если рассматривать 7 найденных точек вместо 8)
		[SP+1]		0		;обнуляем. А "Y" (индексы) можно не обнулять, заведомо будут занесены на первой же итерации		
@@j_loop:	i		j		;сравниваем точку под индексом j+1 со всеми остальными, чей индекс меньше j+1
@@i_loop:	k		1
		[SP]		0		;Начинаем считать квадрат расстояния
		Z		Fx1		;используется с индексом j, чтобы выбрать точки 1..7
@@k_loop:	DIV2		[X+2i+k]	;это Fy[i], если k=1, или Fx[i], если k=0
		DIV2S		[Z+2j+k]	;это Fy[j+1], если k=1, или Fx[j+1], если k=0
;изначально стояло Acc / SUB, но приводило к переполнению на наших убийственных 0,5 метрах, когда "еле-еле впихали всю мишень в кадр"
;поэтому заменили на DIV2 / DIV2S
		SQRD2		Acc		;разность возвести в квадрат и делить пополам
		ADD		[SP]
		[SP]		Acc
		kLOOP		@@k_loop
		;здесь у нас есть квадрат дистанции, лежит и в [SP], и в Acc. Это удобно :)
		SUB		[SP+1]	;сравниваем с самой большим квадратом дистанции на данный момент
		JL		@@skip	;меньше, не интересно
		;обновим значение дальности и индексов, при которых она достигается
		[SP+1]		[SP]		;программисты на ассемблерах x86, AVR и ARM должны завидовать, что у нас такое можно!
		Y		ijk		;и такому
@@skip:		iLOOP		@@i_loop
		jLOOP		@@j_loop		
		;индексы найдены, теперь нужно переместить эти точки куда положено
		ijk		Y
		;начнём с индекса j, с ним правильное смещение даёт Z. А для X поставим i = 7 (запихаем в самую последнюю, это ПРИНЦИПИАЛЬНО)
		i		7
		[SP]		Call(SwapPoints)
		;теперь нужно вытащить индекс i
		ijk		Y
		;с ним правильное смещение даёт X. А чтобы [Z+2i] указывало на точку с индексом 6, нужно выставить j = 5 (Z сдвинуто на 1 вперёд)
		j		5
		[SP]		Call(SwapPoints)	
Find2Points endp

;содержание регистров на этот момент
;X = Points2D (начало массива найденных точек)
;Y содержит индексы самых отдалённых точек (уже потеряло актуальность)
;Z = Fx1 (сдвинуто на 1 точку отн. X)
;k=0
;j=5
;i - индекс одной из самых отдалённых точек (потеряло актуальность)
;[SP] указывает на первую строку SortByProjection (адрес возврата, когда вызывали SwapPoints последний раз)
;[SP+1] содержит 1/8 квадрата расстояния между двумя самыми дальними точками (потеряло актуальность)
SortByProjection proc
		ijk		0x04C4	;{Inv,k,i,j} = {0, 1, 6, 4}
		Y		a14		;нужно временное хранилище под вектор, соединяющий точки 6 и 7. Пусть это будет a14 и a24 (пока они не нужны, и понадобятся нескоро)
@@Dif:		DIV2		[X+2i+k]
		DIV2S		[Z+2i+k]
		[SP]		@@loopEnd	;чтобы сделать условный вызов процедуры и вернуться из неё куда надо (пихнули сюда, чтобы выполнялось "нахаляву")				
		[Y+k]		Acc
		kLOOP		@@Dif
;теперь начинаем сортировать. Более компактный, но более медленный код, где скалярные произведения считаются "на лету", вместо того чтобы посчитать заранее
@@j_loop:	i		j
		;Делаем "сдвинутую" адресацию [Fx1 + 2j + k], т.е j=4..0 пробежит индексы от 5 до 1 
		;но при этом, i = 4..0 пробежит индексы от 4 до 0. 
		;хотим в индекс j+1 поставить "самую большую", для этого надо выбрать между j+1,...0

;вариант без цикла по k (то есть, k=0 всегда)
;самый топорный (с разворачиванием разности в отдельные умножения и накопление в акк)
@@i_loop:	C		[Y+k]
		MUL		[Z+2j+k]
		FMS		[X+2i+k]
		C		[Y+1]
		FMA		[Z+2j+1]
		FMS		[X+2i+1]
;6 команд, под конец проверяем знак. Но 4 умножения вместо 2. 		
		JGE		SwapPoints
@@loopEnd:	iLOOP		@@i_loop
		jLOOP		@@j_loop
SortByProjection endp


;состояние регистров к этому моменту:
;i=j=k=0
;C = Vy (y-компонента вектора, соединяющего крайние точки)
;Acc - последнее скалярное произведение (потеряло актуальность)
;X = Points2D (начало массива найденных точек)
;Y = a14 (здесь лежит вектор V, соединяющий крайние точки, а вообще это "временное хранилище")
;Z = Fx1 (сдвинуто на 1 точку отн. X)
;[SP] = @@loopEnd, адрес возврата для SwapPoints (потеряло актуальность)
;[SP+1] - 1/8 квадрата расстояния между двумя самыми дальними точками (потеряло актуальность)
FindCentralPoints proc
;дважды вызываем процедуру FindCentralPoint, первый раз для "левой" центральной точки, второй раз для "правой"
			Y		a11
			[SP++]		Call(FindCentralPoint)	;классический вызов процедуры, т.к она использует стек [SP] и [SP+1], недопустимо чтобы затёрла адрес возврата
;помещаем "левую" центральную точку в индекс 1.
			[SP]		Call(SwapPoints)		;вызов без инкремента стека, чтобы можно было в неё запрыгивать из условных переходов, зная что SP останется где надо
;меняем местами точки 6 и 7
			ijk		0x00C6	;{Inv, k, i, j} = {0, 0, 6, 6} когда-нибудь надо ввести синтаксис, чтобы указывать биты don't care, чуть поможет компилятору
			[SP]		Call(SwapPoints)
;теперь FindCentralPoint найдёт "правую" центральную точку
			[SP++]		Call(FindCentralPoint)
;помещаем "правую" центральную точку в индекс 4
			j		3
			[SP]		Call(SwapPoints)
;вот и всё. Центральные точки сидят в индексах 1 и 4, крайние - в индексах 6 и 7 (причём поменялись местами), остальные отсортированы "слева направо".
FindCentralPoints endp
			

;содержание регистров и стека к этому моменту:
;i неизвестно (от 5 до 7)
;j = 3
;k = 0
;C, Acc неизвестны
;X = Points2D
;Y = a11 (временное хранилище)
;Z = Fx1 (сдвинуто на 1 точку отн. X)
;[SP], [SP+1] - неизвестны
CheckUpsideDown proc
;на самом деле, проблема не в том, что "вверх ногами", а в возможном зеркальном отражении
			ijk		0x0027	;{Inv,k,i,j} = {0,0,1,7}
			[SP]		0
@@i_loop:		Acc		[X+i]		;Fy0 на первой итерации, Fx0 на второй
			SUB		[X+2j+i]	;вычесть Fy7 на первой итерации, Fx7 на второй
			C		[Y+i^j]	;i^j = 6 на первой итерации, 7 на второй. Должным выбором Y можно это устроить...
			MUL		Acc		;(Fy0-Fy7) * Vx на первой итерации,  (Fx0-Fx7) * Vy на второй
			SUB		[SP]		;вычитаем 0 на первой итерации, получая (Fy0-Fy7) * Vx.  Вычитаем (Fy0-Fy7) * Vx на второй, получая (Fy0-Fy7) * (-Vx) + (Fx0-Fx7) * Vy
			[SP]		Acc
			iLOOP		@@i_loop
;по завершении, флаг знака обозначает знак проекции на "вертикальную" ось (перпендикулярную к отрезку, соед. точки 6-7)
			JGE		@@skip
;7 <-> 6		
;0 <-> 5
;1 <-> 4
;2 <-> 3

;i=0, j=6, k=0, но могли бы k поставить какой хотим.
;Swap меняет (X,i)  с (Z,j) в смысле нумерация i от нулевой точки, j - от первой.
			ijk		0x00C6
			[SP]		Call(SwapPoints)	;поменяли 6 и 7
			ijk		0x0042	;i=2, j=2, k=0, Inv=0, последнее вообще необязательно
@@reverse:		[SP]		Call(SwapPoints)
			j++		0
			iLOOP		@@reverse
;к моменту @@skip, заведомо i=0, j = 5 или 6. k=0. 
;хотим поменять местами 1,7 и 4,6.
@@skip:			ijk		0x0026	;Inv=0, k=0, i=1, j=6
			[SP]		Call(SwapPoints)
			ijk		0x0085	;Inv=0, k=0, i=4, j=5
			[SP]		Call(SwapPoints)
CheckUpsideDown endp

			Z		ShortRangeAffine	;нужно перед ComputeNPointAffine, чтобы она была универсальна, и для дальней, и для ближней дистанции
			[SP+1]		5			;количество точек (нумерация с нуля)
			
;состояние регистров к этому моменту
;X = Points2D
;Y = a11 = Matrix (временное хранилище), куда сунем матрицу аф преобразования
;Z = ShortRangeAffine либо LongRangeAffine (в зависимости от алгоритма)
;Inv=0
;i,j,k - неважно, все переопределим
;C, Acc - неважно
;[SP+1] = количество точек (нумерация с нуля)
ComputeNPointAffine	proc
;по сути, Y = Z * X, где все операнды - матрицы.
			k		1		;номер столбца результата (и столбца Points2D)
	@@k_loop:	i		2		;номер строки результата (и строки AffineMat)
	@@i_loop:	j		[SP+1]	;номер столбца AffineMat и строки Points2D (либо 5 для ближней дистанции, либо 3 для дальней)
			ZAcc		RoundZero	;обнулить до 1/2 мл. разр
	@@j_loop:	C		[Z+3j+i]
			FMA		[X+2j+k]
			jLOOP		@@j_loop
			[Y+2i+k]	Acc
			iLOOP		@@i_loop
			kLOOP		@@k_loop
;вот, какбе и всё...	
ComputeNPointAffine	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
				Z		QuatY 	;в этом кватернионе построим,используя Y/Z значения как временные
								;(потом они занулятся)				
		@@sico:		DIV2		[Y+i^j]
				Inv		i
				DIV2PM		[Y+i]
				[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
	;здесь у нас i=j=k=0
	;теперь у нас должна получиться матрица, включающая в себя масштаб и ракурс (ужатие вдоль некоторой оси), но не крен...
	;ещё кватернион подчистим - по Y,Z может лежать всякий мусор
	;нет, сделаем это внутри цикла в FindScale
	FindRoll	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 лежит манхеттенская метрика, а в [Y] = [Y+k] - временное значение
				ijk		0x0023	;Inv=k=0, i=1, j=3
				[Y+1]		0		;Чебышевская, максимум от модулей
				C		0		;Манхэттенская, сумма модулей (метрика городских кварталов)
		@@loop:		Acc		[X+i]		;
				PM		[X+i^j]	;
				Inv		1		;уже можно! Так что на следующей итерации будет "-"
				[Z+i]		0		;очистим две компоненты кватерниона
				ABS		Acc		;|Txy+Tyx| (i=1), |Txx-Tyy| (i=0)
				[Y+k]		Acc		;сохранили это значение
				ADD		C
				C		Acc		;обновили манхэттенскую
				Acc		[Y+k]		;вернули "текущее значение"
				SUB		[Y+1]
				JL		@@skipCheb
				[Y+1]		[Y+k]
		@@skipCheb:	iLOOP		@@loop
				
	;теперь в C лежит манхэттенская метрика, а в [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:		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
	
	;состояние регистров:
	;X=AfTransf=Exp   (это одно и тоже, просто в разное время)
	;Y=Matrix (первые 4 значения можно использовать как временные)
	;Z=QuatY (два нулевых значения, не надо их трогать)
	;i=k=0
	;j=Exp
	;Inv=1
	;C, Acc - пофиг (сейчас сотрём)
	;в [SP] лежит "мантисса" масштаба, которую надо обратить
	;в [SP+1] - либо 3, если дальняя дистанция, либо 5, если ближняя.
	FindVector	proc	
				k		3
				C		43648	;наше нулевое приближение к обр. величине
		@@Newton:	Acc		0		;занести двойку
				UFMS		[SP]	;Acc = 2 - a * x
				MULU		UAC		;Acc = Acc * x = x * (2- a*x), т.е итерация метода Ньютона
				Y		Rx								
				Z		QuatA
				i		1				
				C		UAC				
				kLOOP		@@Newton			
	;золотой ключик почти у нас в кармане
	;если на входе было 16384, то правильный результат 65536 не влезет в 16 бит, сократившись до нуля. 
	;это мы должны заметить по переполнению, только вот не соображу, оно должно ПОЯВИТЬСЯ, или исчезнуть?
				JGE		@@SkipOflo
				j++		0 			;раз не вмещается в мантиссу, значит добавим экспоненту, а в мантиссу нужно 32768 теперь.Ща найдём...
				C		32768 
		@@SkipOflo:	[X+k]		j						
				[X+1]		C

				X		Ty
	;и ещё осталось найти Ty, Tz
		@@vector:	MULSU		[Y+i]					
				[X+i]		Acc
				iLOOP		@@vector
		
				CALL		NormSiCo
				
	FindVector	endp		

	
endless:	JMP	endless


;точка входа для дальней дистанции

		;состояние регистров неизвестно (за искл. PC и SP)
		FindMDD3	proc
		;перво-наперво, находим МДД3
		;как точка, сумма квадратов расстояний до которой максимальна (т.е самая отдалённая)
		;для каждой точки считаем сумму квадр. расстояний до всех остальных, в т.ч до самой себя (это 0, зато код упрощается)
		;внешний цикл (по строкам) - перем. j
		;внутр. цикл - по индексу перем (X или Y) - перем k
		;самый-самый внутр. цикл (по столбцам) - перем. i
				X		Points2D
				[SP+1]		0	;максимальная отдалённость, инициализируем нулём
				;рег. X будет хранить индекс точки с макс. отд. её иниц. не надо-заведомо на одной из итераций запишется
					
				j		3
		@@j_loop:	k		1	;также от 0 до 3, чтобы все расстояния просуммировать
				[SP]		0	
		@@k_loop:	i		3	;от 0 до 1, т.е значения X и Y
		;а теперь непосредственно прибавляем к акк. ([X+2i+k]-[X+2j+k])^2
		@@i_loop:	Acc		[X+2j+k]	;загрузили одно значение       (3 такта)
				SUB		[X+2i+k]	;вычитаем второе               (2 такта, т.к не дожидаемся окончания ACC, но ещё один т.к нужно загрузить Acc в DataBus)
				SQRD2		Acc		;возводим в квадрат      	 (19 тактов)
				ADD		[SP]		;прибавляем к пред. значению   (2 такта, т.к не дожидаемся SQRD2, но ещё один т.к Acc)
				[SP]		Acc
				iLOOP		@@i_loop		;теперь то же самое для второй координаты
				
				kLOOP		@@k_loop
		;хорошо, нашли сумму расстояний от точки j до всех остальных
		;она лежит в Acc и в [SP]
		;нужно сравнить с самым большим, и если больше - заменить
		;самое большое значение мы храним в [SP+1],
		;а его индекс - в [SP+2]
				SUB		[SP+1]
				Z		Fx3	;вообще, оно перед SwapPoints, но здесь всё равно ждём окончания SUB		
				JL		@@skip
		;дистанция оказалась больше - надо обновить максимум и его индекс
				[SP+1]		[SP]	;двухпортовая память-это зашибись!
				Y		j	;здесь сохраняем индекс самой отдалённой точки
		@@skip:		jLOOP		@@j_loop
		;по окончании этих циклов, у нас в X лежит индекс точки, которая является МДД3
		;осталось эту точку поменять местами с точкой под индексом 0...
		;здесь i=j=k=0 (все циклы завершились!)
				i		Y	;первая точка - нумерация с нуля, индекс i (та, что самая отдалённая)
				;вторая точка - нумерация с тройки, индекс 0, т.е 3 (последняя)
				[SP]		Call(SwapPoints)
		;потёрли текущий максимум (лежал в [SP])-и хрен с ним
		
		;на этом первая часть марлезонского балета закончена.
		FindMDD3	endp

		;давайте самую отдалённую передвинем не в нулевую позицию, а в третью. 

		;состояние регистров к данному моменту:
		;X = Points2D
		;Y, i = индекс наиболее отдалённой точки  (не понадобится)
		;Z = Fx3 (самая отдалённая точка)
		;j=k=0
		;Inv неизвестен
		;C = [Fx0]  (не понадобится)
		;Acc = разность полусумм квадратов (не понадобится)
		
		SortCCW		proc
		;Далее: нужно расставить оставшиеся точки против часовой стрелки
		;Первый шаг к этому: переместить начало координат в МДД3, который сейчас по индексу 3
		;потом вернём как ни в чём не бывало, у нас фикс. точка, все правила арифм. соблюдаются						
				CALL		ShiftOrigin
		;вот теперь пора сортировать!
				[SP]		@@loopEnd				
				ijk		0x0021	;{Inv,k,i,j} = {0,0,1,1}
				Z		Fx1
		;Делаем "сдвинутую" адресацию [Fx2 + j], т.е j=1 указывает на 3-ю точку, j=0 - на 2-ю.
		;но при этом, i = 1 указывает на 2-ю, i=0 - на 1-ю
		;а нулевую пока вообще не рассматриваем - она на своём месте!
		;хотим на позицию j+2 поставить "самую большую", для этого надо выбрать между j+1,...1
		;здесь заведомо k=0, так что X+2i / X+2j не нужны!
		@@loop:		C		[X+2i+1]				
				MUL		[Z+2j+k]
				C		[X+2i+k]
				FMS		[Z+2j+1]	;нахождение "векторного произведения"
				JL		SwapPoints
		@@loopEnd:	iLOOP		@@loop
				jLOOP		@@loop

				;если мы не хотим убирать со своего места МДД3, то можно ещё точки поменять
				;здесь i=j=k=0, X = Points2D, Z=Fx1. Т.е поменяем 0 и 1. 
				[SP]		Call(SwapPoints)			
				Z		Fx3		;чтобы ShiftOrigin правильно отработал							
				CALL		ShiftOrigin		
		SortCCW		endp

Y		Matrix
[SP+1]	3
Z		LongRangeAffine
;состояние регистров к этому моменту
;X = Points2D
;Y = a11 = Matrix (временное хранилище), куда сунем матрицу аф преобразования
;Z = ShortRangeAffine либо LongRangeAffine (в зависимости от алгоритма)
;Inv=0, k=0
;i=4,j=6 (если это мишень ближней дистанции)
;но если удобнее, можем сделать i=1, j=5. 
;C, Acc - неважно
;[SP+1] = количество точек (нумерация с нуля)
JMP		ComputeNPointAffine

;   ВСПОМОГАТЕЛЬНЫЕ ПРОЦЕДУРЫ. МОГУТ РАСПОЛАГАТЬСЯ В ЛЮБОМ ПОРЯДКЕ

FindCentralPoint proc
;1. Находим 0,7914P[6] + 0,2086P[7] - координата левой центральной точки, если смотреть на мишень под прямым углом.
;(в противном случае она уползёт в сторону, но не так уж сильно)
			
			ijk		0x04C5	;одним махом всех иниц. {Inv, k, i, j} = {0, 1, 6, 5}
@@k_loop:		C		25933		;число 0,7914 в формате Q1.15
			MUL		[X+2i+k]	;Fy6 на первой итерации, Fx6 на второй
			C		6835		;число 0,2086 в формате Q1.15
			FMA		[Z+2i+k]	;Fy7 на первой итерации, Fx7 на второй
			[Y+k]		Acc
			kLOOP		@@k_loop

;2. находим точку, которая к этим координатам ближе всего. 		
			[SP+1]	32767		;текущий минимум, инициализируем максимальным значением
			;индекс самой близкой к центру точки будет храниться в i, её инициализировать не требуется			
@@outer_cycle:		k		1
			[SP]		0		;сумма квадратов
@@inner_cycle:		Acc		[X+2j+k]	;Fy[i] если k=1,  Fx[i] если k=0
			SUB		[Y+k]		;Cy если k=1, Cx если k=0
			SQRD2		Acc
			ADD		[SP]
			[SP]		Acc
			kLOOP		@@inner_cycle
			;досчитали квадрат дистанции до центра, лежит как в [SP], так и в Acc, очень удобно
			;проверяем, насколько он мал
			SUB		[SP+1]
			JGE		@@skip
			[SP+1]		[SP]
			i		j
@@skip:			jLOOP		@@outer_cycle
;точка с индексом i ближе всего к центру, а j=0. То, что надо!
			JMP		[--SP]	;ret
FindCentralPoint endp

;используется для алгоритмов и ближней, и дальней дистанций

;меняет местами точку с базой Z, инд. j  и базой X, инд. i
;изменяет регистр k (по окончании выходит 0), и C
;вызов через [SP]  Call(SwapPoints), т.е без инкремента стека
SwapPoints	proc
			k		1
			NOP		0	;хотим избежать warning'ов
@@swap:			C		[Z+2j+k]
			[Z+2j+k]	[X+2i+k]
			[X+2i+k]	C
			kLOOP		@@swap
			JMP		[SP]
SwapPoints	endp	

;по адресу Z лежит два числа, двухмерный вектор
;нужно его отнормировать.
;необходимо, чтобы i=0. 
;после завершения процедуры, i=j=0, значения в Acc и C меняются
NormSiCo 	proc
		j		12 ;если "передавать" j как аргумент, то можно регулировать число итераций
;при первом вызове нужно 13 итераций, при втором хватило бы и 6 итераций, (а вообще-то 4! Не знаю, как я так посчитал криво!)
;но мы пока пытаемся оптимизировать по памяти кода, а не по быстродействию
;по быстродействию запас в 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

	;отражает точки относительно наиболее отдалённой. Повторное применение этой операции возврщает точки на место!
	;состояние регистров к данному моменту:
	;X = Points2D (т.е указывает на точку с индексом 0)
	;Y - неважно
	;Z = Fx3 (самая отдалённая точка)
	;i неважен (вообще это индекс наиболее отдалённой точки, а потом 0)
	;j,k неважны (вообще j=k=0)
	;Inv неизвестен
	;C неважен (вообще [Fx0] при первом вызове, одна из коорд. при втором) 
	;Acc неважен (вобще разность полусумм квадратов при первом вызове, результат сравн. при втором)
ShiftOrigin proc
	;надо из всех точек вычесть коорд. наиболее отдалённой (с индексом 3)
	;кроме неё самой, разумеется
				k		1
@@k_loop:			i		2	;как всегда, чтобы Y и X
@@i_loop:			Acc		[Z+k]
				SUB		[X+2i+k]
				[X+2i+k]	Acc
				iLOOP		@@i_loop
				kLOOP		@@k_loop
				JMP		[--SP]
ShiftOrigin endp



И поглядим, что получается "по дальней дистанции", старые добрые координаты, соответствующие 300 метрам "прямой наводкой":


Ярко-зелёным показаны 4 точки, после того как их идентифицировали. В этот раз наиболее удобным мне показался порядок МБД-МДД2-МДД1-МДД3.

Синим выделен вектор с общей "экспонентой" 9, т.е значения X (от 0 до 2), Y, Z (от -1 до +1) нужно домножить на 29-1 = 256 метра.

Имеем X = 0x9591 = 38289 = 1,168 (в формате Q1.15). Умножаем на 256 - и выходит 299,13 метров.
Далее, Y = 0x0031 = 49 = 0,00149 (в формате Q1.15). Умножаем на 256 - выходит 38 сантиметров. Похоже, я и тут пожадничал в 4 раза (чтобы точность не потерять раньше времени), на самом деле должно быть 9,5 см "сдвиг вправо", т.к смотрим мы "левым глазом", а летим прямо по курсу.

И наконец, Z = 0xFE3D = -451 = -0,0138 (в формате Q1.15). Умножаем на 64 (помня, что "пожадничали") и получаем -0,881 метра. Это мы сейчас установили "началом координат мишени" центр стыковочного узла, но прибор закреплён на те самые 0,881 метра ВЫШЕ, вот и воспринимает картинку как сдвинутую вниз.

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

Это у нас работа по модельным данным, я макет мишени дальней дистанции так и не собрал до сих пор, всё с ближней дистанцией возился. По модельным данным оно приятно - всё совпадает :)
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