2nd_Sem_Bogachev/2025.04.04/dist/Krivoruchenko_SK/solve.c

#include "solve.h"
#include "io_status.h"
#include "array_io.h"
#include "matrix.h"
#include <float.h>
#include <math.h>
#include <stdio.h>

#define EPS 1.2e-16

// c - changes in rows
int t14_solve(int n, double * restrict A, double * restrict X, int * restrict c)
{
	double norm = get_matrix_norm(n, A);
	double eps = EPS*norm;

//	printf("NORM = %lf EPS = %lf\n", norm, eps);

	// Проходимся по главным минорам
	for (int k = 0; k < n; ++k) {
		double maximum = -1.;
		int max_i = 0, max_j = 0;
		
		// Ищем максимальный элемент минора
		for (int i = k; i < n; ++i)
			for (int j = k; j < n; ++j)
				if (fabs(A[i*n + j]) > maximum) {
					maximum = fabs(A[i*n + j]);
					max_i = i;
					max_j = j;
				}
			
//		printf("\n------- K = %d -------\n", k);
//		printf("Maximum = %lf i = %d j = %d\n", maximum, max_i, max_j);

		// Если максимальный по модулю элемент равен нулю, значит матрица вырождена
		if (fabs(maximum) <= eps)
			return SINGULAR;
		
		// Меняем строки местами, если максимум находится не в k строке
		if (max_i != k)
		{
			for (int j = 0; j < k; ++j)
			{
				double swap = X[k*n + j];
				X[k*n + j] = X[max_i*n + j];
				X[max_i*n + j] = swap;
			}
	
			for (int j = k; j < n; ++j)
			{
				double swap = X[k*n + j];
				X[k*n + j] = X[max_i*n + j];
				X[max_i*n + j] = swap;
				
				swap = A[k*n + j];
				A[k*n + j] = A[max_i*n + j];
				A[max_i*n + j] = swap;
			}
		}

		// Меняем столбцы местами
		if (max_j != k)
		{
			int swap_temp = c[max_j];
			c[max_j] = c[k];
			c[k] = swap_temp;

			for (int i = 0; i < n; i++)
			{
				double swap = A[i*n + k];
				A[i*n + k] = A[i*n + max_j];
				A[i*n + max_j] = swap;
			}
		}

//		printf("BEFORE GAUSS\n");
//		printf("Original matrix:\n");
//		print_matrix(A, n, n);
//		printf("Inverse matrix:\n");
//		print_matrix(X, n, n);
		
		gauss_inverse(n, k, A, X);
		
//		printf("AFTER GAUSS\n");
//		printf("Original matrix:\n");
//		print_matrix(A, n, n);
//		printf("Inverse matrix:\n");
//		print_matrix(X, n, n);
	}

	gauss_back_substitution(n, A, X);

/*
	for (int k = 0; k < n; ++k)
	{
		int i = c[k];
		
		if (i != k)
		{
			double last_swap = 0;
			double cur_swap;
			
			for (int j = 0; j < n-1; ++j)
			{
				int cur_p = i;
				last_swap = X[k*n + j];

				do {
					cur_swap = X[cur_p*n + j];
					X[cur_p*n + j] = last_swap;
					cur_p = c[cur_p];
					last_swap = cur_swap;
				} while (cur_p != k);

				X[k*n + j] = last_swap;
			}

			last_swap = X[k*n + n-1];

			do 
			{
				int last_p = i;
				i = c[i];
				c[last_p] = last_p;

				cur_swap = X[last_p*n + n-1];
				X[last_p*n + n-1] = last_swap;
				last_swap = cur_swap;
			} while (i != k);

			X[k*n + n-1] = last_swap;
			c[k] = k;
		}
	}
*/
	
	for (int k = 0; k < n; ++k)
	{
		const int kn = k*n;	
		int i = c[k];
		
		while (i != k)
		{
			const int in = i*n;
			const int swap_int = c[i];
			c[i] = i;
			i = swap_int;

			for (int j = 0; j < n; ++j)
			{
				double swap_temp = X[in+j];
				X[in+j] = X[kn+j];
				X[kn+j] = swap_temp;
			}
		}
	}

	return 0;
}

// Прямой ход Го ----- йда
void gauss_inverse(const int n, const int k, double * restrict A, double * restrict X)
{
	const double inv_akk = 1./A[k*n + k];
	
	for (int j = 0; j < k+1; ++j)
		X[k*n + j] *= inv_akk;

	for (int j = k+1; j < n; ++j)
	{
		A[k*n + j] *= inv_akk;
		X[k*n + j] *= inv_akk;
	}
	
	for (int i = k+1; i < n; ++i)
	{
		const double aik = A[i*n + k];
		
		for (int j = 0; j < k+1; ++j)
			X[i*n + j] -= X[k*n + j] * aik;

		for (int j = k+1; j < n; ++j)
		{
			A[i*n + j] -= A[k*n + j] * aik;
			X[i*n + j] -= X[k*n + j] * aik;
		}
	}
}

// Обратный ход метода Гаусса
void gauss_back_substitution(const int n, double * restrict A, double * restrict X)
{
	// Идём с последней строки и вычитаем её из последующих
	for (int k = n-1; k > 0; --k)
		for (int i = 0; i < k; ++i)
		{
			const double aik = A[i*n + k];

			for (int j = 0; j < n; ++j)
				X[i*n + j] -= X[k*n + j] * aik;	
		}

//		printf("\n------- K = %d -------\n", k);
//		printf("Original matrix:\n");
//		print_matrix(A, n, n);
//		printf("Inverse matrix:\n");
//		print_matrix(X, n, n);
}