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