//------ 演算 -------
/**
* @brief 受け取った２つの行列の掛け算を行う(指定の行列 m2 を右から掛け、dstに設定する)
* @param b EMatrix44をセットする
* @param *dst 変換行列を受け取るポインタをセットする
*/
public void mult(LDMatrix44 b, LDMatrix44 dst, ld_bool omit4thRow = false)
{
//Debug.Assert(this != dst);
if (this == dst)
{
mult_safe(this.m, b.m, dst.m, omit4thRow);
}
else
{
mult_fast(this.m, b.m, dst.m, omit4thRow);
}
}
/**
* @brief 安全な乗算. a , b と dst が同じでも良い安全な乗算(ld_float[16]を別に用意するため負荷大)
* @param *a 行列aをセットする
* @param *b 行列bをセットする
* @param *dst 変換行列を受け取るポインタをセットする
*/
public static void mult_safe(ld_float[] a, ld_float[] b, ld_float[] dst, ld_bool omit4thRow = false)
{
ld_float[] tmp = new ld_float[16];
if (a == dst)
{
mult_fast(a, b, tmp, omit4thRow);
for (int i = 15; i >= 0; --i)
{
dst[i] = tmp[i];
}
}
else
{
mult_fast(a, b, dst, omit4thRow);
}
}
/**
* @brief 高速な乗算. a , b と dst が同じ場合は不正な計算になる
* @param *a 行列aをセットする
* @param *b 行列bをセットする
* @param *dst 乗算した行列を受け取るポインタをセットする
*/
public static void mult_fast(ld_float[] a, ld_float[] b, ld_float[] dst, ld_bool omit4thRow = false)
{
// 透視変換を除けば通常4行は 0,0,0,1 であるため計算を省略できる場合が多い
if (omit4thRow)
{
dst[0] = a[0] * b[0] + a[4] * b[1] + a[8] * b[2];
dst[4] = a[0] * b[4] + a[4] * b[5] + a[8] * b[6];
dst[8] = a[0] * b[8] + a[4] * b[9] + a[8] * b[10];
dst[12] = a[0] * b[12] + a[4] * b[13] + a[8] * b[14] + a[12];
dst[1] = a[1] * b[0] + a[5] * b[1] + a[9] * b[2];
dst[5] = a[1] * b[4] + a[5] * b[5] + a[9] * b[6];
dst[9] = a[1] * b[8] + a[5] * b[9] + a[9] * b[10];
dst[13] = a[1] * b[12] + a[5] * b[13] + a[9] * b[14] + a[13];
dst[2] = a[2] * b[0] + a[6] * b[1] + a[10] * b[2];
dst[6] = a[2] * b[4] + a[6] * b[5] + a[10] * b[6];
dst[10] = a[2] * b[8] + a[6] * b[9] + a[10] * b[10];
dst[14] = a[2] * b[12] + a[6] * b[13] + a[10] * b[14] + a[14];
dst[3] = dst[7] = dst[11] = 0;
dst[15] = 1;
}
else
{
dst[0] = a[0] * b[0] + a[4] * b[1] + a[8] * b[2] + a[12] * b[3];
dst[4] = a[0] * b[4] + a[4] * b[5] + a[8] * b[6] + a[12] * b[7];
dst[8] = a[0] * b[8] + a[4] * b[9] + a[8] * b[10] + a[12] * b[11];
dst[12] = a[0] * b[12] + a[4] * b[13] + a[8] * b[14] + a[12] * b[15];
dst[1] = a[1] * b[0] + a[5] * b[1] + a[9] * b[2] + a[13] * b[3];
dst[5] = a[1] * b[4] + a[5] * b[5] + a[9] * b[6] + a[13] * b[7];
dst[9] = a[1] * b[8] + a[5] * b[9] + a[9] * b[10] + a[13] * b[11];
dst[13] = a[1] * b[12] + a[5] * b[13] + a[9] * b[14] + a[13] * b[15];
dst[2] = a[2] * b[0] + a[6] * b[1] + a[10] * b[2] + a[14] * b[3];
dst[6] = a[2] * b[4] + a[6] * b[5] + a[10] * b[6] + a[14] * b[7];
dst[10] = a[2] * b[8] + a[6] * b[9] + a[10] * b[10] + a[14] * b[11];
dst[14] = a[2] * b[12] + a[6] * b[13] + a[10] * b[14] + a[14] * b[15];
dst[3] = a[3] * b[0] + a[7] * b[1] + a[11] * b[2] + a[15] * b[3];
dst[7] = a[3] * b[4] + a[7] * b[5] + a[11] * b[6] + a[15] * b[7];
dst[11] = a[3] * b[8] + a[7] * b[9] + a[11] * b[10] + a[15] * b[11];
dst[15] = a[3] * b[12] + a[7] * b[13] + a[11] * b[14] + a[15] * b[15];
}
}
//------ Debug ------
/// <summary>
/// 未実装
/// </summary>
/// <param name="title"></param>
/// <param name="newline"></param>
/// <param name="keta"></param>
public void dump(string title = "", ld_bool newline = true, ld_int32 keta = 3)
{
if (keta == 3)
{
Console.WriteLine(title + "{" + x + " ," + y + " ," + z + " }(len=" + length() + ")" + (newline ? "\n" : ""));
}
else
{
if (keta < 0)
{
keta = 0;
}
else if (keta > 5)
{
keta = 5;
}
string dst = new string(' ', 32);//絶対超えないサイズにする
//sprintf(dst, "%%s{%%7.%df,%%7.%df,%%7.%df %7.3f }%s", keta, keta, keta, length(), (newline ? "\n" : ""));
Console.WriteLine(dst, title, x, y, z);
}
}
//------ Debug ------
public void dump(string title = "", ld_bool newline = true, ld_int32 keta = 3)
{
throw new NotImplementedException();
if (keta == 3)
{
//printf("%s{%7.3f ,%7.3f ,%7.3f ,%7.3f }(len=%7.3f)%s", title, w, x, y, z, length(), (newline ? "\n" : ""));
}
else
{
if (keta < 0)
{
keta = 0;
}
else if (keta > 5)
{
keta = 5;
}
//char dst[32];//絶対超えないサイズにする
//sprintf(dst, "%%s{%%7.%df,%%7.%df,%%7.%df,%%7.%df %7.3f }%s", keta, keta, keta, keta, length(), (newline ? "\n" : ""));
//printf(dst, title, w, x, y, z);
}
}
/**
* @brief 高速な乗算. a , b と dst が同じ場合は不正な計算になる
* @param *a 行列aをセットする
* @param *b 行列bをセットする
* @param *dst 乗算した行列を受け取るポインタをセットする
*/
public static void mult_fast(ld_float[] a, ld_float[] b, ld_float[] dst, ld_bool omit4thRow = false)
{
// 透視変換を除けば通常4行は 0,0,0,1 であるため計算を省略できる場合が多い
if (omit4thRow)
{
dst[0] = a[0] * b[0] + a[4] * b[1] + a[8] * b[2];
dst[4] = a[0] * b[4] + a[4] * b[5] + a[8] * b[6];
dst[8] = a[0] * b[8] + a[4] * b[9] + a[8] * b[10];
dst[12] = a[0] * b[12] + a[4] * b[13] + a[8] * b[14] + a[12];
dst[1] = a[1] * b[0] + a[5] * b[1] + a[9] * b[2];
dst[5] = a[1] * b[4] + a[5] * b[5] + a[9] * b[6];
dst[9] = a[1] * b[8] + a[5] * b[9] + a[9] * b[10];
dst[13] = a[1] * b[12] + a[5] * b[13] + a[9] * b[14] + a[13];
dst[2] = a[2] * b[0] + a[6] * b[1] + a[10] * b[2];
dst[6] = a[2] * b[4] + a[6] * b[5] + a[10] * b[6];
dst[10] = a[2] * b[8] + a[6] * b[9] + a[10] * b[10];
dst[14] = a[2] * b[12] + a[6] * b[13] + a[10] * b[14] + a[14];
dst[3] = dst[7] = dst[11] = 0;
dst[15] = 1;
}
else
{
dst[0] = a[0] * b[0] + a[4] * b[1] + a[8] * b[2] + a[12] * b[3];
dst[4] = a[0] * b[4] + a[4] * b[5] + a[8] * b[6] + a[12] * b[7];
dst[8] = a[0] * b[8] + a[4] * b[9] + a[8] * b[10] + a[12] * b[11];
dst[12] = a[0] * b[12] + a[4] * b[13] + a[8] * b[14] + a[12] * b[15];
dst[1] = a[1] * b[0] + a[5] * b[1] + a[9] * b[2] + a[13] * b[3];
dst[5] = a[1] * b[4] + a[5] * b[5] + a[9] * b[6] + a[13] * b[7];
dst[9] = a[1] * b[8] + a[5] * b[9] + a[9] * b[10] + a[13] * b[11];
dst[13] = a[1] * b[12] + a[5] * b[13] + a[9] * b[14] + a[13] * b[15];
dst[2] = a[2] * b[0] + a[6] * b[1] + a[10] * b[2] + a[14] * b[3];
dst[6] = a[2] * b[4] + a[6] * b[5] + a[10] * b[6] + a[14] * b[7];
dst[10] = a[2] * b[8] + a[6] * b[9] + a[10] * b[10] + a[14] * b[11];
dst[14] = a[2] * b[12] + a[6] * b[13] + a[10] * b[14] + a[14] * b[15];
dst[3] = a[3] * b[0] + a[7] * b[1] + a[11] * b[2] + a[15] * b[3];
dst[7] = a[3] * b[4] + a[7] * b[5] + a[11] * b[6] + a[15] * b[7];
dst[11] = a[3] * b[8] + a[7] * b[9] + a[11] * b[10] + a[15] * b[11];
dst[15] = a[3] * b[12] + a[7] * b[13] + a[11] * b[14] + a[15] * b[15];
}
}
/**
* @brief 安全な乗算. a , b と dst が同じでも良い安全な乗算(ld_float[16]を別に用意するため負荷大)
* @param *a 行列aをセットする
* @param *b 行列bをセットする
* @param *dst 変換行列を受け取るポインタをセットする
*/
public static void mult_safe(ld_float[] a, ld_float[] b, ld_float[] dst, ld_bool omit4thRow = false)
{
ld_float[] tmp = new ld_float[16];
if (a == dst)
{
mult_fast(a, b, tmp, omit4thRow);
for (int i = 15; i >= 0; --i)
{
dst[i] = tmp[i];
}
}
else
{
mult_fast(a, b, dst, omit4thRow);
}
}
//------ 演算 -------
/**
* @brief 受け取った２つの行列の掛け算を行う(指定の行列 m2 を右から掛け、dstに設定する)
* @param b EMatrix44をセットする
* @param *dst 変換行列を受け取るポインタをセットする
*/
public void mult(LDMatrix44 b, LDMatrix44 dst, ld_bool omit4thRow = false)
{
Debug.Assert(this != dst);
if (this == dst)
{
mult_safe(this.m, b.m, dst.m, omit4thRow);
}
else
{
mult_fast(this.m, b.m, dst.m, omit4thRow);
}
}
