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
bts_Flt16Mat2D_init(struct bts_Flt16Mat2D * ptrA)41 void 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
bts_Flt16Mat2D_exit(struct bts_Flt16Mat2D * ptrA)52 void 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
bts_Flt16Mat2D_copy(struct bts_Flt16Mat2D * ptrA,const struct bts_Flt16Mat2D * srcPtrA)71 void 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
bts_Flt16Mat2D_equal(const struct bts_Flt16Mat2D * ptrA,const struct bts_Flt16Mat2D * srcPtrA)82 flag 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
bts_Flt16Mat2D_det(const struct bts_Flt16Mat2D * ptrA)171 uint32 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
bts_Flt16Mat2D_createIdentity()182 struct 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
bts_Flt16Mat2D_createRotation(phase16 angleA)190 struct 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
bts_Flt16Mat2D_createScale(int32 scaleA,int32 scaleBbpA)205 struct 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
bts_Flt16Mat2D_createRigid(phase16 angleA,int32 scaleA,int32 scaleBbpA)214 struct 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
bts_Flt16Mat2D_create16(int16 xxA,int16 xyA,int16 yxA,int16 yyA,int16 bbpA)223 struct 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
bts_Flt16Mat2D_create32(int32 xxA,int32 xyA,int32 yxA,int32 yyA,int32 bbpA)236 struct 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
bts_Flt16Mat2D_scale(struct bts_Flt16Mat2D * ptrA,int32 scaleA,int32 scaleBbpA)276 void 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
bts_Flt16Mat2D_map(const struct bts_Flt16Mat2D * matPtrA,const struct bts_Int16Vec2D * vecPtrA)296 struct 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
bts_Flt16Mat2D_mapFlt(const struct bts_Flt16Mat2D * matPtrA,const struct bts_Flt16Vec2D * vecPtrA)322 struct 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
bts_Flt16Mat2D_mul(const struct bts_Flt16Mat2D * mat1PtrA,const struct bts_Flt16Mat2D * mat2PtrA)333 struct 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
bts_Flt16Mat2D_mulTo(struct bts_Flt16Mat2D * mat1PtrA,const struct bts_Flt16Mat2D * mat2PtrA)345 struct 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
bts_Flt16Mat2D_invert(struct bts_Flt16Mat2D * ptrA)354 void 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
bts_Flt16Mat2D_inverted(const struct bts_Flt16Mat2D * ptrA)382 struct 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