Developer Documentation
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Macros Modules Pages
Factor.cpp
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 
30 // Polynomial Roots //
32 #include <math.h>
33 #include "Factor.h"
34 int Factor(double a1,double a0,double roots[1][2],double EPS){
35  if(fabs(a1)<=EPS){return 0;}
36  roots[0][0]=-a0/a1;
37  roots[0][1]=0;
38  return 1;
39 }
40 int Factor(double a2,double a1,double a0,double roots[2][2],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
62 int Factor(double a3,double a2,double a1,double a0,double roots[3][2],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 }
107 double ArcTan2(double y,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 }
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 }
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 }
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 }
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 }
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 }
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
151 int Factor(double a4,double a3,double a2,double a1,double a0,double roots[4][2],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 
210 int Solve(const double* eqns,const double* values,double* solutions,int dim){
211  int i,j;
212  double v;
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  double m=-1;
226  int 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 }