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 42 void bts_Flt16Vec2D_init( struct bts_Flt16Vec2D* ptrA ) 43 { 44 ptrA->xE = 0; 45 ptrA->yE = 0; 46 ptrA->bbpE = 0; 47 } 48 49 /* ------------------------------------------------------------------------- */ 50 51 void 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 68 void 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 77 flag 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 148 struct 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 159 struct 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 170 struct 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 201 int32 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 209 uint32 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 216 uint16 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 223 void 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 233 struct 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 242 phase16 bts_Flt16Vec2D_angle( const struct bts_Flt16Vec2D* vecPtrA ) 243 { 244 return bbs_phase16( vecPtrA->xE, vecPtrA->yE ); 245 } 246 247 /* ------------------------------------------------------------------------- */ 248 249 phase16 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 259 struct 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 282 struct 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 305 struct 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 314 struct 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
