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/Flt16Vec3D.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 
bts_Flt16Vec3D_init(struct bts_Flt16Vec3D * ptrA)42 void bts_Flt16Vec3D_init( struct bts_Flt16Vec3D* ptrA )
43 {
44 	ptrA->xE = 0;
45 	ptrA->yE = 0;
46 	ptrA->zE = 0;
47 	ptrA->bbpE = 0;
48 }
49 
50 /* ------------------------------------------------------------------------- */
51 
bts_Flt16Vec3D_exit(struct bts_Flt16Vec3D * ptrA)52 void bts_Flt16Vec3D_exit( struct bts_Flt16Vec3D* ptrA )
53 {
54 	ptrA->xE = 0;
55 	ptrA->yE = 0;
56 	ptrA->zE = 0;
57 	ptrA->bbpE = 0;
58 }
59 
60 /* ------------------------------------------------------------------------- */
61 
62 /* ========================================================================= */
63 /*                                                                           */
64 /* ---- \ghd{ operators } -------------------------------------------------- */
65 /*                                                                           */
66 /* ========================================================================= */
67 
68 /* ------------------------------------------------------------------------- */
69 
bts_Flt16Vec3D_equal(const struct bts_Flt16Vec3D * ptrA,const struct bts_Flt16Vec3D * srcPtrA)70 flag bts_Flt16Vec3D_equal( const struct bts_Flt16Vec3D* ptrA, const struct bts_Flt16Vec3D* srcPtrA )
71 {
72 	int32 bbpDiffL = ptrA->bbpE - srcPtrA->bbpE;
73 	if( bbpDiffL == 0 )
74 	{
75 		if( ptrA->xE != srcPtrA->xE ) return FALSE;
76 		if( ptrA->yE != srcPtrA->yE ) return FALSE;
77 		if( ptrA->zE != srcPtrA->zE ) return FALSE;
78 		return TRUE;
79 	}
80 
81 	if( bbpDiffL > 0 )
82 	{
83 		int32 xL = ( int32 ) srcPtrA->xE << bbpDiffL;
84 		int32 yL = ( int32 ) srcPtrA->yE << bbpDiffL;
85 		int32 zL = ( int32 ) srcPtrA->zE << bbpDiffL;
86 		if( ptrA->xE != xL ) return FALSE;
87 		if( ptrA->yE != yL ) return FALSE;
88 		if( ptrA->zE != zL ) return FALSE;
89 		return TRUE;
90 	}
91 
92 	if( bbpDiffL < 0 )
93 	{
94 		int32 xL = ( int32 ) ptrA->xE << -bbpDiffL;
95 		int32 yL = ( int32 ) ptrA->yE << -bbpDiffL;
96 		int32 zL = ( int32 ) ptrA->zE << -bbpDiffL;
97 		if( xL != srcPtrA->xE ) return FALSE;
98 		if( yL != srcPtrA->yE ) return FALSE;
99 		if( zL != srcPtrA->zE ) return FALSE;
100 		return TRUE;
101 	}
102 
103 	return TRUE;
104 }
105 
106 /* ------------------------------------------------------------------------- */
107 
108 /* ========================================================================= */
109 /*                                                                           */
110 /* ---- \ghd{ query functions } -------------------------------------------- */
111 /*                                                                           */
112 /* ========================================================================= */
113 
114 /* ------------------------------------------------------------------------- */
115 
116 /* ========================================================================= */
117 /*                                                                           */
118 /* ---- \ghd{ modify functions } ------------------------------------------- */
119 /*                                                                           */
120 /* ========================================================================= */
121 
122 /* ------------------------------------------------------------------------- */
123 
124 /* ========================================================================= */
125 /*                                                                           */
126 /* ---- \ghd{ I/O } -------------------------------------------------------- */
127 /*                                                                           */
128 /* ========================================================================= */
129 
130 /* ------------------------------------------------------------------------- */
131 
bts_Flt16Vec3D_memSize(struct bbs_Context * cpA,const struct bts_Flt16Vec3D * ptrA)132 uint32 bts_Flt16Vec3D_memSize( struct bbs_Context* cpA,
133 							   const struct bts_Flt16Vec3D *ptrA )
134 {
135 	return bbs_SIZEOF16( *ptrA );
136 }
137 
138 /* ------------------------------------------------------------------------- */
139 
bts_Flt16Vec3D_memWrite(struct bbs_Context * cpA,const struct bts_Flt16Vec3D * ptrA,uint16 * memPtrA)140 uint32 bts_Flt16Vec3D_memWrite( struct bbs_Context* cpA,
141 							    const struct bts_Flt16Vec3D* ptrA,
142 								uint16* memPtrA )
143 {
144 	bbs_ERROR0( "not implemented" );
145 	return 0;
146 }
147 
148 /* ------------------------------------------------------------------------- */
149 
bts_Flt16Vec3D_memRead(struct bbs_Context * cpA,struct bts_Flt16Vec3D * ptrA,const uint16 * memPtrA)150 uint32 bts_Flt16Vec3D_memRead( struct bbs_Context* cpA,
151 							   struct bts_Flt16Vec3D* ptrA,
152 							   const uint16* memPtrA )
153 {
154 	if( bbs_Context_error( cpA ) ) return 0;
155 	bbs_ERROR0( "not implemented" );
156 	return 0;
157 }
158 
159 /* ------------------------------------------------------------------------- */
160 
161 /* ========================================================================= */
162 /*                                                                           */
163 /* ---- \ghd{ exec functions } --------------------------------------------- */
164 /*                                                                           */
165 /* ========================================================================= */
166 
167 /* ------------------------------------------------------------------------- */
168 
bts_Flt16Vec3D_create16(int16 xA,int16 yA,int16 zA,int16 bbpA)169 struct bts_Flt16Vec3D bts_Flt16Vec3D_create16( int16 xA, int16 yA, int16 zA, int16 bbpA )
170 {
171 	struct bts_Flt16Vec3D vecL;
172 	vecL.xE = xA;
173 	vecL.yE = yA;
174 	vecL.zE = zA;
175 	vecL.bbpE = bbpA;
176 	return vecL;
177 }
178 
179 /* ------------------------------------------------------------------------- */
180 
bts_Flt16Vec3D_create32(int32 xA,int32 yA,int32 zA,int32 bbpA)181 struct bts_Flt16Vec3D bts_Flt16Vec3D_create32( int32 xA, int32 yA, int32 zA, int32 bbpA )
182 {
183 	struct bts_Flt16Vec3D vecL;
184 	if( ( xA | yA | zA ) == 0 )
185 	{
186 		vecL.xE = 0;
187 		vecL.yE = 0;
188 		vecL.zE = 0;
189 		vecL.bbpE = 0;
190 	}
191 	else
192 	{
193 		int32 shiftL = bts_maxAbsIntLog2Of3( xA, yA, zA ) - 13;
194 
195 		if( shiftL > 0 )
196 		{
197 			int32 sh1L = shiftL - 1;
198 			vecL.xE = ( ( xA >> sh1L ) + 1 ) >> 1;
199 			vecL.yE = ( ( yA >> sh1L ) + 1 ) >> 1;
200 			vecL.zE = ( ( zA >> sh1L ) + 1 ) >> 1;
201 		}
202 		else
203 		{
204 			vecL.xE = xA << -shiftL;
205 			vecL.yE = yA << -shiftL;
206 			vecL.zE = zA << -shiftL;
207 		}
208 		vecL.bbpE = bbpA - shiftL;
209 	}
210 	return vecL;
211 }
212 
213 /* ------------------------------------------------------------------------- */
214 
bts_Flt16Vec3D_norm2(const struct bts_Flt16Vec3D * ptrA)215 uint32 bts_Flt16Vec3D_norm2( const struct bts_Flt16Vec3D* ptrA )
216 {
217 	return ( int32 ) ptrA->xE * ptrA->xE +
218 		   ( int32 ) ptrA->yE * ptrA->yE +
219 		   ( int32 ) ptrA->zE * ptrA->zE;
220 }
221 
222 /* ------------------------------------------------------------------------- */
223 
bts_Flt16Vec3D_norm(const struct bts_Flt16Vec3D * ptrA)224 uint16 bts_Flt16Vec3D_norm( const struct bts_Flt16Vec3D* ptrA )
225 {
226 	return bbs_sqrt32( ( int32 ) ptrA->xE * ptrA->xE +
227 					   ( int32 ) ptrA->yE * ptrA->yE +
228 					   ( int32 ) ptrA->zE * ptrA->zE );
229 }
230 
231 /* ------------------------------------------------------------------------- */
232 
bts_Flt16Vec3D_normalize(struct bts_Flt16Vec3D * ptrA)233 void bts_Flt16Vec3D_normalize( struct bts_Flt16Vec3D* ptrA )
234 {
235 	int32 normL = bbs_sqrt32( ( int32 ) ptrA->xE * ptrA->xE +
236 							  ( int32 ) ptrA->yE * ptrA->yE +
237 							  ( int32 ) ptrA->zE * ptrA->zE );
238 
239 	int32 xL = ( ( int32 ) ptrA->xE << 16 ) / normL;
240 	int32 yL = ( ( int32 ) ptrA->yE << 16 ) / normL;
241 	int32 zL = ( ( int32 ) ptrA->zE << 16 ) / normL;
242 	*ptrA = bts_Flt16Vec3D_create32( xL, yL, zL, 16 );
243 }
244 
245 /* ------------------------------------------------------------------------- */
246 
bts_Flt16Vec3D_normalized(const struct bts_Flt16Vec3D * ptrA)247 struct bts_Flt16Vec3D bts_Flt16Vec3D_normalized( const struct bts_Flt16Vec3D* ptrA )
248 {
249 	struct bts_Flt16Vec3D vecL = *ptrA;
250 	bts_Flt16Vec3D_normalize( &vecL );
251 	return vecL;
252 }
253 
254 /* ------------------------------------------------------------------------- */
255 
bts_Flt16Vec3D_add(struct bts_Flt16Vec3D vec1A,struct bts_Flt16Vec3D vec2A)256 struct bts_Flt16Vec3D bts_Flt16Vec3D_add( struct bts_Flt16Vec3D vec1A, struct bts_Flt16Vec3D vec2A )
257 {
258 	int32 xL, yL, zL, bbpL;
259 	int32 shiftL = vec1A.bbpE - vec2A.bbpE;
260 
261 	if( shiftL > 0 )
262 	{
263 		xL = vec1A.xE + ( ( int32 ) vec2A.xE << shiftL );
264 		yL = vec1A.yE + ( ( int32 ) vec2A.yE << shiftL );
265 		zL = vec1A.zE + ( ( int32 ) vec2A.zE << shiftL );
266 		bbpL = vec1A.bbpE;
267 	}
268 	else
269 	{
270 		xL = ( ( int32 ) vec1A.xE << -shiftL ) + vec2A.xE;
271 		yL = ( ( int32 ) vec1A.yE << -shiftL ) + vec2A.yE;
272 		zL = ( ( int32 ) vec1A.zE << -shiftL ) + vec2A.zE;
273 		bbpL = vec2A.bbpE;
274 	}
275 
276 	return bts_Flt16Vec3D_create32( xL, yL, zL, bbpL );
277 }
278 
279 /* ------------------------------------------------------------------------- */
280 
bts_Flt16Vec3D_sub(struct bts_Flt16Vec3D vec1A,struct bts_Flt16Vec3D vec2A)281 struct bts_Flt16Vec3D bts_Flt16Vec3D_sub( struct bts_Flt16Vec3D vec1A, struct bts_Flt16Vec3D vec2A )
282 {
283 	int32 xL, yL, zL, bbpL;
284 	int32 shiftL = vec1A.bbpE - vec2A.bbpE;
285 
286 	if( shiftL > 0 )
287 	{
288 		xL = vec1A.xE - ( ( int32 ) vec2A.xE << shiftL );
289 		yL = vec1A.yE - ( ( int32 ) vec2A.yE << shiftL );
290 		zL = vec1A.zE - ( ( int32 ) vec2A.zE << 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 		zL = ( ( int32 ) vec1A.zE << -shiftL ) - vec2A.zE;
298 		bbpL = vec2A.bbpE;
299 	}
300 
301 	return bts_Flt16Vec3D_create32( xL, yL, zL, bbpL );
302 }
303 
304 /* ------------------------------------------------------------------------- */
305 
bts_Flt16Vec3D_mul(struct bts_Flt16Vec3D vecA,int16 factorA,int32 bbpFactorA)306 struct bts_Flt16Vec3D bts_Flt16Vec3D_mul( struct bts_Flt16Vec3D vecA, int16 factorA, int32 bbpFactorA )
307 {
308 	int32 xL = ( int32 ) vecA.xE * factorA;
309 	int32 yL = ( int32 ) vecA.yE * factorA;
310 	int32 zL = ( int32 ) vecA.zE * factorA;
311 	return bts_Flt16Vec3D_create32( xL, yL, zL, bbpFactorA + vecA.bbpE );
312 }
313 
314 /* ------------------------------------------------------------------------- */
315 
316 /* ========================================================================= */
317 
318