仿射变换的一个练习题！
问题：已知坐标A(0,2,sqrt(2))、B(1,1,sqrt(2))、C(2,0,sqrt(2))连接成直线，希望围绕其在XOY平面内投影旋转90度，求新的坐标点A'B’C'。
解决方案：仿射变换(参考书籍《交互式计算机图形学——基于OpenGL的自顶向下方法》154——163)
（1） 
（2）
这里出现两次错误，首先是T-1表示的是T（-AXOY），即将A的投影点AXOY移到原点，视AXOY为中心和不动点。第二处T中第二行第三列不是2，应该是0，这个中间结果表示正确。
中间结果：
实现函数：
变换函数
1 public static IPolyline Polyline_3D_2D(IPolyline m_Polyline, double m_BaseHeight, double zFactor) 2 { 3 IPolyline m_2DPolyline = new PolylineClass(); 4 IPointCollection m_2DPointCollection = m_2DPolyline as IPointCollection; 5 6 IPointCollection m3dPtCol = m_Polyline as IPointCollection; 7 Matrix m_P_Matrix = new Matrix(4, m3dPtCol.PointCount); 8 for (int i = 0; i < m3dPtCol.PointCount; i++) 9 { 10 IPoint pt = m3dPtCol.get_Point(i); 11 m_P_Matrix[0, i] = pt.X; 12 m_P_Matrix[1, i] = pt.Y; 13 m_P_Matrix[2, i] = (pt.Z - m_BaseHeight) * zFactor; 14 m_P_Matrix[3, i] = 1; 15 } 16 Matrix m_U_Matrix = new Matrix(4, 1);//旋转方向向量U 17 m_U_Matrix[0, 0] = m3dPtCol.get_Point(m3dPtCol.PointCount - 1).X - m3dPtCol.get_Point(0).X; 18 m_U_Matrix[1, 0] = m3dPtCol.get_Point(m3dPtCol.PointCount - 1).Y - m3dPtCol.get_Point(0).Y; 19 m_U_Matrix[2, 0] = 0; 20 m_U_Matrix[3, 0] = 0; 21 Matrix m_V_Matrix = new Matrix(4, 1);//旋转方向向量U的基V 22 double d2 = m_U_Matrix[0, 0] * m_U_Matrix[0, 0] + m_U_Matrix[1, 0] * m_U_Matrix[1, 0] + m_U_Matrix[2, 0] * m_U_Matrix[2, 0] + m_U_Matrix[3, 0] * m_U_Matrix[3, 0]; 23 m_V_Matrix[0, 0] = m_U_Matrix[0, 0] / Math.Sqrt(d2); 24 m_V_Matrix[1, 0] = m_U_Matrix[1, 0] / Math.Sqrt(d2); 25 m_V_Matrix[2, 0] = m_U_Matrix[2, 0] / Math.Sqrt(d2); 26 m_V_Matrix[3, 0] = m_U_Matrix[3, 0] / Math.Sqrt(d2); 27 28 Matrix m_T_matrix = new Matrix(4);//平移矩阵T（-pf） 29 m_T_matrix.MakeUnitMatrix(4); 30 m_T_matrix[0, 3] = -m_P_Matrix[0, 0]; 31 m_T_matrix[1, 3] = -m_P_Matrix[1, 0]; 32 m_T_matrix[2, 3] = 0; 33 34 double dd = m_V_Matrix[2, 0] * m_V_Matrix[2, 0] + m_V_Matrix[1, 0] * m_V_Matrix[1, 0]; 35 Matrix m_Rx_matrix = new Matrix(4);//x旋转 36 m_Rx_matrix.MakeUnitMatrix(4); 37 m_Rx_matrix[1, 1] = m_V_Matrix[2, 0] / Math.Sqrt(dd); 38 m_Rx_matrix[1, 2] = -m_V_Matrix[1, 0] / Math.Sqrt(dd); 39 m_Rx_matrix[2, 1] = m_V_Matrix[1, 0] / Math.Sqrt(dd); 40 m_Rx_matrix[2, 2] = m_V_Matrix[2, 0] / Math.Sqrt(dd); 41 42 Matrix m_Ry_matrix = new Matrix(4);//y旋转 43 m_Ry_matrix.MakeUnitMatrix(4); 44 m_Ry_matrix[0, 0] = Math.Sqrt(dd); 45 m_Ry_matrix[0, 2] = -m_V_Matrix[0, 0]; 46 m_Ry_matrix[2, 0] = m_V_Matrix[0, 0]; 47 m_Ry_matrix[2, 2] = Math.Sqrt(dd); 48 49 Matrix m_Rz_matrix = new Matrix(4);//z旋转 50 m_Rz_matrix.MakeUnitMatrix(4); 51 m_Rz_matrix[0, 0] = Math.Cos(Math.PI / 2); 52 m_Rz_matrix[0, 1] = -Math.Sin(Math.PI / 2); 53 m_Rz_matrix[1, 0] = Math.Sin(Math.PI / 2); 54 m_Rz_matrix[1, 1] = Math.Cos(Math.PI / 2); 55 56 Matrix m_M_Matrix = new Matrix(4);//级联变换矩阵 57 Matrix m_Tr_matrix = new Matrix(m_T_matrix); 58 m_Tr_matrix.InvertGaussJordan();//平移矩阵的逆 59 Matrix m_Rxr_matrix = new Matrix(m_Rx_matrix); 60 m_Rxr_matrix.InvertGaussJordan();//x旋转的逆 61 Matrix m_Ryr_matrix = new Matrix(m_Ry_matrix); 62 m_Ryr_matrix.InvertGaussJordan();//y旋转的逆 63 m_M_Matrix = m_Tr_matrix * m_Rxr_matrix * m_Ryr_matrix * m_Rz_matrix * m_Ry_matrix * m_Rx_matrix * m_T_matrix; 64 Matrix m_Pnew_Matrix = new Matrix(4, m3dPtCol.PointCount); 65 m_Pnew_Matrix = m_M_Matrix * m_P_Matrix;//实现旋转 66 67 for (int k = 0; k < m_Pnew_Matrix.Columns; k++) 68 { 69 IPoint pt3d = new PointClass(); 70 71 pt3d.X = m_Pnew_Matrix[0, k]; 72 pt3d.Y = m_Pnew_Matrix[1, k]; 73 pt3d.Z = m_Pnew_Matrix[2, k]; 74 m_2DPointCollection.AddPoint(pt3d, ref mis, ref mis); 75 } 76 77 return m_2DPolyline; 78 }
矩阵类：
矩阵类
1 using System; 2 3 namespace CMathematics.MatrixAlgebra 4 { 5 /// <summary> 6 /// C#数值计算算法编程 7 /// 矩阵类，操作矩阵的了Matrix 8 /// 矩阵的赋值是用一维数组，用行列控制 9 /// </summary> 10 public class Matrix : CMathematics.MatrixAlgebra.IMatrix 11 { 12 private int numColumns = 0; //矩阵列数 13 private int numRows = 0; //矩阵行数 14 private double eps = 0.0; //缺省精度 15 private double[] elements = null; //矩阵数据缓冲区 16 17 /// <summary> 18 /// 属性：矩阵列数 19 /// </summary> 20 public int Columns 21 { 22 get 23 { 24 return numColumns; 25 } 26 } 27 28 /// <summary> 29 ///属性：矩阵行数 30 /// </summary> 31 public int Rows 32 { 33 get 34 { 35 return numRows; 36 } 37 } 38 39 /// <summary> 40 /// 索引器：访问矩阵元素 41 /// </summary> 42 /// <param name="row">元素的行</param> 43 /// <param name="col">元素的列</param> 44 /// <returns></returns> 45 public double this[int row, int col] 46 { 47 get 48 { 49 return elements[col + row * numColumns]; 50 } 51 set 52 { 53 elements[col + row * numColumns] = value; 54 } 55 } 56 57 /// <summary> 58 /// 属性：Eps,矩阵运算的精度 59 /// </summary> 60 public double Eps 61 { 62 get 63 { 64 return eps; 65 } 66 set 67 { 68 eps = value; 69 } 70 } 71 /// <summary> 72 /// 基本构造函数 73 /// </summary> 74 75 public Matrix() 76 { 77 numColumns = 1; 78 numRows = 1; 79 Init(numRows, numColumns); 80 } 81 82 /// <summary> 83 /// 指定行列构造函数 84 /// </summary> 85 /// <param name="nRows">指定的矩阵行数</param> 86 /// <param name="nCols">指定的矩阵列数</param> 87 public Matrix(int nRows, int nCols) 88 { 89 numRows = nRows; 90 numColumns = nCols; 91 Init(numRows, numColumns); 92 } 93 94 /// <summary> 95 /// 指定值构造函数 96 /// </summary> 97 /// <param name="nRows">-指定的矩阵行数</param> 98 /// <param name="nCols">指定的矩阵列数</param> 99 /// <param name="value">一维数组,长度为nRows*nCols,存储矩阵各元素的值</param> 100 public Matrix(int nRows, int nCols, double[] value) 101 { 102 numRows = nRows; 103 numColumns = nCols; 104 Init(numRows, numColumns); 105 SetData(value); 106 } 107 108 /// <summary> 109 /// 方阵构造函数 110 /// </summary> 111 /// <param name="nSize">方阵行列数</param> 112 public Matrix(int nSize) 113 { 114 numRows = nSize; 115 numColumns = nSize; 116 Init(nSize, nSize); 117 } 118 /// <summary> 119 /// 方阵构造函数 120 /// </summary> 121 /// <param name="nSize">方阵行列数</param> 122 /// <param name="value">维数组,长度为nRows*nRows,存储方阵各元素的值</param> 123 public Matrix(int nSize, double[] value) 124 { 125 numRows = nSize; 126 numColumns = nSize; 127 Init(nSize, nSize); 128 SetData(value); 129 } 130 131 /// <summary> 132 /// 拷贝构造函数 133 /// </summary> 134 /// <param name="other">源矩阵</param> 135 public Matrix(Matrix other) 136 { 137 numColumns = other.GetNumColumns(); 138 numRows = other.GetNumRows(); 139 Init(numRows, numColumns); 140 SetData(other.elements); 141 } 142 /// <summary> 143 /// 初始化函数 144 /// </summary> 145 /// <param name="nRows">指定的矩阵行数</param> 146 /// <param name="nCols">指定的矩阵列数</param> 147 /// <returns>bool,成功返回true,否则返回false</returns> 148 public bool Init(int nRows, int nCols) 149 { 150 numRows = nRows; 151 numColumns = nCols; 152 int nSize = nCols * nRows; 153 if (nSize < 0) 154 return false; 155 //分配内存 156 elements = new double[nSize]; 157 158 return true; 159 } 160 161 /// <summary> 162 /// 设置矩阵运算的精度 163 /// </summary> 164 /// <param name="newEps">新的精度值</param> 165 public void SetEps(double newEps) 166 { 167 eps = newEps; 168 } 169 170 /// <summary> 171 /// 取矩阵的精度值 172 /// </summary> 173 /// <returns>double型,矩阵的精度值</returns> 174 public double GetEps() 175 { 176 return eps; 177 } 178 /// <summary> 179 /// 重载+运算符 180 /// </summary> 181 /// <param name="m1"></param> 182 /// <param name="m2"></param> 183 /// <returns>Matrix对象</returns> 184 public static Matrix operator +(Matrix m1, Matrix m2) 185 { 186 return m1.Add(m2); 187 188 } 189 /// <summary> 190 /// 重载-运算符 191 /// </summary> 192 /// <param name="m1"></param> 193 /// <param name="m2"></param> 194 /// <returns>Matrix对象</returns> 195 public static Matrix operator -(Matrix m1, Matrix m2) 196 { 197 return m1.Subtract(m2); 198 } 199 /// <summary> 200 /// 重载*运算符 201 /// </summary> 202 /// <param name="m1"></param> 203 /// <param name="m2"></param> 204 /// <returns>Matrix对象</returns> 205 public static Matrix operator *(Matrix m1, Matrix m2) 206 { 207 return m1.Multiply(m2); 208 } 209 /// <summary> 210 /// 重载double[]运算符 211 /// </summary> 212 /// <param name="m"></param> 213 /// <returns>double[]对象</returns> 214 public static implicit operator double[](Matrix m) 215 { 216 return m.elements; 217 } 218 /// <summary> 219 /// 将方阵初始化为单位矩阵 220 /// </summary> 221 /// <param name="nSize">方阵行列数</param> 222 /// <returns>bool型,初始化是否成功</returns> 223 public bool MakeUnitMatrix(int nSize) 224 { 225 if (!Init(nSize, nSize)) 226 return false; 227 for (int i = 0; i < nSize; ++i) 228 for (int j = 0; j < nSize; ++j) 229 if (i == j) 230 SetElement(i, j, 1); 231 return true; 232 } 233 /// <summary> 234 /// 将矩阵各元素的值转化为字符型,元素之间的分隔符为",",行与行之间有回车换行符 235 /// </summary> 236 /// <returns>string型,转换得到的字符串</returns> 237 public override string ToString() 238 { 239 return ToString(",", true); 240 } 241 /// <summary> 242 /// 将矩阵各元素的值转化为字符串 243 /// </summary> 244 /// <param name="sDelim">元素之间的分隔符</param> 245 /// <param name="bLineBreak">行与行之间是否有回车换行符</param> 246 /// <returns>string型,转换得到的字符串</returns> 247 public string ToString(string sDelim, bool bLineBreak) 248 { 249 string s = ""; 250 251 for (int i = 0; i < numRows; ++i) 252 { 253 for (int j = 0; j < numColumns; ++j) 254 { 255 string ss = GetElement(i, j).ToString("F8"); 256 s += ss; 257 258 if (bLineBreak) 259 { 260 if (j != numColumns - 1) 261 s += sDelim; 262 } 263 else 264 { 265 if (i != numRows - 1 || j != numColumns - 1) 266 s += sDelim; 267 } 268 } 269 if (bLineBreak) 270 if (i != numRows - 1) 271 s += "\r\n"; 272 } 273 return s; 274 } 275 /// <summary> 276 /// 将矩阵指定行中各元素的值转化为字符串 277 /// </summary> 278 /// <param name="nRow">指定的矩阵行,nRows=0表示第一行</param> 279 /// <param name="sDelim">元素之间的分隔符</param> 280 /// <returns>string型,转换得到的字符串</returns> 281 public string ToStringRow(int nRow, string sDelim) 282 { 283 string s = ""; 284 if (nRow >= numRows) 285 return s; 286 287 for (int j = 0; j < numColumns; ++j) 288 { 289 string ss = GetElement(nRow, j).ToString("F8"); 290 s += ss; 291 if (j != numColumns - 1) 292 s += sDelim; 293 } 294 return s; 295 } 296 /// <summary> 297 /// 将矩阵指定列中各元素的值转化为字符串 298 /// </summary> 299 /// <param name="nCol">指定的矩阵列,nCol=0表示第一列</param> 300 /// <param name="sDelim">元素之间的分隔符</param> 301 /// <returns>string型,转换得到的字符串</returns> 302 public string ToStringCol(int nCol, string sDelim) 303 { 304 string s = ""; 305 306 if (nCol >= numColumns) 307 return s; 308 309 for (int i = 0; i < numRows; ++i) 310 { 311 string ss = GetElement(i, nCol).ToString("F8"); 312 s += ss; 313 if (i != numRows - 1) 314 s += sDelim; 315 } 316 return s; 317 } 318 /// <summary> 319 /// 设置矩阵各元素的值 320 /// </summary> 321 /// <param name="value">一维数组,长度numColumns*numRows,存储矩阵各元素的值</param> 322 public void SetData(double[] value) 323 { 324 elements = (double[])value.Clone(); 325 } 326 /// <summary> 327 /// 设置指定元素的值 328 /// </summary> 329 /// <param name="nRow">元素的行</param> 330 /// <param name="nCol">元素的列</param> 331 /// <param name="value">指定元素的值</param> 332 /// <returns>bool型,说明设置是否成功</returns> 333 public bool SetElement(int nRow, int nCol, double value) 334 { 335 if (nCol < 0 || nCol >= numColumns || nRow < 0 || nRow >= numRows) 336 return false; 337 338 elements[nCol + nRow * numColumns] = value; 339 340 return true; 341 } 342 343 /// <summary> 344 /// 获取指定元素的值 345 /// </summary> 346 /// <param name="nRow">元素的行</param> 347 /// <param name="nCol">元素的列</param> 348 /// <returns>double型,指定元素的值</returns> 349 public double GetElement(int nRow, int nCol) 350 { 351 return elements[nCol + nRow * numColumns]; 352 } 353 354 /// <summary> 355 /// 获取矩阵的列数 356 /// </summary> 357 /// <returns>int型,矩阵的列数</returns> 358 public int GetNumColumns() 359 { 360 return numColumns; 361 } 362 /// <summary> 363 /// 获取矩阵的行数 364 /// </summary> 365 /// <returns>int型,矩阵的行数</returns> 366 public int GetNumRows() 367 { 368 return numRows; 369 } 370 /// <summary> 371 /// 获取矩阵的数据 372 /// </summary> 373 /// <returns>double型数组,指向矩阵各元素的数据缓冲区</returns> 374 public double[] GetData() 375 { 376 return elements; 377 } 378 /// <summary> 379 /// 获取指定行的向量 380 /// </summary> 381 /// <param name="nRow">向量所在的行</param> 382 /// <param name="pVector">指向向量中各元素的缓冲区</param> 383 /// <returns>int型,向量中元素的个数,即矩阵的列数</returns> 384 public int GetRowVector(int nRow, double[] pVector) 385 { 386 for (int j = 0; j < numColumns; ++j) 387 pVector[j] = GetElement(nRow, j); 388 389 return numColumns; 390 } 391 392 /// <summary> 393 /// 获取指定列的向量 394 /// </summary> 395 /// <param name="nCol">向量所在的列</param> 396 /// <param name="pVector">指向向量中各元素的缓冲区</param> 397 /// <returns>int型,向量中元素的个数,即矩阵的行数</returns> 398 public int GetColVector(int nCol, double[] pVector) 399 { 400 for (int i = 0; i < numRows; ++i) 401 pVector[i] = GetElement(i, nCol); 402 403 return numRows; 404 } 405 /// <summary> 406 /// 给矩阵赋值 407 /// </summary> 408 /// <param name="other">用于给矩阵赋值的源矩阵</param> 409 /// <returns>Matrix型,矩阵与other相等</returns> 410 public Matrix SetValue(Matrix other) 411 { 412 if (other != this) 413 { 414 Init(other.GetNumRows(), other.GetNumColumns()); 415 SetData(other.elements); 416 } 417 418 return this; 419 } 420 /// <summary> 421 /// 判断矩阵是否相等 422 /// </summary> 423 /// <param name="other">用于比较的矩阵</param> 424 /// <returns>bool型,两个矩阵相等则为true,否则为false</returns> 425 public override bool Equals(object other) 426 { 427 Matrix matrix = other as Matrix; 428 if (matrix == null) 429 return false; 430 431 //首先检查行列数是否相等 432 if (numColumns != matrix.GetNumColumns() || numRows != matrix.GetNumRows()) 433 return false; 434 435 for (int i = 0; i < numRows; ++i) 436 { 437 for (int j = 0; j < numColumns; ++j) 438 { 439 if (Math.Abs(GetElement(i, j) - matrix.GetElement(i, j)) > eps) 440 return false; 441 } 442 } 443 return true; 444 445 446 447 } 448 449 /// <summary> 450 /// 因为重写了Equals,因此必须重写GetHashCode 451 /// </summary> 452 /// <returns>int型返回复数对象散列码</returns> 453 public override int GetHashCode() 454 { 455 double sum = 0; 456 for (int i = 0; i < numRows; ++i) 457 { 458 for (int j = 0; j < numColumns; ++j) 459 { 460 sum += Math.Abs(GetElement(i, j)); 461 } 462 } 463 return (int)Math.Sqrt(sum); 464 465 } 466 /// <summary> 467 /// 实现矩阵的加法 468 /// </summary> 469 /// <param name="other">other--与指定矩阵相加的矩阵</param> 470 /// <returns>Matrix型,指定矩阵与other相加之和</returns> 471 public Matrix Add(Matrix other) 472 { 473 //首先检查行列数是否相等 474 if (numColumns != other.GetNumColumns() || numRows != other.GetNumRows()) 475 throw new Exception("矩阵的行/列数不匹配。"); 476 477 //构造结果矩阵 478 Matrix result = new Matrix(this); //拷贝构造 479 480 //矩阵加法 481 for (int i = 0; i < numRows; ++i) 482 { 483 for (int j = 0; j < numColumns; ++j) 484 result.SetElement(i, j, result.GetElement(i, j) + other.GetElement(i, j)); 485 } 486 return result; 487 488 } 489 /// <summary> 490 /// 实现矩阵的减法 491 /// </summary> 492 /// <param name="other">与指定矩阵相减的矩阵</param> 493 /// <returns>Matrix型,指定矩阵与other相减之差</returns> 494 public Matrix Subtract(Matrix other) 495 { 496 if (numColumns != other.GetNumColumns() || numRows != other.GetNumRows()) 497 throw new Exception("矩阵的行/列数不匹配。"); 498 499 //构造结果矩阵 500 Matrix result = new Matrix(this); //拷贝构造 501 502 //进行减法操作 503 for (int i = 0; i < numRows; ++i) 504 { 505 for (int j = 0; j < numColumns; ++j) 506 result.SetElement(i, j, result.GetElement(i, j) - other.GetElement(i, j)); 507 } 508 509 return result; 510 } 511 512 /// <summary> 513 /// 实现矩阵的数乘 514 /// </summary> 515 /// <param name="value">与指定矩阵相乘的实数</param> 516 /// <returns>Matrix型,指定矩阵与value相乘之积</returns> 517 public Matrix Multiply(double value) 518 { 519 //构造目标矩阵 520 Matrix result = new Matrix(this); 521 522 //进行数乘 523 for (int i = 0; i < numRows; ++i) 524 { 525 for (int j = 0; j < numColumns; ++j) 526 result.SetElement(i, j, result.GetElement(i, j) * value); 527 } 528 return result; 529 530 } 531 /// <summary> 532 /// 矩阵的乘法 533 /// </summary> 534 /// <param name="other">与指定矩阵相乘的矩</param> 535 /// <returns>Matrix型,指定矩阵与other相乘之积</returns> 536 public Matrix Multiply(Matrix other) 537 { 538 //首先检查行列是否符合要求 539 if (numColumns != other.GetNumRows()) 540 throw new Exception("矩阵的行/列数不匹配。"); 541 //ruct the object we are going to return 542 Matrix result = new Matrix(numRows, other.GetNumColumns()); 543 //矩阵乘法,即 544 // 545 //[A][B][C] [G][H] [A*G+B*I+C*K][A*H+B*J+C*L] 546 //[D][E][F]*[I][J] = [D*G+E*I+F*K][D*H+E*J+F*L] 547 // [K][L] 548 double value; 549 for (int i = 0; i < result.GetNumRows(); ++i) 550 { 551 for (int j = 0; j < other.GetNumColumns(); ++j) 552 { 553 value = 0.0; 554 for (int k = 0; k < numColumns; ++k) 555 { 556 value += GetElement(i, k) * other.GetElement(k, j); 557 } 558 result.SetElement(i, j, value); 559 560 } 561 } 562 return result; 563 } 564 565 /// <summary> 566 /// 复矩阵的乘法 567 /// </summary> 568 /// <param name="AR">左边复矩阵的实部矩阵</param> 569 /// <param name="AI">左边复矩阵的虚部矩阵</param> 570 /// <param name="BR">右边复矩阵的实部矩阵</param> 571 /// <param name="BI">右边复矩阵的虚部矩阵</param> 572 /// <param name="CR">乘积复矩阵的实部矩阵</param> 573 /// <param name="CI">乘积复矩阵的虚部矩阵</param> 574 /// <returns>bool型,复矩阵乘法是否成功</returns> 575 public bool Multiply(Matrix AR, Matrix AI, Matrix BR, Matrix BI, Matrix CR, Matrix CI) 576 { 577 //首先检查行列数是否符合要求 578 if (AR.GetNumColumns() != AI.GetNumColumns() || AR.GetNumRows() != AI.GetNumRows() || BR.GetNumColumns() != BI.GetNumColumns() || BR.GetNumRows() != BI.GetNumRows() || AR.GetNumColumns() != BR.GetNumRows()) 579 return false; 580 581 //构造乘积矩阵实部和虚部矩阵 582 Matrix mtxCR = new Matrix(AR.GetNumRows(), BR.GetNumColumns()); 583 Matrix mtxCI = new Matrix(AR.GetNumRows(), BR.GetNumColumns()); 584 //复矩阵相乘 585 for (int i = 0; i < AR.GetNumRows(); ++i) 586 { 587 for (int j = 0; j < BR.GetNumColumns(); ++j) 588 { 589 double vr = 0; 590 double vi = 0; 591 for (int k = 0; k < AR.GetNumColumns(); ++k) 592 { 593 double p = AR.GetElement(i, k) * BR.GetElement(k, j); 594 double q = AI.GetElement(i, k) * BI.GetElement(k, j); 595 double s = (AR.GetElement(i, k) + AI.GetElement(i, k)) * (BR.GetElement(k, j) + BI.GetElement(k, j)); 596 vr += p - q; 597 vi += s - p - q; 598 } 599 mtxCR.SetElement(i, j, vr); 600 mtxCI.SetElement(i, j, vi); 601 } 602 } 603 CR = mtxCR; 604 CI = mtxCI; 605 606 return true; 607 608 609 } 610 611 /// <summary> 612 /// 矩阵的转置 613 /// </summary> 614 /// <returns>Matrix型,指定矩阵转置矩阵</returns> 615 public Matrix Transpose() 616 { 617 //构造目标矩阵 618 Matrix Trans = new Matrix(numColumns, numRows); 619 620 //转置各元素 621 for (int i = 0; i < numRows; ++i) 622 { 623 for (int j = 0; j < numColumns; ++j) 624 Trans.SetElement(j, i, GetElement(i, j)); 625 } 626 return Trans; 627 } 628 /// <summary> 629 /// 实矩阵求逆的全选主元高斯-约当法 630 /// </summary> 631 /// <returns>bool型,求逆是否成功</returns> 632 public bool InvertGaussJordan() 633 { 634 int i, j, k, l, u, v; 635 double d = 0, p = 0; 636 637 //分配内存 638 int[] pnRow = new int[numColumns]; 639 int[] pnCol = new int[numColumns]; 640 641 //消元 642 for (k = 0; k <= numColumns - 1; k++) 643 { 644 d = 0.0; 645 for (i = k; i <= numColumns - 1; i++) 646 { 647 for (j = k; j <= numColumns - 1; j++) 648 { 649 l = i * numColumns + j; p = Math.Abs(elements[l]); 650 if (p > d) 651 { 652 d = p; 653 pnRow[k] = i; 654 pnCol[k] = j; 655 } 656 } 657 } 658 //失败 659 if (d == 0.0) 660 { 661 return false; 662 } 663 664 if (pnRow[k] != k) 665 { 666 for (j = 0; j <= numColumns - 1; j++) 667 { 668 u = k * numColumns + j; 669 v = pnRow[k] * numColumns + j; 670 p = elements[u]; 671 elements[u] = elements[v]; 672 elements[v] = p; 673 } 674 } 675 if (pnCol[k] != k) 676 { 677 for (i = 0; i <= numColumns - 1; i++) 678 { 679 u = i * numColumns + k; 680 v = i * numColumns + pnCol[k]; 681 p = elements[u]; 682 elements[u] = elements[v]; 683 elements[v] = p; 684 } 685 } 686 l = k * numColumns + k; 687 elements[l] = 1.0 / elements[l]; 688 for (j = 0; j <= numColumns - 1; j++) 689 { 690 if (j != k) 691 { 692 u = k * numColumns + j; 693 elements[u] = elements[u] * elements[l]; 694 } 695 } 696 697 for (i = 0; i <= numColumns - 1; i++) 698 { 699 if (i != k) 700 { 701 for (j = 0; j <= numColumns - 1; j++) 702 { 703 if (j != k) 704 { 705 u = i * numColumns + j; 706 elements[u] = elements[u] - elements[i * numColumns + k] * elements[k * numColumns + j]; 707 } 708 } 709 } 710 } 711 for (i = 0; i <= numColumns - 1; i++) 712 { 713 if (i != k) 714 { 715 u = i * numColumns + k; 716 elements[u] = -elements[u] * elements[l]; 717 } 718 } 719 } 720 721 //调整恢复行列次序 722 for (k = numColumns - 1; k >= 0; k--) 723 { 724 if (pnCol[k] != k) 725 { 726 for (j = 0; j <= numColumns - 1; j++) 727 { 728 u = k * numColumns + j; 729 v = pnCol[k] * numColumns + j; 730 p = elements[u]; 731 elements[u] = elements[v]; 732 elements[v] = p; 733 } 734 } 735 if (pnRow[k] != k) 736 { 737 for (i = 0; i <= numColumns - 1; i++) 738 { 739 u = i * numColumns + k; 740 v = i * numColumns + pnRow[k]; 741 p = elements[u]; 742 elements[u] = elements[v]; 743 elements[v] = p; 744 } 745 } 746 } 747 //成功返回 748 return true; 749 750 751 } 752 /// <summary> 753 /// 复矩阵求逆的全选主元高斯--约当法 754 /// </summary> 755 /// <param name="mtxImag">复矩阵的虚部矩阵,当前矩阵为复矩阵的实部</param> 756 /// <returns>bool型号，求逆是否成功</returns> 757 public bool InvertGaussJordan(Matrix mtxImag) 758 { 759 int i, j, k, l, u, v, w; 760 double p, q, s, t, d, b; 761 762 // 分配内存 763 int[] pnRow = new int[numColumns]; 764 int[] pnCol = new int[numColumns]; 765 766 // 消元 767 for (k = 0; k <= numColumns - 1; k++) 768 { 769 d = 0.0; 770 for (i = k; i <= numColumns - 1; i++) 771 { 772 for (j = k; j <= numColumns - 1; j++) 773 { 774 u = i * numColumns + j; 775 p = elements[u] * elements[u] + mtxImag.elements[u] * mtxImag.elements[u]; 776 if (p > d) 777 { 778 d = p; 779 pnRow[k] = i; 780 pnCol[k] = j; 781 } 782 } 783 } 784 785 // 失败 786 if (d == 0.0) 787 { 788 return false; 789 } 790 791 if (pnRow[k] != k) 792 { 793 for (j = 0; j <= numColumns - 1; j++) 794 { 795 u = k * numColumns + j; 796 v = pnRow[k] * numColumns + j; 797 t = elements[u]; 798 elements[u] = elements[v]; 799 elements[v] = t; 800 t = mtxImag.elements[u]; 801 mtxImag.elements[u] = mtxImag.elements[v]; 802 mtxImag.elements[v] = t; 803 } 804 } 805 806 if (pnCol[k] != k) 807 { 808 for (i = 0; i <= numColumns - 1; i++) 809 { 810 u = i * numColumns + k; 811 v = i * numColumns + pnCol[k]; 812 t = elements[u]; 813 elements[u] = elements[v]; 814 elements[v] = t; 815 t = mtxImag.elements[u]; 816 mtxImag.elements[u] = mtxImag.elements[v]; 817 mtxImag.elements[v] = t; 818 } 819 } 820 821 l = k * numColumns + k; 822 elements[l] = elements[l] / d; mtxImag.elements[l] = -mtxImag.elements[l] / d; 823 for (j = 0; j <= numColumns - 1; j++) 824 { 825 if (j != k) 826 { 827 u = k * numColumns + j; 828 p = elements[u] * elements[l]; 829 q = mtxImag.elements[u] * mtxImag.elements[l]; 830 s = (elements[u] + mtxImag.elements[u]) * (elements[l] + mtxImag.elements[l]); 831 elements[u] = p - q; 832 mtxImag.elements[u] = s - p - q; 833 } 834 } 835 836 for (i = 0; i <= numColumns - 1; i++) 837 { 838 if (i != k) 839 { 840 v = i * numColumns + k; 841 for (j = 0; j <= numColumns - 1; j++) 842 { 843 if (j != k) 844 { 845 u = k * numColumns + j; 846 w = i * numColumns + j; 847 p = elements[u] * elements[v]; 848 q = mtxImag.elements[u] * mtxImag.elements[v]; 849 s = (elements[u] + mtxImag.elements[u]) * (elements[v] + mtxImag.elements[v]); 850 t = p - q; 851 b = s - p - q; 852 elements[w] = elements[w] - t; 853 mtxImag.elements[w] = mtxImag.elements[w] - b; 854 } 855 } 856 } 857 } 858 859 for (i = 0; i <= numColumns - 1; i++) 860 { 861 if (i != k) 862 { 863 u = i * numColumns + k; 864 p = elements[u] * elements[l]; 865 q = mtxImag.elements[u] * mtxImag.elements[l]; 866 s = (elements[u] + mtxImag.elements[u]) * (elements[l] + mtxImag.elements[l]); 867 elements[u] = q - p; 868 mtxImag.elements[u] = p + q - s; 869 } 870 } 871 } 872 873 // 调整恢复行列次序 874 for (k = numColumns - 1; k >= 0; k--) 875 { 876 if (pnCol[k] != k) 877 { 878 for (j = 0; j <= numColumns - 1; j++) 879 { 880 u = k * numColumns + j; 881 v = pnCol[k] * numColumns + j; 882 t = elements[u]; 883 elements[u] = elements[v]; 884 elements[v] = t; 885 t = mtxImag.elements[u]; 886 mtxImag.elements[u] = mtxImag.elements[v]; 887 mtxImag.elements[v] = t; 888 } 889 } 890 891 if (pnRow[k] != k) 892 { 893 for (i = 0; i <= numColumns - 1; i++) 894 { 895 u = i * numColumns + k; 896 v = i * numColumns + pnRow[k]; 897 t = elements[u]; 898 elements[u] = elements[v]; 899 elements[v] = t; 900 t = mtxImag.elements[u]; 901 mtxImag.elements[u] = mtxImag.elements[v]; 902 mtxImag.elements[v] = t; 903 } 904 } 905 } 906 907 // 成功返回 908 return true; 909 } 910 /// <summary> 911 /// 对称正定矩阵的求逆 912 /// </summary> 913 /// <returns>bool型,求逆是否成功</returns> 914 public bool InvertSsgj() 915 { 916 int i, j, k, m; 917 double w, g; 918 919 //临时内存 920 double[] pTmp = new double[numColumns]; 921 922 //逐行处理 923 for (k = 0; k <= numColumns - 1; k++) 924 { 925 w = elements[0]; 926 if (w == 0.0) 927 { 928 return false; 929 } 930 931 m = numColumns - k - 1; 932 for (i = 1; i <= numColumns - 1; i++) 933 { 934 g = elements[i * numColumns]; 935 pTmp[i] = g / w; 936 if (i <= m) 937 pTmp[i] = -pTmp[i]; 938 for (j = 1; j <= i; j++) 939 elements[(i - 1) * numColumns + j - 1] = elements[i * numColumns + j] + g * pTmp[j]; 940 } 941 942 elements[numColumns * numColumns - 1] = 1.0 / w; 943 for (i = 1; i <= numColumns - 1; i++) 944 elements[(numColumns - 1) * numColumns + i - 1] = pTmp[i]; 945 } 946 947 //行列调整 948 for (i = 0; i <= numColumns - 2; i++) 949 for (j = i + 1; j <= numColumns - 1; j++) 950 elements[i * numColumns + j] = elements[j * numColumns + i]; 951 952 return true; 953 954 } 955 956 //托伯利慈矩阵求逆的埃兰特方法 957 //@return bool型,求逆是否成功 958 959 public bool InvertTrench() 960 { 961 int i, j, k; 962 double a, s; 963 964 // 上三角元素 965 double[] t = new double[numColumns]; 966 // 下三角元素 967 double[] tt = new double[numColumns]; 968 969 // 上、下三角元素赋值 970 for (i = 0; i < numColumns; ++i) 971 { 972 t[i] = GetElement(0, i); 973 tt[i] = GetElement(i, 0); 974 } 975 976 // 临时缓冲区 977 double[] c = new double[numColumns]; 978 double[] r = new double[numColumns]; 979 double[] p = new double[numColumns]; 980 981 // 非Toeplitz矩阵，返回 982 if (t[0] == 0.0) 983 { 984 return false; 985 } 986 987 a = t[0]; 988 c[0] = tt[1] / t[0]; 989 r[0] = t[1] / t[0]; 990 991 for (k = 0; k <= numColumns - 3; k++) 992 { 993 s = 0.0; 994 for (j = 1; j <= k + 1; j++) 995 s = s + c[k + 1 - j] * tt[j]; 996 997 s = (s - tt[k + 2]) / a; 998 for (i = 0; i <= k; i++) 999 p[i] = c[i] + s * r[k - i]; 1000 1001 c[k + 1] = -s; 1002 s = 0.0; 1003 for (j = 1; j <= k + 1; j++) 1004 s = s + r[k + 1 - j] * t[j]; 1005 1006 s = (s - t[k + 2]) / a; 1007 for (i = 0; i <= k; i++) 1008 { 1009 r[i] = r[i] + s * c[k - i]; 1010 c[k - i] = p[k - i]; 1011 } 1012 1013 r[k + 1] = -s; 1014 a = 0.0; 1015 for (j = 1; j <= k + 2; j++) 1016 a = a + t[j] * c[j - 1]; 1017 1018 a = t[0] - a; 1019 1020 // 求解失败 1021 if (a == 0.0) 1022 { 1023 return false; 1024 } 1025 } 1026 1027 elements[0] = 1.0 / a; 1028 for (i = 0; i <= numColumns - 2; i++) 1029 { 1030 k = i + 1; 1031 j = (i + 1) * numColumns; 1032 elements[k] = -r[i] / a; 1033 elements[j] = -c[i] / a; 1034 } 1035 1036 for (i = 0; i <= numColumns - 2; i++) 1037 { 1038 for (j = 0; j <= numColumns - 2; j++) 1039 { 1040 k = (i + 1) * numColumns + j + 1; 1041 elements[k] = elements[i * numColumns + j] - c[i] * elements[j + 1]; 1042 elements[k] = elements[k] + c[numColumns - j - 2] * elements[numColumns - i - 1]; 1043 } 1044 } 1045 1046 return true; 1047 } 1048 1049 //求行列式值的全选主元高斯消去法 1050 //@return double型,行列式的值 1051 public double ComputeDetGauss() 1052 { 1053 int i, j, k, nis = 0, js = 0, l, u, v; 1054 double f, det, q, d; 1055 1056 //初值 1057 f = 1.0; 1058 det = 1.0; 1059 1060 //消元 1061 for (k = 0; k <= numColumns - 2; k++) 1062 { 1063 q = 0.0; 1064 for (i = k; i <= numColumns - 1; i++) 1065 { 1066 for (j = k; j <= numColumns - 1; j++) 1067 { 1068 l = i * numColumns + j; 1069 d = Math.Abs(elements[l]); 1070 if (d > q) 1071 { 1072 q = d; 1073 nis = i; 1074 js = j; 1075 } 1076 } 1077 } 1078 1079 if (q == 0.0) 1080 { 1081 det = 0.0; 1082 return (det); 1083 } 1084 1085 if (nis != k) 1086 { 1087 f = -f; 1088 for (j = k; j <= numColumns - 1; j++) 1089 { 1090 u = k * numColumns + j; 1091 v = nis * numColumns + j; 1092 d = elements[u]; 1093 elements[u] = elements[v]; 1094 elements[v] = d; 1095 } 1096 } 1097 1098 if (js != k) 1099 { 1100 f = -f; 1101 for (i = k; i <= numColumns - 1; i++) 1102 { 1103 u = i * numColumns + js; 1104 v = i * numColumns + k; 1105 d = elements[u]; 1106 elements[u] = elements[v]; 1107 elements[v] = d; 1108 1109 } 1110 } 1111 l = k * numColumns + k; 1112 det = det * elements[l]; 1113 for (i = k + 1; i <= numColumns - 1; i++) 1114 { 1115 d = elements[i * numColumns + k] / elements[l]; 1116 for (j = k + 1; j <= numColumns - 1; j++) 1117 { 1118 u = i * numColumns + j; 1119 elements[u] = elements[u] - d * elements[k * numColumns + j]; 1120 } 1121 } 1122 } 1123 1124 //求值 1125 det = f * det * elements[numColumns * numColumns - 1]; 1126 1127 return (det); 1128 } 1129 //求矩阵秩的全选主元高斯消去法 1130 //@return int型,矩阵的秩 1131 public int ComputeRankGauss() 1132 { 1133 int i, j, k, nn, nis = 0, js = 0, l, ll, u, v; 1134 double q, d; 1135 1136 //秩小于等于行列数 1137 nn = numRows; 1138 if (numRows >= numColumns) 1139 nn = numColumns; 1140 k = 0; 1141 1142 //消元求解 1143 for (l = 0; l <= nn - 1; l++) 1144 { 1145 q = 0.0; 1146 for (i = 1; i <= numRows - 1; i++) 1147 { 1148 for (j = 1; j <= numColumns - 1; j++) 1149 { 1150 ll = i * numColumns + j; 1151 d = Math.Abs(elements[ll]); 1152 if (d > q) 1153 { 1154 q = d; 1155 nis = i; 1156 js = j; 1157 } 1158 } 1159 } 1160 1161 if (q == 0.0) 1162 return (k); 1163 1164 k = k + 1; 1165 if (nis != 1) 1166 { 1167 for (j = 1; j <= numColumns - 1; j++) 1168 { 1169 u = l * numColumns + j; 1170 v = nis * numColumns + j; 1171 d = elements[u]; 1172 elements[u] = elements[v]; 1173 elements[v] = d; 1174 } 1175 } 1176 if (js != 1) 1177 { 1178 for (i = 1; i <= numRows - 1; i++) 1179 { 1180 u = i * numColumns + js; 1181 v = i * numColumns + l; 1182 d = elements[u]; 1183 elements[u] = elements[v]; 1184 elements[v] = d; 1185 } 1186 } 1187 1188 ll = l * numColumns + l; 1189 for (i = l + 1; i <= numColumns - 1; i++) 1190 { 1191 d = elements[i * numColumns + l] / elements[ll]; 1192 for (j = l + 1; j <= numColumns - 1; j++) 1193 { 1194 u = i * numColumns + j; 1195 elements[u] = elements[u] - d * elements[l * numColumns + j]; 1196 } 1197 } 1198 1199 1200 } 1201 return (k); 1202 } 1203 1204 //对称正定矩阵的乔里斯基分解与行列式的求值 1205 //@param realDetValue---返回行列式的值 1206 //@return bool型,求解是否成功 1207 public bool ComputeDetCholesky(ref double realDetValue) 1208 { 1209 int i, j, k, u, l; 1210 double d; 1211 1212 //不满足求解要求 1213 if (elements[0] <= 0.0) 1214 return false; 1215 1216 //乔里斯基分解 1217 1218 elements[0] = Math.Sqrt(elements[0]); 1219 d = elements[0]; 1220 1221 for (i = 1; i <= numColumns - 1; i++) 1222 { 1223 u = i * numColumns; 1224 elements[u] = elements[u] / elements[0]; 1225 } 1226 1227 for (j = 1; j <= numColumns - 1; j++) 1228 { 1229 l = j * numColumns + j; 1230 for (k = 0; k <= j - 1; k++) 1231 { 1232 u = j * numColumns + k; 1233 elements[l] = elements[l] - elements[u] * elements[u]; 1234 1235 1236 } 1237 1238 if (elements[l] <= 0.0) 1239 return false; 1240 1241 elements[l] = Math.Sqrt(elements[l]); 1242 d = d * elements[l]; 1243 1244 for (i = j + 1; i <= numColumns - 1; i++) 1245 { 1246 u = i * numColumns + j; 1247 for (k = 0; k <= j - 1; k++) 1248 elements[u] = elements[u] - elements[i * numColumns + k] * elements[j * numColumns + k]; 1249 elements[u] = elements[u] / elements[l]; 1250 } 1251 1252 } 1253 1254 //行列式求值 1255 realDetValue = d * d; 1256 1257 //下三角矩阵 1258 for (i = 0; i <= numColumns - 2; i++) 1259 for (j = i + 1; j <= numColumns - 1; j++) 1260 elements[i * numColumns + j] = 0.0; 1261 1262 return true; 1263 1264 } 1265 1266 1267 //矩阵的三角分解,分解成功后,原矩阵将成为Q矩阵 1268 //@param mtxL---返回分解后的L矩阵 1269 //@param mtxU---返回分解后的U矩阵 1270 //@return bool型,求解是否成功 1271 public bool SpiltLU(Matrix mtxL, Matrix mtxU) 1272 { 1273 int i, j, k, w, v, ll; 1274 1275 //初始化结果矩阵 1276 if (!mtxL.Init(numColumns, numColumns) || !mtxU.Init(numColumns, numColumns)) 1277 return false; 1278 1279 for (k = 0; k <= numColumns - 2; k++) 1280 { 1281 ll = k * numColumns + k; 1282 if (elements[ll] == 0.0) 1283 return false; 1284 1285 for (i = k + 1; i <= numColumns - 1; i++) 1286 { 1287 w = i * numColumns + k; 1288 elements[w] = elements[w] / elements[ll]; 1289 } 1290 1291 for (i = k + 1; i <= numColumns - 1; i++) 1292 { 1293 w = i * numColumns + k; 1294 for (j = k + 1; j <= numColumns - 1; j++) 1295 { 1296 v = i * numColumns + j; 1297 elements[v] = elements[v] - elements[w] * elements[k * numColumns + j]; 1298 } 1299 } 1300 } 1301 1302 for (i = 0; i <= numColumns - 1; i++) 1303 { 1304 for (j = 0; j < i; j++) 1305 { 1306 w = i * numColumns + j; 1307 mtxL.elements[w] = elements[w]; 1308 mtxU.elements[w] = 0.0; 1309 } 1310 1311 w = i * numColumns + i; 1312 mtxL.elements[w] = 1.0; 1313 mtxU.elements[w] = elements[w]; 1314 1315 for (j = i + 1; j <= numColumns - 1; j++) 1316 { 1317 w = i * numColumns + j; 1318 mtxL.elements[w] = 0.0; 1319 mtxU.elements[w] = elements[w]; 1320 } 1321 } 1322 1323 return true; 1324 } 1325 1326 //一般实矩阵的QR分解,分解成功后,原矩阵将成为R矩阵 1327 //@param mtxQ---返回分解后的Q矩阵 1328 //@return bool型,求解是否成功 1329 public bool SplitQR(Matrix mtxQ) 1330 { 1331 int i, j, k, l, nn, p, jj; 1332 double u, alpha, w, t; 1333 1334 if (numRows < numColumns) 1335 return false; 1336 1337 // 初始化Q矩阵 1338 if (!mtxQ.Init(numRows, numRows)) 1339 return false; 1340 1341 // 对角线元素单位化 1342 for (i = 0; i <= numRows - 1; i++) 1343 { 1344 for (j = 0; j <= numRows - 1; j++) 1345 { 1346 l = i * numRows + j; 1347 mtxQ.elements[l] = 0.0; 1348 if (i == j) 1349 mtxQ.elements[l] = 1.0; 1350 } 1351 } 1352 1353 // 开始分解 1354 1355 nn = numColumns; 1356 if (numRows == numColumns) 1357 nn = numRows - 1; 1358 1359 for (k = 0; k <= nn - 1; k++) 1360 { 1361 u = 0.0; 1362 l = k * numColumns + k; 1363 for (i = k; i <= numRows - 1; i++) 1364 { 1365 w = Math.Abs(elements[i * numColumns + k]); 1366 if (w > u) 1367 u = w; 1368 } 1369 1370 alpha = 0.0; 1371 for (i = k; i <= numRows - 1; i++) 1372 { 1373 t = elements[i * numColumns + k] / u; 1374 alpha = alpha + t * t; 1375 } 1376 1377 if (elements[l] > 0.0) 1378 u = -u; 1379 1380 alpha = u * Math.Sqrt(alpha); 1381 if (alpha == 0.0) 1382 return false; 1383 1384 u = Math.Sqrt(2.0 * alpha * (alpha - elements[l])); 1385 if ((u + 1.0) != 1.0) 1386 { 1387 elements[l] = (elements[l] - alpha) / u; 1388 for (i = k + 1; i <= numRows - 1; i++) 1389 { 1390 p = i * numColumns + k; 1391 elements[p] = elements[p] / u; 1392 } 1393 1394 for (j = 0; j <= numRows - 1; j++) 1395 { 1396 t = 0.0; 1397 for (jj = k; jj <= numRows - 1; jj++) 1398 t = t + elements[jj * numColumns + k] * mtxQ.elements[jj * numRows + j]; 1399 1400 for (i = k; i <= numRows - 1; i++) 1401 { 1402 p = i * numRows + j; 1403 mtxQ.elements[p] = mtxQ.elements[p] - 2.0 * t * elements[i * numColumns + k]; 1404 } 1405 } 1406 1407 for (j = k + 1; j <= numColumns - 1; j++) 1408 { 1409 t = 0.0; 1410 1411 for (jj = k; jj <= numRows - 1; jj++) 1412 t = t + elements[jj * numColumns + k] * elements[jj * numColumns + j]; 1413 1414 for (i = k; i <= numRows - 1; i++) 1415 { 1416 p = i * numColumns + j; 1417 elements[p] = elements[p] - 2.0 * t * elements[i * numColumns + k]; 1418 } 1419 } 1420 1421 elements[l] = alpha; 1422 for (i = k + 1; i <= numRows - 1; i++) 1423 elements[i * numColumns + k] = 0.0; 1424 } 1425 } 1426 1427 // 调整元素 1428 for (i = 0; i <= numRows - 2; i++) 1429 { 1430 for (j = i + 1; j <= numRows - 1; j++) 1431 { 1432 p = i * numRows + j; 1433 l = j * numRows + i; 1434 t = mtxQ.elements[p]; 1435 mtxQ.elements[p] = mtxQ.elements[l]; 1436 mtxQ.elements[l] = t; 1437 } 1438 } 1439 1440 return true; 1441 } 1442 1443 //一般实矩阵的奇异值分解,分解成功后,原矩阵对角线元素就是矩阵的奇异值 1444 //@param mtxU--返回分解后的U矩阵 1445 //@param mtxV--返回分解后的V矩阵 1446 //@param eps---计算精度 1447 //@return bool型,求解是否成功 1448 public bool SplitUV(Matrix mtxU, Matrix mtxV, double eps) 1449 { 1450 int i, j, k, l, it, ll, kk, ix, iy, mm, nn, iz, m1, ks; 1451 double d, dd, t, sm, sm1, em1, sk, ek, b, c, shh; 1452 double[] fg = new double[2]; 1453 double[] cs = new double[2]; 1454 1455 int m = numRows; 1456 int n = numColumns; 1457 1458 // 初始化U, V矩阵 1459 if (!mtxU.Init(m, m) || !mtxV.Init(n, n)) 1460 return false; 1461 1462 // 临时缓冲区 1463 int ka = Math.Max(m, n) + 1; 1464 double[] s = new double[ka]; 1465 double[] e = new double[ka]; 1466 double[] w = new double[ka]; 1467 1468 // 指定迭代次数为60 1469 it = 60; 1470 k = n; 1471 1472 if (m - 1 < n) 1473 k = m - 1; 1474 1475 l = m; 1476 if (n - 2 < m) 1477 l = n - 2; 1478 if (l < 0) 1479 l = 0; 1480 1481 // 循环迭代计算 1482 ll = k; 1483 if (l > k) 1484 ll = l; 1485 if (ll >= 1) 1486 { 1487 for (kk = 1; kk <= ll; kk++) 1488 { 1489 if (kk <= k) 1490 { 1491 d = 0.0; 1492 for (i = kk; i <= m; i++) 1493 { 1494 ix = (i - 1) * n + kk - 1; 1495 d = d + elements[ix] * elements[ix]; 1496 } 1497 1498 s[kk - 1] = Math.Sqrt(d); 1499 if (s[kk - 1] != 0.0) 1500 { 1501 ix = (kk - 1) * n + kk - 1; 1502 if (elements[ix] != 0.0) 1503 { 1504 s[kk - 1] = Math.Abs(s[kk - 1]); 1505 if (elements[ix] < 0.0) 1506 s[kk - 1] = -s[kk - 1]; 1507 } 1508 1509 for (i = kk; i <= m; i++) 1510 { 1511 iy = (i - 1) * n + kk - 1; 1512 elements[iy] = elements[iy] / s[kk - 1]; 1513 } 1514 1515 elements[ix] = 1.0 + elements[ix]; 1516 } 1517 1518 s[kk - 1] = -s[kk - 1]; 1519 } 1520 1521 if (n >= kk + 1) 1522 { 1523 for (j = kk + 1; j <= n; j++) 1524 { 1525 if ((kk <= k) && (s[kk - 1] != 0.0)) 1526 { 1527 d = 0.0; 1528 for (i = kk; i <= m; i++) 1529 { 1530 ix = (i - 1) * n + kk - 1; 1531 iy = (i - 1) * n + j - 1; 1532 d = d + elements[ix] * elements[iy]; 1533 } 1534 1535 d = -d / elements[(kk - 1) * n + kk - 1]; 1536 for (i = kk; i <= m; i++) 1537 { 1538 ix = (i - 1) * n + j - 1; 1539 iy = (i - 1) * n + kk - 1; 1540 elements[ix] = elements[ix] + d * elements[iy]; 1541 } 1542 } 1543 1544 e[j - 1] = elements[(kk - 1) * n + j - 1]; 1545 } 1546 } 1547 1548 if (kk <= k) 1549 { 1550 for (i = kk; i <= m; i++) 1551 { 1552 ix = (i - 1) * m + kk - 1; 1553 iy = (i - 1) * n + kk - 1; 1554 mtxU.elements[ix] = elements[iy]; 1555 } 1556 } 1557 1558 if (kk <= l) 1559 { 1560 d = 0.0; 1561 for (i = kk + 1; i <= n; i++) 1562 d = d + e[i - 1] * e[i - 1]; 1563 1564 e[kk - 1] = Math.Sqrt(d); 1565 if (e[kk - 1] != 0.0) 1566 { 1567 if (e[kk] != 0.0) 1568 { 1569 e[kk - 1] = Math.Abs(e[kk - 1]); 1570 if (e[kk] < 0.0) 1571 e[kk - 1] = -e[kk - 1]; 1572 } 1573 1574 for (i = kk + 1; i <= n; i++) 1575 e[i - 1] = e[i - 1] / e[kk - 1]; 1576 1577 e[kk] = 1.0 + e[kk]; 1578 } 1579 1580 e[kk - 1] = -e[kk - 1]; 1581 if ((kk + 1 <= m) && (e[kk - 1] != 0.0)) 1582 { 1583 for (i = kk + 1; i <= m; i++) 1584 w[i - 1] = 0.0; 1585 1586 for (j = kk + 1; j <= n; j++) 1587 for (i = kk + 1; i <= m; i++) 1588 w[i - 1] = w[i - 1] + e[j - 1] * elements[(i - 1) * n + j - 1]; 1589 1590 for (j = kk + 1; j <= n; j++) 1591 { 1592 for (i = kk + 1; i <= m; i++) 1593 { 1594 ix = (i - 1) * n + j - 1; 1595 elements[ix] = elements[ix] - w[i - 1] * e[j - 1] / e[kk]; 1596 } 1597 } 1598 } 1599 1600 for (i = kk + 1; i <= n; i++) 1601 mtxV.elements[(i - 1) * n + kk - 1] = e[i - 1]; 1602 } 1603 } 1604 } 1605 1606 mm = n; 1607 if (m + 1 < n) 1608 mm = m + 1; 1609 if (k < n) 1610 s[k] = elements[k * n + k]; 1611 if (m < mm) 1612 s[mm - 1] = 0.0; 1613 if (l + 1 < mm) 1614 e[l] = elements[l * n + mm - 1]; 1615 1616 e[mm - 1] = 0.0; 1617 nn = m; 1618 if (m > n) 1619 nn = n; 1620 if (nn >= k + 1) 1621 { 1622 for (j = k + 1; j <= nn; j++) 1623 { 1624 for (i = 1; i <= m; i++) 1625 mtxU.elements[(i - 1) * m + j - 1] = 0.0; 1626 mtxU.elements[(j - 1) * m + j - 1] = 1.0; 1627 } 1628 } 1629 1630 if (k >= 1) 1631 { 1632 for (ll = 1; ll <= k; ll++) 1633 { 1634 kk = k - ll + 1; 1635 iz = (kk - 1) * m + kk - 1; 1636 if (s[kk - 1] != 0.0) 1637 { 1638 if (nn >= kk + 1) 1639 { 1640 for (j = kk + 1; j <= nn; j++) 1641 { 1642 d = 0.0; 1643 for (i = kk; i <= m; i++) 1644 { 1645 ix = (i - 1) * m + kk - 1; 1646 iy = (i - 1) * m + j - 1; 1647 d = d + mtxU.elements[ix] * mtxU.elements[iy] / mtxU.elements[iz]; 1648 } 1649 1650 d = -d; 1651 for (i = kk; i <= m; i++) 1652 { 1653 ix = (i - 1) * m + j - 1; 1654 iy = (i - 1) * m + kk - 1; 1655 mtxU.elements[ix] = mtxU.elements[ix] + d * mtxU.elements[iy]; 1656 } 1657 } 1658 } 1659 1660 for (i = kk; i <= m; i++) 1661 { 1662 ix = (i - 1) * m + kk - 1; 1663 mtxU.elements[ix] = -mtxU.elements[ix]; 1664 } 1665 1666 mtxU.elements[iz] = 1.0 + mtxU.elements[iz]; 1667 if (kk - 1 >= 1) 1668 { 1669 for (i = 1; i <= kk - 1; i++) 1670 mtxU.elements[(i - 1) * m + kk - 1] = 0.0; 1671 } 1672 } 1673 else 1674 { 1675 for (i = 1; i <= m; i++) 1676 mtxU.elements[(i - 1) * m + kk - 1] = 0.0; 1677 mtxU.elements[(kk - 1) * m + kk - 1] = 1.0; 1678 } 1679 } 1680 } 1681 1682 for (ll = 1; ll <= n; ll++) 1683 { 1684 kk = n - ll + 1; 1685 iz = kk * n + kk - 1; 1686 1687 if ((kk <= l) && (e[kk - 1] != 0.0)) 1688 { 1689 for (j = kk + 1; j <= n; j++) 1690 { 1691 d = 0.0; 1692 for (i = kk + 1; i <= n; i++) 1693 { 1694 ix = (i - 1) * n + kk - 1; 1695 iy = (i - 1) * n + j - 1; 1696 d = d + mtxV.elements[ix] * mtxV.elements[iy] / mtxV.elements[iz]; 1697 } 1698 1699 d = -d; 1700 for (i = kk + 1; i <= n; i++) 1701 { 1702 ix = (i - 1) * n + j - 1; 1703 iy = (i - 1) * n + kk - 1; 1704 mtxV.elements[ix] = mtxV.elements[ix] + d * mtxV.elements[iy]; 1705 } 1706 } 1707 } 1708 1709 for (i = 1; i <= n; i++) 1710 mtxV.elements[(i - 1) * n + kk - 1] = 0.0; 1711 1712 mtxV.elements[iz - n] = 1.0; 1713 } 1714 1715 for (i = 1; i <= m; i++) 1716 for (j = 1; j <= n; j++) 1717 elements[(i - 1) * n + j - 1] = 0.0; 1718 1719 m1 = mm; 1720 it = 60; 1721 while (true) 1722 { 1723 if (mm == 0) 1724 { 1725 ppp(elements, e, s, mtxV.elements, m, n); 1726 return true; 1727 } 1728 if (it == 0) 1729 { 1730 ppp(elements, e, s, mtxV.elements, m, n); 1731 return false; 1732 } 1733 1734 kk = mm - 1; 1735 while ((kk != 0) && (Math.Abs(e[kk - 1]) != 0.0)) 1736 { 1737 d = Math.Abs(s[kk - 1]) + Math.Abs(s[kk]); 1738 dd = Math.Abs(e[kk - 1]); 1739 if (dd > eps * d) 1740 kk = kk - 1; 1741 else 1742 e[kk - 1] = 0.0; 1743 } 1744 1745 if (kk == mm - 1) 1746 { 1747 kk = kk + 1; 1748 if (s[kk - 1] < 0.0) 1749 { 1750 s[kk - 1] = -s[kk - 1]; 1751 for (i = 1; i <= n; i++) 1752 { 1753 ix = (i - 1) * n + kk - 1; 1754 mtxV.elements[ix] = -mtxV.elements[ix]; 1755 } 1756 } 1757 1758 while ((kk != m1) && (s[kk - 1] < s[kk])) 1759 { 1760 d = s[kk - 1]; 1761 s[kk - 1] = s[kk]; 1762 s[kk] = d; 1763 if (kk < n) 1764 { 1765 for (i = 1; i <= n; i++) 1766 { 1767 ix = (i - 1) * n + kk - 1; 1768 iy = (i - 1) * n + kk; 1769 d = mtxV.elements[ix]; 1770 mtxV.elements[ix] = mtxV.elements[iy]; 1771 mtxV.elements[iy] = d; 1772 } 1773 } 1774 1775 if (kk < m) 1776 { 1777 for (i = 1; i <= m; i++) 1778 { 1779 ix = (i - 1) * m + kk - 1; 1780 iy = (i - 1) * m + kk; 1781 d = mtxU.elements[ix]; 1782 mtxU.elements[ix] = mtxU.elements[iy]; 1783 mtxU.elements[iy] = d; 1784 } 1785 } 1786 1787 kk = kk + 1; 1788 } 1789 1790 it = 60; 1791 mm = mm - 1; 1792 } 1793 else 1794 { 1795 ks = mm; 1796 while ((ks > kk) && (Math.Abs(s[ks - 1]) != 0.0)) 1797 { 1798 d = 0.0; 1799 if (ks != mm) 1800 d = d + Math.Abs(e[ks - 1]); 1801 if (ks != kk + 1) 1802 d = d + Math.Abs(e[ks - 2]); 1803 1804 dd = Math.Abs(s[ks - 1]); 1805 if (dd > eps * d) 1806 ks = ks - 1; 1807 else 1808 s[ks - 1] = 0.0; 1809 } 1810 1811 if (ks == kk) 1812 { 1813 kk = kk + 1; 1814 d = Math.Abs(s[mm - 1]); 1815 t = Math.Abs(s[mm - 2]); 1816 if (t > d) 1817 d = t; 1818 1819 t = Math.Abs(e[mm - 2]); 1820 if (t > d) 1821 d = t; 1822 1823 t = Math.Abs(s[kk - 1]); 1824 if (t > d) 1825 d = t; 1826 1827 t = Math.Abs(e[kk - 1]); 1828 if (t > d) 1829 d = t; 1830 1831 sm = s[mm - 1] / d; 1832 sm1 = s[mm - 2] / d; 1833 em1 = e[mm - 2] / d; 1834 sk = s[kk - 1] / d; 1835 ek = e[kk - 1] / d; 1836 b = ((sm1 + sm) * (sm1 - sm) + em1 * em1) / 2.0; 1837 c = sm * em1; 1838 c = c * c; 1839 shh = 0.0; 1840 1841 if ((b != 0.0) || (c != 0.0)) 1842 { 1843 shh = Math.Sqrt(b * b + c); 1844 if (b < 0.0) 1845 shh = -shh; 1846 1847 shh = c / (b + shh); 1848 } 1849 1850 fg[0] = (sk + sm) * (sk - sm) - shh; 1851 fg[1] = sk * ek; 1852 for (i = kk; i <= mm - 1; i++) 1853 { 1854 sss(fg, cs); 1855 if (i != kk) 1856 e[i - 2] = fg[0]; 1857 1858 fg[0] = cs[0] * s[i - 1] + cs[1] * e[i - 1]; 1859 e[i - 1] = cs[0] * e[i - 1] - cs[1] * s[i - 1]; 1860 fg[1] = cs[1] * s[i]; 1861 s[i] = cs[0] * s[i]; 1862 1863 if ((cs[0] != 1.0) || (cs[1] != 0.0)) 1864 { 1865 for (j = 1; j <= n; j++) 1866 { 1867 ix = (j - 1) * n + i - 1; 1868 iy = (j - 1) * n + i; 1869 d = cs[0] * mtxV.elements[ix] + cs[1] * mtxV.elements[iy]; 1870 mtxV.elements[iy] = -cs[1] * mtxV.elements[ix] + cs[0] * mtxV.elements[iy]; 1871 mtxV.elements[ix] = d; 1872 } 1873 } 1874 1875 sss(fg, cs); 1876 s[i - 1] = fg[0]; 1877 fg[0] = cs[0] * e[i - 1] + cs[1] * s[i]; 1878 s[i] = -cs[1] * e[i - 1] + cs[0] * s[i]; 1879 fg[1] = cs[1] * e[i]; 1880 e[i] = cs[0] * e[i]; 1881 1882 if (i < m) 1883 { 1884 if ((cs[0] != 1.0) || (cs[1] != 0.0)) 1885 { 1886 for (j = 1; j <= m; j++) 1887 { 1888 ix = (j - 1) * m + i - 1; 1889 iy = (j - 1) * m + i; 1890 d = cs[0] * mtxU.elements[ix] + cs[1] * mtxU.elements[iy]; 1891 mtxU.elements[iy] = -cs[1] * mtxU.elements[ix] + cs[0] * mtxU.elements[iy]; 1892 mtxU.elements[ix] = d; 1893 } 1894 } 1895 } 1896 } 1897 1898 e[mm - 2] = fg[0]; 1899 it = it - 1; 1900 } 1901 else 1902 { 1903 if (ks == mm) 1904 { 1905 kk = kk + 1; 1906 fg[1] = e[mm - 2]; 1907 e[mm - 2] = 0.0; 1908 for (ll = kk; ll <= mm - 1; ll++) 1909 { 1910 i = mm + kk - ll - 1; 1911 fg[0] = s[i - 1]; 1912 sss(fg, cs); 1913 s[i - 1] = fg[0]; 1914 if (i != kk) 1915 { 1916 fg[1] = -cs[1] * e[i - 2]; 1917 e[i - 2] = cs[0] * e[i - 2]; 1918 } 1919 1920 if ((cs[0] != 1.0) || (cs[1] != 0.0)) 1921 { 1922 for (j = 1; j <= n; j++) 1923 { 1924 ix = (j - 1) * n + i - 1; 1925 iy = (j - 1) * n + mm - 1; 1926 d = cs[0] * mtxV.elements[ix] + cs[1] * mtxV.elements[iy]; 1927 mtxV.elements[iy] = -cs[1] * mtxV.elements[ix] + cs[0] * mtxV.elements[iy]; 1928 mtxV.elements[ix] = d; 1929 } 1930 } 1931 } 1932 } 1933 else 1934 { 1935 kk = ks + 1; 1936 fg[1] = e[kk - 2]; 1937 e[kk - 2] = 0.0; 1938 for (i = kk; i <= mm; i++) 1939 { 1940 fg[0] = s[i - 1]; 1941 sss(fg, cs); 1942 s[i - 1] = fg[0]; 1943 fg[1] = -cs[1] * e[i - 1]; 1944 e[i - 1] = cs[0] * e[i - 1]; 1945 if ((cs[0] != 1.0) || (cs[1] != 0.0)) 1946 { 1947 for (j = 1; j <= m; j++) 1948 { 1949 ix = (j - 1) * m + i - 1; 1950 iy = (j - 1) * m + kk - 2; 1951 d = cs[0] * mtxU.elements[ix] + cs[1] * mtxU.elements[iy]; 1952 mtxU.elements[iy] = -cs[1] * mtxU.elements[ix] + cs[0] * mtxU.elements[iy]; 1953 mtxU.elements[ix] = d; 1954 } 1955 } 1956 } 1957 } 1958 } 1959 } 1960 } 1961 } 1962 1963 /** 1964 * 内部函数，由SplitUV函数调用 1965 */ 1966 private void ppp(double[] a, double[] e, double[] s, double[] v, int m, int n) 1967 { 1968 int i, j, p, q; 1969 double d; 1970 1971 if (m >= n) 1972 i = n; 1973 else 1974 i = m; 1975 1976 for (j = 1; j <= i - 1; j++) 1977 { 1978 a[(j - 1) * n + j - 1] = s[j - 1]; 1979 a[(j - 1) * n + j] = e[j - 1]; 1980 } 1981 1982 a[(i - 1) * n + i - 1] = s[i - 1]; 1983 if (m < n) 1984 a[(i - 1) * n + i] = e[i - 1]; 1985 1986 for (i = 1; i <= n - 1; i++) 1987 { 1988 for (j = i + 1; j <= n; j++) 1989 { 1990 p = (i - 1) * n + j - 1; 1991 q = (j - 1) * n + i - 1; 1992 d = v[p]; 1993 v[p] = v[q]; 1994 v[q] = d; 1995 } 1996 } 1997 } 1998 1999 /** 2000 * 内部函数，由SplitUV函数调用 2001 */ 2002 private void sss(double[] fg, double[] cs) 2003 { 2004 double r, d; 2005 2006 if ((Math.Abs(fg[0]) + Math.Abs(fg[1])) == 0.0) 2007 { 2008 cs[0] = 1.0; 2009 cs[1] = 0.0; 2010 d = 0.0; 2011 } 2012 else 2013 { 2014 d = Math.Sqrt(fg[0] * fg[0] + fg[1] * fg[1]); 2015 if (Math.Abs(fg[0]) > Math.Abs(fg[1])) 2016 { 2017 d = Math.Abs(d); 2018 if (fg[0] < 0.0) 2019 d = -d; 2020 } 2021 if (Math.Abs(fg[1]) >= Math.Abs(fg[0])) 2022 { 2023 d = Math.Abs(d); 2024 if (fg[1] < 0.0) 2025 d = -d; 2026 } 2027 2028 cs[0] = fg[0] / d; 2029 cs[1] = fg[1] / d; 2030 } 2031 2032 r = 1.0; 2033 if (Math.Abs(fg[0]) > Math.Abs(fg[1])) 2034 r = cs[1]; 2035 else if (cs[0] != 0.0) 2036 r = 1.0 / cs[0]; 2037 2038 fg[0] = d; 2039 fg[1] = r; 2040 } 2041 2042 /** 2043 * 求广义逆的奇异值分解法，分解成功后，原矩阵对角线元素就是矩阵的奇异值 2044 * 2045 * @param mtxAP - 返回原矩阵的广义逆矩阵 2046 * @param mtxU - 返回分解后的U矩阵 2047 * @param mtxV - 返回分解后的V矩阵 2048 * @param eps - 计算精度 2049 * @return bool型，求解是否成功 2050 */ 2051 public bool InvertUV(Matrix mtxAP, Matrix mtxU, Matrix mtxV, double eps) 2052 { 2053 int i, j, k, l, t, p, q, f; 2054 2055 // 调用奇异值分解 2056 if (!SplitUV(mtxU, mtxV, eps)) 2057 return false; 2058 2059 int m = numRows; 2060 int n = numColumns; 2061 2062 // 初始化广义逆矩阵 2063 if (!mtxAP.Init(n, m)) 2064 return false; 2065 2066 // 计算广义逆矩阵 2067 2068 j = n; 2069 if (m < n) 2070 j = m; 2071 j = j - 1; 2072 k = 0; 2073 while ((k <= j) && (elements[k * n + k] != 0.0)) 2074 k = k + 1; 2075 2076 k = k - 1; 2077 for (i = 0; i <= n - 1; i++) 2078 { 2079 for (j = 0; j <= m - 1; j++) 2080 { 2081 t = i * m + j; 2082 mtxAP.elements[t] = 0.0; 2083 for (l = 0; l <= k; l++) 2084 { 2085 f = l * n + i; 2086 p = j * m + l; 2087 q = l * n + l; 2088 mtxAP.elements[t] = mtxAP.elements[t] + mtxV.elements[f] * mtxU.elements[p] / elements[q]; 2089 } 2090 } 2091 } 2092 2093 return true; 2094 } 2095 2096 /** 2097 * 约化对称矩阵为对称三对角阵的豪斯荷尔德变换法 2098 * 2099 * @param mtxQ - 返回豪斯荷尔德变换的乘积矩阵Q 2100 * @param mtxT - 返回求得的对称三对角阵 2101 * @param dblB - 一维数组，长度为矩阵的阶数，返回对称三对角阵的主对角线元素 2102 * @param dblC - 一维数组，长度为矩阵的阶数，前n-1个元素返回对称三对角阵的 2103 * 次对角线元素 2104 * @return bool型，求解是否成功 2105 */ 2106 public bool MakeSymTri(Matrix mtxQ, Matrix mtxT, double[] dblB, double[] dblC) 2107 { 2108 int i, j, k, u; 2109 double h, f, g, h2; 2110 2111 // 初始化矩阵Q和T 2112 if (!mtxQ.Init(numColumns, numColumns) || 2113 !mtxT.Init(numColumns, numColumns)) 2114 return false; 2115 2116 if (dblB == null || dblC == null) 2117 return false; 2118 2119 for (i = 0; i <= numColumns - 1; i++) 2120 { 2121 for (j = 0; j <= numColumns - 1; j++) 2122 { 2123 u = i * numColumns + j; 2124 mtxQ.elements[u] = elements[u]; 2125 } 2126 } 2127 2128 for (i = numColumns - 1; i >= 1; i--) 2129 { 2130 h = 0.0; 2131 if (i > 1) 2132 { 2133 for (k = 0; k <= i - 1; k++) 2134 { 2135 u = i * numColumns + k; 2136 h = h + mtxQ.elements[u] * mtxQ.elements[u]; 2137 } 2138 } 2139 2140 if (h == 0.0) 2141 { 2142 dblC[i] = 0.0; 2143 if (i == 1) 2144 dblC[i] = mtxQ.elements[i * numColumns + i - 1]; 2145 dblB[i] = 0.0; 2146 } 2147 else 2148 { 2149 dblC[i] = Math.Sqrt(h); 2150 u = i * numColumns + i - 1; 2151 if (mtxQ.elements[u] > 0.0) 2152 dblC[i] = -dblC[i]; 2153 2154 h = h - mtxQ.elements[u] * dblC[i]; 2155 mtxQ.elements[u] = mtxQ.elements[u] - dblC[i]; 2156 f = 0.0; 2157 for (j = 0; j <= i - 1; j++) 2158 { 2159 mtxQ.elements[j * numColumns + i] = mtxQ.elements[i * numColumns + j] / h; 2160 g = 0.0; 2161 for (k = 0; k <= j; k++) 2162 g = g + mtxQ.elements[j * numColumns + k] * mtxQ.elements[i * numColumns + k]; 2163 2164 if (j + 1 <= i - 1) 2165 for (k = j + 1; k <= i - 1; k++) 2166 g = g + mtxQ.elements[k * numColumns + j] * mtxQ.elements[i * numColumns + k]; 2167 2168 dblC[j] = g / h; 2169 f = f + g * mtxQ.elements[j * numColumns + i]; 2170 } 2171 2172 h2 = f / (h + h); 2173 for (j = 0; j <= i - 1; j++) 2174 { 2175 f = mtxQ.elements[i * numColumns + j]; 2176 g = dblC[j] - h2 * f; 2177 dblC[j] = g; 2178 for (k = 0; k <= j; k++) 2179 { 2180 u = j * numColumns + k; 2181 mtxQ.elements[u] = mtxQ.elements[u] - f * dblC[k] - g * mtxQ.elements[i * numColumns + k]; 2182 } 2183 } 2184 2185 dblB[i] = h; 2186 } 2187 } 2188 2189 for (i = 0; i <= numColumns - 2; i++) 2190 dblC[i] = dblC[i + 1]; 2191 2192 dblC[numColumns - 1] = 0.0; 2193 dblB[0] = 0.0; 2194 for (i = 0; i <= numColumns - 1; i++) 2195 { 2196 if ((dblB[i] != (double)0.0) && (i - 1 >= 0)) 2197 { 2198 for (j = 0; j <= i - 1; j++) 2199 { 2200 g = 0.0; 2201 for (k = 0; k <= i - 1; k++) 2202 g = g + mtxQ.elements[i * numColumns + k] * mtxQ.elements[k * numColumns + j]; 2203 2204 for (k = 0; k <= i - 1; k++) 2205 { 2206 u = k * numColumns + j; 2207 mtxQ.elements[u] = mtxQ.elements[u] - g * mtxQ.elements[k * numColumns + i]; 2208 } 2209 } 2210 } 2211 2212 u = i * numColumns + i; 2213 dblB[i] = mtxQ.elements[u]; mtxQ.elements[u] = 1.0; 2214 if (i - 1 >= 0) 2215 { 2216 for (j = 0; j <= i - 1; j++) 2217 { 2218 mtxQ.elements[i * numColumns + j] = 0.0; 2219 mtxQ.elements[j * numColumns + i] = 0.0; 2220 } 2221 } 2222 } 2223 2224 // 构造对称三对角矩阵 2225 for (i = 0; i < numColumns; ++i) 2226 { 2227 for (j = 0; j < numColumns; ++j) 2228 { 2229 mtxT.SetElement(i, j, 0); 2230 k = i - j; 2231 if (k == 0) 2232 mtxT.SetElement(i, j, dblB[j]); 2233 else if (k == 1) 2234 mtxT.SetElement(i, j, dblC[j]); 2235 else if (k == -1) 2236 mtxT.SetElement(i, j, dblC[i]); 2237 } 2238 } 2239 2240 return true; 2241 } 2242 2243 /** 2244 * 实对称三对角阵的全部特征值与特征向量的计算 2245 * 2246 * @param dblB - 一维数组，长度为矩阵的阶数，传入对称三对角阵的主对角线元素； 2247 * 返回时存放全部特征值。 2248 * @param dblC - 一维数组，长度为矩阵的阶数，前n-1个元素传入对称三对角阵的 2249 * 次对角线元素 2250 * @param mtxQ - 如果传入单位矩阵，则返回实对称三对角阵的特征值向量矩阵； 2251 * 如果传入MakeSymTri函数求得的矩阵A的豪斯荷尔德变换的乘积 2252 * 矩阵Q，则返回矩阵A的特征值向量矩阵。其中第i列为与数组dblB 2253 * 中第j个特征值对应的特征向量。 2254 * @param nMaxIt - 迭代次数 2255 * @param eps - 计算精度 2256 * @return bool型，求解是否成功 2257 */ 2258 public bool ComputeEvSymTri(double[] dblB, double[] dblC, Matrix mtxQ, int nMaxIt, double eps) 2259 { 2260 int i, j, k, m, it, u, v; 2261 double d, f, h, g, p, r, e, s; 2262 2263 // 初值 2264 int n = mtxQ.GetNumColumns(); 2265 dblC[n - 1] = 0.0; 2266 d = 0.0; 2267 f = 0.0; 2268 2269 // 迭代计算 2270 2271 for (j = 0; j <= n - 1; j++) 2272 { 2273 it = 0; 2274 h = eps * (Math.Abs(dblB[j]) + Math.Abs(dblC[j])); 2275 if (h > d) 2276 d = h; 2277 2278 m = j; 2279 while ((m <= n - 1) && (Math.Abs(dblC[m]) > d)) 2280 m = m + 1; 2281 2282 if (m != j) 2283 { 2284 do 2285 { 2286 if (it == nMaxIt) 2287 return false; 2288 2289 it = it + 1; 2290 g = dblB[j]; 2291 p = (dblB[j + 1] - g) / (2.0 * dblC[j]); 2292 r = Math.Sqrt(p * p + 1.0); 2293 if (p >= 0.0) 2294 dblB[j] = dblC[j] / (p + r); 2295 else 2296 dblB[j] = dblC[j] / (p - r); 2297 2298 h = g - dblB[j]; 2299 for (i = j + 1; i <= n - 1; i++) 2300 dblB[i] = dblB[i] - h; 2301 2302 f = f + h; 2303 p = dblB[m]; 2304 e = 1.0; 2305 s = 0.0; 2306 for (i = m - 1; i >= j; i--) 2307 { 2308 g = e * dblC[i]; 2309 h = e * p; 2310 if (Math.Abs(p) >= Math.Abs(dblC[i])) 2311 { 2312 e = dblC[i] / p; 2313 r = Math.Sqrt(e * e + 1.0); 2314 dblC[i + 1] = s * p * r; 2315 s = e / r; 2316 e = 1.0 / r; 2317 } 2318 else 2319 { 2320 e = p / dblC[i]; 2321 r = Math.Sqrt(e * e + 1.0); 2322 dblC[i + 1] = s * dblC[i] * r; 2323 s = 1.0 / r; 2324 e = e / r; 2325 } 2326 2327 p = e * dblB[i] - s * g; 2328 dblB[i + 1] = h + s * (e * g + s * dblB[i]); 2329 for (k = 0; k <= n - 1; k++) 2330 { 2331 u = k * n + i + 1; 2332 v = u - 1; 2333 h = mtxQ.elements[u]; 2334 mtxQ.elements[u] = s * mtxQ.elements[v] + e * h; 2335 mtxQ.elements[v] = e * mtxQ.elements[v] - s * h; 2336 } 2337 } 2338 2339 dblC[j] = s * p; 2340 dblB[j] = e * p; 2341 2342 } while (Math.Abs(dblC[j]) > d); 2343 } 2344 2345 dblB[j] = dblB[j] + f; 2346 } 2347 2348 for (i = 0; i <= n - 1; i++) 2349 { 2350 k = i; 2351 p = dblB[i]; 2352 if (i + 1 <= n - 1) 2353 { 2354 j = i + 1; 2355 while ((j <= n - 1) && (dblB[j] <= p)) 2356 { 2357 k = j; 2358 p = dblB[j]; 2359 j = j + 1; 2360 } 2361 } 2362 2363 if (k != i) 2364 { 2365 dblB[k] = dblB[i]; 2366 dblB[i] = p; 2367 for (j = 0; j <= n - 1; j++) 2368 { 2369 u = j * n + i; 2370 v = j * n + k; 2371 p = mtxQ.elements[u]; 2372 mtxQ.elements[u] = mtxQ.elements[v]; 2373 mtxQ.elements[v] = p; 2374 } 2375 } 2376 } 2377 2378 return true; 2379 } 2380 2381 /** 2382 * 约化一般实矩阵为赫申伯格矩阵的初等相似变换法 2383 */ 2384 public void MakeHberg() 2385 { 2386 int i = 0, j, k, u, v; 2387 double d, t; 2388 2389 for (k = 1; k <= numColumns - 2; k++) 2390 { 2391 d = 0.0; 2392 for (j = k; j <= numColumns - 1; j++) 2393 { 2394 u = j * numColumns + k - 1; 2395 t = elements[u]; 2396 if (Math.Abs(t) > Math.Abs(d)) 2397 { 2398 d = t; 2399 i = j; 2400 } 2401 } 2402 2403 if (d != 0.0) 2404 { 2405 if (i != k) 2406 { 2407 for (j = k - 1; j <= numColumns - 1; j++) 2408 { 2409 u = i * numColumns + j; 2410 v = k * numColumns + j; 2411 t = elements[u]; 2412 elements[u] = elements[v]; 2413 elements[v] = t; 2414 } 2415 2416 for (j = 0; j <= numColumns - 1; j++) 2417 { 2418 u = j * numColumns + i; 2419 v = j * numColumns + k; 2420 t = elements[u]; 2421 elements[u] = elements[v]; 2422 elements[v] = t; 2423 } 2424 } 2425 2426 for (i = k + 1; i <= numColumns - 1; i++) 2427 { 2428 u = i * numColumns + k - 1; 2429 t = elements[u] / d; 2430 elements[u] = 0.0; 2431 for (j = k; j <= numColumns - 1; j++) 2432 { 2433 v = i * numColumns + j; 2434 elements[v] = elements[v] - t * elements[k * numColumns + j]; 2435 } 2436 2437 for (j = 0; j <= numColumns - 1; j++) 2438 { 2439 v = j * numColumns + k; 2440 elements[v] = elements[v] + t * elements[j * numColumns + i]; 2441 } 2442 } 2443 } 2444 } 2445 } 2446 2447 /** 2448 * 求赫申伯格矩阵全部特征值的QR方法 2449 * 2450 * @param dblU - 一维数组，长度为矩阵的阶数，返回时存放特征值的实部 2451 * @param dblV - 一维数组，长度为矩阵的阶数，返回时存放特征值的虚部 2452 * @param nMaxIt - 迭代次数 2453 * @param eps - 计算精度 2454 * @return bool型，求解是否成功 2455 */ 2456 public bool ComputeEvHBerg(double[] dblU, double[] dblV, int nMaxIt, double eps) 2457 { 2458 int m, it, i, j, k, l, ii, jj, kk, ll; 2459 double b, c, w, g, xy, p, q, r, x, s, e, f, z, y; 2460 2461 int n = numColumns; 2462 2463 it = 0; 2464 m = n; 2465 while (m != 0) 2466 { 2467 l = m - 1; 2468 while ((l > 0) && (Math.Abs(elements[l * n + l - 1]) > 2469 eps * (Math.Abs(elements[(l - 1) * n + l - 1]) + Math.Abs(elements[l * n + l])))) 2470 l = l - 1; 2471 2472 ii = (m - 1) * n + m - 1; 2473 jj = (m - 1) * n + m - 2; 2474 kk = (m - 2) * n + m - 1; 2475 ll = (m - 2) * n + m - 2; 2476 if (l == m - 1) 2477 { 2478 dblU[m - 1] = elements[(m - 1) * n + m - 1]; 2479 dblV[m - 1] = 0.0; 2480 m = m - 1; 2481 it = 0; 2482 } 2483 else if (l == m - 2) 2484 { 2485 b = -(elements[ii] + elements[ll]); 2486 c = elements[ii] * elements[ll] - elements[jj] * elements[kk]; 2487 w = b * b - 4.0 * c; 2488 y = Math.Sqrt(Math.Abs(w)); 2489 if (w > 0.0) 2490 { 2491 xy = 1.0; 2492 if (b < 0.0) 2493 xy = -1.0; 2494 dblU[m - 1] = (-b - xy * y) / 2.0; 2495 dblU[m - 2] = c / dblU[m - 1]; 2496 dblV[m - 1] = 0.0; dblV[m - 2] = 0.0; 2497 } 2498 else 2499 { 2500 dblU[m - 1] = -b / 2.0; 2501 dblU[m - 2] = dblU[m - 1]; 2502 dblV[m - 1] = y / 2.0; 2503 dblV[m - 2] = -dblV[m - 1]; 2504 } 2505 2506 m = m - 2; 2507 it = 0; 2508 } 2509 else 2510 { 2511 if (it >= nMaxIt) 2512 return false; 2513 2514 it = it + 1; 2515 for (j = l + 2; j <= m - 1; j++) 2516 elements[j * n + j - 2] = 0.0; 2517 for (j = l + 3; j <= m - 1; j++) 2518 elements[j * n + j - 3] = 0.0; 2519 for (k = l; k <= m - 2; k++) 2520 { 2521 if (k != l) 2522 { 2523 p = elements[k * n + k - 1]; 2524 q = elements[(k + 1) * n + k - 1]; 2525 r = 0.0; 2526 if (k != m - 2) 2527 r = elements[(k + 2) * n + k - 1]; 2528 } 2529 else 2530 { 2531 x = elements[ii] + elements[ll]; 2532 y = elements[ll] * elements[ii] - elements[kk] * elements[jj]; 2533 ii = l * n + l; 2534 jj = l * n + l + 1; 2535 kk = (l + 1) * n + l; 2536 ll = (l + 1) * n + l + 1; 2537 p = elements[ii] * (elements[ii] - x) + elements[jj] * elements[kk] + y; 2538 q = elements[kk] * (elements[ii] + elements[ll] - x); 2539 r = elements[kk] * elements[(l + 2) * n + l + 1]; 2540 } 2541 2542 if ((Math.Abs(p) + Math.Abs(q) + Math.Abs(r)) != 0.0) 2543 { 2544 xy = 1.0; 2545 if (p < 0.0) 2546 xy = -1.0; 2547 s = xy * Math.Sqrt(p * p + q * q + r * r); 2548 if (k != l) 2549 elements[k * n + k - 1] = -s; 2550 e = -q / s; 2551 f = -r / s; 2552 x = -p / s; 2553 y = -x - f * r / (p + s); 2554 g = e * r / (p + s); 2555 z = -x - e * q / (p + s); 2556 for (j = k; j <= m - 1; j++) 2557 { 2558 ii = k * n + j; 2559 jj = (k + 1) * n + j; 2560 p = x * elements[ii] + e * elements[jj]; 2561 q = e * elements[ii] + y * elements[jj]; 2562 r = f * elements[ii] + g * elements[jj]; 2563 if (k != m - 2) 2564 { 2565 kk = (k + 2) * n + j; 2566 p = p + f * elements[kk]; 2567 q = q + g * elements[kk]; 2568 r = r + z * elements[kk]; 2569 elements[kk] = r; 2570 } 2571 2572 elements[jj] = q; elements[ii] = p; 2573 } 2574 2575 j = k + 3; 2576 if (j >= m - 1) 2577 j = m - 1; 2578 2579 for (i = l; i <= j; i++) 2580 { 2581 ii = i * n + k; 2582 jj = i * n + k + 1; 2583 p = x * elements[ii] + e * elements[jj]; 2584 q = e * elements[ii] + y * elements[jj]; 2585 r = f * elements[ii] + g * elements[jj]; 2586 if (k != m - 2) 2587 { 2588 kk = i * n + k + 2; 2589 p = p + f * elements[kk]; 2590 q = q + g * elements[kk]; 2591 r = r + z * elements[kk]; 2592 elements[kk] = r; 2593 } 2594 2595 elements[jj] = q; 2596 elements[ii] = p; 2597 } 2598 } 2599 } 2600 } 2601 } 2602 2603 return true; 2604 } 2605 2606 /** 2607 * 求实对称矩阵特征值与特征向量的雅可比法 2608 * 2609 * @param dblEigenValue - 一维数组，长度为矩阵的阶数，返回时存放特征值 2610 * @param mtxEigenVector - 返回时存放特征向量矩阵，其中第i列为与数组 2611 * dblEigenValue中第j个特征值对应的特征向量 2612 * @param nMaxIt - 迭代次数 2613 * @param eps - 计算精度 2614 * @return bool型，求解是否成功 2615 */ 2616 public bool ComputeEvJacobi(double[] dblEigenValue, Matrix mtxEigenVector, int nMaxIt, double eps) 2617 { 2618 int i, j, p = 0, q = 0, u, w, t, s, l; 2619 double fm, cn, sn, omega, x, y, d; 2620 2621 if (!mtxEigenVector.Init(numColumns, numColumns)) 2622 return false; 2623 2624 l = 1; 2625 for (i = 0; i <= numColumns - 1; i++) 2626 { 2627 mtxEigenVector.elements[i * numColumns + i] = 1.0; 2628 for (j = 0; j <= numColumns - 1; j++) 2629 if (i != j) 2630 mtxEigenVector.elements[i * numColumns + j] = 0.0; 2631 } 2632 2633 while (true) 2634 { 2635 fm = 0.0; 2636 for (i = 1; i <= numColumns - 1; i++) 2637 { 2638 for (j = 0; j <= i - 1; j++) 2639 { 2640 d = Math.Abs(elements[i * numColumns + j]); 2641 if ((i != j) && (d > fm)) 2642 { 2643 fm = d; 2644 p = i; 2645 q = j; 2646 } 2647 } 2648 } 2649 2650 if (fm < eps) 2651 { 2652 for (i = 0; i < numColumns; ++i) 2653 dblEigenValue[i] = GetElement(i, i); 2654 return true; 2655 } 2656 2657 if (l > nMaxIt) 2658 return false; 2659 2660 l = l + 1; 2661 u = p * numColumns + q; 2662 w = p * numColumns + p; 2663 t = q * numColumns + p; 2664 s = q * numColumns + q; 2665 x = -elements[u]; 2666 y = (elements[s] - elements[w]) / 2.0; 2667 omega = x / Math.Sqrt(x * x + y * y); 2668 2669 if (y < 0.0) 2670 omega = -omega; 2671 2672 sn = 1.0 + Math.Sqrt(1.0 - omega * omega); 2673 sn = omega / Math.Sqrt(2.0 * sn); 2674 cn = Math.Sqrt(1.0 - sn * sn); 2675 fm = elements[w]; 2676 elements[w] = fm * cn * cn + elements[s] * sn * sn + elements[u] * omega; 2677 elements[s] = fm * sn * sn + elements[s] * cn * cn - elements[u] * omega; 2678 elements[u] = 0.0; 2679 elements[t] = 0.0; 2680 for (j = 0; j <= numColumns - 1; j++) 2681 { 2682 if ((j != p) && (j != q)) 2683 { 2684 u = p * numColumns + j; w = q * numColumns + j; 2685 fm = elements[u]; 2686 elements[u] = fm * cn + elements[w] * sn; 2687 elements[w] = -fm * sn + elements[w] * cn; 2688 } 2689 } 2690 2691 for (i = 0; i <= numColumns - 1; i++) 2692 { 2693 if ((i != p) && (i != q)) 2694 { 2695 u = i * numColumns + p; 2696 w = i * numColumns + q; 2697 fm = elements[u]; 2698 elements[u] = fm * cn + elements[w] * sn; 2699 elements[w] = -fm * sn + elements[w] * cn; 2700 } 2701 } 2702 2703 for (i = 0; i <= numColumns - 1; i++) 2704 { 2705 u = i * numColumns + p; 2706 w = i * numColumns + q; 2707 fm = mtxEigenVector.elements[u]; 2708 mtxEigenVector.elements[u] = fm * cn + mtxEigenVector.elements[w] * sn; 2709 mtxEigenVector.elements[w] = -fm * sn + mtxEigenVector.elements[w] * cn; 2710 } 2711 } 2712 } 2713 2714 /** 2715 * 求实对称矩阵特征值与特征向量的雅可比过关法 2716 * 2717 * @param dblEigenValue - 一维数组，长度为矩阵的阶数，返回时存放特征值 2718 * @param mtxEigenVector - 返回时存放特征向量矩阵，其中第i列为与数组 2719 * dblEigenValue中第j个特征值对应的特征向量 2720 * @param eps - 计算精度 2721 * @return bool型，求解是否成功 2722 */ 2723 public bool ComputeEvJacobi(double[] dblEigenValue, Matrix mtxEigenVector, double eps) 2724 { 2725 int i, j, p, q, u, w, t, s; 2726 double ff, fm, cn, sn, omega, x, y, d; 2727 2728 if (!mtxEigenVector.Init(numColumns, numColumns)) 2729 return false; 2730 2731 for (i = 0; i <= numColumns - 1; i++) 2732 { 2733 mtxEigenVector.elements[i * numColumns + i] = 1.0; 2734 for (j = 0; j <= numColumns - 1; j++) 2735 if (i != j) 2736 mtxEigenVector.elements[i * numColumns + j] = 0.0; 2737 } 2738 2739 ff = 0.0; 2740 for (i = 1; i <= numColumns - 1; i++) 2741 { 2742 for (j = 0; j <= i - 1; j++) 2743 { 2744 d = elements[i * numColumns + j]; 2745 ff = ff + d * d; 2746 } 2747 } 2748 2749 ff = Math.Sqrt(2.0 * ff); 2750 ff = ff / (1.0 * numColumns); 2751 2752 bool nextLoop = false; 2753 while (true) 2754 { 2755 for (i = 1; i <= numColumns - 1; i++) 2756 { 2757 for (j = 0; j <= i - 1; j++) 2758 { 2759 d = Math.Abs(elements[i * numColumns + j]); 2760 if (d > ff) 2761 { 2762 p = i; 2763 q = j; 2764 2765 u = p * numColumns + q; 2766 w = p * numColumns + p; 2767 t = q * numColumns + p; 2768 s = q * numColumns + q; 2769 x = -elements[u]; 2770 y = (elements[s] - elements[w]) / 2.0; 2771 omega = x / Math.Sqrt(x * x + y * y); 2772 if (y < 0.0) 2773 omega = -omega; 2774 2775 sn = 1.0 + Math.Sqrt(1.0 - omega * omega); 2776 sn = omega / Math.Sqrt(2.0 * sn); 2777 cn = Math.Sqrt(1.0 - sn * sn); 2778 fm = elements[w]; 2779 elements[w] = fm * cn * cn + elements[s] * sn * sn + elements[u] * omega; 2780 elements[s] = fm * sn * sn + elements[s] * cn * cn - elements[u] * omega; 2781 elements[u] = 0.0; elements[t] = 0.0; 2782 2783 for (j = 0; j <= numColumns - 1; j++) 2784 { 2785 if ((j != p) && (j != q)) 2786 { 2787 u = p * numColumns + j; 2788 w = q * numColumns + j; 2789 fm = elements[u]; 2790 elements[u] = fm * cn + elements[w] * sn; 2791 elements[w] = -fm * sn + elements[w] * cn; 2792 } 2793 } 2794 2795 for (i = 0; i <= numColumns - 1; i++) 2796 { 2797 if ((i != p) && (i != q)) 2798 { 2799 u = i * numColumns + p; 2800 w = i * numColumns + q; 2801 fm = elements[u]; 2802 elements[u] = fm * cn + elements[w] * sn; 2803 elements[w] = -fm * sn + elements[w] * cn; 2804 } 2805 } 2806 2807 for (i = 0; i <= numColumns - 1; i++) 2808 { 2809 u = i * numColumns + p; 2810 w = i * numColumns + q; 2811 fm = mtxEigenVector.elements[u]; 2812 mtxEigenVector.elements[u] = fm * cn + mtxEigenVector.elements[w] * sn; 2813 mtxEigenVector.elements[w] = -fm * sn + mtxEigenVector.elements[w] * cn; 2814 } 2815 2816 nextLoop = true; 2817 break; 2818 } 2819 } 2820 2821 if (nextLoop) 2822 break; 2823 } 2824 2825 if (nextLoop) 2826 { 2827 nextLoop = false; 2828 continue; 2829 } 2830 2831 nextLoop = false; 2832 2833 // 如果达到精度要求，退出循环，返回结果 2834 if (ff < eps) 2835 { 2836 for (i = 0; i < numColumns; ++i) 2837 dblEigenValue[i] = GetElement(i, i); 2838 return true; 2839 } 2840 2841 ff = ff / (1.0 * numColumns); 2842 } 2843 } 2844 2845 2846 2847 2848 } 2849 }