auto import from //depot/cupcake/@135843
[android/platform/external/neven.git] / Embedded / common / src / b_TensorEm / Cluster3D.c
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
42 void 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
54 void 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
75 void 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
95 flag 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
128 struct 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
156 struct 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
182 int32 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
209 int32 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
237 int32 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         
273 void 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
304 void 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         
321 void 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         
368 struct 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         
396 uint32 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         
408 uint32 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         
425 uint32 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