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