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/Flt16Vec2D.h"
20#include "b_TensorEm/Functions.h"
21#include "b_BasicEm/Math.h"
22#include "b_BasicEm/Memory.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_Flt16Vec2D_init( struct bts_Flt16Vec2D* ptrA )
43{
44	ptrA->xE = 0;
45	ptrA->yE = 0;
46	ptrA->bbpE = 0;
47}
48
49/* ------------------------------------------------------------------------- */
50
51void bts_Flt16Vec2D_exit( struct bts_Flt16Vec2D* ptrA )
52{
53	ptrA->xE = 0;
54	ptrA->yE = 0;
55	ptrA->bbpE = 0;
56}
57
58/* ------------------------------------------------------------------------- */
59
60/* ========================================================================= */
61/*                                                                           */
62/* ---- \ghd{ operators } -------------------------------------------------- */
63/*                                                                           */
64/* ========================================================================= */
65
66/* ------------------------------------------------------------------------- */
67
68void bts_Flt16Vec2D_copy( struct bts_Flt16Vec2D* ptrA, const struct bts_Flt16Vec2D* srcPtrA )
69{
70	ptrA->bbpE = srcPtrA->bbpE;
71	ptrA->xE = srcPtrA->xE;
72	ptrA->yE = srcPtrA->yE;
73}
74
75/* ------------------------------------------------------------------------- */
76
77flag bts_Flt16Vec2D_equal( const struct bts_Flt16Vec2D* ptrA, const struct bts_Flt16Vec2D* srcPtrA )
78{
79	int32 bbpDiffL = ptrA->bbpE - srcPtrA->bbpE;
80	if( bbpDiffL == 0 )
81	{
82		if( ptrA->xE != srcPtrA->xE ) return FALSE;
83		if( ptrA->yE != srcPtrA->yE ) return FALSE;
84		return TRUE;
85	}
86
87	if( bbpDiffL > 0 )
88	{
89		int32 xL = ( int32 ) srcPtrA->xE << bbpDiffL;
90		int32 yL = ( int32 ) srcPtrA->yE << bbpDiffL;
91		if( ptrA->xE != xL ) return FALSE;
92		if( ptrA->yE != yL ) return FALSE;
93		/* check if bits were lost by the shifting */
94		if( srcPtrA->xE != ( xL >> bbpDiffL ) ) return FALSE;
95		if( srcPtrA->yE != ( yL >> bbpDiffL ) ) return FALSE;
96		return TRUE;
97	}
98
99	if( bbpDiffL < 0 )
100	{
101		int32 xL = ( int32 ) ptrA->xE << -bbpDiffL;
102		int32 yL = ( int32 ) ptrA->yE << -bbpDiffL;
103		if( xL != srcPtrA->xE ) return FALSE;
104		if( yL != srcPtrA->yE ) return FALSE;
105		/* check if bits were lost by the shifting */
106		if( ptrA->xE != ( xL >> -bbpDiffL ) ) return FALSE;
107		if( ptrA->yE != ( yL >> -bbpDiffL ) ) return FALSE;
108		return TRUE;
109	}
110
111	return TRUE;
112}
113
114/* ------------------------------------------------------------------------- */
115
116/* ========================================================================= */
117/*                                                                           */
118/* ---- \ghd{ query functions } -------------------------------------------- */
119/*                                                                           */
120/* ========================================================================= */
121
122/* ------------------------------------------------------------------------- */
123
124/* ========================================================================= */
125/*                                                                           */
126/* ---- \ghd{ modify functions } ------------------------------------------- */
127/*                                                                           */
128/* ========================================================================= */
129
130/* ------------------------------------------------------------------------- */
131
132/* ========================================================================= */
133/*                                                                           */
134/* ---- \ghd{ I/O } -------------------------------------------------------- */
135/*                                                                           */
136/* ========================================================================= */
137
138/* ------------------------------------------------------------------------- */
139
140/* ========================================================================= */
141/*                                                                           */
142/* ---- \ghd{ exec functions } --------------------------------------------- */
143/*                                                                           */
144/* ========================================================================= */
145
146/* ------------------------------------------------------------------------- */
147
148struct bts_Flt16Vec2D bts_Flt16Vec2D_create16( int16 xA, int16 yA, int16 bbpA )
149{
150	struct bts_Flt16Vec2D vecL;
151	vecL.xE = xA;
152	vecL.yE = yA;
153	vecL.bbpE = bbpA;
154	return vecL;
155}
156
157/* ------------------------------------------------------------------------- */
158
159struct bts_Flt16Vec2D bts_Flt16Vec2D_createVec16( struct bts_Int16Vec2D vecA, int16 bbpA )
160{
161	struct bts_Flt16Vec2D vecL;
162	vecL.xE = vecA.xE;
163	vecL.yE = vecA.yE;
164	vecL.bbpE = bbpA;
165	return vecL;
166}
167
168/* ------------------------------------------------------------------------- */
169
170struct bts_Flt16Vec2D bts_Flt16Vec2D_create32( int32 xA, int32 yA, int32 bbpA )
171{
172	struct bts_Flt16Vec2D vecL;
173	if( ( xA | yA ) == 0 )
174	{
175		vecL.xE = 0;
176		vecL.yE = 0;
177		vecL.bbpE = 0;
178	}
179	else
180	{
181		int32 shiftL = bts_maxAbsIntLog2Of2( xA, yA ) - 13;
182
183		if( shiftL > 0 )
184		{
185			int32 sh1L = shiftL - 1;
186			vecL.xE = ( ( xA >> sh1L ) + 1 ) >> 1;
187			vecL.yE = ( ( yA >> sh1L ) + 1 ) >> 1;
188		}
189		else
190		{
191			vecL.xE = xA << -shiftL;
192			vecL.yE = yA << -shiftL;
193		}
194		vecL.bbpE = bbpA - shiftL;
195	}
196	return vecL;
197}
198
199/* ------------------------------------------------------------------------- */
200
201int32 bts_Flt16Vec2D_dotPrd( const struct bts_Flt16Vec2D* vec1PtrA,
202							 const struct bts_Flt16Vec2D* vec2PtrA )
203{
204	return ( int32 ) vec1PtrA->xE * vec2PtrA->xE + ( int32 ) vec1PtrA->yE * vec2PtrA->yE;
205}
206
207/* ------------------------------------------------------------------------- */
208
209uint32 bts_Flt16Vec2D_norm2( const struct bts_Flt16Vec2D* ptrA )
210{
211	return ( int32 ) ptrA->xE * ptrA->xE + ( int32 ) ptrA->yE * ptrA->yE;
212}
213
214/* ------------------------------------------------------------------------- */
215
216uint16 bts_Flt16Vec2D_norm( const struct bts_Flt16Vec2D* ptrA )
217{
218	return bbs_sqrt32( ( int32 ) ptrA->xE * ptrA->xE + ( int32 ) ptrA->yE * ptrA->yE );
219}
220
221/* ------------------------------------------------------------------------- */
222
223void bts_Flt16Vec2D_normalize( struct bts_Flt16Vec2D* ptrA )
224{
225	int32 normL = bbs_sqrt32( ( int32 ) ptrA->xE * ptrA->xE + ( int32 ) ptrA->yE * ptrA->yE );
226	int32 xL = ( ( int32 ) ptrA->xE << 16 ) / normL;
227	int32 yL = ( ( int32 ) ptrA->yE << 16 ) / normL;
228	*ptrA = bts_Flt16Vec2D_create32( xL, yL, 16 );
229}
230
231/* ------------------------------------------------------------------------- */
232
233struct bts_Flt16Vec2D bts_Flt16Vec2D_normalized( const struct bts_Flt16Vec2D* ptrA )
234{
235	struct bts_Flt16Vec2D vecL = *ptrA;
236	bts_Flt16Vec2D_normalize( &vecL );
237	return vecL;
238}
239
240/* ------------------------------------------------------------------------- */
241
242phase16 bts_Flt16Vec2D_angle( const struct bts_Flt16Vec2D* vecPtrA )
243{
244	return bbs_phase16( vecPtrA->xE, vecPtrA->yE );
245}
246
247/* ------------------------------------------------------------------------- */
248
249phase16 bts_Flt16Vec2D_enclosedAngle( const struct bts_Flt16Vec2D* vec1PtrA,
250									  const struct bts_Flt16Vec2D* vec2PtrA )
251{
252	int32 xL = ( int32 ) vec1PtrA->xE * vec2PtrA->xE + ( int32 ) vec1PtrA->yE * vec2PtrA->yE;
253	int32 yL = ( int32 ) vec1PtrA->yE * vec2PtrA->xE - ( int32 ) vec1PtrA->xE * vec2PtrA->yE;
254	return bbs_phase16( xL, yL );
255}
256
257/* ------------------------------------------------------------------------- */
258
259struct bts_Flt16Vec2D bts_Flt16Vec2D_add( struct bts_Flt16Vec2D vec1A, struct bts_Flt16Vec2D vec2A )
260{
261	int32 xL, yL, bbpL;
262	int32 shiftL = vec1A.bbpE - vec2A.bbpE;
263
264	if( shiftL > 0 )
265	{
266		xL = ( ( int32 ) vec2A.xE << shiftL ) + vec1A.xE;
267		yL = ( ( int32 ) vec2A.yE << shiftL ) + vec1A.yE;
268		bbpL = vec1A.bbpE;
269	}
270	else
271	{
272		xL = ( ( int32 ) vec1A.xE << -shiftL ) + vec2A.xE;
273		yL = ( ( int32 ) vec1A.yE << -shiftL ) + vec2A.yE;
274		bbpL = vec2A.bbpE;
275	}
276
277	return bts_Flt16Vec2D_create32( xL, yL, bbpL );
278}
279
280/* ------------------------------------------------------------------------- */
281
282struct bts_Flt16Vec2D bts_Flt16Vec2D_sub( struct bts_Flt16Vec2D vec1A, struct bts_Flt16Vec2D vec2A )
283{
284	int32 xL, yL, bbpL;
285	int32 shiftL = vec1A.bbpE - vec2A.bbpE;
286
287	if( shiftL > 0 )
288	{
289		xL = ( int32 ) vec1A.xE - ( ( int32 ) vec2A.xE << shiftL );
290		yL = ( int32 ) vec1A.yE - ( ( int32 ) vec2A.yE << shiftL );
291		bbpL = vec1A.bbpE;
292	}
293	else
294	{
295		xL = ( ( int32 ) vec1A.xE << -shiftL ) - vec2A.xE;
296		yL = ( ( int32 ) vec1A.yE << -shiftL ) - vec2A.yE;
297		bbpL = vec2A.bbpE;
298	}
299
300	return bts_Flt16Vec2D_create32( xL, yL, bbpL );
301}
302
303/* ------------------------------------------------------------------------- */
304
305struct bts_Flt16Vec2D bts_Flt16Vec2D_mul( struct bts_Flt16Vec2D vecA, int16 factorA, int32 bbpFactorA )
306{
307	int32 xL = ( int32 ) vecA.xE * factorA;
308	int32 yL = ( int32 ) vecA.yE * factorA;
309	return bts_Flt16Vec2D_create32( xL, yL, bbpFactorA + vecA.bbpE );
310}
311
312/* ------------------------------------------------------------------------- */
313
314struct bts_Int16Vec2D bts_Flt16Vec2D_int16Vec2D( struct bts_Flt16Vec2D vecA, int32 dstBbpA )
315{
316	struct bts_Int16Vec2D vecL;
317	int32 shiftL = vecA.bbpE - dstBbpA;
318
319	if( shiftL > 0 )
320	{
321		vecL.xE = ( ( vecA.xE >> ( shiftL - 1 ) ) + 1 ) >> 1;
322		vecL.yE = ( ( vecA.yE >> ( shiftL - 1 ) ) + 1 ) >> 1;
323	}
324	else
325	{
326		vecL.xE = vecA.xE << ( -shiftL );
327		vecL.yE = vecA.yE << ( -shiftL );
328	}
329
330	return vecL;
331}
332
333/* ========================================================================= */
334
335
336