/// <summary> /// 4方向ベクトルと4x4行列の積 /// </summary> /// <param name="dv">出力先ベクトル</param> /// <param name="v">計算用ベクトル</param> /// <param name="m">計算用行列</param> public static void MatApply4(CVector4 dv, CVector4 v, CMatrix4 m) { dv.x = v.x * m.m[0, 0] + v.y * m.m[1, 0] + v.z * m.m[2, 0] + v.w * m.m[3, 0]; dv.y = v.x * m.m[0, 1] + v.y * m.m[1, 1] + v.z * m.m[2, 1] + v.w * m.m[3, 1]; dv.z = v.x * m.m[0, 2] + v.y * m.m[1, 2] + v.z * m.m[2, 2] + v.w * m.m[3, 2]; dv.z = v.x * m.m[0, 3] + v.y * m.m[1, 3] + v.z * m.m[2, 3] + v.w * m.m[3, 3]; }
public CMatrix4(CMatrix4 mat) { m[0, 0] = mat.m[0, 0]; m[0, 1] = mat.m[0, 1]; m[0, 2] = mat.m[0, 2]; m[0, 3] = mat.m[0, 3]; m[1, 0] = mat.m[1, 0]; m[1, 1] = mat.m[1, 1]; m[1, 2] = mat.m[1, 2]; m[1, 3] = mat.m[1, 3]; m[2, 0] = mat.m[2, 0]; m[2, 1] = mat.m[2, 1]; m[2, 2] = mat.m[2, 2]; m[2, 3] = mat.m[2, 3]; m[3, 0] = mat.m[3, 0]; m[3, 1] = mat.m[3, 1]; m[3, 2] = mat.m[3, 2]; m[3, 3] = mat.m[3, 3]; }
/// <summary> /// 転置行列の取得 /// </summary> /// <returns>転置行列</returns> public CMatrix4 Transpose() { CMatrix4 tm = new CMatrix4(); for (int i = 0; i < 4; i++) { for (int j = 0; j < 4; j++) { tm.m[i, j] = m[j, i]; } } return(tm); }
public static CMatrix4 operator *(CMatrix4 m1, CMatrix4 m2) { CMatrix4 tm = new CMatrix4(); for (int i = 0; i < 4; i++) { for (int j = 0; j < 4; j++) { tm.m[i, j] = m1.m[i, 0] * m2.m[0, j] + m1.m[i, 1] * m2.m[1, j] + m1.m[i, 2] * m2.m[2, j] + m1.m[i, 3] * m2.m[3, j]; } } return(tm); }
/// <summary> /// 逆行列の取得 /// 備考:正則行列であることが前提 /// </summary> /// <returns>逆行列</returns> public CMatrix4 InversTrans() { CMatrix4 tm = this.Transpose(); tm.m[0, 3] = -(m[0, 3] * tm.m[0, 0] + m[1, 3] * tm.m[0, 1] + m[2, 3] * tm.m[0, 2]); tm.m[1, 3] = -(m[0, 3] * tm.m[1, 0] + m[1, 3] * tm.m[1, 1] + m[2, 3] * tm.m[1, 2]); tm.m[2, 3] = -(m[0, 3] * tm.m[2, 0] + m[1, 3] * tm.m[2, 1] + m[2, 3] * tm.m[2, 2]); #if CLEAR_TRANS tm.m[3, 0] = tm.m[3, 1] = tm.m[3, 2] = 0.0f; #else tm.m[3, 0] = -tm.m[3, 0]; tm.m[3, 1] = -tm.m[3, 1]; tm.m[3, 2] = -tm.m[3, 2]; #endif tm.m[3, 3] = 1.0f; return(tm); }
// 評価 public override bool Equals(object obj) { if (obj == null) { return(false); } CMatrix4 tm = obj as CMatrix4; if ((object)tm == null) { return(false); } return((m[0, 0] == tm.m[0, 0]) && (m[0, 1] == tm.m[0, 1]) && (m[0, 2] == tm.m[0, 2]) && (m[0, 3] == tm.m[0, 3]) && (m[1, 0] == tm.m[1, 0]) && (m[1, 1] == tm.m[1, 1]) && (m[1, 2] == tm.m[1, 2]) && (m[1, 3] == tm.m[1, 3]) && (m[2, 0] == tm.m[2, 0]) && (m[2, 1] == tm.m[2, 1]) && (m[2, 2] == tm.m[2, 2]) && (m[2, 3] == tm.m[2, 3]) && (m[3, 0] == tm.m[3, 0]) && (m[3, 1] == tm.m[3, 1]) && (m[3, 2] == tm.m[3, 2]) && (m[3, 3] == tm.m[3, 3])); }
/// <summary> /// 3方向ベクトルと4x4行列の積 /// 備考:4行目の行列には1をかけます /// </summary> /// <param name="dv">出力先ベクトル</param> /// <param name="v">計算用ベクトル</param> /// <param name="m">計算用行列</param> public static void MatApply3(CVector3 dv, CVector3 v, CMatrix4 m) { dv.x = v.x * m.m[0, 0] + v.y * m.m[1, 0] + v.z * m.m[2, 0] + 1.0f * m.m[3, 0]; dv.y = v.x * m.m[0, 1] + v.y * m.m[1, 1] + v.z * m.m[2, 1] + 1.0f * m.m[3, 1]; dv.z = v.x * m.m[0, 2] + v.y * m.m[1, 2] + v.z * m.m[2, 2] + 1.0f * m.m[3, 2]; }
/// <summary> /// 逆行列の取得 /// </summary> /// <param name="mat">出力先</param> /// <returns>逆行列がない場合は単位行列が代入され、falseが返る</returns> public bool Invers(CMatrix4 mat) { int i, j, row; float tmp; float[,] mat84 = new float[4, 8]; // 8x4行列に4x4行列と単位行列入れる for (i = 0; i < 4; i++) { for (j = 0; j < 4; j++) { mat84[i, j] = m[i, j]; if (i == j) { mat84[i, (j + 4)] = 1.0f; } else { mat84[i, (j + 4)] = 0.0f; } } } for (row = 0; row < 4; row++) { tmp = mat84[row, row]; if (tmp != 1.0f) { if (tmp == 0.0f) { for (i = row + 1; i < 4; i++) { tmp = mat84[i, row]; if (tmp != 0.0f) { break; } } // 全て0なら逆行列なし if (i >= 4) { mat.Identity(); // 単位行列を入れておく(保険) return(false); } // 行を入れ替える for (j = 0; j < 8; j++) { tmp = mat84[i, j]; mat84[i, j] = mat84[row, j]; mat84[row, j] = tmp; } tmp = mat84[row, row]; } for (i = 0; i < 8; i++) { mat84[row, i] /= tmp; } } // mat84[i][row]が1になるよう計算 for (i = 0; i < 4; i++) { if (i != row) { tmp = mat84[i, row]; if (tmp != 0) { for (j = 0; j < 8; j++) { mat84[i, j] -= mat84[row, j] * tmp; } } } } } // 求まった逆行列を4x4行列にコピー for (i = 0; i < 4; i++) { for (j = 0; j < 4; j++) { mat.m[i, j] = mat84[i, (j + 4)]; } } return(true); }