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/Flt16Alt2D.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_Flt16Alt2D_init( struct bts_Flt16Alt2D* ptrA ) 43 { 44 bts_Flt16Mat2D_init( &ptrA->matE ); 45 bts_Flt16Vec2D_init( &ptrA->vecE ); 46 } 47 48 /* ------------------------------------------------------------------------- */ 49 50 void bts_Flt16Alt2D_exit( struct bts_Flt16Alt2D* ptrA ) 51 { 52 bts_Flt16Mat2D_exit( &ptrA->matE ); 53 bts_Flt16Vec2D_exit( &ptrA->vecE ); 54 } 55 56 /* ------------------------------------------------------------------------- */ 57 58 /* ========================================================================= */ 59 /* */ 60 /* ---- \ghd{ operators } -------------------------------------------------- */ 61 /* */ 62 /* ========================================================================= */ 63 64 /* ------------------------------------------------------------------------- */ 65 66 /* ------------------------------------------------------------------------- */ 67 68 void bts_Flt16Alt2D_copy( struct bts_Flt16Alt2D* ptrA, const struct bts_Flt16Alt2D* srcPtrA ) 69 { 70 bts_Flt16Mat2D_copy( &ptrA->matE, &srcPtrA->matE ); 71 bts_Flt16Vec2D_copy( &ptrA->vecE, &srcPtrA->vecE ); 72 } 73 74 /* ------------------------------------------------------------------------- */ 75 76 flag bts_Flt16Alt2D_equal( const struct bts_Flt16Alt2D* ptrA, const struct bts_Flt16Alt2D* srcPtrA ) 77 { 78 if( ! bts_Flt16Mat2D_equal( &ptrA->matE, &srcPtrA->matE ) ) return FALSE; 79 if( ! bts_Flt16Vec2D_equal( &ptrA->vecE, &srcPtrA->vecE ) ) return FALSE; 80 return TRUE; 81 } 82 83 /* ========================================================================= */ 84 /* */ 85 /* ---- \ghd{ query functions } -------------------------------------------- */ 86 /* */ 87 /* ========================================================================= */ 88 89 /* ------------------------------------------------------------------------- */ 90 91 /* ========================================================================= */ 92 /* */ 93 /* ---- \ghd{ modify functions } ------------------------------------------- */ 94 /* */ 95 /* ========================================================================= */ 96 97 /* ------------------------------------------------------------------------- */ 98 99 /* ========================================================================= */ 100 /* */ 101 /* ---- \ghd{ I/O } -------------------------------------------------------- */ 102 /* */ 103 /* ========================================================================= */ 104 105 /* ------------------------------------------------------------------------- */ 106 107 uint32 bts_Flt16Alt2D_memSize( struct bbs_Context* cpA, 108 const struct bts_Flt16Alt2D *ptrA ) 109 { 110 bbs_ERROR0( "unimplemented function" ); 111 return 0; 112 } 113 114 /* ------------------------------------------------------------------------- */ 115 116 uint32 bts_Flt16Alt2D_memWrite( struct bbs_Context* cpA, 117 const struct bts_Flt16Alt2D* ptrA, 118 uint16* memPtrA ) 119 { 120 bbs_ERROR0( "unimplemented function" ); 121 return 0; 122 } 123 124 /* ------------------------------------------------------------------------- */ 125 126 uint32 bts_Flt16Alt2D_memRead( struct bbs_Context* cpA, 127 struct bts_Flt16Alt2D* ptrA, 128 const uint16* memPtrA ) 129 { 130 if( bbs_Context_error( cpA ) ) return 0; 131 bbs_ERROR0( "unimplemented function" ); 132 return 0; 133 } 134 135 /* ------------------------------------------------------------------------- */ 136 137 /* ========================================================================= */ 138 /* */ 139 /* ---- \ghd{ exec functions } --------------------------------------------- */ 140 /* */ 141 /* ========================================================================= */ 142 143 /* ------------------------------------------------------------------------- */ 144 145 struct bts_Flt16Alt2D bts_Flt16Alt2D_createIdentity() 146 { 147 struct bts_Flt16Alt2D altL = { { 1, 0, 0, 1, 0 }, { 0, 0, 0 } }; 148 return altL; 149 } 150 151 /* ------------------------------------------------------------------------- */ 152 153 struct bts_Flt16Alt2D bts_Flt16Alt2D_createRotation( phase16 angleA, 154 const struct bts_Flt16Vec2D* centerPtrA ) 155 { 156 struct bts_Flt16Alt2D altL; 157 altL.matE = bts_Flt16Mat2D_createRotation( angleA ); 158 altL.vecE = bts_Flt16Vec2D_sub( *centerPtrA, bts_Flt16Mat2D_mapFlt( &altL.matE, centerPtrA ) ); 159 return altL; 160 } 161 162 /* ------------------------------------------------------------------------- */ 163 164 struct bts_Flt16Alt2D bts_Flt16Alt2D_createScale( int32 scaleA, 165 int32 scaleBbpA, 166 const struct bts_Flt16Vec2D* centerPtrA ) 167 { 168 struct bts_Flt16Alt2D altL; 169 altL.matE = bts_Flt16Mat2D_createScale( scaleA, scaleBbpA ); 170 altL.vecE = bts_Flt16Vec2D_sub( *centerPtrA, bts_Flt16Mat2D_mapFlt( &altL.matE, centerPtrA ) ); 171 return altL; 172 } 173 174 /* ------------------------------------------------------------------------- */ 175 176 struct bts_Flt16Alt2D bts_Flt16Alt2D_createRigid( phase16 angleA, 177 int32 scaleA, 178 int32 scaleBbpA, 179 const struct bts_Flt16Vec2D* centerPtrA ) 180 { 181 struct bts_Flt16Alt2D altL; 182 altL.matE = bts_Flt16Mat2D_createRigid( angleA, scaleA, scaleBbpA ); 183 altL.vecE = bts_Flt16Vec2D_sub( *centerPtrA, bts_Flt16Mat2D_mapFlt( &altL.matE, centerPtrA ) ); 184 return altL; 185 } 186 187 /* ------------------------------------------------------------------------- */ 188 189 struct bts_Flt16Alt2D bts_Flt16Alt2D_createRigidMap( struct bts_Flt16Vec2D vecIn1A, 190 struct bts_Flt16Vec2D vecIn2A, 191 struct bts_Flt16Vec2D vecOut1A, 192 struct bts_Flt16Vec2D vecOut2A ) 193 { 194 struct bts_Flt16Vec2D diffInL = bts_Flt16Vec2D_sub( vecIn1A, vecIn2A ); 195 struct bts_Flt16Vec2D diffOutL = bts_Flt16Vec2D_sub( vecOut1A, vecOut2A ); 196 struct bts_Flt16Vec2D centerInL = bts_Flt16Vec2D_mul( bts_Flt16Vec2D_add( vecIn1A, vecIn2A ), 1, 1 ); /* mul by 0.5 */ 197 struct bts_Flt16Vec2D centerOutL = bts_Flt16Vec2D_mul( bts_Flt16Vec2D_add( vecOut1A, vecOut2A ), 1, 1 ); /* mul by 0.5 */ 198 199 struct bts_Flt16Vec2D transL = bts_Flt16Vec2D_sub( centerOutL, centerInL ); 200 phase16 angleL = bts_Flt16Vec2D_enclosedAngle( &diffOutL, &diffInL ); 201 uint32 normInL = bts_Flt16Vec2D_norm( &diffInL ); 202 uint32 normOutL = bts_Flt16Vec2D_norm( &diffOutL ); 203 204 uint32 scaleL = ( normInL > 0 ) ? ( normOutL << 16 ) / normInL : 0xFFFFFFFF; 205 uint32 scaleBbpL = 16 + diffOutL.bbpE - diffInL.bbpE; 206 207 struct bts_Flt16Alt2D altL; 208 209 /* fit scale factor in 15 bit */ 210 uint32 scaleExpL = bbs_intLog2( scaleL ); 211 if( scaleExpL > 14 ) 212 { 213 scaleL >>= scaleExpL - 14; 214 scaleBbpL -= scaleExpL - 14; 215 } 216 217 altL = bts_Flt16Alt2D_createRigid( angleL, ( int16 )scaleL, scaleBbpL, ¢erInL ); 218 219 altL.vecE = bts_Flt16Vec2D_add( altL.vecE, transL ); 220 221 return altL; 222 } 223 224 /* ------------------------------------------------------------------------- */ 225 226 struct bts_Flt16Alt2D bts_Flt16Alt2D_create16( int16 xxA, 227 int16 xyA, 228 int16 yxA, 229 int16 yyA, 230 int16 matBbpA, 231 int16 xA, 232 int16 yA, 233 int16 vecBbpA ) 234 { 235 struct bts_Flt16Alt2D altL; 236 altL.matE = bts_Flt16Mat2D_create16( xxA, xyA, yxA, yyA, matBbpA ); 237 altL.vecE = bts_Flt16Vec2D_create16( xA, yA, vecBbpA ); 238 return altL; 239 } 240 241 /* ------------------------------------------------------------------------- */ 242 243 struct bts_Flt16Alt2D bts_Flt16Alt2D_create32( int32 xxA, 244 int32 xyA, 245 int32 yxA, 246 int32 yyA, 247 int32 matBbpA, 248 int32 xA, 249 int32 yA, 250 int32 vecBbpA ) 251 { 252 struct bts_Flt16Alt2D altL; 253 altL.matE = bts_Flt16Mat2D_create32( xxA, xyA, yxA, yyA, matBbpA ); 254 altL.vecE = bts_Flt16Vec2D_create32( xA, yA, vecBbpA ); 255 return altL; 256 } 257 258 /* ------------------------------------------------------------------------- */ 259 260 struct bts_Flt16Vec2D bts_Flt16Alt2D_mapFlt( const struct bts_Flt16Alt2D* altPtrA, 261 const struct bts_Flt16Vec2D* vecPtrA ) 262 { 263 return bts_Flt16Vec2D_add( altPtrA->vecE, bts_Flt16Mat2D_mapFlt( &altPtrA->matE, vecPtrA ) ); 264 } 265 266 /* ------------------------------------------------------------------------- */ 267 268 struct bts_Flt16Alt2D bts_Flt16Alt2D_mul( const struct bts_Flt16Alt2D* alt1PtrA, 269 const struct bts_Flt16Alt2D* alt2PtrA ) 270 { 271 struct bts_Flt16Alt2D altL; 272 altL.vecE = bts_Flt16Alt2D_mapFlt( alt1PtrA, &alt2PtrA->vecE ); 273 altL.matE = bts_Flt16Mat2D_mul( &alt1PtrA->matE, &alt2PtrA->matE ); 274 return altL; 275 } 276 277 /* ------------------------------------------------------------------------- */ 278 279 /** multiplies matrix with matA; returns pointer to resulting matrix */ 280 struct bts_Flt16Alt2D* bts_Flt16Alt2D_mulTo( struct bts_Flt16Alt2D* alt1PtrA, 281 const struct bts_Flt16Alt2D* alt2PtrA ) 282 { 283 *alt1PtrA = bts_Flt16Alt2D_mul( alt1PtrA, alt2PtrA ); 284 return alt1PtrA; 285 } 286 287 /* ------------------------------------------------------------------------- */ 288 289 void bts_Flt16Alt2D_invert( struct bts_Flt16Alt2D* ptrA ) 290 { 291 bts_Flt16Mat2D_invert( &ptrA->matE ); 292 293 ptrA->vecE = bts_Flt16Vec2D_create32( - ( ( ( int32 ) ptrA->matE.xxE * ptrA->vecE.xE + ( int32 ) ptrA->matE.xyE * ptrA->vecE.yE ) ), 294 - ( ( ( int32 ) ptrA->matE.yxE * ptrA->vecE.xE + ( int32 ) ptrA->matE.yyE * ptrA->vecE.yE ) ), 295 ptrA->vecE.bbpE + ptrA->matE.bbpE ); 296 } 297 298 /* ------------------------------------------------------------------------- */ 299 300 struct bts_Flt16Alt2D bts_Flt16Alt2D_inverted( const struct bts_Flt16Alt2D* ptrA ) 301 { 302 struct bts_Flt16Alt2D altL = *ptrA; 303 bts_Flt16Alt2D_invert( &altL ); 304 return altL; 305 } 306 307 /* ------------------------------------------------------------------------- */ 308 309 /* ========================================================================= */ 310 311 312
