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
42void bts_Flt16Alt2D_init( struct bts_Flt16Alt2D* ptrA )
43{
44	bts_Flt16Mat2D_init( &ptrA->matE );
45	bts_Flt16Vec2D_init( &ptrA->vecE );
46}
47
48/* ------------------------------------------------------------------------- */
49
50void 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
68void 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
76flag 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
107uint32 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
116uint32 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
126uint32 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
145struct 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
153struct 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
164struct 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
176struct 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
189struct 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
226struct 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
243struct 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
260struct 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
268struct 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 */
280struct 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
289void 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
300struct 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