diff options
Diffstat (limited to 'jni/feature_stab/db_vlvm/db_utilities_poly.h')
| -rw-r--r-- | jni/feature_stab/db_vlvm/db_utilities_poly.h | 383 |
1 files changed, 383 insertions, 0 deletions
diff --git a/jni/feature_stab/db_vlvm/db_utilities_poly.h b/jni/feature_stab/db_vlvm/db_utilities_poly.h new file mode 100644 index 000000000..1f8789077 --- /dev/null +++ b/jni/feature_stab/db_vlvm/db_utilities_poly.h @@ -0,0 +1,383 @@ +/* + * 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 */ |