1 /* 2 * Copyright (C) 2011 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 /* $Id: db_utilities_poly.h,v 1.2 2010/09/03 12:00:11 bsouthall Exp $ */ 18 19 #ifndef DB_UTILITIES_POLY 20 #define DB_UTILITIES_POLY 21 22 #include "db_utilities.h" 23 24 25 26 /***************************************************************** 27 * Lean and mean begins here * 28 *****************************************************************/ 29 /*! 30 * \defgroup LMPolynomial (LM) Polynomial utilities (solvers, arithmetic, evaluation, etc.) 31 */ 32 /*\{*/ 33 34 /*! 35 In debug mode closed form quadratic solving takes on the order of 15 microseconds 36 while eig of the companion matrix takes about 1.1 milliseconds 37 Speed-optimized code in release mode solves a quadratic in 0.3 microseconds on 450MHz 38 */ 39 inline void db_SolveQuadratic(double *roots,int *nr_roots,double a,double b,double c) 40 { 41 double rs,srs,q; 42 43 /*For non-degenerate quadratics 44 [5 mult 2 add 1 sqrt=7flops 1func]*/ 45 if(a==0.0) 46 { 47 if(b==0.0) *nr_roots=0; 48 else 49 { 50 roots[0]= -c/b; 51 *nr_roots=1; 52 } 53 } 54 else 55 { 56 rs=b*b-4.0*a*c; 57 if(rs>=0.0) 58 { 59 *nr_roots=2; 60 srs=sqrt(rs); 61 q= -0.5*(b+db_sign(b)*srs); 62 roots[0]=q/a; 63 /*If b is zero db_sign(b) returns 1, 64 so q is only zero when b=0 and c=0*/ 65 if(q==0.0) *nr_roots=1; 66 else roots[1]=c/q; 67 } 68 else *nr_roots=0; 69 } 70 } 71 72 /*! 73 In debug mode closed form cubic solving takes on the order of 45 microseconds 74 while eig of the companion matrix takes about 1.3 milliseconds 75 Speed-optimized code in release mode solves a cubic in 1.5 microseconds on 450MHz 76 For a non-degenerate cubic with two roots, the first root is the single root and 77 the second root is the double root 78 */ 79 DB_API void db_SolveCubic(double *roots,int *nr_roots,double a,double b,double c,double d); 80 /*! 81 In debug mode closed form quartic solving takes on the order of 0.1 milliseconds 82 while eig of the companion matrix takes about 1.5 milliseconds 83 Speed-optimized code in release mode solves a quartic in 2.6 microseconds on 450MHz*/ 84 DB_API void db_SolveQuartic(double *roots,int *nr_roots,double a,double b,double c,double d,double e); 85 /*! 86 Quartic solving where a solution is forced when splitting into quadratics, which 87 can be good if the quartic is sometimes in fact a quadratic, such as in absolute orientation 88 when the data is planar*/ 89 DB_API void db_SolveQuarticForced(double *roots,int *nr_roots,double a,double b,double c,double d,double e); 90 91 inline double db_PolyEval1(const double p[2],double x) 92 { 93 return(p[0]+x*p[1]); 94 } 95 96 inline void db_MultiplyPoly1_1(double *d,const double *a,const double *b) 97 { 98 double a0,a1; 99 double b0,b1; 100 a0=a[0];a1=a[1]; 101 b0=b[0];b1=b[1]; 102 103 d[0]=a0*b0; 104 d[1]=a0*b1+a1*b0; 105 d[2]= a1*b1; 106 } 107 108 inline void db_MultiplyPoly0_2(double *d,const double *a,const double *b) 109 { 110 double a0; 111 double b0,b1,b2; 112 a0=a[0]; 113 b0=b[0];b1=b[1];b2=b[2]; 114 115 d[0]=a0*b0; 116 d[1]=a0*b1; 117 d[2]=a0*b2; 118 } 119 120 inline void db_MultiplyPoly1_2(double *d,const double *a,const double *b) 121 { 122 double a0,a1; 123 double b0,b1,b2; 124 a0=a[0];a1=a[1]; 125 b0=b[0];b1=b[1];b2=b[2]; 126 127 d[0]=a0*b0; 128 d[1]=a0*b1+a1*b0; 129 d[2]=a0*b2+a1*b1; 130 d[3]= a1*b2; 131 } 132 133 134 inline void db_MultiplyPoly1_3(double *d,const double *a,const double *b) 135 { 136 double a0,a1; 137 double b0,b1,b2,b3; 138 a0=a[0];a1=a[1]; 139 b0=b[0];b1=b[1];b2=b[2];b3=b[3]; 140 141 d[0]=a0*b0; 142 d[1]=a0*b1+a1*b0; 143 d[2]=a0*b2+a1*b1; 144 d[3]=a0*b3+a1*b2; 145 d[4]= a1*b3; 146 } 147 /*! 148 Multiply d=a*b where a is one degree and b is two degree*/ 149 inline void db_AddPolyProduct0_1(double *d,const double *a,const double *b) 150 { 151 double a0; 152 double b0,b1; 153 a0=a[0]; 154 b0=b[0];b1=b[1]; 155 156 d[0]+=a0*b0; 157 d[1]+=a0*b1; 158 } 159 inline void db_AddPolyProduct0_2(double *d,const double *a,const double *b) 160 { 161 double a0; 162 double b0,b1,b2; 163 a0=a[0]; 164 b0=b[0];b1=b[1];b2=b[2]; 165 166 d[0]+=a0*b0; 167 d[1]+=a0*b1; 168 d[2]+=a0*b2; 169 } 170 /*! 171 Multiply d=a*b where a is one degree and b is two degree*/ 172 inline void db_SubtractPolyProduct0_0(double *d,const double *a,const double *b) 173 { 174 double a0; 175 double b0; 176 a0=a[0]; 177 b0=b[0]; 178 179 d[0]-=a0*b0; 180 } 181 182 inline void db_SubtractPolyProduct0_1(double *d,const double *a,const double *b) 183 { 184 double a0; 185 double b0,b1; 186 a0=a[0]; 187 b0=b[0];b1=b[1]; 188 189 d[0]-=a0*b0; 190 d[1]-=a0*b1; 191 } 192 193 inline void db_SubtractPolyProduct0_2(double *d,const double *a,const double *b) 194 { 195 double a0; 196 double b0,b1,b2; 197 a0=a[0]; 198 b0=b[0];b1=b[1];b2=b[2]; 199 200 d[0]-=a0*b0; 201 d[1]-=a0*b1; 202 d[2]-=a0*b2; 203 } 204 205 inline void db_SubtractPolyProduct1_3(double *d,const double *a,const double *b) 206 { 207 double a0,a1; 208 double b0,b1,b2,b3; 209 a0=a[0];a1=a[1]; 210 b0=b[0];b1=b[1];b2=b[2];b3=b[3]; 211 212 d[0]-=a0*b0; 213 d[1]-=a0*b1+a1*b0; 214 d[2]-=a0*b2+a1*b1; 215 d[3]-=a0*b3+a1*b2; 216 d[4]-= a1*b3; 217 } 218 219 inline void db_CharacteristicPolynomial4x4(double p[5],const double A[16]) 220 { 221 /*All two by two determinants of the first two rows*/ 222 double two01[3],two02[3],two03[3],two12[3],two13[3],two23[3]; 223 /*Polynomials representing third and fourth row of A*/ 224 double P0[2],P1[2],P2[2],P3[2]; 225 double P4[2],P5[2],P6[2],P7[2]; 226 /*All three by three determinants of the first three rows*/ 227 double neg_three0[4],neg_three1[4],three2[4],three3[4]; 228 229 /*Compute 2x2 determinants*/ 230 two01[0]=A[0]*A[5]-A[1]*A[4]; 231 two01[1]= -(A[0]+A[5]); 232 two01[2]=1.0; 233 234 two02[0]=A[0]*A[6]-A[2]*A[4]; 235 two02[1]= -A[6]; 236 237 two03[0]=A[0]*A[7]-A[3]*A[4]; 238 two03[1]= -A[7]; 239 240 two12[0]=A[1]*A[6]-A[2]*A[5]; 241 two12[1]=A[2]; 242 243 two13[0]=A[1]*A[7]-A[3]*A[5]; 244 two13[1]=A[3]; 245 246 two23[0]=A[2]*A[7]-A[3]*A[6]; 247 248 P0[0]=A[8]; 249 P1[0]=A[9]; 250 P2[0]=A[10];P2[1]= -1.0; 251 P3[0]=A[11]; 252 253 P4[0]=A[12]; 254 P5[0]=A[13]; 255 P6[0]=A[14]; 256 P7[0]=A[15];P7[1]= -1.0; 257 258 /*Compute 3x3 determinants.Note that the highest 259 degree polynomial goes first and the smaller ones 260 are added or subtracted from it*/ 261 db_MultiplyPoly1_1( neg_three0,P2,two13); 262 db_SubtractPolyProduct0_0(neg_three0,P1,two23); 263 db_SubtractPolyProduct0_1(neg_three0,P3,two12); 264 265 db_MultiplyPoly1_1( neg_three1,P2,two03); 266 db_SubtractPolyProduct0_1(neg_three1,P3,two02); 267 db_SubtractPolyProduct0_0(neg_three1,P0,two23); 268 269 db_MultiplyPoly0_2( three2,P3,two01); 270 db_AddPolyProduct0_1( three2,P0,two13); 271 db_SubtractPolyProduct0_1(three2,P1,two03); 272 273 db_MultiplyPoly1_2( three3,P2,two01); 274 db_AddPolyProduct0_1( three3,P0,two12); 275 db_SubtractPolyProduct0_1(three3,P1,two02); 276 277 /*Compute 4x4 determinants*/ 278 db_MultiplyPoly1_3( p,P7,three3); 279 db_AddPolyProduct0_2( p,P4,neg_three0); 280 db_SubtractPolyProduct0_2(p,P5,neg_three1); 281 db_SubtractPolyProduct0_2(p,P6,three2); 282 } 283 284 inline void db_RealEigenvalues4x4(double lambda[4],int *nr_roots,const double A[16],int forced=0) 285 { 286 double p[5]; 287 288 db_CharacteristicPolynomial4x4(p,A); 289 if(forced) db_SolveQuarticForced(lambda,nr_roots,p[4],p[3],p[2],p[1],p[0]); 290 else db_SolveQuartic(lambda,nr_roots,p[4],p[3],p[2],p[1],p[0]); 291 } 292 293 /*! 294 Compute the unit norm eigenvector v of the matrix A corresponding 295 to the eigenvalue lambda 296 [96mult 60add 1sqrt=156flops 1sqrt]*/ 297 inline void db_EigenVector4x4(double v[4],double lambda,const double A[16]) 298 { 299 double a0,a5,a10,a15; 300 double d01,d02,d03,d12,d13,d23; 301 double e01,e02,e03,e12,e13,e23; 302 double C[16],n0,n1,n2,n3,m; 303 304 /*Compute diagonal 305 [4add=4flops]*/ 306 a0=A[0]-lambda; 307 a5=A[5]-lambda; 308 a10=A[10]-lambda; 309 a15=A[15]-lambda; 310 311 /*Compute 2x2 determinants of rows 1,2 and 3,4 312 [24mult 12add=36flops]*/ 313 d01=a0*a5 -A[1]*A[4]; 314 d02=a0*A[6] -A[2]*A[4]; 315 d03=a0*A[7] -A[3]*A[4]; 316 d12=A[1]*A[6]-A[2]*a5; 317 d13=A[1]*A[7]-A[3]*a5; 318 d23=A[2]*A[7]-A[3]*A[6]; 319 320 e01=A[8]*A[13]-A[9] *A[12]; 321 e02=A[8]*A[14]-a10 *A[12]; 322 e03=A[8]*a15 -A[11]*A[12]; 323 e12=A[9]*A[14]-a10 *A[13]; 324 e13=A[9]*a15 -A[11]*A[13]; 325 e23=a10 *a15 -A[11]*A[14]; 326 327 /*Compute matrix of cofactors 328 [48mult 32 add=80flops*/ 329 C[0]= (a5 *e23-A[6]*e13+A[7]*e12); 330 C[1]= -(A[4]*e23-A[6]*e03+A[7]*e02); 331 C[2]= (A[4]*e13-a5 *e03+A[7]*e01); 332 C[3]= -(A[4]*e12-a5 *e02+A[6]*e01); 333 334 C[4]= -(A[1]*e23-A[2]*e13+A[3]*e12); 335 C[5]= (a0 *e23-A[2]*e03+A[3]*e02); 336 C[6]= -(a0 *e13-A[1]*e03+A[3]*e01); 337 C[7]= (a0 *e12-A[1]*e02+A[2]*e01); 338 339 C[8]= (A[13]*d23-A[14]*d13+a15 *d12); 340 C[9]= -(A[12]*d23-A[14]*d03+a15 *d02); 341 C[10]= (A[12]*d13-A[13]*d03+a15 *d01); 342 C[11]= -(A[12]*d12-A[13]*d02+A[14]*d01); 343 344 C[12]= -(A[9]*d23-a10 *d13+A[11]*d12); 345 C[13]= (A[8]*d23-a10 *d03+A[11]*d02); 346 C[14]= -(A[8]*d13-A[9]*d03+A[11]*d01); 347 C[15]= (A[8]*d12-A[9]*d02+a10 *d01); 348 349 /*Compute square sums of rows 350 [16mult 12add=28flops*/ 351 n0=db_sqr(C[0]) +db_sqr(C[1]) +db_sqr(C[2]) +db_sqr(C[3]); 352 n1=db_sqr(C[4]) +db_sqr(C[5]) +db_sqr(C[6]) +db_sqr(C[7]); 353 n2=db_sqr(C[8]) +db_sqr(C[9]) +db_sqr(C[10])+db_sqr(C[11]); 354 n3=db_sqr(C[12])+db_sqr(C[13])+db_sqr(C[14])+db_sqr(C[15]); 355 356 /*Take the largest norm row and normalize 357 [4mult 1 sqrt=4flops 1sqrt]*/ 358 if(n0>=n1 && n0>=n2 && n0>=n3) 359 { 360 m=db_SafeReciprocal(sqrt(n0)); 361 db_MultiplyScalarCopy4(v,C,m); 362 } 363 else if(n1>=n2 && n1>=n3) 364 { 365 m=db_SafeReciprocal(sqrt(n1)); 366 db_MultiplyScalarCopy4(v,&(C[4]),m); 367 } 368 else if(n2>=n3) 369 { 370 m=db_SafeReciprocal(sqrt(n2)); 371 db_MultiplyScalarCopy4(v,&(C[8]),m); 372 } 373 else 374 { 375 m=db_SafeReciprocal(sqrt(n3)); 376 db_MultiplyScalarCopy4(v,&(C[12]),m); 377 } 378 } 379 380 381 382 /*\}*/ 383 #endif /* DB_UTILITIES_POLY */ 384
