Обратная Матрица (Inverse Matrix)09.12.2024 23:00

#pragma GCC optimize ("Ofast,unroll-loops,-ffast-math")
#pragma GCC target   ("sse,sse4.2")
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
//обозначим необходимый минимум по структурам
typedef float f32;

struct vec3_t
{
    f32 x;
    f32 y;
    f32 z;
};
typedef struct vec3_t vec3;

struct mat3_t
{
    f32 v[9];
};
typedef struct mat3_t mat3;

//функция печати матрицы
void printMatrix(const mat3 m)
{
  printf("%f %f %f\n%f %f %f\n%f %f %f\n\n",
	 m.v[0],m.v[1],m.v[2],
	 m.v[3],m.v[4],m.v[5],
	 m.v[6],m.v[7],m.v[8]
	 );
}

//установка матрицы через 3 вектора
const mat3 setMatrix(const vec3 v,const vec3 v1,const vec3 v2)
{
  return (mat3)
    {
      v.x,v.y,v.z,
      v1.x,v1.y,v1.z,
      v2.x,v2.y,v2.z,
    };
}

//матрица транспонирования
const mat3 Transposedm3(const mat3 m)
{
    return (mat3)
      {
	m.v[0], m.v[3],m.v[6],
	m.v[1], m.v[4],m.v[7],
	m.v[2], m.v[5],m.v[8]
      };
}

//матрица минора 
mat3 Minorm3(mat3 m)
{
  return (mat3)
    {
      (m.v[4]*m.v[8]-m.v[5]*m.v[7]),
      (m.v[3]*m.v[8]-m.v[5]*m.v[6]),
      (m.v[3]*m.v[7]-m.v[4]*m.v[6]),

      (m.v[1]*m.v[8]-m.v[2]*m.v[7]),
      (m.v[0]*m.v[8]-m.v[2]*m.v[6]),
      (m.v[0]*m.v[7]-m.v[1]*m.v[6]),

      (m.v[1]*m.v[5]-m.v[2]*m.v[4]),
      (m.v[0]*m.v[5]-m.v[2]*m.v[3]),
      (m.v[0]*m.v[4]-m.v[1]*m.v[3]),
    };
}

//применение знаковой матрицы
mat3 Cofactorm3(mat3 m)
{
  return (mat3)
    {
       m.v[0],-m.v[1],m.v[2],
      -m.v[3], m.v[4],-m.v[5],
       m.v[6],-m.v[7],m.v[8]
    };
}

//результирующая функция Инверсной матрицы
const mat3 Inversem3(mat3 m) 
{
  mat3 as;
  as=Minorm3(m);
  as=Cofactorm3(as);

  float DetA=(m.v[0]*as.v[0])+(m.v[1]*as.v[1])+(m.v[2]*as.v[2]);
  printf("Det %f\n\n",DetA);
  //проверка !0
  DetA=1/DetA;
  
  
  as=Transposedm3(as);
  
  return (mat3)
    {
      as.v[0]*DetA,as.v[1]*DetA,as.v[2]*DetA,
      as.v[3]*DetA,as.v[4]*DetA,as.v[5]*DetA,
      as.v[6]*DetA,as.v[7]*DetA,as.v[8]*DetA
    };
}

int main()
{
  
  //printf("Chapter: Find Inverse Matrix!\nExample Matrix for start calculate:\n\n");
  mat3 matA;

  matA=setMatrix(
		 (vec3){1,2,3},
		 (vec3){4,5,6},
		 (vec3){7,8,9}
                );
  printMatrix(matA);

  mat3 as;

  as=Inversem3(matA);
  printf("Inverse Result: \n");
  printMatrix(as);

  printf("\n\n");
  return 0;
  
}