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.cpp,v 1.2 2010/09/03 12:00:10 bsouthall Exp $ */
18
19 #include "db_utilities_poly.h"
20 #include "db_utilities.h"
21
22
23
24 /*****************************************************************
25 * Lean and mean begins here *
26 *****************************************************************/
27
db_SolveCubic(double * roots,int * nr_roots,double a,double b,double c,double d)28 void db_SolveCubic(double *roots,int *nr_roots,double a,double b,double c,double d)
29 {
30 double bp,bp2,cp,dp,q,r,srq;
31 double r2_min_q3,theta,bp_through3,theta_through3;
32 double cos_theta_through3,sin_theta_through3,min2_cos_theta_plu,min2_cos_theta_min;
33 double si_r_srq,A;
34
35 /*For nondegenerate cubics with three roots
36 [24 mult 9 add 2sqrt 1acos 1cos=33flops 4func]
37 For nondegenerate cubics with one root
38 [16 mult 6 add 1sqrt 1qbrt=24flops 3func]*/
39
40 if(a==0.0) db_SolveQuadratic(roots,nr_roots,b,c,d);
41 else
42 {
43 bp=b/a;
44 bp2=bp*bp;
45 cp=c/a;
46 dp=d/a;
47
48 q=(bp2-3.0*cp)/9.0;
49 r=(2.0*bp2*bp-9.0*bp*cp+27.0*dp)/54.0;
50 r2_min_q3=r*r-q*q*q;
51 if(r2_min_q3<0.0)
52 {
53 *nr_roots=3;
54 /*q has to be > 0*/
55 srq=sqrt(q);
56 theta=acos(db_maxd(-1.0,db_mind(1.0,r/(q*srq))));
57 bp_through3=bp/3.0;
58 theta_through3=theta/3.0;
59 cos_theta_through3=cos(theta_through3);
60 sin_theta_through3=sqrt(db_maxd(0.0,1.0-cos_theta_through3*cos_theta_through3));
61
62 /*cos(theta_through3+2*pi/3)=cos_theta_through3*cos(2*pi/3)-sin_theta_through3*sin(2*pi/3)
63 = -0.5*cos_theta_through3-sqrt(3)/2.0*sin_theta_through3
64 = -0.5*(cos_theta_through3+sqrt(3)*sin_theta_through3)*/
65 min2_cos_theta_plu=cos_theta_through3+DB_SQRT3*sin_theta_through3;
66 min2_cos_theta_min=cos_theta_through3-DB_SQRT3*sin_theta_through3;
67
68 roots[0]= -2.0*srq*cos_theta_through3-bp_through3;
69 roots[1]=srq*min2_cos_theta_plu-bp_through3;
70 roots[2]=srq*min2_cos_theta_min-bp_through3;
71 }
72 else if(r2_min_q3>0.0)
73 {
74 *nr_roots=1;
75 A= -db_sign(r)*db_CubRoot(db_absd(r)+sqrt(r2_min_q3));
76 bp_through3=bp/3.0;
77 if(A!=0.0) roots[0]=A+q/A-bp_through3;
78 else roots[0]= -bp_through3;
79 }
80 else
81 {
82 *nr_roots=2;
83 bp_through3=bp/3.0;
84 /*q has to be >= 0*/
85 si_r_srq=db_sign(r)*sqrt(q);
86 /*Single root*/
87 roots[0]= -2.0*si_r_srq-bp_through3;
88 /*Double root*/
89 roots[1]=si_r_srq-bp_through3;
90 }
91 }
92 }
93
db_SolveQuartic(double * roots,int * nr_roots,double a,double b,double c,double d,double e)94 void db_SolveQuartic(double *roots,int *nr_roots,double a,double b,double c,double d,double e)
95 {
96 /*Normalized coefficients*/
97 double c0,c1,c2,c3;
98 /*Temporary coefficients*/
99 double c3through2,c3through4,c3c3through4_min_c2,min4_c0;
100 double lz,ms,ns,mn,m,n,lz_through2;
101 /*Cubic polynomial roots, nr of roots and coefficients*/
102 double c_roots[3];
103 int nr_c_roots;
104 double k0,k1;
105 /*nr additional roots from second quadratic*/
106 int addroots;
107
108 /*For nondegenerate quartics
109 [16mult 11add 2sqrt 1cubic 2quadratic=74flops 8funcs]*/
110
111 if(a==0.0) db_SolveCubic(roots,nr_roots,b,c,d,e);
112 else if(e==0.0)
113 {
114 db_SolveCubic(roots,nr_roots,a,b,c,d);
115 roots[*nr_roots]=0.0;
116 *nr_roots+=1;
117 }
118 else
119 {
120 /*Compute normalized coefficients*/
121 c3=b/a;
122 c2=c/a;
123 c1=d/a;
124 c0=e/a;
125 /*Compute temporary coefficients*/
126 c3through2=c3/2.0;
127 c3through4=c3/4.0;
128 c3c3through4_min_c2=c3*c3through4-c2;
129 min4_c0= -4.0*c0;
130 /*Compute coefficients of cubic*/
131 k0=min4_c0*c3c3through4_min_c2-c1*c1;
132 k1=c1*c3+min4_c0;
133 /*k2= -c2*/
134 /*k3=1.0*/
135
136 /*Solve it for roots*/
137 db_SolveCubic(c_roots,&nr_c_roots,1.0,-c2,k1,k0);
138
139 if(nr_c_roots>0)
140 {
141 lz=c_roots[0];
142 lz_through2=lz/2.0;
143 ms=lz+c3c3through4_min_c2;
144 ns=lz_through2*lz_through2-c0;
145 mn=lz*c3through4-c1/2.0;
146
147 if((ms>=0.0)&&(ns>=0.0))
148 {
149 m=sqrt(ms);
150 n=sqrt(ns)*db_sign(mn);
151
152 db_SolveQuadratic(roots,nr_roots,
153 1.0,c3through2+m,lz_through2+n);
154
155 db_SolveQuadratic(&roots[*nr_roots],&addroots,
156 1.0,c3through2-m,lz_through2-n);
157
158 *nr_roots+=addroots;
159 }
160 else *nr_roots=0;
161 }
162 else *nr_roots=0;
163 }
164 }
165
db_SolveQuarticForced(double * roots,int * nr_roots,double a,double b,double c,double d,double e)166 void db_SolveQuarticForced(double *roots,int *nr_roots,double a,double b,double c,double d,double e)
167 {
168 /*Normalized coefficients*/
169 double c0,c1,c2,c3;
170 /*Temporary coefficients*/
171 double c3through2,c3through4,c3c3through4_min_c2,min4_c0;
172 double lz,ms,ns,mn,m,n,lz_through2;
173 /*Cubic polynomial roots, nr of roots and coefficients*/
174 double c_roots[3];
175 int nr_c_roots;
176 double k0,k1;
177 /*nr additional roots from second quadratic*/
178 int addroots;
179
180 /*For nondegenerate quartics
181 [16mult 11add 2sqrt 1cubic 2quadratic=74flops 8funcs]*/
182
183 if(a==0.0) db_SolveCubic(roots,nr_roots,b,c,d,e);
184 else if(e==0.0)
185 {
186 db_SolveCubic(roots,nr_roots,a,b,c,d);
187 roots[*nr_roots]=0.0;
188 *nr_roots+=1;
189 }
190 else
191 {
192 /*Compute normalized coefficients*/
193 c3=b/a;
194 c2=c/a;
195 c1=d/a;
196 c0=e/a;
197 /*Compute temporary coefficients*/
198 c3through2=c3/2.0;
199 c3through4=c3/4.0;
200 c3c3through4_min_c2=c3*c3through4-c2;
201 min4_c0= -4.0*c0;
202 /*Compute coefficients of cubic*/
203 k0=min4_c0*c3c3through4_min_c2-c1*c1;
204 k1=c1*c3+min4_c0;
205 /*k2= -c2*/
206 /*k3=1.0*/
207
208 /*Solve it for roots*/
209 db_SolveCubic(c_roots,&nr_c_roots,1.0,-c2,k1,k0);
210
211 if(nr_c_roots>0)
212 {
213 lz=c_roots[0];
214 lz_through2=lz/2.0;
215 ms=lz+c3c3through4_min_c2;
216 ns=lz_through2*lz_through2-c0;
217 mn=lz*c3through4-c1/2.0;
218
219 if(ms<0.0) ms=0.0;
220 if(ns<0.0) ns=0.0;
221
222 m=sqrt(ms);
223 n=sqrt(ns)*db_sign(mn);
224
225 db_SolveQuadratic(roots,nr_roots,
226 1.0,c3through2+m,lz_through2+n);
227
228 db_SolveQuadratic(&roots[*nr_roots],&addroots,
229 1.0,c3through2-m,lz_through2-n);
230
231 *nr_roots+=addroots;
232 }
233 else *nr_roots=0;
234 }
235 }
236