おまけ

3x3行列の逆行列を計算するルーチンです。意外と使えます。

// 逆行列の公式による3x3の逆行列計算
// A:入力行列, B：出力行列, 戻り値=0：逆行列なし、=1：あり
int mat_inv_3x3(double *A, double *B)
{
	double detA;
	// サラスの公式による行列式の計算
	// detA=a11a22a33+a21a32a13+a31a12a23
	//     -a11a32a23-a31a22a13-a21a12a33
	detA = 
		 A[0 * 3 + 0]*A[1 * 3 + 1]*A[2 * 3 + 2]
    		+A[1 * 3 + 0]*A[2 * 3 + 1]*A[0 * 3 + 2]
		+A[2 * 3 + 0]*A[0 * 3 + 1]*A[1 * 3 + 2]
		-A[0 * 3 + 0]*A[2 * 3 + 1]*A[1 * 3 + 2]
		-A[2 * 3 + 0]*A[1 * 3 + 1]*A[0 * 3 + 2]
		-A[1 * 3 + 0]*A[0 * 3 + 1]*A[2 * 3 + 2];
	if(detA == 0)
		return 0;
	// 3x3逆行列の公式（余韻子の転置/detA）
	B[0 * 3 + 0] = (A[1 * 3 + 1]*A[2 * 3 + 2] - A[1 * 3 + 2]*A[2 * 3 + 1]) / detA;
	B[1 * 3 + 0] = (A[1 * 3 + 2]*A[2 * 3 + 0] - A[1 * 3 + 0]*A[2 * 3 + 2]) / detA;
	B[2 * 3 + 0] = (A[1 * 3 + 0]*A[2 * 3 + 1] - A[1 * 3 + 1]*A[2 * 3 + 0]) / detA;

	B[0 * 3 + 1] = (A[0 * 3 + 2]*A[2 * 3 + 1] - A[0 * 3 + 1]*A[2 * 3 + 2]) / detA;
	B[1 * 3 + 1] = (A[0 * 3 + 0]*A[2 * 3 + 2] - A[0 * 3 + 2]*A[2 * 3 + 0]) / detA;
	B[2 * 3 + 1] = (A[0 * 3 + 1]*A[2 * 3 + 0] - A[0 * 3 + 0]*A[2 * 3 + 1]) / detA;

	B[0 * 3 + 2] = (A[0 * 3 + 1]*A[1 * 3 + 2] - A[0 * 3 + 2]*A[1 * 3 + 1]) / detA;
	B[1 * 3 + 2] = (A[0 * 3 + 2]*A[1 * 3 + 0] - A[0 * 3 + 0]*A[1 * 3 + 2]) / detA;
	B[2 * 3 + 2] = (A[0 * 3 + 0]*A[1 * 3 + 1] - A[0 * 3 + 1]*A[1 * 3 + 0]) / detA;

	return 1;
}