1 /*
2 Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho
3 All rights reserved.
4
5 Redistribution and use in source and binary forms, with or without modification,
6 are permitted provided that the following conditions are met:
7
8 Redistributions of source code must retain the above copyright notice, this list of
9 conditions and the following disclaimer. Redistributions in binary form must reproduce
10 the above copyright notice, this list of conditions and the following disclaimer
11 in the documentation and/or other materials provided with the distribution.
12
13 Neither the name of the Johns Hopkins University nor the names of its contributors
14 may be used to endorse or promote products derived from this software without specific
15 prior written permission.
16
17 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
18 EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES
19 OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
20 SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
21 INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
22 TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
23 BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
24 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
25 ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
26 DAMAGE.
27 */
28
29 //////////////////////
30 // Polynomial Roots //
31 //////////////////////
32 #include <math.h>
33 #include "Factor.h"
Factor(double a1,double a0,double roots[1][2],const double & EPS)34 int Factor(double a1,double a0,double roots[1][2],const double& EPS){
35 if(fabs(a1)<=EPS){return 0;}
36 roots[0][0]=-a0/a1;
37 roots[0][1]=0;
38 return 1;
39 }
Factor(double a2,double a1,double a0,double roots[2][2],const double & EPS)40 int Factor(double a2,double a1,double a0,double roots[2][2],const double& EPS){
41 double d;
42 if(fabs(a2)<=EPS){return Factor(a1,a0,roots,EPS);}
43
44 d=a1*a1-4*a0*a2;
45 a1/=(2*a2);
46 if(d<0){
47 d=sqrt(-d)/(2*a2);
48 roots[0][0]=roots[1][0]=-a1;
49 roots[0][1]=-d;
50 roots[1][1]= d;
51 }
52 else{
53 d=sqrt(d)/(2*a2);
54 roots[0][1]=roots[1][1]=0;
55 roots[0][0]=-a1-d;
56 roots[1][0]=-a1+d;
57 }
58 return 2;
59 }
60 // Solution taken from: http://mathworld.wolfram.com/CubicFormula.html
61 // and http://www.csit.fsu.edu/~burkardt/f_src/subpak/subpak.f90
Factor(double a3,double a2,double a1,double a0,double roots[3][2],const double & EPS)62 int Factor(double a3,double a2,double a1,double a0,double roots[3][2],const double& EPS){
63 double q,r,r2,q3;
64
65 if(fabs(a3)<=EPS){return Factor(a2,a1,a0,roots,EPS);}
66 a2/=a3;
67 a1/=a3;
68 a0/=a3;
69
70 q=-(3*a1-a2*a2)/9;
71 r=-(9*a2*a1-27*a0-2*a2*a2*a2)/54;
72 r2=r*r;
73 q3=q*q*q;
74
75 if(r2<q3){
76 double sqrQ=sqrt(q);
77 double theta = acos ( r / (sqrQ*q) );
78 double cTheta=cos(theta/3)*sqrQ;
79 double sTheta=sin(theta/3)*sqrQ*SQRT_3/2;
80 roots[0][1]=roots[1][1]=roots[2][1]=0;
81 roots[0][0]=-2*cTheta;
82 roots[1][0]=-2*(-cTheta*0.5-sTheta);
83 roots[2][0]=-2*(-cTheta*0.5+sTheta);
84 }
85 else{
86 double s1,s2,sqr=sqrt(r2-q3);
87 double t;
88 t=-r+sqr;
89 if(t<0){s1=-pow(-t,1.0/3);}
90 else{s1=pow(t,1.0/3);}
91 t=-r-sqr;
92 if(t<0){s2=-pow(-t,1.0/3);}
93 else{s2=pow(t,1.0/3);}
94 roots[0][1]=0;
95 roots[0][0]=s1+s2;
96 s1/=2;
97 s2/=2;
98 roots[1][0]= roots[2][0]=-s1-s2;
99 roots[1][1]= SQRT_3*(s1-s2);
100 roots[2][1]=-roots[1][1];
101 }
102 roots[0][0]-=a2/3;
103 roots[1][0]-=a2/3;
104 roots[2][0]-=a2/3;
105 return 3;
106 }
ArcTan2(const double & y,const double & x)107 double ArcTan2(const double& y,const double& x){
108 /* This first case should never happen */
109 if(y==0 && x==0){return 0;}
110 if(x==0){
111 if(y>0){return PI/2.0;}
112 else{return -PI/2.0;}
113 }
114 if(x>=0){return atan(y/x);}
115 else{
116 if(y>=0){return atan(y/x)+PI;}
117 else{return atan(y/x)-PI;}
118 }
119 }
Angle(const double in[2])120 double Angle(const double in[2]){
121 if((in[0]*in[0]+in[1]*in[1])==0.0){return 0;}
122 else{return ArcTan2(in[1],in[0]);}
123 }
Sqrt(const double in[2],double out[2])124 void Sqrt(const double in[2],double out[2]){
125 double r=sqrt(sqrt(in[0]*in[0]+in[1]*in[1]));
126 double a=Angle(in)*0.5;
127 out[0]=r*cos(a);
128 out[1]=r*sin(a);
129 }
Add(const double in1[2],const double in2[2],double out[2])130 void Add(const double in1[2],const double in2[2],double out[2]){
131 out[0]=in1[0]+in2[0];
132 out[1]=in1[1]+in2[1];
133 }
Subtract(const double in1[2],const double in2[2],double out[2])134 void Subtract(const double in1[2],const double in2[2],double out[2]){
135 out[0]=in1[0]-in2[0];
136 out[1]=in1[1]-in2[1];
137 }
Multiply(const double in1[2],const double in2[2],double out[2])138 void Multiply(const double in1[2],const double in2[2],double out[2]){
139 out[0]=in1[0]*in2[0]-in1[1]*in2[1];
140 out[1]=in1[0]*in2[1]+in1[1]*in2[0];
141 }
Divide(const double in1[2],const double in2[2],double out[2])142 void Divide(const double in1[2],const double in2[2],double out[2]){
143 double temp[2];
144 double l=in2[0]*in2[0]+in2[1]*in2[1];
145 temp[0]= in2[0]/l;
146 temp[1]=-in2[1]/l;
147 Multiply(in1,temp,out);
148 }
149 // Solution taken from: http://mathworld.wolfram.com/QuarticEquation.html
150 // and http://www.csit.fsu.edu/~burkardt/f_src/subpak/subpak.f90
Factor(double a4,double a3,double a2,double a1,double a0,double roots[4][2],const double & EPS)151 int Factor(double a4,double a3,double a2,double a1,double a0,double roots[4][2],const double& EPS){
152 double R[2],D[2],E[2],R2[2];
153
154 if(fabs(a4)<EPS){return Factor(a3,a2,a1,a0,roots,EPS);}
155 a3/=a4;
156 a2/=a4;
157 a1/=a4;
158 a0/=a4;
159
160 Factor(1.0,-a2,a3*a1-4.0*a0,-a3*a3*a0+4.0*a2*a0-a1*a1,roots,EPS);
161
162 R2[0]=a3*a3/4.0-a2+roots[0][0];
163 R2[1]=0;
164 Sqrt(R2,R);
165 if(fabs(R[0])>10e-8){
166 double temp1[2],temp2[2];
167 double p1[2],p2[2];
168
169 p1[0]=a3*a3*0.75-2.0*a2-R2[0];
170 p1[1]=0;
171
172 temp2[0]=((4.0*a3*a2-8.0*a1-a3*a3*a3)/4.0);
173 temp2[1]=0;
174 Divide(temp2,R,p2);
175
176 Add (p1,p2,temp1);
177 Subtract(p1,p2,temp2);
178
179 Sqrt(temp1,D);
180 Sqrt(temp2,E);
181 }
182 else{
183 R[0]=R[1]=0;
184 double temp1[2],temp2[2];
185 temp1[0]=roots[0][0]*roots[0][0]-4.0*a0;
186 temp1[1]=0;
187 Sqrt(temp1,temp2);
188 temp1[0]=a3*a3*0.75-2.0*a2+2.0*temp2[0];
189 temp1[1]= 2.0*temp2[1];
190 Sqrt(temp1,D);
191 temp1[0]=a3*a3*0.75-2.0*a2-2.0*temp2[0];
192 temp1[1]= -2.0*temp2[1];
193 Sqrt(temp1,E);
194 }
195
196 roots[0][0]=-a3/4.0+R[0]/2.0+D[0]/2.0;
197 roots[0][1]= R[1]/2.0+D[1]/2.0;
198
199 roots[1][0]=-a3/4.0+R[0]/2.0-D[0]/2.0;
200 roots[1][1]= R[1]/2.0-D[1]/2.0;
201
202 roots[2][0]=-a3/4.0-R[0]/2.0+E[0]/2.0;
203 roots[2][1]= -R[1]/2.0+E[1]/2.0;
204
205 roots[3][0]=-a3/4.0-R[0]/2.0-E[0]/2.0;
206 roots[3][1]= -R[1]/2.0-E[1]/2.0;
207 return 4;
208 }
209
Solve(const double * eqns,const double * values,double * solutions,const int & dim)210 int Solve(const double* eqns,const double* values,double* solutions,const int& dim){
211 int i,j,eIndex;
212 double v,m;
213 int *index=new int[dim];
214 int *set=new int[dim];
215 double* myEqns=new double[dim*dim];
216 double* myValues=new double[dim];
217
218 for(i=0;i<dim*dim;i++){myEqns[i]=eqns[i];}
219 for(i=0;i<dim;i++){
220 myValues[i]=values[i];
221 set[i]=0;
222 }
223 for(i=0;i<dim;i++){
224 // Find the largest equation that has a non-zero entry in the i-th index
225 m=-1;
226 eIndex=-1;
227 for(j=0;j<dim;j++){
228 if(set[j]){continue;}
229 if(myEqns[j*dim+i]!=0 && fabs(myEqns[j*dim+i])>m){
230 m=fabs(myEqns[j*dim+i]);
231 eIndex=j;
232 }
233 }
234 if(eIndex==-1){
235 delete[] index;
236 delete[] myValues;
237 delete[] myEqns;
238 delete[] set;
239 return 0;
240 }
241 // The position in which the solution for the i-th variable can be found
242 index[i]=eIndex;
243 set[eIndex]=1;
244
245 // Normalize the equation
246 v=myEqns[eIndex*dim+i];
247 for(j=0;j<dim;j++){myEqns[eIndex*dim+j]/=v;}
248 myValues[eIndex]/=v;
249
250 // Subtract it off from everything else
251 for(j=0;j<dim;j++){
252 if(j==eIndex){continue;}
253 double vv=myEqns[j*dim+i];
254 for(int k=0;k<dim;k++){myEqns[j*dim+k]-=myEqns[eIndex*dim+k]*vv;}
255 myValues[j]-=myValues[eIndex]*vv;
256 }
257 }
258 for(i=0;i<dim;i++){solutions[i]=myValues[index[i]];}
259 delete[] index;
260 delete[] myValues;
261 delete[] myEqns;
262 delete[] set;
263 return 1;
264 }
265