 |
00001 #include "SystemSolver.h" 00002 #include "MathTools.h" 00003 #include <stdlib.h> 00004 #include <assert.h> 00005 00006 #define TINY 1.0e-20; 00007 00009 // Construction/Destruction 00011 00012 SystemSolver::SystemSolver(){ 00013 } 00014 00015 SystemSolver::~SystemSolver(){ 00016 } 00017 00018 void SystemSolver::luDecompose(real** a, int n, int* index, real* vv){ 00019 int i,imax,j,k; 00020 real big,dum,sum,temp; 00021 00022 //vv=(real*)malloc(n * sizeof(real)); 00023 for (i=0;i<n;i++) { 00024 big=0.0; 00025 for (j=0;j<n;j++) 00026 if ((temp=fabs(a[i][j])) > big) big=temp; 00027 assert (big != 0.0); 00028 vv[i]=1.0/big; 00029 } 00030 for (j=0;j<n;j++) { 00031 for (i=0;i<j;i++) { 00032 sum=a[i][j]; 00033 for (k=0;k<i;k++) sum -= a[i][k]*a[k][j]; 00034 a[i][j]=sum; 00035 } 00036 big=0.0; 00037 for (i=j;i<n;i++) { 00038 sum=a[i][j]; 00039 for (k=0;k<j;k++) 00040 sum -= a[i][k]*a[k][j]; 00041 a[i][j]=sum; 00042 if ( (dum=vv[i]*fabs(sum)) >= big) { 00043 big=dum; 00044 imax=i; 00045 } 00046 } 00047 if (j != imax) { 00048 for (k=0;k<n;k++) { 00049 dum=a[imax][k]; 00050 a[imax][k]=a[j][k]; 00051 a[j][k]=dum; 00052 } 00053 //*d = -(*d); 00054 vv[imax]=vv[j]; 00055 } 00056 index[j]=imax; 00057 if (a[j][j] == 0.0f) a[j][j]=TINY; 00058 if (j != n-1) { 00059 dum=1.0/(a[j][j]); 00060 for (i=j+1;i<n;i++) a[i][j] *= dum; 00061 } 00062 } 00063 //free(vv); 00064 } 00065 00066 void SystemSolver::luSubstitute(real** a, int n, int* index, real* b){ 00067 int i,ii=-1,ip,j; 00068 real sum; 00069 00070 for (i=0;i<n;i++) { 00071 ip=index[i]; 00072 sum=b[ip]; 00073 b[ip]=b[i]; 00074 if (ii>=0) 00075 for (j=ii;j<=i-1;j++) sum -= a[i][j]*b[j]; 00076 else if (sum) ii=i; 00077 b[i]=sum; 00078 } 00079 for (i=n-1;i>=0;i--) { 00080 sum=b[i]; 00081 for (j=i+1;j<n;j++) sum -= a[i][j]*b[j]; 00082 b[i]=sum/a[i][i]; 00083 } 00084 } 00085 00086 void SystemSolver::svDecompose(real** a, int n, real* w, real** v, real* rv1){ 00087 int flag,i,its,j,jj,k,L,nm; 00088 real anorm,c,f,g,h,s,scale,x,y,z; 00089 00090 g=scale=anorm=0.0; 00091 for (i=0;i<n;i++) { 00092 L=i+1; 00093 rv1[i]=scale*g; 00094 g=s=scale=0.0; 00095 00096 for (k=i;k<n;k++) scale += fabs(a[k][i]); 00097 if (scale) { 00098 for (k=i;k<n;k++) { 00099 a[k][i] /= scale; 00100 s += a[k][i]*a[k][i]; 00101 } 00102 f=a[i][i]; 00103 g = -MathTools::setSign(sqrt(s),f); 00104 h=f*g-s; 00105 a[i][i]=f-g; 00106 for (j=L;j<n;j++) { 00107 for (s=0.0,k=i;k<n;k++) s += a[k][i]*a[k][j]; 00108 f=s/h; 00109 for (k=i;k<n;k++) a[k][j] += f*a[k][i]; 00110 } 00111 for (k=i;k<n;k++) a[k][i] *= scale; 00112 } 00113 00114 w[i]=scale *g; 00115 g=s=scale=0.0; 00116 if (i < n-1) { 00117 for (k=L;k<n;k++) scale += fabs(a[i][k]); 00118 if (scale) { 00119 for (k=L;k<n;k++) { 00120 a[i][k] /= scale; 00121 s += a[i][k]*a[i][k]; 00122 } 00123 f=a[i][L]; 00124 g = -MathTools::setSign(sqrt(s),f); 00125 h=f*g-s; 00126 a[i][L]=f-g; 00127 for (k=L;k<n;k++) rv1[k]=a[i][k]/h; 00128 for (j=L;j<n;j++) { 00129 for (s=0.0,k=L;k<n;k++) s += a[j][k]*a[i][k]; 00130 for (k=L;k<n;k++) a[j][k] += s*rv1[k]; 00131 } 00132 for (k=L;k<n;k++) a[i][k] *= scale; 00133 } 00134 } 00135 anorm=MathTools::max(anorm,(fabs(w[i])+fabs(rv1[i]))); 00136 } 00137 for (i=n-1;i>=0;i--) { 00138 if (i < n-1) { 00139 if (g) { 00140 for (j=L;j<n;j++) 00141 v[j][i]=(a[i][j]/a[i][L])/g; 00142 for (j=L;j<n;j++) { 00143 for (s=0.0,k=L;k<n;k++) s += a[i][k]*v[k][j]; 00144 for (k=L;k<n;k++) v[k][j] += s*v[k][i]; 00145 } 00146 } 00147 for (j=L;j<n;j++) v[i][j]=v[j][i]=0.0; 00148 } 00149 v[i][i]=1.0; 00150 g=rv1[i]; 00151 L=i; 00152 } 00153 for (i=n-1;i>=0;i--) { 00154 L=i+1; 00155 g=w[i]; 00156 for (j=L;j<n;j++) a[i][j]=0.0; 00157 if (g) { 00158 g=1.0/g; 00159 for (j=L;j<n;j++) { 00160 for (s=0.0,k=L;k<n;k++) s += a[k][i]*a[k][j]; 00161 f=(s/a[i][i])*g; 00162 for (k=i;k<n;k++) a[k][j] += f*a[k][i]; 00163 } 00164 for (j=i;j<n;j++) a[j][i] *= g; 00165 } else for (j=i;j<n;j++) a[j][i]=0.0; 00166 ++a[i][i]; 00167 } 00168 for (k=n-1;k>=0;k--) { 00169 for (its=1;its<=30;its++) { 00170 flag=1; 00171 for (L=k;L>=0;L--) { 00172 nm=L-1; 00173 if ((float)(fabs(rv1[L])+anorm) == anorm) { 00174 flag=0; 00175 break; 00176 } 00177 if ((float)(fabs(w[nm])+anorm) == anorm) break; 00178 } 00179 if (flag) { 00180 c=0.0; 00181 s=1.0; 00182 for (i=L;i<=k;i++) { 00183 f=s*rv1[i]; 00184 rv1[i]=c*rv1[i]; 00185 if ((float)(fabs(f)+anorm) == anorm) break; 00186 g=w[i]; 00187 h=MathTools::modulus(f,g); 00188 w[i]=h; 00189 h=1.0/h; 00190 c=g*h; 00191 s = -f*h; 00192 for (j=0;j<n;j++) { 00193 y=a[j][nm]; 00194 z=a[j][i]; 00195 a[j][nm]=y*c+z*s; 00196 a[j][i]=z*c-y*s; 00197 } 00198 } 00199 } 00200 z=w[k]; 00201 if (L == k) { 00202 if (z < 0.0) { 00203 w[k] = -z; 00204 for (j=0;j<n;j++) v[j][k] = -v[j][k]; 00205 } 00206 break; 00207 } 00208 if (its == 30) exit(0); //no convergence in 30 svdcmp iterations 00209 x=w[L]; 00210 nm=k-1; 00211 y=w[nm]; 00212 g=rv1[nm]; 00213 h=rv1[k]; 00214 f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y); 00215 g=MathTools::modulus(f,1.0); 00216 f=((x-z)*(x+z)+h*((y/(f+MathTools::setSign(g,f)))-h))/x; 00217 c=s=1.0; 00218 for (j=L;j<=nm;j++) { 00219 i=j+1; 00220 g=rv1[i]; 00221 y=w[i]; 00222 h=s*g; 00223 g=c*g; 00224 z=MathTools::modulus(f,h); 00225 rv1[j]=z; 00226 c=f/z; 00227 s=h/z; 00228 f=x*c+g*s; 00229 g = g*c-x*s; 00230 h=y*s; 00231 y *= c; 00232 for (jj=0;jj<n;jj++) { 00233 x=v[jj][j]; 00234 z=v[jj][i]; 00235 v[jj][j]=x*c+z*s; 00236 v[jj][i]=z*c-x*s; 00237 } 00238 z=MathTools::modulus(f,h); 00239 w[j]=z; 00240 if (z) { 00241 z=1.0/z; 00242 c=f*z; 00243 s=h*z; 00244 } 00245 f=c*g+s*y; 00246 x=c*y-s*g; 00247 for (jj=0;jj<n;jj++) { 00248 y=a[jj][j]; 00249 z=a[jj][i]; 00250 a[jj][j]=y*c+z*s; 00251 a[jj][i]=z*c-y*s; 00252 } 00253 } 00254 rv1[L]=0.0; 00255 rv1[k]=f; 00256 w[k]=x; 00257 } 00258 } 00259 } 00260 00261 00262 void SystemSolver::svSubstitute(real** u, int n, real* w, real** v, real* b, real* t){ 00263 real wmax=-1e10, wmin, s; 00264 int i,j,k; 00265 for(i=0;i<n;i++) 00266 if(w[i]>wmax) wmax=w[i]; 00267 wmin=wmax*1e-6; 00268 for(i=0;i<n;i++) 00269 if(w[i]<wmin) w[i]=0; 00270 00271 for (j=0;j<n;j++) { 00272 s=0.0; 00273 if (w[j]) { 00274 for (i=0;i<n;i++) s += u[i][j]*b[i]; 00275 s /= w[j]; 00276 } 00277 t[j]=s; 00278 } 00279 for (j=0;j<n;j++) { 00280 s=0.0; 00281 for (k=0;k<n;k++) s += v[j][k]*t[k]; 00282 w[j]=s; 00283 } 00284 }
|
Thyrix homepage • Users' guide
(C) Arxia 2004-2005