/// <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);
}