#include <string>
#include <iterator>
#include <iostream>
#include <math.h>
using namespace std;
const int N = 4;
//Функция signum
int sgn(double val)
{
return (val>0) ? (1) : ((val<0) ? (-1) : (0));
}
//Транспонирование
void transpose(double matrix[1][N], double m1[N][1])
{
for (int i = 0; i < N; ++i)
{
m1[i][0] = matrix[0][i];
}
}
//Матрица wwT перемножения двух векторов
void mult_vector(double m1[1][N], double m2[N][1], double res[N][N])
{
for (int i = 0; i < N; i++)
{
for (int j = 0; j < N; j++)
{
res[i][j] = 2 * m1[0][i] * m2[j][0];
}
}
}
void mult_matrix(double m1[N][N], double m2[N][N], double res[N][N])
{
double temp = 0;
for (int i = 0; i < N; i++)
{
for (int j = 0; j < N; j++)
{
temp = 0;
for (int k = 0; k < N; k++)
{
temp += m1[i][k] * m2[k][j];
}
res[i][j] = temp;
}
}
for (int i = 0; i < N; i++)
{
for (int j = 0; j < N; j++)
{
m2[i][j] = res[i][j];
}
}
}
//Зануление первой строки и первого столбца
void cut(double m1[N][N])
{
for (int i = 0; i < N; i++)
{
for (int j = 0; j < N; j++)
{
m1[0][j] = 0;
m1[i][0] = 0;
}
}
}
//Обнуление векторов
void zeroing(double v1[1][N], double v2[N][1])
{
for (int i = 0; i < N; i++)
{
v1[0][i] = 0;
v2[i][0] = 0;
}
}
//Копирование матрицы
void copy(double m1[N][N], double m2[N][N])
{
for (int i = 0; i < N; i++)
{
for (int j = 0; j < N; j++)
{
m2[i][j] = m1[i][j];
}
}
}
//Умножение матрицы на вектор
void mult_vec(double m1[N][N], double v1[N])
{
double temp = 0;
double res[N];
for (int i = 0; i < N; i++)
{
temp = 0;
for (int k = 0; k < N; k++)
{
temp += m1[i][k] * v1[k];
}
res[i] = temp;
}
for (int i = 0; i < N; i++)
{
v1[i] = res[i];
}
}
int main()
{
double res[N][N];
double HA_temp[N][N] = { 0 };
double HA[N][N] = { { 1, -1, 3, 1 },
{ 4, -1, 5, 4 },
{ 2, -2, 4, 1 },
{ 1, -4, 5, -1 } };
double wwT[N][N] = { 0 };
double b[N] = { 5, 4, 6, 3 };
double beta;
double mu;
double w[1][N] = { 0 };
double wT[N][1] = { 0 };
double H[N][N];
double E[N][N] = { { 1, 0, 0, 0 }, { 0, 1, 0, 0 }, { 0, 0, 1, 0 }, { 0, 0, 0, 1 } };
double sum;
cout << "Matrix A:\t\t\tB:" << endl;
for (int i = 0; i < N; i++)
{
for (int j = 0; j < N; j++)
{
cout << HA[i][j] << "\t";
}
cout << "| " << b[i] << " |" << endl;
}
cout << endl;
for (int i = 0; i < N - 1; i++)
{
zeroing(w, wT);
beta = 0;
mu = 0;
sum = 0;
for (int j = i; j < N; j++)
{
sum += HA[j][i] * HA[j][i];
}
sum = sqrt(sum);
beta = sgn(-HA[i][i]) * sum;
mu = 1 / sqrt(2 * beta*beta - 2 * beta*HA[i][i]);
if (i != 0)
{
copy(HA, HA_temp);
cut(HA_temp);
}
w[0][i] = HA[i][i] - beta;
for (int j = i + 1; j < N; j++)
{
w[0][j] = HA[j][i];
}
transpose(w, wT);
mult_vector(w, wT, wwT);
for (int i = 0; i < N; i++)
{
for (int j = 0; j < N; j++)
{
H[i][j] = E[i][j] - mu*mu*wwT[i][j];
}
}
mult_matrix(H, HA, res);
mult_vec(H, b);
cout << "step " << i + 1 << endl;
cout << "HA:" << endl;
for (int i = 0; i < N; i++)
{
for (int j = 0; j < N; j++)
{
printf("%.3f\t", HA[i][j]);
}
printf("|%.3f|\t\n", b[i]);
}
cout << endl;
}
cout << "result" << endl;
cout << "HA:\t\t\t\tB:" << endl;
for (int i = 0; i < N; i++)
{
for (int j = 0; j < N; j++)
{
printf("%.3f\t", HA[i][j]);
}
printf("|%.3f|\t\n", b[i]);
}
cout << endl;
double X[N];
cout << "solutions:" << endl;
//Обратный ход Гаусса
for (int i = N - 1; i >= 0; i--)
{
sum = 0;
for (int k = N - 1; k > i; k--)
{
sum += X[k] * HA[i][k];
}
X[i] = (b[i] - sum) / HA[i][i];
cout << "X[" << N - i << "] = " << X[i] << endl;
}
system("pause");
return 0;
}