The Android Open Source Project | 30957f5 | 2008-10-21 07:00:00 -0700 | [diff] [blame^] | 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 | |