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 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 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 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
