| /* |
| * Copyright (C) 2011 The Android Open Source Project |
| * |
| * Licensed under the Apache License, Version 2.0 (the "License"); |
| * you may not use this file except in compliance with the License. |
| * You may obtain a copy of the License at |
| * |
| * http://www.apache.org/licenses/LICENSE-2.0 |
| * |
| * Unless required by applicable law or agreed to in writing, software |
| * distributed under the License is distributed on an "AS IS" BASIS, |
| * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| * See the License for the specific language governing permissions and |
| * limitations under the License. |
| */ |
| |
| /* $Id: db_utilities_poly.h,v 1.2 2010/09/03 12:00:11 bsouthall Exp $ */ |
| |
| #ifndef DB_UTILITIES_POLY |
| #define DB_UTILITIES_POLY |
| |
| #include "db_utilities.h" |
| |
| |
| |
| /***************************************************************** |
| * Lean and mean begins here * |
| *****************************************************************/ |
| /*! |
| * \defgroup LMPolynomial (LM) Polynomial utilities (solvers, arithmetic, evaluation, etc.) |
| */ |
| /*\{*/ |
| |
| /*! |
| In debug mode closed form quadratic solving takes on the order of 15 microseconds |
| while eig of the companion matrix takes about 1.1 milliseconds |
| Speed-optimized code in release mode solves a quadratic in 0.3 microseconds on 450MHz |
| */ |
| inline void db_SolveQuadratic(double *roots,int *nr_roots,double a,double b,double c) |
| { |
| double rs,srs,q; |
| |
| /*For non-degenerate quadratics |
| [5 mult 2 add 1 sqrt=7flops 1func]*/ |
| if(a==0.0) |
| { |
| if(b==0.0) *nr_roots=0; |
| else |
| { |
| roots[0]= -c/b; |
| *nr_roots=1; |
| } |
| } |
| else |
| { |
| rs=b*b-4.0*a*c; |
| if(rs>=0.0) |
| { |
| *nr_roots=2; |
| srs=sqrt(rs); |
| q= -0.5*(b+db_sign(b)*srs); |
| roots[0]=q/a; |
| /*If b is zero db_sign(b) returns 1, |
| so q is only zero when b=0 and c=0*/ |
| if(q==0.0) *nr_roots=1; |
| else roots[1]=c/q; |
| } |
| else *nr_roots=0; |
| } |
| } |
| |
| /*! |
| In debug mode closed form cubic solving takes on the order of 45 microseconds |
| while eig of the companion matrix takes about 1.3 milliseconds |
| Speed-optimized code in release mode solves a cubic in 1.5 microseconds on 450MHz |
| For a non-degenerate cubic with two roots, the first root is the single root and |
| the second root is the double root |
| */ |
| DB_API void db_SolveCubic(double *roots,int *nr_roots,double a,double b,double c,double d); |
| /*! |
| In debug mode closed form quartic solving takes on the order of 0.1 milliseconds |
| while eig of the companion matrix takes about 1.5 milliseconds |
| Speed-optimized code in release mode solves a quartic in 2.6 microseconds on 450MHz*/ |
| DB_API void db_SolveQuartic(double *roots,int *nr_roots,double a,double b,double c,double d,double e); |
| /*! |
| Quartic solving where a solution is forced when splitting into quadratics, which |
| can be good if the quartic is sometimes in fact a quadratic, such as in absolute orientation |
| when the data is planar*/ |
| DB_API void db_SolveQuarticForced(double *roots,int *nr_roots,double a,double b,double c,double d,double e); |
| |
| inline double db_PolyEval1(const double p[2],double x) |
| { |
| return(p[0]+x*p[1]); |
| } |
| |
| inline void db_MultiplyPoly1_1(double *d,const double *a,const double *b) |
| { |
| double a0,a1; |
| double b0,b1; |
| a0=a[0];a1=a[1]; |
| b0=b[0];b1=b[1]; |
| |
| d[0]=a0*b0; |
| d[1]=a0*b1+a1*b0; |
| d[2]= a1*b1; |
| } |
| |
| inline void db_MultiplyPoly0_2(double *d,const double *a,const double *b) |
| { |
| double a0; |
| double b0,b1,b2; |
| a0=a[0]; |
| b0=b[0];b1=b[1];b2=b[2]; |
| |
| d[0]=a0*b0; |
| d[1]=a0*b1; |
| d[2]=a0*b2; |
| } |
| |
| inline void db_MultiplyPoly1_2(double *d,const double *a,const double *b) |
| { |
| double a0,a1; |
| double b0,b1,b2; |
| a0=a[0];a1=a[1]; |
| b0=b[0];b1=b[1];b2=b[2]; |
| |
| d[0]=a0*b0; |
| d[1]=a0*b1+a1*b0; |
| d[2]=a0*b2+a1*b1; |
| d[3]= a1*b2; |
| } |
| |
| |
| inline void db_MultiplyPoly1_3(double *d,const double *a,const double *b) |
| { |
| double a0,a1; |
| double b0,b1,b2,b3; |
| a0=a[0];a1=a[1]; |
| b0=b[0];b1=b[1];b2=b[2];b3=b[3]; |
| |
| d[0]=a0*b0; |
| d[1]=a0*b1+a1*b0; |
| d[2]=a0*b2+a1*b1; |
| d[3]=a0*b3+a1*b2; |
| d[4]= a1*b3; |
| } |
| /*! |
| Multiply d=a*b where a is one degree and b is two degree*/ |
| inline void db_AddPolyProduct0_1(double *d,const double *a,const double *b) |
| { |
| double a0; |
| double b0,b1; |
| a0=a[0]; |
| b0=b[0];b1=b[1]; |
| |
| d[0]+=a0*b0; |
| d[1]+=a0*b1; |
| } |
| inline void db_AddPolyProduct0_2(double *d,const double *a,const double *b) |
| { |
| double a0; |
| double b0,b1,b2; |
| a0=a[0]; |
| b0=b[0];b1=b[1];b2=b[2]; |
| |
| d[0]+=a0*b0; |
| d[1]+=a0*b1; |
| d[2]+=a0*b2; |
| } |
| /*! |
| Multiply d=a*b where a is one degree and b is two degree*/ |
| inline void db_SubtractPolyProduct0_0(double *d,const double *a,const double *b) |
| { |
| double a0; |
| double b0; |
| a0=a[0]; |
| b0=b[0]; |
| |
| d[0]-=a0*b0; |
| } |
| |
| inline void db_SubtractPolyProduct0_1(double *d,const double *a,const double *b) |
| { |
| double a0; |
| double b0,b1; |
| a0=a[0]; |
| b0=b[0];b1=b[1]; |
| |
| d[0]-=a0*b0; |
| d[1]-=a0*b1; |
| } |
| |
| inline void db_SubtractPolyProduct0_2(double *d,const double *a,const double *b) |
| { |
| double a0; |
| double b0,b1,b2; |
| a0=a[0]; |
| b0=b[0];b1=b[1];b2=b[2]; |
| |
| d[0]-=a0*b0; |
| d[1]-=a0*b1; |
| d[2]-=a0*b2; |
| } |
| |
| inline void db_SubtractPolyProduct1_3(double *d,const double *a,const double *b) |
| { |
| double a0,a1; |
| double b0,b1,b2,b3; |
| a0=a[0];a1=a[1]; |
| b0=b[0];b1=b[1];b2=b[2];b3=b[3]; |
| |
| d[0]-=a0*b0; |
| d[1]-=a0*b1+a1*b0; |
| d[2]-=a0*b2+a1*b1; |
| d[3]-=a0*b3+a1*b2; |
| d[4]-= a1*b3; |
| } |
| |
| inline void db_CharacteristicPolynomial4x4(double p[5],const double A[16]) |
| { |
| /*All two by two determinants of the first two rows*/ |
| double two01[3],two02[3],two03[3],two12[3],two13[3],two23[3]; |
| /*Polynomials representing third and fourth row of A*/ |
| double P0[2],P1[2],P2[2],P3[2]; |
| double P4[2],P5[2],P6[2],P7[2]; |
| /*All three by three determinants of the first three rows*/ |
| double neg_three0[4],neg_three1[4],three2[4],three3[4]; |
| |
| /*Compute 2x2 determinants*/ |
| two01[0]=A[0]*A[5]-A[1]*A[4]; |
| two01[1]= -(A[0]+A[5]); |
| two01[2]=1.0; |
| |
| two02[0]=A[0]*A[6]-A[2]*A[4]; |
| two02[1]= -A[6]; |
| |
| two03[0]=A[0]*A[7]-A[3]*A[4]; |
| two03[1]= -A[7]; |
| |
| two12[0]=A[1]*A[6]-A[2]*A[5]; |
| two12[1]=A[2]; |
| |
| two13[0]=A[1]*A[7]-A[3]*A[5]; |
| two13[1]=A[3]; |
| |
| two23[0]=A[2]*A[7]-A[3]*A[6]; |
| |
| P0[0]=A[8]; |
| P1[0]=A[9]; |
| P2[0]=A[10];P2[1]= -1.0; |
| P3[0]=A[11]; |
| |
| P4[0]=A[12]; |
| P5[0]=A[13]; |
| P6[0]=A[14]; |
| P7[0]=A[15];P7[1]= -1.0; |
| |
| /*Compute 3x3 determinants.Note that the highest |
| degree polynomial goes first and the smaller ones |
| are added or subtracted from it*/ |
| db_MultiplyPoly1_1( neg_three0,P2,two13); |
| db_SubtractPolyProduct0_0(neg_three0,P1,two23); |
| db_SubtractPolyProduct0_1(neg_three0,P3,two12); |
| |
| db_MultiplyPoly1_1( neg_three1,P2,two03); |
| db_SubtractPolyProduct0_1(neg_three1,P3,two02); |
| db_SubtractPolyProduct0_0(neg_three1,P0,two23); |
| |
| db_MultiplyPoly0_2( three2,P3,two01); |
| db_AddPolyProduct0_1( three2,P0,two13); |
| db_SubtractPolyProduct0_1(three2,P1,two03); |
| |
| db_MultiplyPoly1_2( three3,P2,two01); |
| db_AddPolyProduct0_1( three3,P0,two12); |
| db_SubtractPolyProduct0_1(three3,P1,two02); |
| |
| /*Compute 4x4 determinants*/ |
| db_MultiplyPoly1_3( p,P7,three3); |
| db_AddPolyProduct0_2( p,P4,neg_three0); |
| db_SubtractPolyProduct0_2(p,P5,neg_three1); |
| db_SubtractPolyProduct0_2(p,P6,three2); |
| } |
| |
| inline void db_RealEigenvalues4x4(double lambda[4],int *nr_roots,const double A[16],int forced=0) |
| { |
| double p[5]; |
| |
| db_CharacteristicPolynomial4x4(p,A); |
| if(forced) db_SolveQuarticForced(lambda,nr_roots,p[4],p[3],p[2],p[1],p[0]); |
| else db_SolveQuartic(lambda,nr_roots,p[4],p[3],p[2],p[1],p[0]); |
| } |
| |
| /*! |
| Compute the unit norm eigenvector v of the matrix A corresponding |
| to the eigenvalue lambda |
| [96mult 60add 1sqrt=156flops 1sqrt]*/ |
| inline void db_EigenVector4x4(double v[4],double lambda,const double A[16]) |
| { |
| double a0,a5,a10,a15; |
| double d01,d02,d03,d12,d13,d23; |
| double e01,e02,e03,e12,e13,e23; |
| double C[16],n0,n1,n2,n3,m; |
| |
| /*Compute diagonal |
| [4add=4flops]*/ |
| a0=A[0]-lambda; |
| a5=A[5]-lambda; |
| a10=A[10]-lambda; |
| a15=A[15]-lambda; |
| |
| /*Compute 2x2 determinants of rows 1,2 and 3,4 |
| [24mult 12add=36flops]*/ |
| d01=a0*a5 -A[1]*A[4]; |
| d02=a0*A[6] -A[2]*A[4]; |
| d03=a0*A[7] -A[3]*A[4]; |
| d12=A[1]*A[6]-A[2]*a5; |
| d13=A[1]*A[7]-A[3]*a5; |
| d23=A[2]*A[7]-A[3]*A[6]; |
| |
| e01=A[8]*A[13]-A[9] *A[12]; |
| e02=A[8]*A[14]-a10 *A[12]; |
| e03=A[8]*a15 -A[11]*A[12]; |
| e12=A[9]*A[14]-a10 *A[13]; |
| e13=A[9]*a15 -A[11]*A[13]; |
| e23=a10 *a15 -A[11]*A[14]; |
| |
| /*Compute matrix of cofactors |
| [48mult 32 add=80flops*/ |
| C[0]= (a5 *e23-A[6]*e13+A[7]*e12); |
| C[1]= -(A[4]*e23-A[6]*e03+A[7]*e02); |
| C[2]= (A[4]*e13-a5 *e03+A[7]*e01); |
| C[3]= -(A[4]*e12-a5 *e02+A[6]*e01); |
| |
| C[4]= -(A[1]*e23-A[2]*e13+A[3]*e12); |
| C[5]= (a0 *e23-A[2]*e03+A[3]*e02); |
| C[6]= -(a0 *e13-A[1]*e03+A[3]*e01); |
| C[7]= (a0 *e12-A[1]*e02+A[2]*e01); |
| |
| C[8]= (A[13]*d23-A[14]*d13+a15 *d12); |
| C[9]= -(A[12]*d23-A[14]*d03+a15 *d02); |
| C[10]= (A[12]*d13-A[13]*d03+a15 *d01); |
| C[11]= -(A[12]*d12-A[13]*d02+A[14]*d01); |
| |
| C[12]= -(A[9]*d23-a10 *d13+A[11]*d12); |
| C[13]= (A[8]*d23-a10 *d03+A[11]*d02); |
| C[14]= -(A[8]*d13-A[9]*d03+A[11]*d01); |
| C[15]= (A[8]*d12-A[9]*d02+a10 *d01); |
| |
| /*Compute square sums of rows |
| [16mult 12add=28flops*/ |
| n0=db_sqr(C[0]) +db_sqr(C[1]) +db_sqr(C[2]) +db_sqr(C[3]); |
| n1=db_sqr(C[4]) +db_sqr(C[5]) +db_sqr(C[6]) +db_sqr(C[7]); |
| n2=db_sqr(C[8]) +db_sqr(C[9]) +db_sqr(C[10])+db_sqr(C[11]); |
| n3=db_sqr(C[12])+db_sqr(C[13])+db_sqr(C[14])+db_sqr(C[15]); |
| |
| /*Take the largest norm row and normalize |
| [4mult 1 sqrt=4flops 1sqrt]*/ |
| if(n0>=n1 && n0>=n2 && n0>=n3) |
| { |
| m=db_SafeReciprocal(sqrt(n0)); |
| db_MultiplyScalarCopy4(v,C,m); |
| } |
| else if(n1>=n2 && n1>=n3) |
| { |
| m=db_SafeReciprocal(sqrt(n1)); |
| db_MultiplyScalarCopy4(v,&(C[4]),m); |
| } |
| else if(n2>=n3) |
| { |
| m=db_SafeReciprocal(sqrt(n2)); |
| db_MultiplyScalarCopy4(v,&(C[8]),m); |
| } |
| else |
| { |
| m=db_SafeReciprocal(sqrt(n3)); |
| db_MultiplyScalarCopy4(v,&(C[12]),m); |
| } |
| } |
| |
| |
| |
| /*\}*/ |
| #endif /* DB_UTILITIES_POLY */ |