//------ 演算 ------- /** * @brief 受け取った2つの行列の掛け算を行う(指定の行列 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 受け取った2つの行列の掛け算を行う(指定の行列 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); } }