Gitiles
Code Review Sign In
gerrit-public.fairphone.software / platform / external / neven / 30957f56b33a18c5a4af787f25b9f145f2803634 / . / Embedded / common / src / b_TensorEm / Cluster2D.c
blob: e6acf6e66d5442ac6f8472ed27aaf7a058fc9759 [file] [log] [blame]
/*
* Copyright (C) 2008 The Android Open Source Project
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
/* ---- includes ----------------------------------------------------------- */
#include "b_TensorEm/Cluster2D.h"
#include "b_TensorEm/RBFMap2D.h"
#include "b_BasicEm/Math.h"
#include "b_BasicEm/Memory.h"
#include "b_BasicEm/Functions.h"
/* ------------------------------------------------------------------------- */
/* ========================================================================= */
/* */
/* ---- \ghd{ auxiliary functions } ---------------------------------------- */
/* */
/* ========================================================================= */
/* ------------------------------------------------------------------------- */
/** Computes relative scale factor from the 2 mean square node distances to the
* cluster centers for 2 clusters.
*/
void bts_Cluster2D_computeScale( uint32 enumA, /* mean square radius, dst cluster */
int32 bbp_enumA, /* bbp of enumA */
uint32 denomA, /* mean square radius, src cluster */
int32 bbp_denomA, /* bbp of denomA */
uint32* scaleA, /* resulting scale factor */
int32* bbp_scaleA )/* bbp of scale factor */
{
uint32 shiftL, quotientL;
int32 posL, bbp_denomL;
/* how far can we shift enumA to the left */
shiftL = 31 - bbs_intLog2( enumA );
/* how far do we have to shift denomA to the right */
posL = bbs_intLog2( denomA ) + 1;
bbp_denomL = bbp_denomA;
if( posL - bbp_denomL > 12 )
{
/* if denomA has more than 12 bit before the point, discard bits behind the point */
denomA >>= bbp_denomL;
bbp_denomL = 0;
}
else
{
/* otherwise reduce denomA to 12 bit */
bbs_uint32ReduceToNBits( &denomA, &bbp_denomL, 12 );
}
/* make result bbp even for call of sqrt */
if( ( bbp_enumA + shiftL - bbp_denomL ) & 1 ) shiftL--;
quotientL = ( enumA << shiftL ) / denomA;
*scaleA = bbs_fastSqrt32( quotientL );
*bbp_scaleA = ( bbp_enumA + shiftL - bbp_denomL ) >> 1;
}
/* ------------------------------------------------------------------------- */
/* ========================================================================= */
/* */
/* ---- \ghd{ constructor / destructor } ----------------------------------- */
/* */
/* ========================================================================= */
/* ------------------------------------------------------------------------- */
void bts_Cluster2D_init( struct bbs_Context* cpA,
struct bts_Cluster2D* ptrA )
{
ptrA->mspE = NULL;
ptrA->vecArrE = NULL;
ptrA->allocatedSizeE = 0;
ptrA->sizeE = 0;
ptrA->bbpE = 0;
}
/* ------------------------------------------------------------------------- */
void bts_Cluster2D_exit( struct bbs_Context* cpA,
struct bts_Cluster2D* ptrA )
{
bbs_MemSeg_free( cpA, ptrA->mspE, ptrA->vecArrE );
ptrA->vecArrE = NULL;
ptrA->mspE = NULL;
ptrA->allocatedSizeE = 0;
ptrA->sizeE = 0;
ptrA->bbpE = 0;
}
/* ------------------------------------------------------------------------- */
/* ========================================================================= */
/* */
/* ---- \ghd{ operators } -------------------------------------------------- */
/* */
/* ========================================================================= */
/* ------------------------------------------------------------------------- */
void bts_Cluster2D_copy( struct bbs_Context* cpA,
struct bts_Cluster2D* ptrA,
const struct bts_Cluster2D* srcPtrA )
{
#ifdef DEBUG2
if( ptrA->allocatedSizeE < srcPtrA->sizeE )
{
bbs_ERROR0( "void bts_Cluster2D_copy( struct bts_Cluster2D* ptrA, const struct bts_Cluster2D* srcPtrA ): allocated size too low in destination cluster" );
return;
}
#endif
bbs_memcpy32( ptrA->vecArrE, srcPtrA->vecArrE, bbs_SIZEOF32( struct bts_Int16Vec2D ) * srcPtrA->sizeE );
ptrA->bbpE = srcPtrA->bbpE;
ptrA->sizeE = srcPtrA->sizeE;
}
/* ------------------------------------------------------------------------- */
flag bts_Cluster2D_equal( struct bbs_Context* cpA,
const struct bts_Cluster2D* ptrA,
const struct bts_Cluster2D* srcPtrA )
{
uint32 iL;
const struct bts_Int16Vec2D* src1L = ptrA->vecArrE;
const struct bts_Int16Vec2D* src2L = srcPtrA->vecArrE;
if( ptrA->sizeE != srcPtrA->sizeE ) return FALSE;
if( ptrA->bbpE != srcPtrA->bbpE ) return FALSE;
for( iL = ptrA->sizeE; iL > 0; iL-- )
{
if( ( src1L->xE != src2L->xE ) || ( src1L->yE != src2L->yE ) ) return FALSE;
src1L++;
src2L++;
}
return TRUE;
}
/* ------------------------------------------------------------------------- */
/* ========================================================================= */
/* */
/* ---- \ghd{ query functions } -------------------------------------------- */
/* */
/* ========================================================================= */
/* ------------------------------------------------------------------------- */
struct bts_Flt16Vec2D bts_Cluster2D_center( struct bbs_Context* cpA,
const struct bts_Cluster2D* ptrA )
{
struct bts_Int16Vec2D* vecPtrL = ptrA->vecArrE;
uint32 iL;
int32 xL = 0;
int32 yL = 0;
if( ptrA->sizeE == 0 ) return bts_Flt16Vec2D_create16( 0, 0, 0 );
for( iL = ptrA->sizeE; iL > 0; iL-- )
{
xL += vecPtrL->xE;
yL += vecPtrL->yE;
vecPtrL++;
}
xL = ( ( ( xL << 1 ) / ( int32 )ptrA->sizeE ) + 1 ) >> 1;
yL = ( ( ( yL << 1 ) / ( int32 )ptrA->sizeE ) + 1 ) >> 1;
return bts_Flt16Vec2D_create16( ( int16 )xL, ( int16 )yL, ( int16 )ptrA->bbpE );
}
/* ------------------------------------------------------------------------- */
uint32 bts_Cluster2D_checkSum( struct bbs_Context* cpA,
const struct bts_Cluster2D* ptrA )
{
struct bts_Int16Vec2D* vecPtrL = ptrA->vecArrE;
uint32 iL;
int32 sumL = ptrA->bbpE;
for( iL = ptrA->sizeE; iL > 0; iL-- )
{
sumL += vecPtrL->xE;
sumL += vecPtrL->yE;
vecPtrL++;
}
return (uint32)sumL;
}
/* ------------------------------------------------------------------------- */
struct bts_Int16Rect bts_Cluster2D_boundingBox( struct bbs_Context* cpA,
const struct bts_Cluster2D* ptrA )
{
struct bts_Int16Vec2D* vecPtrL = ptrA->vecArrE;
uint32 iL;
int32 xMinL = 65536;
int32 yMinL = 65536;
int32 xMaxL = -65536;
int32 yMaxL = -65536;
if( ptrA->sizeE == 0 ) return bts_Int16Rect_create( 0, 0, 0, 0 );
for( iL = ptrA->sizeE; iL > 0; iL-- )
{
xMinL = bbs_min( xMinL, vecPtrL->xE );
yMinL = bbs_min( yMinL, vecPtrL->yE );
xMaxL = bbs_max( xMaxL, vecPtrL->xE );
yMaxL = bbs_max( yMaxL, vecPtrL->yE );
vecPtrL++;
}
return bts_Int16Rect_create( ( int16 )xMinL, ( int16 )yMinL, ( int16 )xMaxL, ( int16 )yMaxL );
}
/* ------------------------------------------------------------------------- */
int32 bts_Cluster2D_int32X( struct bbs_Context* cpA,
const struct bts_Cluster2D* ptrA,
uint32 indexA, int32 bbpA )
{
#ifdef DEBUG2
if( indexA >= ptrA->sizeE )
{
bbs_ERROR2( "int32 bts_Cluster2D_int32X( .... )\n"
"indexA = %i is out of range [0,%i]",
indexA,
ptrA->sizeE - 1 );
return 0;
}
#endif
{
int32 shiftL = bbpA - ptrA->bbpE;
int32 xL = ptrA->vecArrE[ indexA ].xE;
if( shiftL >= 0 )
{
xL <<= shiftL;
}
else
{
xL = ( ( xL >> ( -shiftL - 1 ) ) + 1 ) >> 1;
}
return xL;
}
}
/* ------------------------------------------------------------------------- */
int32 bts_Cluster2D_int32Y( struct bbs_Context* cpA,
const struct bts_Cluster2D* ptrA,
uint32 indexA,
int32 bbpA )
{
#ifdef DEBUG2
if( indexA >= ptrA->sizeE )
{
bbs_ERROR2( "int32 bts_Cluster2D_int32Y( .... )\n"
"indexA = %i is out of range [0,%i]",
indexA,
ptrA->sizeE - 1 );
return 0;
}
#endif
{
int32 shiftL = bbpA - ptrA->bbpE;
int32 yL = ptrA->vecArrE[ indexA ].yE;
if( shiftL >= 0 )
{
yL <<= shiftL;
}
else
{
yL = ( ( yL >> ( -shiftL - 1 ) ) + 1 ) >> 1;
}
return yL;
}
}
/* ------------------------------------------------------------------------- */
/* ========================================================================= */
/* */
/* ---- \ghd{ modify functions } ------------------------------------------- */
/* */
/* ========================================================================= */
/* ------------------------------------------------------------------------- */
void bts_Cluster2D_create( struct bbs_Context* cpA,
struct bts_Cluster2D* ptrA,
uint32 sizeA,
struct bbs_MemSeg* mspA )
{
if( bbs_Context_error( cpA ) ) return;
if( ptrA->mspE == NULL )
{
ptrA->sizeE = 0;
ptrA->allocatedSizeE = 0;
ptrA->vecArrE = NULL;
}
if( ptrA->sizeE == sizeA ) return;
if( ptrA->vecArrE != 0 )
{
bbs_ERROR0( "void bts_Cluster2D_create( const struct bts_Cluster2D*, uint32 ):\n"
"object has already been created and cannot be resized." );
return;
}
ptrA->vecArrE = bbs_MemSeg_alloc( cpA, mspA, sizeA * bbs_SIZEOF16( struct bts_Int16Vec2D ) );
if( bbs_Context_error( cpA ) ) return;
ptrA->sizeE = sizeA;
ptrA->allocatedSizeE = sizeA;
if( !mspA->sharedE ) ptrA->mspE = mspA;
}
/* ------------------------------------------------------------------------- */
void bts_Cluster2D_size( struct bbs_Context* cpA,
struct bts_Cluster2D* ptrA,
uint32 sizeA )
{
if( ptrA->allocatedSizeE < sizeA )
{
bbs_ERROR2( "void bts_Cluster2D_size( struct bts_Cluster2D* ptrA, uint32 sizeA ):\n"
"Allocated size (%i) of cluster is smaller than requested size (%i).",
ptrA->allocatedSizeE,
sizeA );
return;
}
ptrA->sizeE = sizeA;
}
/* ------------------------------------------------------------------------- */
void bts_Cluster2D_transform( struct bbs_Context* cpA,
struct bts_Cluster2D* ptrA,
struct bts_Flt16Alt2D altA )
{
uint32 iL;
for( iL = 0; iL < ptrA->sizeE; iL++ )
{
struct bts_Flt16Vec2D vL = bts_Flt16Vec2D_createVec16( ptrA->vecArrE[ iL ], ptrA->bbpE );
ptrA->vecArrE[ iL ] = bts_Flt16Vec2D_int16Vec2D( bts_Flt16Alt2D_mapFlt( &altA, &vL ), ptrA->bbpE );
}
}
/* ------------------------------------------------------------------------- */
void bts_Cluster2D_transformBbp( struct bbs_Context* cpA,
struct bts_Cluster2D* ptrA,
struct bts_Flt16Alt2D altA,
uint32 dstBbpA )
{
uint32 iL;
for( iL = 0; iL < ptrA->sizeE; iL++ )
{
struct bts_Flt16Vec2D vL = bts_Flt16Vec2D_createVec16( ptrA->vecArrE[ iL ], ptrA->bbpE );
ptrA->vecArrE[ iL ] = bts_Flt16Vec2D_int16Vec2D( bts_Flt16Alt2D_mapFlt( &altA, &vL ), dstBbpA );
}
ptrA->bbpE = dstBbpA;
}
/* ------------------------------------------------------------------------- */
void bts_Cluster2D_rbfTransform( struct bbs_Context* cpA,
struct bts_Cluster2D* ptrA,
const struct bts_RBFMap2D* rbfMapPtrA )
{
bts_RBFMap2D_mapCluster( cpA, rbfMapPtrA, ptrA, ptrA, ptrA->bbpE );
}
/* ------------------------------------------------------------------------- */
void bts_Cluster2D_copyTransform( struct bbs_Context* cpA,
struct bts_Cluster2D* ptrA,
const struct bts_Cluster2D* srcPtrA,
struct bts_Flt16Alt2D altA,
uint32 dstBbpA )
{
uint32 iL;
/* prepare destination cluster */
if( ptrA->allocatedSizeE < srcPtrA->sizeE )
{
bbs_ERROR0( "void bts_Cluster2D_copyTransform( struct bts_Cluster2D* ptrA, const struct bts_Cluster2D* srcPtrA, struct bts_Flt16Alt2D altA, uint32 dstBbpA ): allocated size too low in destination cluster" );
return;
}
ptrA->sizeE = srcPtrA->sizeE;
ptrA->bbpE = dstBbpA;
/* transform */
for( iL = 0; iL < ptrA->sizeE; iL++ )
{
struct bts_Flt16Vec2D vL = bts_Flt16Vec2D_createVec16( srcPtrA->vecArrE[ iL ], srcPtrA->bbpE );
ptrA->vecArrE[ iL ] = bts_Flt16Vec2D_int16Vec2D( bts_Flt16Alt2D_mapFlt( &altA, &vL ), ptrA->bbpE );
}
}
/* ------------------------------------------------------------------------- */
/* ========================================================================= */
/* */
/* ---- \ghd{ I/O } -------------------------------------------------------- */
/* */
/* ========================================================================= */
/* ------------------------------------------------------------------------- */
uint32 bts_Cluster2D_memSize( struct bbs_Context* cpA,
const struct bts_Cluster2D *ptrA )
{
return bbs_SIZEOF16( uint32 ) /* mem size */
+ bbs_SIZEOF16( uint32 ) /* version */
+ bbs_SIZEOF16( ptrA->sizeE )
+ bbs_SIZEOF16( ptrA->bbpE )
+ bbs_SIZEOF16( struct bts_Int16Vec2D ) * ptrA->sizeE;
}
/* ------------------------------------------------------------------------- */
uint32 bts_Cluster2D_memWrite( struct bbs_Context* cpA,
const struct bts_Cluster2D* ptrA,
uint16* memPtrA )
{
uint32 memSizeL = bts_Cluster2D_memSize( cpA, ptrA );
memPtrA += bbs_memWrite32( &memSizeL, memPtrA );
memPtrA += bbs_memWriteUInt32( bts_CLUSTER2D_VERSION, memPtrA );
memPtrA += bbs_memWrite32( &ptrA->sizeE, memPtrA );
memPtrA += bbs_memWrite32( &ptrA->bbpE, memPtrA );
memPtrA += bbs_memWrite16Arr( cpA, ptrA->vecArrE, ptrA->sizeE * 2, memPtrA );
return memSizeL;
}
/* ------------------------------------------------------------------------- */
uint32 bts_Cluster2D_memRead( struct bbs_Context* cpA,
struct bts_Cluster2D* ptrA,
const uint16* memPtrA,
struct bbs_MemSeg* mspA )
{
uint32 memSizeL;
uint32 sizeL;
uint32 versionL;
if( bbs_Context_error( cpA ) ) return 0;
memPtrA += bbs_memRead32( &memSizeL, memPtrA );
memPtrA += bbs_memReadVersion32( cpA, &versionL, bts_CLUSTER2D_VERSION, memPtrA );
memPtrA += bbs_memRead32( &sizeL, memPtrA );
memPtrA += bbs_memRead32( &ptrA->bbpE, memPtrA );
if( ptrA->allocatedSizeE < sizeL )
{
bts_Cluster2D_create( cpA, ptrA, sizeL, mspA );
}
else
{
bts_Cluster2D_size( cpA, ptrA, sizeL );
}
memPtrA += bbs_memRead16Arr( cpA, ptrA->vecArrE, ptrA->sizeE * 2, memPtrA );
if( memSizeL != bts_Cluster2D_memSize( cpA, ptrA ) )
{
bbs_ERR0( bbs_ERR_CORRUPT_DATA, "uint32 bts_Cluster2D_memRead( const struct bts_Cluster2D* ptrA, const void* memPtrA ):\n"
"size mismatch" );
return 0;
}
return memSizeL;
}
/* ------------------------------------------------------------------------- */
/* ========================================================================= */
/* */
/* ---- \ghd{ exec functions } --------------------------------------------- */
/* */
/* ========================================================================= */
/* ------------------------------------------------------------------------- */
struct bts_Flt16Alt2D bts_Cluster2D_alt( struct bbs_Context* cpA,
const struct bts_Cluster2D* srcPtrA,
const struct bts_Cluster2D* dstPtrA,
enum bts_AltType altTypeA )
{
struct bts_Flt16Alt2D altL = bts_Flt16Alt2D_createIdentity();
enum bts_AltType altTypeL = altTypeA;
uint32 sizeL = srcPtrA->sizeE;
int32 srcBbpL = srcPtrA->bbpE;
int32 dstBbpL = dstPtrA->bbpE;
struct bts_Flt16Vec2D cpL, cqL, cpMappedL, cpAdjustedL;
if( dstPtrA->sizeE != srcPtrA->sizeE )
{
bbs_ERROR2( "struct bts_Flt16Alt2D bts_Cluster2D_alt( ... ):\n"
"the 2 input clusters differ in size: %d vs %d", srcPtrA->sizeE, dstPtrA->sizeE );
}
if( sizeL <= 2 )
{
if( altTypeL == bts_ALT_LINEAR )
{
altTypeL = bts_ALT_RIGID;
}
}
if( sizeL <= 1 )
{
if( altTypeL == bts_ALT_RIGID )
{
altTypeL = bts_ALT_TRANS;
}
else if( altTypeL == bts_ALT_TRANS_SCALE )
{
altTypeL = bts_ALT_TRANS;
}
}
if( sizeL == 0 || altTypeL == bts_ALT_IDENTITY )
{
/* return identity */
return altL;
}
cpL = bts_Cluster2D_center( cpA, srcPtrA );
cqL = bts_Cluster2D_center( cpA, dstPtrA );
if( altTypeL == bts_ALT_TRANS )
{
/* return translation only */
altL.vecE = bts_Flt16Vec2D_sub( cqL, cpL );
return altL;
}
switch( altTypeL )
{
case bts_ALT_TRANS_SCALE:
{
uint32 spL = 0;
uint32 sqL = 0;
struct bts_Int16Vec2D* srcPtrL = srcPtrA->vecArrE;
struct bts_Int16Vec2D* dstPtrL = dstPtrA->vecArrE;
int32 iL = sizeL;
while( iL-- )
{
int32 pxL = srcPtrL->xE - cpL.xE;
int32 pyL = srcPtrL->yE - cpL.yE;
int32 qxL = dstPtrL->xE - cqL.xE;
int32 qyL = dstPtrL->yE - cqL.yE;
srcPtrL++;
dstPtrL++;
/* overflow estimate: no problem with 100 nodes, bbp = 6, x = y = 500 */
spL += ( pxL * pxL ) >> srcBbpL;
spL += ( pyL * pyL ) >> srcBbpL;
sqL += ( qxL * qxL ) >> dstBbpL;
sqL += ( qyL * qyL ) >> dstBbpL;
}
spL /= sizeL;
sqL /= sizeL;
if( spL == 0 )
{
bbs_ERROR0( "struct bts_Flt16Alt2D bts_Cluster2D_alt( ... ):\n"
"All nodes of the src cluster are sitting in the center -> "
"unable to compute scale matrix between clusters" );
}
else
{
uint32 scaleL;
int32 factor32L, bbp_scaleL;
int16 factor16L;
bts_Cluster2D_computeScale( sqL, dstBbpL, spL, srcBbpL, &scaleL, &bbp_scaleL );
/* create scale matrix */
factor32L = ( int32 )scaleL;
altL.matE = bts_Flt16Mat2D_createScale( factor32L, bbp_scaleL );
/* create translation vector */
factor16L = scaleL;
cpMappedL = bts_Flt16Vec2D_mul( cpL, factor16L, bbp_scaleL );
altL.vecE = bts_Flt16Vec2D_sub( cqL, cpMappedL );
return altL;
}
}
break;
case bts_ALT_RIGID:
{
/* smaller of the 2 bbp's */
int32 minBbpL = bbs_min( srcBbpL, dstBbpL );
uint32 spL = 0;
uint32 sqL = 0;
int32 pxqxL = 0;
int32 pxqyL = 0;
int32 pyqxL = 0;
int32 pyqyL = 0;
struct bts_Int16Vec2D* srcPtrL = srcPtrA->vecArrE;
struct bts_Int16Vec2D* dstPtrL = dstPtrA->vecArrE;
int32 iL = sizeL;
while( iL-- )
{
int32 pxL = srcPtrL->xE - cpL.xE;
int32 pyL = srcPtrL->yE - cpL.yE;
int32 qxL = dstPtrL->xE - cqL.xE;
int32 qyL = dstPtrL->yE - cqL.yE;
srcPtrL++;
dstPtrL++;
/* overflow estimate: no problem with 100 nodes, bbp = 6, x = y = 500 */
spL += ( pxL * pxL ) >> srcBbpL;
spL += ( pyL * pyL ) >> srcBbpL;
sqL += ( qxL * qxL ) >> dstBbpL;
sqL += ( qyL * qyL ) >> dstBbpL;
pxqxL += ( pxL * qxL ) >> minBbpL;
pxqyL += ( pxL * qyL ) >> minBbpL;
pyqxL += ( pyL * qxL ) >> minBbpL;
pyqyL += ( pyL * qyL ) >> minBbpL;
}
spL /= sizeL;
sqL /= sizeL;
pxqxL /= ( int32 )sizeL;
pxqyL /= ( int32 )sizeL;
pyqxL /= ( int32 )sizeL;
pyqyL /= ( int32 )sizeL;
if( spL == 0 )
{
bbs_ERROR0( "struct bts_Flt16Alt2D bts_Cluster2D_alt( ... ):\n"
"All nodes of the src cluster are sitting in the center -> "
"unable to compute scale matrix between clusters" );
}
else
{
uint32 scaleL, shiftL, quotientL, enumL, denomL, bitsTaken0L, bitsTaken1L;
int32 bbp_scaleL, cL, rL, c1L, r1L;
int32 ppL, pmL, mpL, mmL, maxL;
int32 quotientBbpL, bbp_crL, posL;
/* find scale factor: */
bts_Cluster2D_computeScale( sqL, dstBbpL, spL, srcBbpL, &scaleL, &bbp_scaleL );
/* find rotation matrix: */
/* sign not needed any more */
enumL = bbs_abs( pxqyL - pyqxL );
denomL = bbs_abs( pxqxL + pyqyL );
if( denomL == 0 )
{
cL = 0;
rL = 1;
quotientBbpL = 0;
}
else
{
/* original formula:
float aL = enumL / denomL;
cL = sqrt( 1.0 / ( 1.0 + ebs_sqr( aL ) ) );
rL = sqrt( 1 - ebs_sqr( cL ) );
*/
/* how far can we shift enumL to the left */
shiftL = 31 - bbs_intLog2( enumL );
/* result has bbp = shiftL */
quotientL = ( enumL << shiftL ) / denomL;
quotientBbpL = shiftL;
posL = bbs_intLog2( quotientL );
/* if enumL much larger than denomL, then we cannot square the quotient */
if( posL > ( quotientBbpL + 14 ) )
{
cL = 0;
rL = 1;
quotientBbpL = 0;
}
else if( quotientBbpL > ( posL + 14 ) )
{
cL = 1;
rL = 0;
quotientBbpL = 0;
}
else
{
bbs_uint32ReduceToNBits( &quotientL, &quotientBbpL, 15 );
/* to avoid an overflow in the next operation */
if( quotientBbpL > 15 )
{
quotientL >>= ( quotientBbpL - 15 );
quotientBbpL -= ( quotientBbpL - 15 );
}
/* result has again bbp = quotientBbpL */
denomL = bbs_fastSqrt32( quotientL * quotientL + ( ( int32 )1 << ( quotientBbpL << 1 ) ) );
quotientL = ( ( uint32 )1 << 31 ) / denomL;
quotientBbpL = 31 - quotientBbpL;
bbs_uint32ReduceToNBits( &quotientL, &quotientBbpL, 15 );
/* to avoid an overflow in the next operation */
if( quotientBbpL > 15 )
{
quotientL >>= ( quotientBbpL - 15 );
quotientBbpL -= ( quotientBbpL - 15 );
}
cL = quotientL;
rL = bbs_fastSqrt32( ( ( int32 )1 << ( quotientBbpL << 1 ) ) - quotientL * quotientL );
}
}
/* save cL and rL with this accuracy for later */
c1L = cL;
r1L = rL;
bbp_crL = quotientBbpL;
/* prepare the next computations */
bitsTaken0L = bts_maxAbsIntLog2Of4( pxqxL, pxqyL, pyqxL, pyqyL ) + 1;
bitsTaken1L = bts_maxAbsIntLog2Of2( cL, rL ) + 1;
if( ( bitsTaken0L + bitsTaken1L ) > 29 )
{
int32 shiftL = bitsTaken0L + bitsTaken1L - 29;
cL >>= shiftL;
rL >>= shiftL;
quotientBbpL -= shiftL;
}
/* best combination: */
ppL = cL * pxqxL - rL * pyqxL + cL * pyqyL + rL * pxqyL;
pmL = cL * pxqxL + rL * pyqxL + cL * pyqyL - rL * pxqyL;
mpL = - cL * pxqxL - rL * pyqxL - cL * pyqyL + rL * pxqyL;
mmL = - cL * pxqxL + rL * pyqxL - cL * pyqyL - rL * pxqyL;
maxL = bbs_max( bbs_max( ppL, pmL ), bbs_max( mpL, mmL ) );
/* restore cL and rL, bbp = bbp_crL */
cL = c1L;
rL = r1L;
/* rotation matrix */
if( ppL == maxL )
{
altL.matE = bts_Flt16Mat2D_create32( cL, -rL, rL, cL, bbp_crL );
}
else if( pmL == maxL )
{
altL.matE = bts_Flt16Mat2D_create32( cL, rL, -rL, cL, bbp_crL );
}
else if( mpL == maxL )
{
altL.matE = bts_Flt16Mat2D_create32( -cL, -rL, rL, -cL, bbp_crL );
}
else
{
altL.matE = bts_Flt16Mat2D_create32( -cL, rL, -rL, -cL, bbp_crL );
}
/* find translation: */
/* original formula:
ets_Float2DVec transL = cqL - ( scaleL * ( rotL * cpL ) );
altL.mat( rotL * scaleL );
altL.vec( transL );
*/
bts_Flt16Mat2D_scale( &altL.matE, scaleL, bbp_scaleL );
cpMappedL = bts_Flt16Mat2D_mapFlt( &altL.matE, &cpL );
altL.vecE = bts_Flt16Vec2D_sub( cqL, cpMappedL );
}
return altL;
}
case bts_ALT_LINEAR:
{
/* smaller of the 2 bbp's */
int32 minBbpL = bbs_min( srcBbpL, dstBbpL );
int32 iL = 0;
int32 pxpxL = 0;
int32 pxpyL = 0;
int32 pypyL = 0;
int32 pxqxL = 0;
int32 pxqyL = 0;
int32 pyqxL = 0;
int32 pyqyL = 0;
struct bts_Int16Vec2D* srcPtrL = srcPtrA->vecArrE;
struct bts_Int16Vec2D* dstPtrL = dstPtrA->vecArrE;
/* get cp adjusted to dstBbpL */
int32 shiftL = dstBbpL - srcBbpL;
if( shiftL > 0 )
{
cpAdjustedL.xE = cpL.xE << shiftL;
cpAdjustedL.yE = cpL.yE << shiftL;
cpAdjustedL.bbpE = dstBbpL;
}
else
{
cpAdjustedL.xE = ( ( cpL.xE >> ( -shiftL - 1 ) ) + 1 ) >> 1;
cpAdjustedL.yE = ( ( cpL.yE >> ( -shiftL - 1 ) ) + 1 ) >> 1;
cpAdjustedL.bbpE = dstBbpL;
}
iL = sizeL;
while( iL-- )
{
int32 pxL = srcPtrL->xE - cpL.xE;
int32 pyL = srcPtrL->yE - cpL.yE;
int32 qxL = dstPtrL->xE - cpAdjustedL.xE; /* cp, not cq! */
int32 qyL = dstPtrL->yE - cpAdjustedL.yE;
srcPtrL++;
dstPtrL++;
/* overflow estimate: no problem with 100 nodes, bbp = 6, x = y = 500 */
pxpxL += ( pxL * pxL ) >> srcBbpL;
pxpyL += ( pxL * pyL ) >> srcBbpL;
pypyL += ( pyL * pyL ) >> srcBbpL;
pxqxL += ( pxL * qxL ) >> minBbpL;
pxqyL += ( pxL * qyL ) >> minBbpL;
pyqxL += ( pyL * qxL ) >> minBbpL;
pyqyL += ( pyL * qyL ) >> minBbpL;
}
pxpxL /= ( int32 )sizeL;
pxpyL /= ( int32 )sizeL;
pypyL /= ( int32 )sizeL;
pxqxL /= ( int32 )sizeL;
pxqyL /= ( int32 )sizeL;
pyqxL /= ( int32 )sizeL;
pyqyL /= ( int32 )sizeL;
{
/* original code:
float detPL = ( pxpxL * pypyL ) - ( pxpyL * pxpyL );
if( ebs_neglectable( detPL ) )
{
matL.setIdentity();
}
else
{
matL.xx( ( pxqxL * pypyL - pyqxL * pxpyL ) / detPL );
matL.xy( ( pyqxL * pxpxL - pxqxL * pxpyL ) / detPL );
matL.yx( ( pxqyL * pypyL - pyqyL * pxpyL ) / detPL );
matL.yy( ( pyqyL * pxpxL - pxqyL * pxpyL ) / detPL );
}
*/
/* compute det first */
uint32 bitsTaken0L = bts_maxAbsIntLog2Of4( pxpxL, pypyL, pxpyL, pxpyL ) + 1;
int32 shL = 0;
int32 detL = 0;
int32 detBbpL = 0;
if( bitsTaken0L > 15 )
{
shL = bitsTaken0L - 15;
}
detL = ( pxpxL >> shL ) * ( pypyL >> shL ) - ( pxpyL >> shL ) * ( pxpyL >> shL );
/* this can be negative */
detBbpL = ( srcBbpL - shL ) << 1;
/* reduce to 15 bit */
shL = ( int32 )bts_absIntLog2( detL );
if( shL > 15 )
{
detL >>= ( shL - 15 );
detBbpL -= ( shL - 15 );
}
if( detL != 0 )
{
int32 sh0L, sh1L, xxL, xyL, yxL, yyL, bbp_enumL;
uint32 bitsTaken1L, highestBitL;
sh0L = 0;
if( bitsTaken0L > 15 )
{
sh0L = bitsTaken0L - 15;
}
bitsTaken1L = bts_maxAbsIntLog2Of4( pxqxL, pxqyL, pyqxL, pyqyL ) + 1;
sh1L = 0;
if( bitsTaken1L > 15 )
{
sh1L = bitsTaken1L - 15;
}
xxL = ( pxqxL >> sh1L ) * ( pypyL >> sh0L ) - ( pyqxL >> sh1L ) * ( pxpyL >> sh0L );
xyL = ( pyqxL >> sh1L ) * ( pxpxL >> sh0L ) - ( pxqxL >> sh1L ) * ( pxpyL >> sh0L );
yxL = ( pxqyL >> sh1L ) * ( pypyL >> sh0L ) - ( pyqyL >> sh1L ) * ( pxpyL >> sh0L );
yyL = ( pyqyL >> sh1L ) * ( pxpxL >> sh0L ) - ( pxqyL >> sh1L ) * ( pxpyL >> sh0L );
/* again, can be negative */
bbp_enumL = ( srcBbpL - sh0L ) + ( bbs_max( srcBbpL, dstBbpL ) - sh1L );
highestBitL = bts_maxAbsIntLog2Of4( xxL, xyL, yxL, yyL ) + 1;
/* shift left */
xxL <<= ( 31 - highestBitL );
xyL <<= ( 31 - highestBitL );
yxL <<= ( 31 - highestBitL );
yyL <<= ( 31 - highestBitL );
bbp_enumL += ( 31 - highestBitL );
xxL /= detL;
xyL /= detL;
yxL /= detL;
yyL /= detL;
bbp_enumL -= detBbpL;
altL.matE = bts_Flt16Mat2D_create32( xxL, xyL, yxL, yyL, bbp_enumL );
}
cpMappedL = bts_Flt16Mat2D_mapFlt( &altL.matE, &cpL );
altL.vecE = bts_Flt16Vec2D_sub( cqL, cpMappedL );
}
return altL;
}
default:
{
bbs_ERROR1( "struct bts_Flt16Alt2D bts_Cluster2D_alt( ... ):\n"
"altType %d is not handled", altTypeL );
}
}
return altL;
}
/* ------------------------------------------------------------------------- */
/* ========================================================================= */