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/Cluster3D.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_Cluster3D_init( struct bbs_Context* cpA,
43						 struct bts_Cluster3D* ptrA )
44{
45	ptrA->mspE = NULL;
46	ptrA->vecArrE = NULL;
47	ptrA->allocatedSizeE = 0;
48	ptrA->sizeE = 0;
49	ptrA->bbpE = 0;
50}
51
52/* ------------------------------------------------------------------------- */
53
54void bts_Cluster3D_exit( struct bbs_Context* cpA,
55						 struct bts_Cluster3D* ptrA )
56{
57	bbs_MemSeg_free( cpA, ptrA->mspE, ptrA->vecArrE );
58	ptrA->vecArrE = NULL;
59	ptrA->mspE = NULL;
60	ptrA->allocatedSizeE = 0;
61	ptrA->sizeE = 0;
62	ptrA->bbpE = 0;
63}
64
65/* ------------------------------------------------------------------------- */
66
67/* ========================================================================= */
68/*                                                                           */
69/* ---- \ghd{ operators } -------------------------------------------------- */
70/*                                                                           */
71/* ========================================================================= */
72
73/* ------------------------------------------------------------------------- */
74
75void bts_Cluster3D_copy( struct bbs_Context* cpA,
76						 struct bts_Cluster3D* ptrA,
77						 const struct bts_Cluster3D* srcPtrA )
78{
79#ifdef DEBUG1
80	if( ptrA->allocatedSizeE < srcPtrA->sizeE )
81	{
82		bbs_ERROR0( "void bts_Cluster3D_copy( struct bts_Cluster2D* ptrA, const struct bts_Cluster2D* srcPtrA ): allocated size too low in destination cluster" );
83		return;
84	}
85#endif
86
87	bbs_memcpy16( ptrA->vecArrE, srcPtrA->vecArrE, bbs_SIZEOF16( struct bts_Int16Vec3D ) * srcPtrA->sizeE );
88
89	ptrA->bbpE = srcPtrA->bbpE;
90	ptrA->sizeE = srcPtrA->sizeE;
91}
92
93/* ------------------------------------------------------------------------- */
94
95flag bts_Cluster3D_equal( struct bbs_Context* cpA,
96						  const struct bts_Cluster3D* ptrA,
97						  const struct bts_Cluster3D* srcPtrA )
98{
99	uint32 iL;
100	const struct bts_Int16Vec3D* src1L = ptrA->vecArrE;
101	const struct bts_Int16Vec3D* src2L = srcPtrA->vecArrE;
102
103	if( ptrA->sizeE != srcPtrA->sizeE ) return FALSE;
104	if( ptrA->bbpE != srcPtrA->bbpE ) return FALSE;
105
106	for( iL = ptrA->sizeE; iL > 0; iL-- )
107	{
108		if( ( src1L->xE != src2L->xE ) ||
109			( src1L->yE != src2L->yE ) ||
110			( src1L->zE != src2L->zE ) ) return FALSE;
111		src1L++;
112		src2L++;
113	}
114
115	return TRUE;
116}
117
118/* ------------------------------------------------------------------------- */
119
120/* ========================================================================= */
121/*                                                                           */
122/* ---- \ghd{ query functions } -------------------------------------------- */
123/*                                                                           */
124/* ========================================================================= */
125
126/* ------------------------------------------------------------------------- */
127
128struct bts_Flt16Vec3D bts_Cluster3D_center( struct bbs_Context* cpA,
129										    const struct bts_Cluster3D* ptrA )
130{
131	struct bts_Int16Vec3D* vecPtrL = ptrA->vecArrE;
132	uint32 iL;
133	int32 xL = 0;
134	int32 yL = 0;
135	int32 zL = 0;
136
137	if( ptrA->sizeE == 0 ) return bts_Flt16Vec3D_create16( 0, 0, 0, 0 );
138
139	for( iL = ptrA->sizeE; iL > 0; iL-- )
140	{
141		xL += vecPtrL->xE;
142		yL += vecPtrL->yE;
143		zL += vecPtrL->zE;
144		vecPtrL++;
145	}
146
147	xL = ( ( ( xL << 1 ) / ( int32 )ptrA->sizeE ) + 1 ) >> 1;
148	yL = ( ( ( yL << 1 ) / ( int32 )ptrA->sizeE ) + 1 ) >> 1;
149	zL = ( ( ( zL << 1 ) / ( int32 )ptrA->sizeE ) + 1 ) >> 1;
150
151	return bts_Flt16Vec3D_create16( ( int16 )xL, ( int16 )yL, ( int16 )zL, ( int16 )ptrA->bbpE );
152}
153
154/* ------------------------------------------------------------------------- */
155
156struct bts_Int16Rect bts_Cluster3D_boundingBox( struct bbs_Context* cpA,
157											    const struct bts_Cluster3D* ptrA )
158{
159	struct bts_Int16Vec3D* vecPtrL = ptrA->vecArrE;
160	uint32 iL;
161	int32 xMinL = 65536; /*( 1 << 16 )*/
162	int32 yMinL = 65536; /*( 1 << 16 )*/
163	int32 xMaxL = 0;
164	int32 yMaxL = 0;
165
166	if( ptrA->sizeE == 0 ) return bts_Int16Rect_create( 0, 0, 0, 0 );
167
168	for( iL = ptrA->sizeE; iL > 0; iL-- )
169	{
170		xMinL = bbs_min( xMinL, vecPtrL->xE );
171		yMinL = bbs_min( yMinL, vecPtrL->yE );
172		xMaxL = bbs_max( xMaxL, vecPtrL->xE );
173		yMaxL = bbs_max( yMaxL, vecPtrL->yE );
174		vecPtrL++;
175	}
176
177	return bts_Int16Rect_create( ( int16 )xMinL, ( int16 )yMinL, ( int16 )xMaxL, ( int16 )yMaxL );
178}
179
180/* ------------------------------------------------------------------------- */
181
182int32 bts_Cluster3D_int32X( struct bbs_Context* cpA,
183						    const struct bts_Cluster3D* ptrA,
184							uint32 indexA, int32 bbpA )
185{
186	int32 shiftL = bbpA - ptrA->bbpE;
187#ifdef DEBUG2
188	if( indexA >= ptrA->sizeE )
189	{
190		bbs_ERROR2( "int32 bts_Cluster2D_int32X( .... )\n"
191			       "indexA = %i is out of range [0,%i]",
192				   indexA,
193				   ptrA->sizeE - 1 );
194		return 0;
195	}
196#endif
197	if( shiftL >= 0 )
198	{
199		return ( int32 ) ptrA->vecArrE[ indexA ].xE << shiftL;
200	}
201	else
202	{
203		return ( ( ( int32 ) ptrA->vecArrE[ indexA ].xE >> ( -shiftL - 1 ) ) + 1 ) >> 1;
204	}
205}
206
207/* ------------------------------------------------------------------------- */
208
209int32 bts_Cluster3D_int32Y( struct bbs_Context* cpA,
210						    const struct bts_Cluster3D* ptrA,
211							uint32 indexA,
212							int32 bbpA )
213{
214	int32 shiftL = bbpA - ptrA->bbpE;
215#ifdef DEBUG2
216	if( indexA >= ptrA->sizeE )
217	{
218		bbs_ERROR2( "int32 bts_Cluster2D_int32Y( .... )\n"
219			       "indexA = %i is out of range [0,%i]",
220				   indexA,
221				   ptrA->sizeE - 1 );
222		return 0;
223	}
224#endif
225	if( shiftL >= 0 )
226	{
227		return ( int32 ) ptrA->vecArrE[ indexA ].yE << shiftL;
228	}
229	else
230	{
231		return ( ( ( int32 ) ptrA->vecArrE[ indexA ].yE >> ( -shiftL - 1 ) ) + 1 ) >> 1;
232	}
233}
234
235/* ------------------------------------------------------------------------- */
236
237int32 bts_Cluster3D_int32Z( struct bbs_Context* cpA,
238						    const struct bts_Cluster3D* ptrA,
239							uint32 indexA,
240							int32 bbpA )
241{
242	int32 shiftL = bbpA - ptrA->bbpE;
243#ifdef DEBUG2
244	if( indexA >= ptrA->sizeE )
245	{
246		bbs_ERROR2( "int32 bts_Cluster2D_int32Z( .... )\n"
247			       "indexA = %i is out of range [0,%i]",
248				   indexA,
249				   ptrA->sizeE - 1 );
250		return 0;
251	}
252#endif
253	if( shiftL >= 0 )
254	{
255		return ( int32 ) ptrA->vecArrE[ indexA ].zE << shiftL;
256	}
257	else
258	{
259		return ( ( ( int32 ) ptrA->vecArrE[ indexA ].zE >> ( -shiftL - 1 ) ) + 1 ) >> 1;
260	}
261}
262
263/* ------------------------------------------------------------------------- */
264
265/* ========================================================================= */
266/*                                                                           */
267/* ---- \ghd{ modify functions } ------------------------------------------- */
268/*                                                                           */
269/* ========================================================================= */
270
271/* ------------------------------------------------------------------------- */
272
273void bts_Cluster3D_create( struct bbs_Context* cpA,
274						   struct bts_Cluster3D* ptrA,
275						   uint32 sizeA,
276						   struct bbs_MemSeg* mspA )
277{
278	if( bbs_Context_error( cpA ) ) return;
279	if( ptrA->mspE == NULL )
280	{
281		ptrA->sizeE = 0;
282		ptrA->allocatedSizeE = 0;
283		ptrA->vecArrE = NULL;
284	}
285
286	if( ptrA->sizeE == sizeA ) return;
287
288	if( ptrA->vecArrE != 0 )
289	{
290		bbs_ERROR0( "void bts_Cluster3D_create( const struct bts_Cluster3D*, uint32 ):\n"
291				   "object has already been created and cannot be resized." );
292		return;
293	}
294
295	ptrA->vecArrE = bbs_MemSeg_alloc( cpA, mspA, sizeA * bbs_SIZEOF16( struct bts_Int16Vec3D ) );
296	if( bbs_Context_error( cpA ) ) return;
297	ptrA->sizeE = sizeA;
298	ptrA->allocatedSizeE = sizeA;
299	if( !mspA->sharedE ) ptrA->mspE = mspA;
300}
301
302/* ------------------------------------------------------------------------- */
303
304void bts_Cluster3D_size( struct bbs_Context* cpA,
305						 struct bts_Cluster3D* ptrA,
306						 uint32 sizeA )
307{
308	if( ptrA->allocatedSizeE < sizeA )
309	{
310		bbs_ERROR2( "void bts_Cluster3D_size( struct bts_Cluster3D* ptrA, uint32 sizeA ):\n"
311				   "Allocated size (%i) of cluster is smaller than requested size (%i).",
312				   ptrA->allocatedSizeE,
313				   sizeA );
314		return;
315	}
316	ptrA->sizeE = sizeA;
317}
318
319/* ------------------------------------------------------------------------- */
320
321void bts_Cluster3D_transform( struct bbs_Context* cpA,
322							  struct bts_Cluster3D* ptrA,
323							  struct bts_Flt16Alt3D altA )
324{
325	struct bts_Int16Vec3D* vecPtrL = ptrA->vecArrE;
326	uint32 iL;
327
328	int32 x0L = altA.vecE.xE;
329	int32 y0L = altA.vecE.yE;
330	int32 z0L = altA.vecE.zE;
331
332	int32 shiftL = altA.matE.bbpE + ptrA->bbpE - altA.vecE.bbpE;
333
334	if( shiftL < 0 )
335	{
336		x0L = ( ( x0L >> ( -shiftL - 1 ) ) + 1 ) >> 1;
337		y0L = ( ( y0L >> ( -shiftL - 1 ) ) + 1 ) >> 1;
338		z0L = ( ( z0L >> ( -shiftL - 1 ) ) + 1 ) >> 1;
339	}
340	else
341	{
342		x0L <<= shiftL;
343		y0L <<= shiftL;
344		z0L <<= shiftL;
345	}
346
347	if( altA.matE.bbpE > 0 )
348	{
349		x0L += (int32)1 << ( altA.matE.bbpE - 1 );
350		y0L += (int32)1 << ( altA.matE.bbpE - 1 );
351		z0L += (int32)1 << ( altA.matE.bbpE - 1 );
352	}
353
354	for( iL = ptrA->sizeE; iL > 0; iL-- )
355	{
356		int32 xL = vecPtrL->xE;
357		int32 yL = vecPtrL->yE;
358		int32 zL = vecPtrL->zE;
359		vecPtrL->xE = ( x0L + xL * altA.matE.xxE + yL * altA.matE.xyE + zL * altA.matE.xzE ) >> altA.matE.bbpE;
360		vecPtrL->yE = ( y0L + xL * altA.matE.yxE + yL * altA.matE.yyE + zL * altA.matE.yzE ) >> altA.matE.bbpE;
361		vecPtrL->zE = ( z0L + xL * altA.matE.zxE + yL * altA.matE.zyE + zL * altA.matE.zzE ) >> altA.matE.bbpE;
362		vecPtrL++;
363	}
364}
365
366/* ------------------------------------------------------------------------- */
367
368struct bts_Flt16Vec3D bts_Cluster3D_centerFree( struct bbs_Context* cpA,
369											    struct bts_Cluster3D* ptrA )
370{
371	struct bts_Flt16Vec3D centerL = bts_Cluster3D_center( cpA, ptrA );
372	struct bts_Int16Vec3D* vecPtrL = ptrA->vecArrE;
373	uint32 iL;
374
375	for( iL = ptrA->sizeE; iL > 0; iL-- )
376	{
377		vecPtrL->xE -= centerL.xE;
378		vecPtrL->yE -= centerL.yE;
379		vecPtrL->zE -= centerL.zE;
380		vecPtrL++;
381	}
382
383	return centerL;
384}
385
386/* ------------------------------------------------------------------------- */
387
388/* ========================================================================= */
389/*                                                                           */
390/* ---- \ghd{ I/O } -------------------------------------------------------- */
391/*                                                                           */
392/* ========================================================================= */
393
394/* ------------------------------------------------------------------------- */
395
396uint32 bts_Cluster3D_memSize( struct bbs_Context* cpA,
397							  const struct bts_Cluster3D *ptrA )
398{
399	return  bbs_SIZEOF16( uint32 )
400		  + bbs_SIZEOF16( uint32 ) /* version */
401		  + bbs_SIZEOF16( ptrA->sizeE )
402		  + bbs_SIZEOF16( ptrA->bbpE )
403		  + bbs_SIZEOF16( struct bts_Int16Vec3D ) * ptrA->sizeE;
404}
405
406/* ------------------------------------------------------------------------- */
407
408uint32 bts_Cluster3D_memWrite( struct bbs_Context* cpA,
409							   const struct bts_Cluster3D* ptrA,
410							   uint16* memPtrA )
411{
412	uint32 memSizeL = bts_Cluster3D_memSize( cpA, ptrA );
413	memPtrA += bbs_memWrite32( &memSizeL, memPtrA );
414	memPtrA += bbs_memWriteUInt32( bts_CLUSTER3D_VERSION, memPtrA );
415	memPtrA += bbs_memWrite32( &ptrA->sizeE, memPtrA );
416	memPtrA += bbs_memWrite32( &ptrA->bbpE, memPtrA );
417	memPtrA += bbs_memWrite16Arr( cpA, ptrA->vecArrE,
418								  ptrA->sizeE * bbs_SIZEOF16( struct bts_Int16Vec3D ),
419								  memPtrA );
420	return memSizeL;
421}
422
423/* ------------------------------------------------------------------------- */
424
425uint32 bts_Cluster3D_memRead( struct bbs_Context* cpA,
426							  struct bts_Cluster3D* ptrA,
427							  const uint16* memPtrA,
428						      struct bbs_MemSeg* mspA )
429{
430	uint32 memSizeL;
431	uint32 sizeL;
432	uint32 versionL;
433	if( bbs_Context_error( cpA ) ) return 0;
434	memPtrA += bbs_memRead32( &memSizeL, memPtrA );
435	memPtrA += bbs_memReadVersion32( cpA, &versionL, bts_CLUSTER3D_VERSION, memPtrA );
436	memPtrA += bbs_memRead32( &sizeL, memPtrA );
437	memPtrA += bbs_memRead32( &ptrA->bbpE, memPtrA );
438
439	if( ptrA->allocatedSizeE < sizeL )
440	{
441		bts_Cluster3D_create( cpA, ptrA, sizeL, mspA );
442	}
443	else
444	{
445		bts_Cluster3D_size( cpA, ptrA, sizeL );
446	}
447
448
449	bbs_memcpy16( ptrA->vecArrE, memPtrA, bbs_SIZEOF16( struct bts_Int16Vec3D ) * ptrA->sizeE );
450	memPtrA += bbs_memRead16Arr( cpA, ptrA->vecArrE,
451								 ptrA->sizeE * bbs_SIZEOF16( struct bts_Int16Vec3D ),
452								 memPtrA );
453
454	if( memSizeL != bts_Cluster3D_memSize( cpA, ptrA ) )
455	{
456		bbs_ERR0( bbs_ERR_CORRUPT_DATA, "uint32 bts_Cluster3D_memRead( const struct bts_Cluster3D* ptrA, const void* memPtrA ):\n"
457                   "size mismatch" );
458		return 0;
459	}
460	return memSizeL;
461}
462
463/* ------------------------------------------------------------------------- */
464
465/* ========================================================================= */
466/*                                                                           */
467/* ---- \ghd{ exec functions } --------------------------------------------- */
468/*                                                                           */
469/* ========================================================================= */
470
471/* ------------------------------------------------------------------------- */
472
473/* ========================================================================= */
474
475