Обращение матрицы 6х6.

[Smorodov 579]

В задачах, связанных с механикой и фильтрами, частенько возникает необходимость найти обратную матрицу 6x6 (по количеству обобщенных координат). Этот метод хорошо параллелится (хорошо для реализации на GPU, что я и делал).

Используемый метод: http://en.wikipedia.org/wiki/Schur_complement

// My6х6MatrixInverse.cpp : Defines the entry point for the console application.

//


#include "stdafx.h"

#include <iostream>

#include <vector>

using namespace std;


void Mul (float *A,float *B,float* C)

{

	C[0]=A[0] * B[0] + A[3] * B[1] + A[6] * B[2];

	C[3]=A[0] * B[3] + A[3] * B[4] + A[6] * B[5];

	C[6]=A[0] * B[6] + A[3] * B[7] + A[6] * B[8];


	C[1]=A[1] * B[0] + A[4] * B[1] + A[7] * B[2];

	C[4]=A[1] * B[3] + A[4] * B[4] + A[7] * B[5];

	C[7]=A[1] * B[6] + A[4] * B[7] + A[7] * B[8];


	C[2]=A[2] * B[0] + A[5] * B[1] + A[8] * B[2];

	C[5]=A[2] * B[3] + A[5] * B[4] + A[8] * B[5]; 

	C[8]=A[2] * B[6] + A[5] * B[7] + A[8] * B[8];

}


void Add (float *A,float *B,float* C)

{

	for(int i=0;i<9;i++)

	{

		C[i]=A[i]+B[i];

	}

}


void Sub (float *A,float *B,float* C)

{

	for(int i=0;i<9;i++)

	{

		C[i]=A[i]-B[i];

	}

}


void Neg (float *A,float* C)

{

	for(int i=0;i<9;i++)

	{

		C[i]=-A[i];

	}

}



bool Inv33(float* src,float* dst)

{

	float determinant =    +src[0]*(src[4]*src[8]-src[5]*src[7])

		-src[3]*(src[1]*src[8]-src[7]*src[2])

		+src[6]*(src[1]*src[5]-src[4]*src[2]);

	if(fabs(determinant)<1e-10){return false;}

	float invdet = 1/determinant;


	dst[0] =  (src[4]*src[8]-src[5]*src[7])*invdet;

	dst[3] = -(src[3]*src[8]-src[6]*src[5])*invdet;

	dst[6] =  (src[3]*src[7]-src[6]*src[4])*invdet;


	dst[1] = -(src[1]*src[8]-src[7]*src[2])*invdet;

	dst[4] =  (src[0]*src[8]-src[6]*src[2])*invdet;

	dst[7] = -(src[0]*src[7]-src[1]*src[6])*invdet;


	dst[2] =  (src[1]*src[5]-src[2]*src[4])*invdet;

	dst[5] = -(src[0]*src[5]-src[2]*src[3])*invdet;

	dst[8] =  (src[0]*src[4]-src[1]*src[3])*invdet;


	return true;

}



bool Inv6х6 (float *src,float *dst)

{

	float A[9];

	float B[9];

	float C[9];

	float D[9];

	float A_i[9];

	float temp[9];

	float temp_i[9];

	float temp1[9];

	float temp2[9];


	A[0]=src[0];A[3]=src[6];A[6]=src[12];

	A[1]=src[1];A[4]=src[7];A[7]=src[13];

	A[2]=src[2];A[5]=src[8];A[8]=src[14];

	//-----------

	B[0]=src[18];B[3]=src[24];B[6]=src[30];

	B[1]=src[19];B[4]=src[25];B[7]=src[31];

	B[2]=src[20];B[5]=src[26];B[8]=src[32];

	//----------

	C[0]=src[3];C[3]=src[9];C[6]=src[15];

	C[1]=src[4];C[4]=src[10];C[7]=src[16];

	C[2]=src[5];C[5]=src[11];C[8]=src[17];

	//-----------

	D[0]=src[21];D[3]=src[27];D[6]=src[33];

	D[1]=src[22];D[4]=src[28];D[7]=src[34];

	D[2]=src[23];D[5]=src[29];D[8]=src[35];


	if(!Inv33(A,A_i)){return false;}


	//---------------

	Mul(C,A_i,temp1);

	Mul(temp1,B,temp2);

	Sub(D,temp2,temp); // Schur complement of A


	if(!Inv33(temp,temp_i)){return false;} // Первый результат (D)


	dst[21]=temp_i[0];dst[27]=temp_i[3];dst[33]=temp_i[6];

	dst[22]=temp_i[1];dst[28]=temp_i[4];dst[34]=temp_i[7];

	dst[23]=temp_i[2];dst[29]=temp_i[5];dst[35]=temp_i[8];


	Neg(temp_i,temp);

	Mul(temp,temp1,temp2);  // второй результат (С)


	dst[3]=temp2[0];dst[9]=temp2[3];dst[15]=temp2[6];

	dst[4]=temp2[1];dst[10]=temp2[4];dst[16]=temp2[7];

	dst[5]=temp2[2];dst[11]=temp2[5];dst[17]=temp2[8];


	float A_iB[9];

	float temp4[9];

	Mul(A_i,B,A_iB);

	Mul(A_iB,temp_i,temp2);

	Mul(temp2,temp1,temp);

	Add(temp,A_i,temp1); // третий результат (A)


	dst[0]=temp1[0];dst[6]=temp1[3];dst[12]=temp1[6];

	dst[1]=temp1[1];dst[7]=temp1[4];dst[13]=temp1[7];

	dst[2]=temp1[2];dst[8]=temp1[5];dst[14]=temp1[8];


	Neg(temp2,temp4); // четвертый результат (


	dst[18]=temp4[0];dst[24]=temp4[3];dst[30]=temp4[6];

	dst[19]=temp4[1];dst[25]=temp4[4];dst[31]=temp4[7];

	dst[20]=temp4[2];dst[26]=temp4[5];dst[32]=temp4[8];

}



int _tmain(int argc, _TCHAR* argv[])

{

// матрица транспонированная отностительно ее вида здесь

// в памяти сначала идут столбцы, затем строки

float mat[36]=

{

	 1,	1,	1,	2,	2,	3

	,2,	8,	1,	2,	2,	3

	,3,	9,	15,	2,	2,	3

	,4,	1,	1,	20,	2,	3

	,5,	1,	1,	23,	29,	3

	,6,	1,	1,	2,	30,	36

};


	float mat_i[36];


	if(!Inv6х6(mat,mat_i)){return -1;}


	for(int i=0;i<6;i++)

	{

		for(int j=0;j<6;j++)

		{

			cout<< " " << mat_i[i+j*6]; 

		}

		cout << endl;

	}


	getchar();

	return 0;

}

[/code]