1 #include <cstdio> 2 #include <cstdlib> 3 #include <algorithm> 4 #include <cmath> 5 #include <cassert> 6 #include <vector> 7 #include <ctime> 8 9 10 class MclVector 11 { 12 public: 13 int n; 14 double *Mat; 15 /** 16 type=0: 列向量(默认) 17 type=1: 行向量 18 **/ 19 int type; 20 21 MclVector() { Mat=NULL; n=0; } 22 MclVector(int len,double initVal=0.0) 23 { 24 n=len; 25 Mat=new double[n+1]; 26 for(int i=0;i<=n;i++) Mat[i]=initVal; 27 type=0; 28 } 29 30 31 double operator[](int id) const 32 { 33 return Mat[id]; 34 } 35 36 37 double& operator[](int id) 38 { 39 return Mat[id]; 40 } 41 42 double length() const 43 { 44 double sum=0; 45 for(int i=1;i<=n;i++) sum+=Mat[i]*Mat[i]; 46 return sqrt(sum); 47 } 48 49 MclVector operator*(double val) const 50 { 51 MclVector ans=MclVector(n); 52 for(int i=1;i<=n;i++) ans[i]=Mat[i]*val; 53 return ans; 54 } 55 56 MclVector operator/(double val) const 57 { 58 MclVector ans=MclVector(n); 59 for(int i=1;i<=n;i++) ans[i]=Mat[i]/val; 60 return ans; 61 } 62 63 MclVector operator+(const MclVector &newVector) const 64 { 65 MclVector ans=MclVector(n); 66 for(int i=1;i<=n;i++) ans[i]=Mat[i]+newVector[i]; 67 return ans; 68 } 69 70 71 MclVector operator-(const MclVector &newVector) const 72 { 73 MclVector ans=MclVector(n); 74 for(int i=1;i<=n;i++) ans[i]=Mat[i]-newVector[i]; 75 return ans; 76 } 77 78 MclVector operator*=(double val) 79 { 80 for(int i=1;i<=n;i++) Mat[i]=Mat[i]*val; 81 return *this; 82 } 83 84 MclVector operator/=(double val) 85 { 86 for(int i=1;i<=n;i++) Mat[i]=Mat[i]/val; 87 return *this; 88 } 89 90 91 MclVector operator+=(const MclVector &newVector) 92 { 93 for(int i=1;i<=n;i++) Mat[i]+=newVector[i]; 94 return *this; 95 } 96 97 MclVector operator-=(const MclVector &newVector) 98 { 99 for(int i=1;i<=n;i++) Mat[i]-=newVector[i]; 100 return *this; 101 } 102 103 MclVector GetTranspose() const 104 { 105 MclVector ans=*this; 106 ans.type=1; 107 return ans; 108 } 109 110 111 void print() const 112 { 113 for(int i=1;i<=n;i++) printf("%8.3lf ",Mat[i]); 114 puts(""); 115 } 116 117 118 }; 119 120 class MclMatrix 121 { 122 public: 123 int row,col; 124 MclVector *Mat; 125 126 MclMatrix() {Mat=NULL;} 127 MclMatrix(int _row,int _col,double initVal=0.0) 128 { 129 row=_row; 130 col=_col; 131 Mat=new MclVector[row+1]; 132 for(int i=0;i<=row;i++) Mat[i]=MclVector(col,initVal); 133 } 134 135 void setIdentityMatrix() 136 { 137 for(int i=1;i<=row;i++) 138 { 139 for(int j=1;j<=col;j++) 140 { 141 if(i==j) Mat[i][j]=1; 142 else Mat[i][j]=0; 143 } 144 } 145 } 146 147 MclMatrix GetTranspose() const 148 { 149 MclMatrix ans=MclMatrix(col,row); 150 for(int i=1;i<=ans.row;i++) 151 { 152 for(int j=1;j<=ans.col;j++) 153 { 154 ans[i][j]=Mat[j][i]; 155 } 156 } 157 return ans; 158 } 159 160 void print() const 161 { 162 for(int i=1;i<=row;i++) Mat[i].print(); 163 puts(""); 164 } 165 166 MclVector& operator[](int id) const 167 { 168 return Mat[id]; 169 } 170 171 172 MclVector& operator[](int id) 173 { 174 return Mat[id]; 175 } 176 177 MclMatrix operator*(const MclMatrix &Matrix) const 178 { 179 MclMatrix ans=MclMatrix(row,Matrix.col); 180 for(int i=1;i<=row;i++) 181 { 182 for(int j=1;j<=Matrix.col;j++) 183 { 184 for(int k=1;k<=col;k++) 185 { 186 ans[i][j]+=Mat[i][k]*Matrix[k][j]; 187 } 188 } 189 } 190 return ans; 191 } 192 193 MclMatrix operator+(const MclMatrix &Matrix) const 194 { 195 MclMatrix ans=MclMatrix(row,Matrix.col); 196 for(int i=1;i<=row;i++) 197 { 198 for(int j=1;j<=Matrix.col;j++) 199 { 200 ans[i][j]=Mat[i][j]+Matrix[i][j]; 201 } 202 } 203 return ans; 204 } 205 206 MclMatrix operator-(const MclMatrix &Matrix) const 207 { 208 MclMatrix ans=MclMatrix(row,Matrix.col); 209 for(int i=1;i<=row;i++) 210 { 211 for(int j=1;j<=Matrix.col;j++) 212 { 213 ans[i][j]=Mat[i][j]-Matrix[i][j]; 214 } 215 } 216 return ans; 217 } 218 219 MclVector GetCol(int colId) const 220 { 221 MclVector ans=MclVector(row); 222 for(int i=1;i<=row;i++) ans[i]=Mat[i][colId]; 223 return ans; 224 } 225 MclVector GetRow(int rowId) const 226 { 227 MclVector ans=MclVector(row); 228 for(int i=1;i<=col;i++) ans[i]=Mat[rowId][i]; 229 return ans; 230 } 231 232 233 MclMatrix operator*=(const MclMatrix &Matrix) 234 { 235 return *this=*this*Matrix; 236 } 237 MclMatrix operator+=(const MclMatrix &Matrix) 238 { 239 return *this=*this+Matrix; 240 } 241 MclMatrix operator-=(const MclMatrix &Matrix) 242 { 243 return *this=*this-Matrix; 244 } 245 246 MclMatrix operator*(double x) const 247 { 248 MclMatrix ans=*this; 249 for(int i=1;i<=row;i++) 250 { 251 for(int j=1;j<=col;j++) 252 { 253 ans[i][j]*=x; 254 } 255 } 256 return ans; 257 } 258 259 }; 260 261 MclMatrix vectorMulVector(const MclVector &A,const MclVector& B) 262 { 263 if(A.type==0) 264 { 265 MclMatrix ans=MclMatrix(A.n,B.n); 266 for(int i=1;i<=A.n;i++) 267 { 268 for(int j=1;j<=B.n;j++) 269 { 270 ans[i][j]+=A[i]*B[j]; 271 } 272 } 273 return ans; 274 } 275 else 276 { 277 assert(A.n==B.n); 278 MclMatrix ans=MclMatrix(1,1); 279 for(int i=1;i<=A.n;i++) 280 { 281 ans[1][1]+=A[i]*B[i]; 282 } 283 return ans; 284 } 285 } 286 287 int sgn(double x) 288 { 289 if(x<-0.000001) return -1; 290 if(x>0.000001) return 1; 291 return 0; 292 } 293 294 295 296 /** 297 矩阵的 Doolittle分解: 298 [1] Mat是方阵 299 [2] Mat的前n-1阶主子式行列式不为0 300 [3] 分解的L为单位下三角阵 301 [4] 分解的U为上三角阵 302 [5] 返回值为<L,R> 303 **/ 304 std::pair<MclMatrix,MclMatrix> DoolittleSplit(const MclMatrix &Mat) 305 { 306 int n=Mat.row; 307 MclMatrix L=MclMatrix(n,n); 308 MclMatrix U=MclMatrix(n,n); 309 for(int k=1;k<=n;k++) 310 { 311 for(int j=k;j<=n;j++) 312 { 313 U[k][j]=Mat[k][j]; 314 for(int t=1;t<=k-1;t++) U[k][j]-=L[k][t]*U[t][j]; 315 } 316 if(k==n) continue; 317 318 for(int i=k+1;i<=n;i++) 319 { 320 L[i][k]=Mat[i][k]; 321 for(int t=1;t<=k-1;t++) L[i][k]-=L[i][t]*U[t][k]; 322 L[i][k]/=U[k][k]; 323 } 324 } 325 for(int i=1;i<=n;i++) L[i][i]=1; 326 return std::make_pair(L,U); 327 } 328 329 330 331 /** 332 三角矩阵分解: 333 [1] Mat是方阵 334 [2] j<i且i-j>r时 Mat[i][j]=0 335 [2] j>i且j-i>s时 Mat[i][j]=0 336 **/ 337 std::pair<MclMatrix,MclMatrix> TriangleSplit(const MclMatrix &Mat,int r,int s) 338 { 339 int n=Mat.row; 340 MclMatrix L=MclMatrix(n,n); 341 MclMatrix U=MclMatrix(n,n); 342 for(int k=1;k<=n;k++) 343 { 344 for(int j=k;j<=n;j++) 345 { 346 U[k][j]=Mat[k][j]; 347 for(int t=std::max(1,std::max(k-r,j-s));t<=k-1;t++) U[k][j]-=L[k][t]*U[t][j]; 348 } 349 if(k==n) continue; 350 351 for(int i=k+1;i<=n;i++) 352 { 353 L[i][k]=Mat[i][k]; 354 for(int t=std::max(1,std::max(i-r,k-s));t<=k-1;t++) L[i][k]-=L[i][t]*U[t][k]; 355 L[i][k]/=U[k][k]; 356 } 357 } 358 for(int i=1;i<=n;i++) L[i][i]=1; 359 return std::make_pair(L,U); 360 } 361 362 /** 363 拟三对角矩阵分解 364 对n=5 矩阵A样子如下: 365 a1 c1 0 0 d1 366 d2 a2 c2 0 0 367 0 d3 a3 c3 0 368 0 0 d4 a4 c4 369 c5 0 0 d5 a5 370 即输入为三个长度为n的向量 371 372 A=LU 373 L样子如下: 374 p1 0 0 0 0 375 d2 p2 0 0 0 376 0 d3 p3 0 0 377 0 0 d3 p4 0 378 r1 r2 r3 r4 r5 379 380 U样子如下: 381 1 q1 0 0 s1 382 0 1 q2 0 s2 383 0 0 1 q3 s3 384 0 0 0 1 s4 385 0 0 0 0 1 386 387 即将返回p,q,s,r四个向量(所有的向量长度都是n) 388 vector[0]=p 389 vector[1]=q 390 vector[2]=s 391 vector[3]=r 392 **/ 393 394 std::vector<MclVector> QuasiDiagonalSplit(const MclVector &a,const MclVector &c,const MclVector &d) 395 { 396 int n=a.n; 397 assert(c.n==n); 398 assert(d.n==n); 399 assert(n>0); 400 401 MclVector p=MclVector(n); 402 MclVector q=MclVector(n); 403 MclVector s=MclVector(n); 404 MclVector r=MclVector(n); 405 406 p[1]=a[1]; 407 for(int i=1;i<=n-2;i++) 408 { 409 q[i]=c[i]/p[i]; 410 p[i+1]=a[i+1]-d[i+1]*q[i]; 411 } 412 413 s[1]=d[1]/p[1]; 414 for(int i=2;i<=n-2;i++) s[i]=-d[i]*s[i-1]/p[i]; 415 s[n-1]=(c[n-1]-d[n-1]*s[n-2])/p[n-1]; 416 417 r[1]=c[n]; 418 for(int j=2;j<=n-2;j++) r[j]=-r[j-1]*q[j-1]; 419 r[n-1]=d[n]-r[n-2]*q[n-2]; 420 r[n]=a[n]; 421 for(int j=1;j<=n-1;j++) r[n]=r[n]-r[j]*s[j]; 422 423 std::vector<MclVector> ans; 424 ans.push_back(p); 425 ans.push_back(q); 426 ans.push_back(s); 427 ans.push_back(r); 428 429 return ans; 430 }