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/*!
35In debug mode closed form quadratic solving takes on the order of 15 microseconds
36while eig of the companion matrix takes about 1.1 milliseconds
37Speed-optimized code in release mode solves a quadratic in 0.3 microseconds on 450MHz
38*/
39inline 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/*!
73In debug mode closed form cubic solving takes on the order of 45 microseconds
74while eig of the companion matrix takes about 1.3 milliseconds
75Speed-optimized code in release mode solves a cubic in 1.5 microseconds on 450MHz
76For a non-degenerate cubic with two roots, the first root is the single root and
77the second root is the double root
78*/
79DB_API void db_SolveCubic(double *roots,int *nr_roots,double a,double b,double c,double d);
80/*!
81In debug mode closed form quartic solving takes on the order of 0.1 milliseconds
82while eig of the companion matrix takes about 1.5 milliseconds
83Speed-optimized code in release mode solves a quartic in 2.6 microseconds on 450MHz*/
84DB_API void db_SolveQuartic(double *roots,int *nr_roots,double a,double b,double c,double d,double e);
85/*!
86Quartic solving where a solution is forced when splitting into quadratics, which
87can be good if the quartic is sometimes in fact a quadratic, such as in absolute orientation
88when the data is planar*/
89DB_API void db_SolveQuarticForced(double *roots,int *nr_roots,double a,double b,double c,double d,double e);
90
91inline double db_PolyEval1(const double p[2],double x)
92{
93    return(p[0]+x*p[1]);
94}
95
96inline 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
108inline 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
120inline 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
134inline 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/*!
148Multiply d=a*b where a is one degree and b is two degree*/
149inline 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}
159inline 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/*!
171Multiply d=a*b where a is one degree and b is two degree*/
172inline 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
182inline 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
193inline 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
205inline 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
219inline 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
284inline 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/*!
294Compute the unit norm eigenvector v of the matrix A corresponding
295to the eigenvalue lambda
296[96mult 60add 1sqrt=156flops 1sqrt]*/
297inline 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