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
bts_Flt16Alt2D_init(struct bts_Flt16Alt2D * ptrA)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
bts_Flt16Alt2D_exit(struct bts_Flt16Alt2D * ptrA)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
bts_Flt16Alt2D_copy(struct bts_Flt16Alt2D * ptrA,const struct bts_Flt16Alt2D * srcPtrA)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
bts_Flt16Alt2D_equal(const struct bts_Flt16Alt2D * ptrA,const struct bts_Flt16Alt2D * srcPtrA)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
bts_Flt16Alt2D_memSize(struct bbs_Context * cpA,const struct bts_Flt16Alt2D * ptrA)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
bts_Flt16Alt2D_memWrite(struct bbs_Context * cpA,const struct bts_Flt16Alt2D * ptrA,uint16 * memPtrA)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
bts_Flt16Alt2D_memRead(struct bbs_Context * cpA,struct bts_Flt16Alt2D * ptrA,const uint16 * memPtrA)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
bts_Flt16Alt2D_createIdentity()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
bts_Flt16Alt2D_createRotation(phase16 angleA,const struct bts_Flt16Vec2D * centerPtrA)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
bts_Flt16Alt2D_createScale(int32 scaleA,int32 scaleBbpA,const struct bts_Flt16Vec2D * centerPtrA)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
bts_Flt16Alt2D_createRigid(phase16 angleA,int32 scaleA,int32 scaleBbpA,const struct bts_Flt16Vec2D * centerPtrA)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
bts_Flt16Alt2D_createRigidMap(struct bts_Flt16Vec2D vecIn1A,struct bts_Flt16Vec2D vecIn2A,struct bts_Flt16Vec2D vecOut1A,struct bts_Flt16Vec2D vecOut2A)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, ¢erInL );
218
219 altL.vecE = bts_Flt16Vec2D_add( altL.vecE, transL );
220
221 return altL;
222 }
223
224 /* ------------------------------------------------------------------------- */
225
bts_Flt16Alt2D_create16(int16 xxA,int16 xyA,int16 yxA,int16 yyA,int16 matBbpA,int16 xA,int16 yA,int16 vecBbpA)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
bts_Flt16Alt2D_create32(int32 xxA,int32 xyA,int32 yxA,int32 yyA,int32 matBbpA,int32 xA,int32 yA,int32 vecBbpA)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
bts_Flt16Alt2D_mapFlt(const struct bts_Flt16Alt2D * altPtrA,const struct bts_Flt16Vec2D * vecPtrA)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
bts_Flt16Alt2D_mul(const struct bts_Flt16Alt2D * alt1PtrA,const struct bts_Flt16Alt2D * alt2PtrA)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 */
bts_Flt16Alt2D_mulTo(struct bts_Flt16Alt2D * alt1PtrA,const struct bts_Flt16Alt2D * alt2PtrA)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
bts_Flt16Alt2D_invert(struct bts_Flt16Alt2D * ptrA)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
bts_Flt16Alt2D_inverted(const struct bts_Flt16Alt2D * ptrA)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
312