34 int Factor(
double a1,
double a0,
double roots[1][2],
double EPS){
35 if(fabs(a1)<=EPS){
return 0;}
40 int Factor(
double a2,
double a1,
double a0,
double roots[2][2],
double EPS){
42 if(fabs(a2)<=EPS){
return Factor(a1,a0,roots,EPS);}
48 roots[0][0]=roots[1][0]=-a1;
54 roots[0][1]=roots[1][1]=0;
62 int Factor(
double a3,
double a2,
double a1,
double a0,
double roots[3][2],
double EPS){
65 if(fabs(a3)<=EPS){
return Factor(a2,a1,a0,roots,EPS);}
71 r=-(9*a2*a1-27*a0-2*a2*a2*a2)/54;
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);
86 double s1,s2,sqr=sqrt(r2-q3);
89 if(t<0){s1=-pow(-t,1.0/3);}
90 else{s1=pow(t,1.0/3);}
92 if(t<0){s2=-pow(-t,1.0/3);}
93 else{s2=pow(t,1.0/3);}
98 roots[1][0]= roots[2][0]=-s1-s2;
99 roots[1][1]= SQRT_3*(s1-s2);
100 roots[2][1]=-roots[1][1];
107 double ArcTan2(
double y,
double x){
109 if(y==0 && x==0){
return 0;}
111 if(y>0){
return PI/2.0;}
112 else{
return -PI/2.0;}
114 if(x>=0){
return atan(y/x);}
116 if(y>=0){
return atan(y/x)+PI;}
117 else{
return atan(y/x)-PI;}
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]);}
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;
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];
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];
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];
142 void Divide(
const double in1[2],
const double in2[2],
double out[2]){
144 double l=in2[0]*in2[0]+in2[1]*in2[1];
147 Multiply(in1,temp,out);
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];
154 if(fabs(a4)<EPS){
return Factor(a3,a2,a1,a0,roots,EPS);}
160 Factor(1.0,-a2,a3*a1-4.0*a0,-a3*a3*a0+4.0*a2*a0-a1*a1,roots,EPS);
162 R2[0]=a3*a3/4.0-a2+roots[0][0];
165 if(fabs(R[0])>10e-8){
166 double temp1[2],temp2[2];
169 p1[0]=a3*a3*0.75-2.0*a2-R2[0];
172 temp2[0]=((4.0*a3*a2-8.0*a1-a3*a3*a3)/4.0);
177 Subtract(p1,p2,temp2);
184 double temp1[2],temp2[2];
185 temp1[0]=roots[0][0]*roots[0][0]-4.0*a0;
188 temp1[0]=a3*a3*0.75-2.0*a2+2.0*temp2[0];
189 temp1[1]= 2.0*temp2[1];
191 temp1[0]=a3*a3*0.75-2.0*a2-2.0*temp2[0];
192 temp1[1]= -2.0*temp2[1];
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;
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;
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;
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;
210 int Solve(
const double* eqns,
const double* values,
double* solutions,
int dim){
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];
218 for(i=0;i<dim*dim;i++){myEqns[i]=eqns[i];}
220 myValues[i]=values[i];
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]);
246 v=myEqns[eIndex*dim+i];
247 for(j=0;j<dim;j++){myEqns[eIndex*dim+j]/=v;}
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;
258 for(i=0;i<dim;i++){solutions[i]=myValues[index[i]];}