1 #include 
     
        2 #include 
      
         3 #include 
       
          4 #include 
        
           5 #include 
         
            6 #include 
          
            7 #include 
           
             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() const104 {105 MclVector ans=*this;106 ans.type=1;107 return ans;108 }109 110 111 void print() const112 {113 for(int i=1;i<=n;i++) printf("%8.3lf ",Mat[i]);114 puts("");115 }116 117 118 };119 120 class MclMatrix121 {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() const148 {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() const161 {162 for(int i=1;i<=row;i++) Mat[i].print();163 puts("");164 }165 166 MclVector& operator[](int id) const167 {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) const178 {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) const194 {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) const207 {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) const220 {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) const226 {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) const247 {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 else276 {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阶主子式行列式不为0300 [3] 分解的L为单位下三角阵301 [4] 分解的U为上三角阵302 [5] 返回值为
            
             303 **/304 std::pair
             
               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
              
               r时 Mat[i][j]=0335 [2] j>i且j-i>s时 Mat[i][j]=0336 **/337 std::pair
               
                 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 d1366 d2 a2 c2 0 0367 0 d3 a3 c3 0368 0 0 d4 a4 c4369 c5 0 0 d5 a5370 即输入为三个长度为n的向量371 372 A=LU373 L样子如下：374 p1 0 0 0 0375 d2 p2 0 0 0376 0 d3 p3 0 0377 0 0 d3 p4 0378 r1 r2 r3 r4 r5379 380 U样子如下:381 1 q1 0 0 s1382 0 1 q2 0 s2383 0 0 1 q3 s3384 0 0 0 1 s4385 0 0 0 0 1386 387 即将返回p,q,s,r四个向量(所有的向量长度都是n)388 vector[0]=p389 vector[1]=q390 vector[2]=s391 vector[3]=r392 **/393 394 std::vector
                
                  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
                 
                   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 }