Jacobi迭代

#include<stdio.h>
#include<math.h>
#define N 3
void print(double s[N][N])
{
	for (int i = 0; i <N; i++)
	{
		for (int j = 0; j<N; j++)
		{
			printf("%lf ",s[i][j]);
		}
		printf("\n");
	}
}
void multiply(double g[N][N],double v[N][N])//乘法
{
	double c[N][N] = {0};

	for (int i = 0; i <= N - 1; i++) {
		for (int j = 0; j <= N - 1; j++) {
			for (int k = 0; k <= N - 1; k++) {
				c[i][j] += g[i][k] * v[k][j];
			}
		}
	}
	for (int i = 0; i < N; i++)
		for (int j = 0; j < N; j++)
		{
			g[i][j] = c[i][j];
		}
	printf("\nG=\n");
	print(g);

}
bool S_(double s[N][N], double s_[N][N], double g[N][N],double e)//前两个参数为定点，第三个参数为估值点
{
	double v[N][N] = {0};//特征向量V
	double max = fabs(s[0][1]), tan2 = 0, x = 0, sinn = 0, coss = 0;//x为弧度
	int i = 1, j = 2;//坐标从1开始
	//找到最大值坐标
	for (int k = 1; k <= N*N; k++)
	{
		if (k % (N + 1) == 1)//跳过对角线
		{
			continue;
		}
		if ( fabs(s[(k - 1) / N][(k - 1) % N])> max)//保存最大值
		{	//最大值坐标
			max = fabs(s[(k - 1) / N][(k - 1) % N]);
			j = (k - 1) % N + 1;
			i = (k - 1) / N + 1;
		}
	}
	if (max < e)//合格
		return true;

	tan2 = 2 * s[i - 1][j - 1] / (s[j - 1][j - 1] - s[i - 1][i - 1]);
	x = atan(tan2) / 2.0;
	sinn = sin(x);
	coss = cos(x);
	s_[i - 1][i - 1] = s[i - 1][i - 1] * (coss)*(coss)-2 * (coss)*(sinn)*s[i - 1][j - 1] + (sinn)*(sinn)*s[j - 1][j - 1];
	s_[j - 1][j - 1] = s[i - 1][i - 1] * (sinn)*(sinn)+2 * (coss)*(sinn)*s[i - 1][j - 1] + (coss)*(coss)*s[j - 1][j - 1];
	s_[i - 1][j - 1] = ((coss)*(coss)-(sinn)*(sinn))*s[i - 1][j - 1] + (coss)*(sinn)*(s[i - 1][i - 1] - s[j - 1][j - 1]);
	for (int k = 1; k <= N; k++)
	{
		if (k == i || k == j)
			continue;
		s_[i - 1][k - 1] = (coss)*s[i - 1][k - 1] - (sinn)*s[j - 1][k - 1];
		s_[k - 1][i - 1] = (coss)*s[i - 1][k - 1] - (sinn)*s[j - 1][k - 1];
		s_[j - 1][k - 1] = (sinn)*s[i - 1][k - 1] + (coss)*s[j - 1][k - 1];
		s_[k - 1][j - 1] = (sinn)*s[i - 1][k - 1] + (coss)*s[j - 1][k - 1];
		for (int l = 1; l <= N; l++)
		{
			if (l == i || l == j)
				continue;
			s_[k - 1][l - 1] = s[k - 1][l - 1];
		}
	}
	//求旋转矩阵
	if (i + j == 3)//绕z-轴的主动旋转
	{
		v[0][0] = cos(x);
		v[0][1] = sin(x);
		v[1][0] = -sin(x);
		v[1][1] = cos(x);
		v[2][2] = 1;
	}
	else if(i + j == 4)//绕y-轴的主动旋转
	{
		v[0][0] = cos(x);
		v[0][2] = sin(x);
		v[2][0] = -sin(x);
		v[1][1] = 1;
		v[2][2] = cos(x);
	}
	else//绕x-轴的主动旋转
	{
		v[0][0] = 1;
		v[1][1] = cos(x);
		v[2][2] = cos(x);
		v[1][2] = sin(x);
		v[2][1] = -sin(x);
	}
	printf("\ns_=\n");
	print(s_);
	multiply(g,v);
	return false;
}
int main()
{

		//double	s[N][N] = { {1,1,0.5},{ 1,1,0.25 },{ 0.5,0.25,2 } },//例题
		double	s[N][N] = { { 2,-1,0 },{ -1,2,-1 },{ 0,-1,2 } },  //习题
			s_[N][N] = { 0 },
			g[N][N] = { { 1,0,0 },{ 0,1,0 },{ 0,0,1 } },e=0.0005;
	while(!S_(s,s_,g,e))//不合格
		for (int i = 0; i < N; i++)//迭代
			for (int j = 0; j < N; j++)
			{
				s[i][j] = s_[i][j];
				s_[i][j] = 0;
			};

	getchar();
}