1/*
2 * Copyright (C) 2008 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 *      http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17/* ---- includes ----------------------------------------------------------- */
18
19#include "b_TensorEm/Flt16Mat2D.h"
20#include "b_TensorEm/Functions.h"
21#include "b_BasicEm/Math.h"
22
23/* ------------------------------------------------------------------------- */
24
25/* ========================================================================= */
26/*                                                                           */
27/* ---- \ghd{ auxiliary functions } ---------------------------------------- */
28/*                                                                           */
29/* ========================================================================= */
30
31/* ------------------------------------------------------------------------- */
32
33/* ========================================================================= */
34/*                                                                           */
35/* ---- \ghd{ constructor / destructor } ----------------------------------- */
36/*                                                                           */
37/* ========================================================================= */
38
39/* ------------------------------------------------------------------------- */
40
41void bts_Flt16Mat2D_init( struct bts_Flt16Mat2D* ptrA )
42{
43	ptrA->bbpE = 0;
44	ptrA->xxE = 0;
45	ptrA->xyE = 0;
46	ptrA->yxE = 0;
47	ptrA->yyE = 0;
48}
49
50/* ------------------------------------------------------------------------- */
51
52void bts_Flt16Mat2D_exit( struct bts_Flt16Mat2D* ptrA )
53{
54	ptrA->bbpE = 0;
55	ptrA->xxE = 0;
56	ptrA->xyE = 0;
57	ptrA->yxE = 0;
58	ptrA->yyE = 0;
59}
60
61/* ------------------------------------------------------------------------- */
62
63/* ========================================================================= */
64/*                                                                           */
65/* ---- \ghd{ operators } -------------------------------------------------- */
66/*                                                                           */
67/* ========================================================================= */
68
69/* ------------------------------------------------------------------------- */
70
71void bts_Flt16Mat2D_copy( struct bts_Flt16Mat2D* ptrA, const struct bts_Flt16Mat2D* srcPtrA )
72{
73	ptrA->bbpE = srcPtrA->bbpE;
74	ptrA->xxE = srcPtrA->xxE;
75	ptrA->xyE = srcPtrA->xyE;
76	ptrA->yxE = srcPtrA->yxE;
77	ptrA->yyE = srcPtrA->yyE;
78}
79
80/* ------------------------------------------------------------------------- */
81
82flag bts_Flt16Mat2D_equal( const struct bts_Flt16Mat2D* ptrA, const struct bts_Flt16Mat2D* srcPtrA )
83{
84	int32 bbpDiffL = ptrA->bbpE - srcPtrA->bbpE;
85	if( bbpDiffL == 0 )
86	{
87		if( ptrA->xxE != srcPtrA->xxE ) return FALSE;
88		if( ptrA->xyE != srcPtrA->xyE ) return FALSE;
89		if( ptrA->yxE != srcPtrA->yxE ) return FALSE;
90		if( ptrA->yyE != srcPtrA->yyE ) return FALSE;
91		return TRUE;
92	}
93
94	if( bbpDiffL > 0 )
95	{
96		int32 xxL = ( int32 ) srcPtrA->xxE << bbpDiffL;
97		int32 xyL = ( int32 ) srcPtrA->xyE << bbpDiffL;
98		int32 yxL = ( int32 ) srcPtrA->yxE << bbpDiffL;
99		int32 yyL = ( int32 ) srcPtrA->yyE << bbpDiffL;
100
101		if( ptrA->xxE != xxL ) return FALSE;
102		if( ptrA->xyE != xyL ) return FALSE;
103		if( ptrA->yxE != yxL ) return FALSE;
104		if( ptrA->yyE != yyL ) return FALSE;
105
106		/* check if bits were lost by the shifting */
107		if( srcPtrA->xxE != ( xxL >> bbpDiffL ) ) return FALSE;
108		if( srcPtrA->xyE != ( xyL >> bbpDiffL ) ) return FALSE;
109		if( srcPtrA->yxE != ( yxL >> bbpDiffL ) ) return FALSE;
110		if( srcPtrA->yyE != ( yyL >> bbpDiffL ) ) return FALSE;
111
112		return TRUE;
113	}
114
115	if( bbpDiffL < 0 )
116	{
117		int32 xxL = ( int32 ) ptrA->xxE << -bbpDiffL;
118		int32 xyL = ( int32 ) ptrA->xyE << -bbpDiffL;
119		int32 yxL = ( int32 ) ptrA->yxE << -bbpDiffL;
120		int32 yyL = ( int32 ) ptrA->yyE << -bbpDiffL;
121
122		if( xxL != srcPtrA->xxE ) return FALSE;
123		if( xyL != srcPtrA->xyE ) return FALSE;
124		if( yxL != srcPtrA->yxE ) return FALSE;
125		if( yyL != srcPtrA->yyE ) return FALSE;
126
127		/* check if bits were lost by the shifting */
128		if( ptrA->xxE != ( xxL >> -bbpDiffL ) ) return FALSE;
129		if( ptrA->xyE != ( xyL >> -bbpDiffL ) ) return FALSE;
130		if( ptrA->yxE != ( yxL >> -bbpDiffL ) ) return FALSE;
131		if( ptrA->yyE != ( yyL >> -bbpDiffL ) ) return FALSE;
132
133		return TRUE;
134	}
135
136	return TRUE;
137}
138
139/* ========================================================================= */
140/*                                                                           */
141/* ---- \ghd{ query functions } -------------------------------------------- */
142/*                                                                           */
143/* ========================================================================= */
144
145/* ------------------------------------------------------------------------- */
146
147/* ========================================================================= */
148/*                                                                           */
149/* ---- \ghd{ modify functions } ------------------------------------------- */
150/*                                                                           */
151/* ========================================================================= */
152
153/* ------------------------------------------------------------------------- */
154
155/* ========================================================================= */
156/*                                                                           */
157/* ---- \ghd{ I/O } -------------------------------------------------------- */
158/*                                                                           */
159/* ========================================================================= */
160
161/* ------------------------------------------------------------------------- */
162
163/* ========================================================================= */
164/*                                                                           */
165/* ---- \ghd{ exec functions } --------------------------------------------- */
166/*                                                                           */
167/* ========================================================================= */
168
169/* ------------------------------------------------------------------------- */
170
171uint32 bts_Flt16Mat2D_det( const struct bts_Flt16Mat2D* ptrA )
172{
173	/* This could be negativ, in theory. But almost always det > 0 for us,
174	   matrix is a rotation or scaling matrix.
175	   Then uint32 makes sure there is no overflow. */
176	uint32 detL = ( int32 ) ptrA->xxE * ptrA->yyE - ( int32 ) ptrA->xyE * ptrA->yxE;
177	return detL;
178}
179
180/* ------------------------------------------------------------------------- */
181
182struct bts_Flt16Mat2D bts_Flt16Mat2D_createIdentity()
183{
184	struct bts_Flt16Mat2D matL = { 1 << 14, 0, 0, 1 << 14, 14 };
185	return matL;
186}
187
188/* ------------------------------------------------------------------------- */
189
190struct bts_Flt16Mat2D bts_Flt16Mat2D_createRotation( phase16 angleA )
191{
192	int16 cL = bbs_cos16( angleA );
193	int16 sL = bbs_sin16( angleA );
194	struct bts_Flt16Mat2D matL;
195	matL.xxE =  cL;
196	matL.xyE = -sL;
197	matL.yxE =  sL;
198	matL.yyE =  cL;
199	matL.bbpE = 14;
200	return matL;
201}
202
203/* ------------------------------------------------------------------------- */
204
205struct bts_Flt16Mat2D bts_Flt16Mat2D_createScale( int32 scaleA, int32 scaleBbpA )
206{
207	struct bts_Flt16Mat2D matL = bts_Flt16Mat2D_createIdentity();
208	bts_Flt16Mat2D_scale( &matL, scaleA, scaleBbpA );
209	return matL;
210}
211
212/* ------------------------------------------------------------------------- */
213
214struct bts_Flt16Mat2D bts_Flt16Mat2D_createRigid( phase16 angleA, int32 scaleA, int32 scaleBbpA )
215{
216	struct bts_Flt16Mat2D matL = bts_Flt16Mat2D_createRotation( angleA );
217	bts_Flt16Mat2D_scale( &matL, scaleA, scaleBbpA );
218	return matL;
219}
220
221/* ------------------------------------------------------------------------- */
222
223struct bts_Flt16Mat2D bts_Flt16Mat2D_create16( int16 xxA, int16 xyA, int16 yxA, int16 yyA, int16 bbpA )
224{
225	struct bts_Flt16Mat2D matL;
226	matL.xxE = xxA;
227	matL.xyE = xyA;
228	matL.yxE = yxA;
229	matL.yyE = yyA;
230	matL.bbpE = bbpA;
231	return matL;
232}
233
234/* ------------------------------------------------------------------------- */
235
236struct bts_Flt16Mat2D bts_Flt16Mat2D_create32( int32 xxA, int32 xyA, int32 yxA, int32 yyA, int32 bbpA )
237{
238	struct bts_Flt16Mat2D matL;
239
240	if( ( xxA | xyA | yxA | yyA ) == 0 )
241	{
242		matL.xxE = 0;
243		matL.xyE = 0;
244		matL.yxE = 0;
245		matL.yyE = 0;
246		matL.bbpE = 0;
247	}
248	else
249	{
250		int32 shiftL = bts_maxAbsIntLog2Of4( xxA, xyA, yxA, yyA ) - 13;
251
252		if( shiftL > 0 )
253		{
254			int32 sh1L = shiftL - 1;
255			matL.xxE = ( ( xxA >> sh1L ) + 1 ) >> 1;
256			matL.xyE = ( ( xyA >> sh1L ) + 1 ) >> 1;
257			matL.yxE = ( ( yxA >> sh1L ) + 1 ) >> 1;
258			matL.yyE = ( ( yyA >> sh1L ) + 1 ) >> 1;
259		}
260		else
261		{
262			matL.xxE = xxA << -shiftL;
263			matL.xyE = xyA << -shiftL;
264			matL.yxE = yxA << -shiftL;
265			matL.yyE = yyA << -shiftL;
266		}
267
268		matL.bbpE = bbpA - shiftL;
269	}
270
271	return matL;
272}
273
274/* ------------------------------------------------------------------------- */
275
276void bts_Flt16Mat2D_scale( struct bts_Flt16Mat2D* ptrA, int32 scaleA, int32 scaleBbpA )
277{
278	/* fit scale in 15 bit */
279	uint32 scaleExpL = bts_absIntLog2( scaleA );
280	if( scaleExpL > 14 )
281	{
282		int32 shiftL = scaleExpL - 14;
283		scaleA = ( ( scaleA >> ( shiftL - 1 ) ) + 1 ) >> 1;
284		scaleBbpA -= shiftL;
285	}
286
287	*ptrA = bts_Flt16Mat2D_create32( (int32)ptrA->xxE * scaleA,
288									 (int32)ptrA->xyE * scaleA,
289									 (int32)ptrA->yxE * scaleA,
290									 (int32)ptrA->yyE * scaleA,
291									 ptrA->bbpE + scaleBbpA );
292}
293
294/* ------------------------------------------------------------------------- */
295
296struct bts_Int16Vec2D bts_Flt16Mat2D_map( const struct bts_Flt16Mat2D* matPtrA,
297								          const struct bts_Int16Vec2D* vecPtrA )
298{
299	struct bts_Int16Vec2D vecL;
300
301	int32 xL = ( int32 ) matPtrA->xxE * vecPtrA->xE + ( int32 ) matPtrA->xyE * vecPtrA->yE;
302	int32 yL = ( int32 ) matPtrA->yxE * vecPtrA->xE + ( int32 ) matPtrA->yyE * vecPtrA->yE;
303
304	if( matPtrA->bbpE > 0 )
305	{
306		int32 sh1L = matPtrA->bbpE - 1;
307		vecL.xE = ( ( xL >> sh1L ) + 1 ) >> 1;
308		vecL.yE = ( ( yL >> sh1L ) + 1 ) >> 1;
309	}
310	else
311	{
312		/* not overflow safe */
313		vecL.xE = xL << -matPtrA->bbpE;
314		vecL.yE = yL << -matPtrA->bbpE;
315	}
316
317	return vecL;
318}
319
320/* ------------------------------------------------------------------------- */
321
322struct bts_Flt16Vec2D bts_Flt16Mat2D_mapFlt( const struct bts_Flt16Mat2D* matPtrA,
323								             const struct bts_Flt16Vec2D* vecPtrA )
324{
325	int32 xL = ( int32 ) matPtrA->xxE * vecPtrA->xE + ( int32 ) matPtrA->xyE * vecPtrA->yE;
326	int32 yL = ( int32 ) matPtrA->yxE * vecPtrA->xE + ( int32 ) matPtrA->yyE * vecPtrA->yE;
327	int32 bbpL = matPtrA->bbpE + vecPtrA->bbpE;
328	return bts_Flt16Vec2D_create32( xL, yL, bbpL );
329}
330
331/* ------------------------------------------------------------------------- */
332
333struct bts_Flt16Mat2D bts_Flt16Mat2D_mul( const struct bts_Flt16Mat2D* mat1PtrA,
334								          const struct bts_Flt16Mat2D* mat2PtrA )
335{
336	return bts_Flt16Mat2D_create32( ( int32 ) mat1PtrA->xxE * mat2PtrA->xxE + ( int32 ) mat1PtrA->xyE * mat2PtrA->yxE,
337									( int32 ) mat1PtrA->xxE * mat2PtrA->xyE + ( int32 ) mat1PtrA->xyE * mat2PtrA->yyE,
338									( int32 ) mat1PtrA->yxE * mat2PtrA->xxE + ( int32 ) mat1PtrA->yyE * mat2PtrA->yxE,
339									( int32 ) mat1PtrA->yxE * mat2PtrA->xyE + ( int32 ) mat1PtrA->yyE * mat2PtrA->yyE,
340									mat1PtrA->bbpE + mat2PtrA->bbpE );
341}
342
343/* ------------------------------------------------------------------------- */
344
345struct bts_Flt16Mat2D* bts_Flt16Mat2D_mulTo( struct bts_Flt16Mat2D* mat1PtrA,
346				                             const struct bts_Flt16Mat2D* mat2PtrA )
347{
348	*mat1PtrA = bts_Flt16Mat2D_mul( mat1PtrA, mat2PtrA );
349	return mat1PtrA;
350}
351
352/* ------------------------------------------------------------------------- */
353
354void bts_Flt16Mat2D_invert( struct bts_Flt16Mat2D* ptrA )
355{
356	int32 detL = ( int32 ) ptrA->xxE * ptrA->yyE - ( int32 ) ptrA->xyE * ptrA->yxE;
357	int32 detExpL = bbs_intLog2( detL );
358	int32 dShrL = 0;
359	if( detExpL > 15 )
360	{
361		dShrL = detExpL - 15;
362		detL = ( ( detL >> ( dShrL - 1 ) ) + 1 ) >> 1;
363	}
364
365	if( detL == 0 )
366	{
367		ptrA->xxE = ptrA->yyE = ptrA->xyE = ptrA->yxE = 0;
368	}
369	else
370	{
371		/* bbp: bbpE + 16 - ( bbpE * 2 - dShrL ) = 16 + dShrL - bbpE */
372		int32 xxL = ( ( int32 )ptrA->xxE << 16 ) / detL;
373		int32 xyL = ( ( int32 )ptrA->xyE << 16 ) / detL;
374		int32 yxL = ( ( int32 )ptrA->yxE << 16 ) / detL;
375		int32 yyL = ( ( int32 )ptrA->yyE << 16 ) / detL;
376		*ptrA = bts_Flt16Mat2D_create32( xxL, -xyL, -yxL, yyL, 16 + dShrL - ptrA->bbpE );
377	}
378}
379
380/* ------------------------------------------------------------------------- */
381
382struct bts_Flt16Mat2D bts_Flt16Mat2D_inverted( const struct bts_Flt16Mat2D* ptrA )
383{
384	struct bts_Flt16Mat2D matL = *ptrA;
385	bts_Flt16Mat2D_invert( &matL );
386	return matL;
387}
388
389/* ------------------------------------------------------------------------- */
390
391/* ========================================================================= */
392
393
394