auto import from //depot/cupcake/@135843
[android/platform/external/neven.git] / Embedded / common / src / b_TensorEm / Flt16Mat2D.c
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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