ヤコビ法

Jacobi.cpp

//
//  Jacobi.cpp
//  ヤコビ法(行列の固有値計算)
//  Created by knuu on 2014/06/23.
//
//

#include <iostream>
#include <iomanip> // std::setpricision
#include <Eigen/Dense> // 行列計算のライブラリ
#include <algorithm>
#include <cmath>

#define M_PRE 10 // 小数点以下の桁数
#define SIZE_A 3 // 行列のサイズ
#define EPS 0.00000000001 // 誤差
#define square(x) (x) * (x)

void yacobi(Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> A)
{
  std::cout<<std::fixed<<std::setprecision(M_PRE); // 小数点以下の桁数設定
	
  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> P_n;
  P_n = Eigen::MatrixXd::Identity(A.cols(), A.rows());
  int count = 1; // 計算回数をカウント
	
  while (true) {
    double a_km[2] = {0, 1}; // 非対角要素の絶対値最大の位置a_km = {k, m}
    // 非対角要素の絶対値最大の位置を見つける
    double a_m;
    for (int i = 0; i < A.cols(); i++) {
      for (int j = 0; j < A.rows(); j++) {
	if (i == j) continue;
	a_m = A(a_km[0], a_km[1]);
	double a_ij = A(i, j); // std::abs(A(i,j))が0になってしまう？
	if (std::abs(a_m) < std::abs(a_ij)) {
	  a_km[0] = i, a_km[1] = j;
	}
      }
    }
		
    // 非対角要素の大きさ判定
    a_m = A(a_km[0], a_km[1]);
    if (std::abs(a_m) < EPS) {
      break;
    }
		
    // sinphi, cosphiを求める
    double s = (A(a_km[0], a_km[0]) - A(a_km[1], a_km[1]))/ 2.0;
    double r = std::sqrt(s * s + a_m * a_m);
    double sigphi;
    s >= 0 ? sigphi = 1.0 : sigphi = -1.0;
		
    double cosphi = std::sqrt(0.5 * (1 + sigphi * s / r));
    double sinphi = sigphi * a_m / (2.0 * r * cosphi);
		
    // 求めたcosphiとsinphiについて行列Pを求める
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> P;
    P = Eigen::MatrixXd::Identity(A.cols(), A.rows());
    P(a_km[0], a_km[0]) = cosphi, P(a_km[1], a_km[1]) = cosphi;
    P(a_km[0], a_km[1]) = -sinphi, P(a_km[1], a_km[0]) = sinphi;
		
    // A=(P^T)APを計算
    P_n = P_n * P;
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Pt = P.transpose();
    A = Pt * A * P;
		
    // 対角化途中の行列を表示
    /*
      std::cout<<"対角化途中("<<c<<"回目)の行列A_i = "<<std::endl;
      std::cout<<A<<std::endl;
      std::cout<<"対角化途中("<<count<<"回目)の対角化行列P_i = "<<std::endl;
      std::cout<<P<<std::endl;
    */
		
    // カウントを増やす
    count++;
		
  }
	
  // 対角化された行列を表示
  std::cout<<"計算回数: "<<count<<std::endl;
  std::cout<<"対角化された行列A_n = "<<std::endl;
  std::cout<<A<<std::endl;
	
  // 固有ベクトルを表示
  for (int i = 0; i < A.rows(); i++) {
    std::cout<<i+1<<"番目の固有ベクトル = \t"<<std::endl;
    std::cout<<P_n.col(i)<<std::endl;
  }
	
}


int main()
{
  Eigen::Matrix<double, SIZE_A, SIZE_A> A;
  A << 3,0.01,0.1, 0.01,2,0.01, 0.1,0.01,1;
  yacobi(A);
}