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/Flt16Mat2D.h" 20 #include "b_TensorEm/Functions.h" 21 #include "b_BasicEm/Math.h" 22 23 /* ------------------------------------------------------------------------- */ 24 25 /* ========================================================================= */ 26 /* */ 27 /* ---- \ghd{ auxiliary functions } ---------------------------------------- */ 28 /* */ 29 /* ========================================================================= */ 30 31 /* ------------------------------------------------------------------------- */ 32 33 /* ========================================================================= */ 34 /* */ 35 /* ---- \ghd{ constructor / destructor } ----------------------------------- */ 36 /* */ 37 /* ========================================================================= */ 38 39 /* ------------------------------------------------------------------------- */ 40 41 void bts_Flt16Mat2D_init( struct bts_Flt16Mat2D* ptrA ) 42 { 43 ptrA->bbpE = 0; 44 ptrA->xxE = 0; 45 ptrA->xyE = 0; 46 ptrA->yxE = 0; 47 ptrA->yyE = 0; 48 } 49 50 /* ------------------------------------------------------------------------- */ 51 52 void bts_Flt16Mat2D_exit( struct bts_Flt16Mat2D* ptrA ) 53 { 54 ptrA->bbpE = 0; 55 ptrA->xxE = 0; 56 ptrA->xyE = 0; 57 ptrA->yxE = 0; 58 ptrA->yyE = 0; 59 } 60 61 /* ------------------------------------------------------------------------- */ 62 63 /* ========================================================================= */ 64 /* */ 65 /* ---- \ghd{ operators } -------------------------------------------------- */ 66 /* */ 67 /* ========================================================================= */ 68 69 /* ------------------------------------------------------------------------- */ 70 71 void bts_Flt16Mat2D_copy( struct bts_Flt16Mat2D* ptrA, const struct bts_Flt16Mat2D* srcPtrA ) 72 { 73 ptrA->bbpE = srcPtrA->bbpE; 74 ptrA->xxE = srcPtrA->xxE; 75 ptrA->xyE = srcPtrA->xyE; 76 ptrA->yxE = srcPtrA->yxE; 77 ptrA->yyE = srcPtrA->yyE; 78 } 79 80 /* ------------------------------------------------------------------------- */ 81 82 flag bts_Flt16Mat2D_equal( const struct bts_Flt16Mat2D* ptrA, const struct bts_Flt16Mat2D* srcPtrA ) 83 { 84 int32 bbpDiffL = ptrA->bbpE - srcPtrA->bbpE; 85 if( bbpDiffL == 0 ) 86 { 87 if( ptrA->xxE != srcPtrA->xxE ) return FALSE; 88 if( ptrA->xyE != srcPtrA->xyE ) return FALSE; 89 if( ptrA->yxE != srcPtrA->yxE ) return FALSE; 90 if( ptrA->yyE != srcPtrA->yyE ) return FALSE; 91 return TRUE; 92 } 93 94 if( bbpDiffL > 0 ) 95 { 96 int32 xxL = ( int32 ) srcPtrA->xxE << bbpDiffL; 97 int32 xyL = ( int32 ) srcPtrA->xyE << bbpDiffL; 98 int32 yxL = ( int32 ) srcPtrA->yxE << bbpDiffL; 99 int32 yyL = ( int32 ) srcPtrA->yyE << bbpDiffL; 100 101 if( ptrA->xxE != xxL ) return FALSE; 102 if( ptrA->xyE != xyL ) return FALSE; 103 if( ptrA->yxE != yxL ) return FALSE; 104 if( ptrA->yyE != yyL ) return FALSE; 105 106 /* check if bits were lost by the shifting */ 107 if( srcPtrA->xxE != ( xxL >> bbpDiffL ) ) return FALSE; 108 if( srcPtrA->xyE != ( xyL >> bbpDiffL ) ) return FALSE; 109 if( srcPtrA->yxE != ( yxL >> bbpDiffL ) ) return FALSE; 110 if( srcPtrA->yyE != ( yyL >> bbpDiffL ) ) return FALSE; 111 112 return TRUE; 113 } 114 115 if( bbpDiffL < 0 ) 116 { 117 int32 xxL = ( int32 ) ptrA->xxE << -bbpDiffL; 118 int32 xyL = ( int32 ) ptrA->xyE << -bbpDiffL; 119 int32 yxL = ( int32 ) ptrA->yxE << -bbpDiffL; 120 int32 yyL = ( int32 ) ptrA->yyE << -bbpDiffL; 121 122 if( xxL != srcPtrA->xxE ) return FALSE; 123 if( xyL != srcPtrA->xyE ) return FALSE; 124 if( yxL != srcPtrA->yxE ) return FALSE; 125 if( yyL != srcPtrA->yyE ) return FALSE; 126 127 /* check if bits were lost by the shifting */ 128 if( ptrA->xxE != ( xxL >> -bbpDiffL ) ) return FALSE; 129 if( ptrA->xyE != ( xyL >> -bbpDiffL ) ) return FALSE; 130 if( ptrA->yxE != ( yxL >> -bbpDiffL ) ) return FALSE; 131 if( ptrA->yyE != ( yyL >> -bbpDiffL ) ) return FALSE; 132 133 return TRUE; 134 } 135 136 return TRUE; 137 } 138 139 /* ========================================================================= */ 140 /* */ 141 /* ---- \ghd{ query functions } -------------------------------------------- */ 142 /* */ 143 /* ========================================================================= */ 144 145 /* ------------------------------------------------------------------------- */ 146 147 /* ========================================================================= */ 148 /* */ 149 /* ---- \ghd{ modify functions } ------------------------------------------- */ 150 /* */ 151 /* ========================================================================= */ 152 153 /* ------------------------------------------------------------------------- */ 154 155 /* ========================================================================= */ 156 /* */ 157 /* ---- \ghd{ I/O } -------------------------------------------------------- */ 158 /* */ 159 /* ========================================================================= */ 160 161 /* ------------------------------------------------------------------------- */ 162 163 /* ========================================================================= */ 164 /* */ 165 /* ---- \ghd{ exec functions } --------------------------------------------- */ 166 /* */ 167 /* ========================================================================= */ 168 169 /* ------------------------------------------------------------------------- */ 170 171 uint32 bts_Flt16Mat2D_det( const struct bts_Flt16Mat2D* ptrA ) 172 { 173 /* This could be negativ, in theory. But almost always det > 0 for us, 174 matrix is a rotation or scaling matrix. 175 Then uint32 makes sure there is no overflow. */ 176 uint32 detL = ( int32 ) ptrA->xxE * ptrA->yyE - ( int32 ) ptrA->xyE * ptrA->yxE; 177 return detL; 178 } 179 180 /* ------------------------------------------------------------------------- */ 181 182 struct bts_Flt16Mat2D bts_Flt16Mat2D_createIdentity() 183 { 184 struct bts_Flt16Mat2D matL = { 1 << 14, 0, 0, 1 << 14, 14 }; 185 return matL; 186 } 187 188 /* ------------------------------------------------------------------------- */ 189 190 struct bts_Flt16Mat2D bts_Flt16Mat2D_createRotation( phase16 angleA ) 191 { 192 int16 cL = bbs_cos16( angleA ); 193 int16 sL = bbs_sin16( angleA ); 194 struct bts_Flt16Mat2D matL; 195 matL.xxE = cL; 196 matL.xyE = -sL; 197 matL.yxE = sL; 198 matL.yyE = cL; 199 matL.bbpE = 14; 200 return matL; 201 } 202 203 /* ------------------------------------------------------------------------- */ 204 205 struct bts_Flt16Mat2D bts_Flt16Mat2D_createScale( int32 scaleA, int32 scaleBbpA ) 206 { 207 struct bts_Flt16Mat2D matL = bts_Flt16Mat2D_createIdentity(); 208 bts_Flt16Mat2D_scale( &matL, scaleA, scaleBbpA ); 209 return matL; 210 } 211 212 /* ------------------------------------------------------------------------- */ 213 214 struct bts_Flt16Mat2D bts_Flt16Mat2D_createRigid( phase16 angleA, int32 scaleA, int32 scaleBbpA ) 215 { 216 struct bts_Flt16Mat2D matL = bts_Flt16Mat2D_createRotation( angleA ); 217 bts_Flt16Mat2D_scale( &matL, scaleA, scaleBbpA ); 218 return matL; 219 } 220 221 /* ------------------------------------------------------------------------- */ 222 223 struct bts_Flt16Mat2D bts_Flt16Mat2D_create16( int16 xxA, int16 xyA, int16 yxA, int16 yyA, int16 bbpA ) 224 { 225 struct bts_Flt16Mat2D matL; 226 matL.xxE = xxA; 227 matL.xyE = xyA; 228 matL.yxE = yxA; 229 matL.yyE = yyA; 230 matL.bbpE = bbpA; 231 return matL; 232 } 233 234 /* ------------------------------------------------------------------------- */ 235 236 struct bts_Flt16Mat2D bts_Flt16Mat2D_create32( int32 xxA, int32 xyA, int32 yxA, int32 yyA, int32 bbpA ) 237 { 238 struct bts_Flt16Mat2D matL; 239 240 if( ( xxA | xyA | yxA | yyA ) == 0 ) 241 { 242 matL.xxE = 0; 243 matL.xyE = 0; 244 matL.yxE = 0; 245 matL.yyE = 0; 246 matL.bbpE = 0; 247 } 248 else 249 { 250 int32 shiftL = bts_maxAbsIntLog2Of4( xxA, xyA, yxA, yyA ) - 13; 251 252 if( shiftL > 0 ) 253 { 254 int32 sh1L = shiftL - 1; 255 matL.xxE = ( ( xxA >> sh1L ) + 1 ) >> 1; 256 matL.xyE = ( ( xyA >> sh1L ) + 1 ) >> 1; 257 matL.yxE = ( ( yxA >> sh1L ) + 1 ) >> 1; 258 matL.yyE = ( ( yyA >> sh1L ) + 1 ) >> 1; 259 } 260 else 261 { 262 matL.xxE = xxA << -shiftL; 263 matL.xyE = xyA << -shiftL; 264 matL.yxE = yxA << -shiftL; 265 matL.yyE = yyA << -shiftL; 266 } 267 268 matL.bbpE = bbpA - shiftL; 269 } 270 271 return matL; 272 } 273 274 /* ------------------------------------------------------------------------- */ 275 276 void bts_Flt16Mat2D_scale( struct bts_Flt16Mat2D* ptrA, int32 scaleA, int32 scaleBbpA ) 277 { 278 /* fit scale in 15 bit */ 279 uint32 scaleExpL = bts_absIntLog2( scaleA ); 280 if( scaleExpL > 14 ) 281 { 282 int32 shiftL = scaleExpL - 14; 283 scaleA = ( ( scaleA >> ( shiftL - 1 ) ) + 1 ) >> 1; 284 scaleBbpA -= shiftL; 285 } 286 287 *ptrA = bts_Flt16Mat2D_create32( (int32)ptrA->xxE * scaleA, 288 (int32)ptrA->xyE * scaleA, 289 (int32)ptrA->yxE * scaleA, 290 (int32)ptrA->yyE * scaleA, 291 ptrA->bbpE + scaleBbpA ); 292 } 293 294 /* ------------------------------------------------------------------------- */ 295 296 struct bts_Int16Vec2D bts_Flt16Mat2D_map( const struct bts_Flt16Mat2D* matPtrA, 297 const struct bts_Int16Vec2D* vecPtrA ) 298 { 299 struct bts_Int16Vec2D vecL; 300 301 int32 xL = ( int32 ) matPtrA->xxE * vecPtrA->xE + ( int32 ) matPtrA->xyE * vecPtrA->yE; 302 int32 yL = ( int32 ) matPtrA->yxE * vecPtrA->xE + ( int32 ) matPtrA->yyE * vecPtrA->yE; 303 304 if( matPtrA->bbpE > 0 ) 305 { 306 int32 sh1L = matPtrA->bbpE - 1; 307 vecL.xE = ( ( xL >> sh1L ) + 1 ) >> 1; 308 vecL.yE = ( ( yL >> sh1L ) + 1 ) >> 1; 309 } 310 else 311 { 312 /* not overflow safe */ 313 vecL.xE = xL << -matPtrA->bbpE; 314 vecL.yE = yL << -matPtrA->bbpE; 315 } 316 317 return vecL; 318 } 319 320 /* ------------------------------------------------------------------------- */ 321 322 struct bts_Flt16Vec2D bts_Flt16Mat2D_mapFlt( const struct bts_Flt16Mat2D* matPtrA, 323 const struct bts_Flt16Vec2D* vecPtrA ) 324 { 325 int32 xL = ( int32 ) matPtrA->xxE * vecPtrA->xE + ( int32 ) matPtrA->xyE * vecPtrA->yE; 326 int32 yL = ( int32 ) matPtrA->yxE * vecPtrA->xE + ( int32 ) matPtrA->yyE * vecPtrA->yE; 327 int32 bbpL = matPtrA->bbpE + vecPtrA->bbpE; 328 return bts_Flt16Vec2D_create32( xL, yL, bbpL ); 329 } 330 331 /* ------------------------------------------------------------------------- */ 332 333 struct bts_Flt16Mat2D bts_Flt16Mat2D_mul( const struct bts_Flt16Mat2D* mat1PtrA, 334 const struct bts_Flt16Mat2D* mat2PtrA ) 335 { 336 return bts_Flt16Mat2D_create32( ( int32 ) mat1PtrA->xxE * mat2PtrA->xxE + ( int32 ) mat1PtrA->xyE * mat2PtrA->yxE, 337 ( int32 ) mat1PtrA->xxE * mat2PtrA->xyE + ( int32 ) mat1PtrA->xyE * mat2PtrA->yyE, 338 ( int32 ) mat1PtrA->yxE * mat2PtrA->xxE + ( int32 ) mat1PtrA->yyE * mat2PtrA->yxE, 339 ( int32 ) mat1PtrA->yxE * mat2PtrA->xyE + ( int32 ) mat1PtrA->yyE * mat2PtrA->yyE, 340 mat1PtrA->bbpE + mat2PtrA->bbpE ); 341 } 342 343 /* ------------------------------------------------------------------------- */ 344 345 struct bts_Flt16Mat2D* bts_Flt16Mat2D_mulTo( struct bts_Flt16Mat2D* mat1PtrA, 346 const struct bts_Flt16Mat2D* mat2PtrA ) 347 { 348 *mat1PtrA = bts_Flt16Mat2D_mul( mat1PtrA, mat2PtrA ); 349 return mat1PtrA; 350 } 351 352 /* ------------------------------------------------------------------------- */ 353 354 void bts_Flt16Mat2D_invert( struct bts_Flt16Mat2D* ptrA ) 355 { 356 int32 detL = ( int32 ) ptrA->xxE * ptrA->yyE - ( int32 ) ptrA->xyE * ptrA->yxE; 357 int32 detExpL = bbs_intLog2( detL ); 358 int32 dShrL = 0; 359 if( detExpL > 15 ) 360 { 361 dShrL = detExpL - 15; 362 detL = ( ( detL >> ( dShrL - 1 ) ) + 1 ) >> 1; 363 } 364 365 if( detL == 0 ) 366 { 367 ptrA->xxE = ptrA->yyE = ptrA->xyE = ptrA->yxE = 0; 368 } 369 else 370 { 371 /* bbp: bbpE + 16 - ( bbpE * 2 - dShrL ) = 16 + dShrL - bbpE */ 372 int32 xxL = ( ( int32 )ptrA->xxE << 16 ) / detL; 373 int32 xyL = ( ( int32 )ptrA->xyE << 16 ) / detL; 374 int32 yxL = ( ( int32 )ptrA->yxE << 16 ) / detL; 375 int32 yyL = ( ( int32 )ptrA->yyE << 16 ) / detL; 376 *ptrA = bts_Flt16Mat2D_create32( xxL, -xyL, -yxL, yyL, 16 + dShrL - ptrA->bbpE ); 377 } 378 } 379 380 /* ------------------------------------------------------------------------- */ 381 382 struct bts_Flt16Mat2D bts_Flt16Mat2D_inverted( const struct bts_Flt16Mat2D* ptrA ) 383 { 384 struct bts_Flt16Mat2D matL = *ptrA; 385 bts_Flt16Mat2D_invert( &matL ); 386 return matL; 387 } 388 389 /* ------------------------------------------------------------------------- */ 390 391 /* ========================================================================= */ 392 393 394
