4x4までの逆行列の直接解法の変更点

追加された行はこの色です。
削除された行はこの色です。
4x4までの逆行列の直接解法へ行く。
----
#contents
----

*2x2の行列式と逆行列 [#m2676220]
#code(C){{
/*!
 * 2x2行列の行列式の計算
 *  | m[0] m[1] |
 *  | m[2] m[3] |
 * @param[in] m 元の行列
 * @return 行列式の値
 */
double CalDetMat2x2(const vector<double> &m)
{
	return m[0]*m[3]-m[1]*m[2];
}

/*!
 * 2x2行列の逆行列の計算
 *  | m[0] m[1] |
 *  | m[2] m[3] |
 * @param[in] m 元の行列
 * @param[out] invm 逆行列
 * @return 逆行列の存在
 */
bool CalInvMat2x2(const vector<double> &m, vector<double> &invm)
{
	double det = CalDetMat2x2(m);
	if(fabs(det) < RX_FEQ_EPS){
		return false;
	}
	else{
		double inv_det = 1.0/det;
		invm[0] =  inv_det*m[3];
		invm[1] = -inv_det*m[1];
		invm[2] = -inv_det*m[2];
		invm[3] =  inv_det*m[0];
		return true;
	}
}
}}

*3x3の行列式と逆行列 [#z4a49ac4]
#code(C){{
/*!
 * 3x3行列の行列式の計算
 *  | m[0] m[1] m[2] |
 *  | m[3] m[4] m[5] |
 *  | m[6] m[7] m[8] |
 * @param[in] m 元の行列
 * @return 行列式の値
 */
double CalDetMat3x3(const vector<double> &m)
{
	// a11a22a33+a21a32a13+a31a12a23-a11a32a23-a31a22a13-a21a12a33
	return m[0]*m[4]*m[8]+m[3]*m[7]*m[2]+m[6]*m[1]*m[5]
		  -m[0]*m[7]*m[5]-m[6]*m[4]*m[2]-m[3]*m[1]*m[8];
}

/*!
 * 3x3行列の逆行列の計算
 *  | m[0] m[1] m[2] |
 *  | m[3] m[4] m[5] |
 *  | m[6] m[7] m[8] |
 * @param[in] m 元の行列
 * @param[out] invm 逆行列
 * @return 逆行列の存在
 */
bool CalInvMat3x3(const vector<double> &m, vector<double> &invm)
{
	double det = CalDetMat3x3(m);
	if(fabs(det) < RX_FEQ_EPS){
		return false;
	}
	else{
		double inv_det = 1.0/det;

		invm[0] = inv_det*(m[4]*m[8]-m[5]*m[7]);
		invm[1] = inv_det*(m[2]*m[7]-m[1]*m[8]);
		invm[2] = inv_det*(m[1]*m[5]-m[2]*m[4]);

		invm[3] = inv_det*(m[5]*m[6]-m[3]*m[8]);
		invm[4] = inv_det*(m[0]*m[8]-m[2]*m[6]);
		invm[5] = inv_det*(m[2]*m[3]-m[0]*m[5]);

		invm[6] = inv_det*(m[3]*m[7]-m[4]*m[6]);
		invm[7] = inv_det*(m[1]*m[6]-m[0]*m[7]);
		invm[8] = inv_det*(m[0]*m[4]-m[1]*m[3]);

		return true;
	}
}
}}

*4x4の行列式と逆行列 [#s5a304ab]
#code(C){{
/*!
 * 4x4行列の行列式の計算
 *  | m[0]  m[1]  m[2]  m[3]  |
 *  | m[4]  m[5]  m[6]  m[7]  |
 *  | m[8]  m[9]  m[10] m[11] |
 *  | m[12] m[13] m[14] m[15] |
 * @param[in] m 元の行列
 * @return 行列式の値
 */
inline double CalDetMat4x4(const vector<double> &m)
{
	return m[0]*m[5]*m[10]*m[15]+m[0]*m[6]*m[11]*m[13]+m[0]*m[7]*m[9]*m[14]
		  +m[1]*m[4]*m[11]*m[14]+m[1]*m[6]*m[8]*m[15]+m[1]*m[7]*m[10]*m[12]
		  +m[2]*m[4]*m[9]*m[15]+m[2]*m[5]*m[11]*m[12]+m[2]*m[7]*m[8]*m[13]
		  +m[3]*m[4]*m[10]*m[13]+m[3]*m[5]*m[8]*m[14]+m[3]*m[6]*m[9]*m[12]
		  -m[0]*m[5]*m[11]*m[14]-m[0]*m[6]*m[9]*m[15]-m[0]*m[7]*m[10]*m[13]
		  -m[1]*m[4]*m[10]*m[15]-m[1]*m[6]*m[11]*m[12]-m[1]*m[7]*m[8]*m[14]
		  -m[2]*m[4]*m[11]*m[13]-m[2]*m[5]*m[8]*m[15]-m[2]*m[7]*m[9]*m[12]
		  -m[3]*m[4]*m[9]*m[14]-m[3]*m[5]*m[10]*m[12]-m[3]*m[6]*m[8]*m[13];
}

/*!
 * 4x4行列の行列式の計算
 *  | m[0]  m[1]  m[2]  m[3]  |
 *  | m[4]  m[5]  m[6]  m[7]  |
 *  | m[8]  m[9]  m[10] m[11] |
 *  | m[12] m[13] m[14] m[15] |
 * @param[in] m 元の行列
 * @param[out] invm 逆行列
 * @return 逆行列の存在
 */
inline bool CalInvMat4x4(const vector<double> &m, vector<double> &invm)
{
	double det = CalDetMat4x4(m);
	if(fabs(det) < RX_FEQ_EPS){
		return false;
	}
	else{
		double inv_det = 1.0/det;

		invm[0]  = inv_det*(m[5]*m[10]*m[15]+m[6]*m[11]*m[13]+m[7]*m[9]*m[14]-m[5]*m[11]*m[14]-m[6]*m[9]*m[15]-m[7]*m[10]*m[13]);
		invm[1]  = inv_det*(m[1]*m[11]*m[14]+m[2]*m[9]*m[15]+m[3]*m[10]*m[13]-m[1]*m[10]*m[15]-m[2]*m[11]*m[13]-m[3]*m[9]*m[14]);
		invm[2]  = inv_det*(m[1]*m[6]*m[15]+m[2]*m[7]*m[13]+m[3]*m[5]*m[14]-m[1]*m[7]*m[14]-m[2]*m[5]*m[15]-m[3]*m[6]*m[13]);
		invm[3]  = inv_det*(m[1]*m[7]*m[10]+m[2]*m[5]*m[11]+m[3]*m[6]*m[9]-m[1]*m[6]*m[11]-m[2]*m[7]*m[9]-m[3]*m[5]*m[10]);

		invm[4]  = inv_det*(m[4]*m[11]*m[14]+m[6]*m[8]*m[15]+m[7]*m[10]*m[12]-m[4]*m[10]*m[15]-m[6]*m[11]*m[12]-m[7]*m[8]*m[14]);
		invm[5]  = inv_det*(m[0]*m[10]*m[15]+m[2]*m[11]*m[12]+m[3]*m[8]*m[14]-m[0]*m[11]*m[14]-m[2]*m[8]*m[15]-m[3]*m[10]*m[12]);
		invm[6]  = inv_det*(m[0]*m[7]*m[14]+m[2]*m[4]*m[15]+m[3]*m[6]*m[12]-m[0]*m[6]*m[15]-m[2]*m[7]*m[12]-m[3]*m[4]*m[14]);
		invm[7]  = inv_det*(m[0]*m[6]*m[11]+m[2]*m[7]*m[8]+m[3]*m[4]*m[10]-m[0]*m[7]*m[10]-m[2]*m[4]*m[11]-m[3]*m[6]*m[8]);

		invm[8]  = inv_det*(m[4]*m[9]*m[15]+m[5]*m[11]*m[12]+m[7]*m[8]*m[13]-m[4]*m[11]*m[13]-m[5]*m[8]*m[15]-m[7]*m[9]*m[12]);
		invm[9]  = inv_det*(m[0]*m[11]*m[13]+m[1]*m[8]*m[15]+m[3]*m[9]*m[12]-m[0]*m[9]*m[15]-m[1]*m[11]*m[12]-m[3]*m[8]*m[13]);
		invm[10] = inv_det*(m[0]*m[5]*m[15]+m[1]*m[7]*m[12]+m[3]*m[4]*m[13]-m[0]*m[7]*m[13]-m[1]*m[4]*m[15]-m[3]*m[5]*m[12]);
		invm[11] = inv_det*(m[0]*m[7]*m[9]+m[1]*m[4]*m[11]+m[3]*m[5]*m[8]-m[0]*m[5]*m[11]-m[1]*m[7]*m[8]-m[3]*m[4]*m[9]);

		invm[12] = inv_det*(m[4]*m[10]*m[13]+m[5]*m[8]*m[14]+m[6]*m[9]*m[12]-m[4]*m[9]*m[14]-m[5]*m[10]*m[12]-m[6]*m[8]*m[13]);
		invm[13] = inv_det*(m[0]*m[9]*m[14]+m[1]*m[10]*m[12]+m[2]*m[8]*m[13]-m[0]*m[10]*m[13]-m[1]*m[8]*m[14]-m[2]*m[9]*m[12]);
		invm[14] = inv_det*(m[0]*m[6]*m[13]+m[1]*m[4]*m[14]+m[2]*m[5]*m[12]-m[0]*m[5]*m[14]-m[1]*m[6]*m[12]-m[2]*m[4]*m[13]);
		invm[15] = inv_det*(m[0]*m[5]*m[10]+m[1]*m[6]*m[8]+m[2]*m[4]*m[9]-m[0]*m[6]*m[9]-m[1]*m[4]*m[10]-m[2]*m[5]*m[8]);

		return true;
	}
}
}}
4x4までの逆行列の直接解法 の変更点

4x4までの逆行列の直接解法の変更点
