auto import from //depot/cupcake/@135843
[android/platform/external/neven.git] / Embedded / common / src / b_TensorEm / Flt16Alt2D.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/Flt16Alt2D.h"
20 #include "b_BasicEm/Math.h"
21 #include "b_BasicEm/Memory.h"
22 #include "b_BasicEm/Functions.h"
23
24 /* ------------------------------------------------------------------------- */
25
26 /* ========================================================================= */
27 /*                                                                           */
28 /* ---- \ghd{ auxiliary functions } ---------------------------------------- */
29 /*                                                                           */
30 /* ========================================================================= */
31
32 /* ------------------------------------------------------------------------- */
33
34 /* ========================================================================= */
35 /*                                                                           */
36 /* ---- \ghd{ constructor / destructor } ----------------------------------- */
37 /*                                                                           */
38 /* ========================================================================= */
39
40 /* ------------------------------------------------------------------------- */
41
42 void bts_Flt16Alt2D_init( struct bts_Flt16Alt2D* ptrA )
43 {
44         bts_Flt16Mat2D_init( &ptrA->matE );
45         bts_Flt16Vec2D_init( &ptrA->vecE );
46 }
47
48 /* ------------------------------------------------------------------------- */
49
50 void bts_Flt16Alt2D_exit( struct bts_Flt16Alt2D* ptrA )
51 {
52         bts_Flt16Mat2D_exit( &ptrA->matE );
53         bts_Flt16Vec2D_exit( &ptrA->vecE );
54 }
55
56 /* ------------------------------------------------------------------------- */
57
58 /* ========================================================================= */
59 /*                                                                           */
60 /* ---- \ghd{ operators } -------------------------------------------------- */
61 /*                                                                           */
62 /* ========================================================================= */
63
64 /* ------------------------------------------------------------------------- */
65
66 /* ------------------------------------------------------------------------- */
67
68 void bts_Flt16Alt2D_copy( struct bts_Flt16Alt2D* ptrA, const struct bts_Flt16Alt2D* srcPtrA )
69 {
70         bts_Flt16Mat2D_copy( &ptrA->matE, &srcPtrA->matE );
71         bts_Flt16Vec2D_copy( &ptrA->vecE, &srcPtrA->vecE );
72 }
73
74 /* ------------------------------------------------------------------------- */
75
76 flag bts_Flt16Alt2D_equal( const struct bts_Flt16Alt2D* ptrA, const struct bts_Flt16Alt2D* srcPtrA )
77 {
78         if( ! bts_Flt16Mat2D_equal( &ptrA->matE, &srcPtrA->matE ) ) return FALSE;
79         if( ! bts_Flt16Vec2D_equal( &ptrA->vecE, &srcPtrA->vecE ) ) return FALSE;
80         return TRUE;
81 }
82
83 /* ========================================================================= */
84 /*                                                                           */
85 /* ---- \ghd{ query functions } -------------------------------------------- */
86 /*                                                                           */
87 /* ========================================================================= */
88
89 /* ------------------------------------------------------------------------- */
90
91 /* ========================================================================= */
92 /*                                                                           */
93 /* ---- \ghd{ modify functions } ------------------------------------------- */
94 /*                                                                           */
95 /* ========================================================================= */
96
97 /* ------------------------------------------------------------------------- */
98         
99 /* ========================================================================= */
100 /*                                                                           */
101 /* ---- \ghd{ I/O } -------------------------------------------------------- */
102 /*                                                                           */
103 /* ========================================================================= */
104
105 /* ------------------------------------------------------------------------- */
106         
107 uint32 bts_Flt16Alt2D_memSize( struct bbs_Context* cpA,
108                                                            const struct bts_Flt16Alt2D *ptrA )
109 {
110         bbs_ERROR0( "unimplemented function" );
111         return 0;
112 }
113
114 /* ------------------------------------------------------------------------- */
115         
116 uint32 bts_Flt16Alt2D_memWrite( struct bbs_Context* cpA,
117                                                             const struct bts_Flt16Alt2D* ptrA, 
118                                                                 uint16* memPtrA )
119 {
120         bbs_ERROR0( "unimplemented function" );
121         return 0;
122 }
123
124 /* ------------------------------------------------------------------------- */
125         
126 uint32 bts_Flt16Alt2D_memRead( struct bbs_Context* cpA,
127                                                            struct bts_Flt16Alt2D* ptrA, 
128                                                            const uint16* memPtrA )
129 {
130         if( bbs_Context_error( cpA ) ) return 0;
131         bbs_ERROR0( "unimplemented function" );
132         return 0;
133 }
134
135 /* ------------------------------------------------------------------------- */
136         
137 /* ========================================================================= */
138 /*                                                                           */
139 /* ---- \ghd{ exec functions } --------------------------------------------- */
140 /*                                                                           */
141 /* ========================================================================= */
142         
143 /* ------------------------------------------------------------------------- */
144
145 struct bts_Flt16Alt2D bts_Flt16Alt2D_createIdentity()
146 {
147         struct bts_Flt16Alt2D altL = { { 1, 0, 0, 1, 0 }, { 0, 0, 0 } };
148         return altL;
149 }
150
151 /* ------------------------------------------------------------------------- */
152
153 struct bts_Flt16Alt2D bts_Flt16Alt2D_createRotation( phase16 angleA, 
154                                                                                                          const struct bts_Flt16Vec2D* centerPtrA )
155 {
156         struct bts_Flt16Alt2D altL;
157         altL.matE = bts_Flt16Mat2D_createRotation( angleA );
158         altL.vecE = bts_Flt16Vec2D_sub( *centerPtrA, bts_Flt16Mat2D_mapFlt( &altL.matE, centerPtrA ) );
159         return altL;
160 }
161
162 /* ------------------------------------------------------------------------- */
163
164 struct bts_Flt16Alt2D bts_Flt16Alt2D_createScale( int32 scaleA, 
165                                                                                                   int32 scaleBbpA, 
166                                                                                                   const struct bts_Flt16Vec2D* centerPtrA )
167 {
168         struct bts_Flt16Alt2D altL;
169         altL.matE = bts_Flt16Mat2D_createScale( scaleA, scaleBbpA );
170         altL.vecE = bts_Flt16Vec2D_sub( *centerPtrA, bts_Flt16Mat2D_mapFlt( &altL.matE, centerPtrA ) );
171         return altL;
172 }
173
174 /* ------------------------------------------------------------------------- */
175
176 struct bts_Flt16Alt2D bts_Flt16Alt2D_createRigid( phase16 angleA, 
177                                                                                                   int32 scaleA, 
178                                                                                                   int32 scaleBbpA,
179                                                                                                   const struct bts_Flt16Vec2D* centerPtrA )
180 {
181         struct bts_Flt16Alt2D altL;
182         altL.matE = bts_Flt16Mat2D_createRigid( angleA, scaleA, scaleBbpA );
183         altL.vecE = bts_Flt16Vec2D_sub( *centerPtrA, bts_Flt16Mat2D_mapFlt( &altL.matE, centerPtrA ) );
184         return altL;
185 }
186
187 /* ------------------------------------------------------------------------- */
188
189 struct bts_Flt16Alt2D bts_Flt16Alt2D_createRigidMap( struct bts_Flt16Vec2D vecIn1A,
190                                                                                                      struct bts_Flt16Vec2D vecIn2A,
191                                                                                                      struct bts_Flt16Vec2D vecOut1A,
192                                                                                                      struct bts_Flt16Vec2D vecOut2A )
193 {
194         struct bts_Flt16Vec2D diffInL = bts_Flt16Vec2D_sub( vecIn1A, vecIn2A );
195         struct bts_Flt16Vec2D diffOutL = bts_Flt16Vec2D_sub( vecOut1A, vecOut2A );
196         struct bts_Flt16Vec2D centerInL = bts_Flt16Vec2D_mul( bts_Flt16Vec2D_add( vecIn1A, vecIn2A ), 1, 1 ); /* mul by 0.5 */ 
197         struct bts_Flt16Vec2D centerOutL = bts_Flt16Vec2D_mul( bts_Flt16Vec2D_add( vecOut1A, vecOut2A ), 1, 1 ); /* mul by 0.5 */ 
198
199         struct bts_Flt16Vec2D transL = bts_Flt16Vec2D_sub( centerOutL, centerInL );
200         phase16 angleL = bts_Flt16Vec2D_enclosedAngle( &diffOutL, &diffInL );
201         uint32 normInL = bts_Flt16Vec2D_norm( &diffInL );
202         uint32 normOutL = bts_Flt16Vec2D_norm( &diffOutL );
203
204         uint32 scaleL = ( normInL > 0 ) ? ( normOutL << 16 ) / normInL : 0xFFFFFFFF;
205         uint32 scaleBbpL = 16 + diffOutL.bbpE - diffInL.bbpE;
206
207         struct bts_Flt16Alt2D altL;
208
209         /* fit scale factor in 15 bit */
210         uint32 scaleExpL = bbs_intLog2( scaleL );
211         if( scaleExpL > 14 )
212         {
213                 scaleL >>= scaleExpL - 14;
214                 scaleBbpL -= scaleExpL - 14;
215         }       
216
217         altL = bts_Flt16Alt2D_createRigid( angleL, ( int16 )scaleL, scaleBbpL, &centerInL );
218
219         altL.vecE = bts_Flt16Vec2D_add( altL.vecE, transL );
220
221         return altL;
222 }
223
224 /* ------------------------------------------------------------------------- */
225
226 struct bts_Flt16Alt2D bts_Flt16Alt2D_create16( int16 xxA, 
227                                                                                            int16 xyA, 
228                                                                                            int16 yxA, 
229                                                                                            int16 yyA, 
230                                                                                            int16 matBbpA,
231                                                                                            int16 xA, 
232                                                                                            int16 yA, 
233                                                                                            int16 vecBbpA )
234 {
235         struct bts_Flt16Alt2D altL;
236         altL.matE = bts_Flt16Mat2D_create16( xxA, xyA, yxA, yyA, matBbpA );
237         altL.vecE = bts_Flt16Vec2D_create16( xA, yA, vecBbpA );
238         return altL;
239 }
240
241 /* ------------------------------------------------------------------------- */
242
243 struct bts_Flt16Alt2D bts_Flt16Alt2D_create32( int32 xxA, 
244                                                                                            int32 xyA, 
245                                                                                            int32 yxA, 
246                                                                                            int32 yyA, 
247                                                                                            int32 matBbpA,
248                                                                                            int32 xA, 
249                                                                                            int32 yA, 
250                                                                                            int32 vecBbpA )
251 {
252         struct bts_Flt16Alt2D altL;
253         altL.matE = bts_Flt16Mat2D_create32( xxA, xyA, yxA, yyA, matBbpA );
254         altL.vecE = bts_Flt16Vec2D_create32( xA, yA, vecBbpA );
255         return altL;
256 }
257
258 /* ------------------------------------------------------------------------- */
259
260 struct bts_Flt16Vec2D bts_Flt16Alt2D_mapFlt( const struct bts_Flt16Alt2D* altPtrA, 
261                                                                              const struct bts_Flt16Vec2D* vecPtrA )
262 {
263         return bts_Flt16Vec2D_add( altPtrA->vecE, bts_Flt16Mat2D_mapFlt( &altPtrA->matE, vecPtrA ) );
264 }
265
266 /* ------------------------------------------------------------------------- */
267
268 struct bts_Flt16Alt2D bts_Flt16Alt2D_mul( const struct bts_Flt16Alt2D* alt1PtrA, 
269                                                                           const struct bts_Flt16Alt2D* alt2PtrA )
270 {
271         struct bts_Flt16Alt2D altL;
272         altL.vecE = bts_Flt16Alt2D_mapFlt( alt1PtrA, &alt2PtrA->vecE );
273         altL.matE = bts_Flt16Mat2D_mul( &alt1PtrA->matE, &alt2PtrA->matE );
274         return altL;
275 }
276
277 /* ------------------------------------------------------------------------- */
278
279 /** multiplies matrix with matA; returns pointer to resulting matrix */
280 struct bts_Flt16Alt2D* bts_Flt16Alt2D_mulTo( struct bts_Flt16Alt2D* alt1PtrA, 
281                                                              const struct bts_Flt16Alt2D* alt2PtrA )
282 {
283         *alt1PtrA = bts_Flt16Alt2D_mul( alt1PtrA, alt2PtrA );
284         return alt1PtrA;
285 }
286
287 /* ------------------------------------------------------------------------- */
288
289 void bts_Flt16Alt2D_invert( struct bts_Flt16Alt2D* ptrA )
290 {
291         bts_Flt16Mat2D_invert( &ptrA->matE );
292         
293         ptrA->vecE = bts_Flt16Vec2D_create32( - ( ( ( int32 ) ptrA->matE.xxE * ptrA->vecE.xE + ( int32 ) ptrA->matE.xyE * ptrA->vecE.yE ) ),
294                                                                                   - ( ( ( int32 ) ptrA->matE.yxE * ptrA->vecE.xE + ( int32 ) ptrA->matE.yyE * ptrA->vecE.yE ) ),
295                                                                                         ptrA->vecE.bbpE + ptrA->matE.bbpE );
296 }
297
298 /* ------------------------------------------------------------------------- */
299
300 struct bts_Flt16Alt2D bts_Flt16Alt2D_inverted( const struct bts_Flt16Alt2D* ptrA )
301 {
302         struct bts_Flt16Alt2D altL = *ptrA;
303         bts_Flt16Alt2D_invert( &altL );
304         return altL;
305 }
306
307 /* ------------------------------------------------------------------------- */
308
309 /* ========================================================================= */
310
311