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 */
db_SolveQuadratic(double * roots,int * nr_roots,double a,double b,double c)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 
db_PolyEval1(const double p[2],double x)91 inline double db_PolyEval1(const double p[2],double x)
92 {
93     return(p[0]+x*p[1]);
94 }
95 
db_MultiplyPoly1_1(double * d,const double * a,const double * b)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 
db_MultiplyPoly0_2(double * d,const double * a,const double * b)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 
db_MultiplyPoly1_2(double * d,const double * a,const double * b)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 
db_MultiplyPoly1_3(double * d,const double * a,const double * b)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*/
db_AddPolyProduct0_1(double * d,const double * a,const double * b)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 }
db_AddPolyProduct0_2(double * d,const double * a,const double * b)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*/
db_SubtractPolyProduct0_0(double * d,const double * a,const double * b)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 
db_SubtractPolyProduct0_1(double * d,const double * a,const double * b)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 
db_SubtractPolyProduct0_2(double * d,const double * a,const double * b)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 
db_SubtractPolyProduct1_3(double * d,const double * a,const double * b)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 
db_CharacteristicPolynomial4x4(double p[5],const double A[16])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]*/
db_EigenVector4x4(double v[4],double lambda,const double A[16])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