ぬの部屋(仮)
nu-no-he-ya
  •    1234
    567891011
    12131415161718
    19202122232425
    262728293031 
           
     123456
    78910111213
    14151617181920
    21222324252627
    282930    
           
         12
    3456789
    10111213141516
    17181920212223
    24252627282930
    31      
      12345
    6789101112
    13141516171819
    20212223242526
    2728293031  
           
    1234567
    891011121314
    15161718192021
    22232425262728
    2930     
           
        123
    45678910
    11121314151617
    18192021222324
    25262728293031
           
      12345
    6789101112
    13141516171819
    20212223242526
    27282930   
           
          1
    2345678
    9101112131415
    16171819202122
    23242526272829
    3031     
         12
    3456789
    10111213141516
    17181920212223
    242526272829 
           
      12345
    6789101112
    13141516171819
    20212223242526
    2728293031  
           
          1
    2345678
    9101112131415
    16171819202122
    23242526272829
    3031     
        123
    45678910
    11121314151617
    18192021222324
    252627282930 
           
     123456
    78910111213
    14151617181920
    21222324252627
    28293031   
           
          1
    2345678
    9101112131415
    16171819202122
    23242526272829
    30      
       1234
    567891011
    12131415161718
    19202122232425
    262728293031 
           
    1234567
    891011121314
    15161718192021
    22232425262728
    293031    
           
         12
    3456789
    10111213141516
    17181920212223
    24252627282930
           
      12345
    6789101112
    13141516171819
    20212223242526
    2728293031  
           
    1234567
    891011121314
    15161718192021
    22232425262728
    2930     
           
        123
    45678910
    11121314151617
    18192021222324
    25262728293031
           
        123
    45678910
    11121314151617
    18192021222324
    25262728   
           
     123456
    78910111213
    14151617181920
    21222324252627
    28293031   
           
         12
    3456789
    10111213141516
    17181920212223
    24252627282930
    31      
       1234
    567891011
    12131415161718
    19202122232425
    2627282930  
           
    1234567
    15161718192021
    293031    
           
         12
    3456789
    10111213141516
           
      12345
    6789101112
    13141516171819
    20212223242526
    2728293031  
           
          1
    2345678
    9101112131415
    16171819202122
    23242526272829
    3031     
        123
    45678910
    11121314151617
    18192021222324
    252627282930 
           
     123456
    78910111213
    14151617181920
    21222324252627
    28293031   
           
          1
    2345678
    9101112131415
    16171819202122
    23242526272829
    30      
       1234
    567891011
    12131415161718
    19202122232425
    262728293031 
           
    1234567
    891011121314
    15161718192021
    22232425262728
    293031    
           
        123
    45678910
    11121314151617
    18192021222324
    25262728293031
           
      12345
    6789101112
    13141516171819
    20212223242526
    27282930   
           
        123
    45678910
    11121314151617
    18192021222324
    252627282930 
           
     123456
    78910111213
    14151617181920
    21222324252627
    28293031   
           
       1234
    567891011
    12131415161718
    19202122232425
    2627282930  
           
    1234567
    891011121314
    15161718192021
    22232425262728
    293031    
           
         12
    3456789
    10111213141516
    17181920212223
    24252627282930
           
      12345
    6789101112
    13141516171819
    20212223242526
    2728293031  
           
      12345
    6789101112
    13141516171819
    20212223242526
    2728     
           
          1
    2345678
    9101112131415
    16171819202122
    23242526272829
    3031     
     123456
    78910111213
    14151617181920
    21222324252627
    282930    
           
         12
    3456789
    10111213141516
    17181920212223
    24252627282930
    31      
    1234567
    891011121314
    15161718192021
    22232425262728
    293031    
           
        123
    45678910
    11121314151617
    18192021222324
    252627282930 
           
     123456
    78910111213
    14151617181920
    21222324252627
    28293031   
           
     123456
    78910111213
    14151617181920
    21222324252627
    28293031   
           
       1234
    567891011
    12131415161718
    19202122232425
    262728293031 
           
     123456
    78910111213
    14151617181920
    21222324252627
    282930    
           
         12
    3456789
    10111213141516
    17181920212223
    24252627282930
    31      
      12345
    6789101112
    13141516171819
    20212223242526
    2728293031  
           
    1234567
    891011121314
    15161718192021
    22232425262728
    2930     
           
        123
    45678910
    11121314151617
    18192021222324
    25262728293031
           
      12345
    6789101112
    13141516171819
    20212223242526
    27282930   
           
          1
    2345678
    9101112131415
    16171819202122
    23242526272829
    3031     
          1
    2345678
    9101112131415
    16171819202122
    232425262728 
           
       1234
    567891011
    12131415161718
    19202122232425
    262728293031 
           
    1234567
    891011121314
    15161718192021
    22232425262728
    293031    
           
         12
    3456789
    10111213141516
    17181920212223
    24252627282930
           
      12345
    6789101112
    13141516171819
    20212223242526
    2728293031  
           
    1234567
    891011121314
    15161718192021
    22232425262728
    2930     
           
        123
    45678910
    11121314151617
    18192021222324
    25262728293031
           
  • 4×4の逆行列を求める関数

    3DCGには欠かせない4×4行列の逆行列を求める関数を自前実装する。

    動作チェックはEigenとの差を見て判断する。

    逆行列を求める関数

    namespace matcalc {
    
      //誤差の定義
      template<typename T>
      struct real_error {static constexpr float value = (float)1e-7;};
      template<>
      struct real_error<double> { static constexpr double value = 1e-15; };
    
      //! @brief 誤差の閾値未満かをチェック
      //! @param [in] value 検査する値
      //! @retval true 値の絶対値は閾値より小さい
      //! @retval false 値の絶対値は閾値より大きい
      template<typename T>
      inline bool is_error(const T value) {
        return
          abs(value) < real_error<T>::value;
      }
    
          
      //! @brief 4x4行列の行列式を取得
      //! @param [in] dm16 4x4行列
      //! @return 行列式
      template<typename real_t>
      inline auto determinant44(const real_t* dm16) {
    
        using scalar_t = decltype(dm16[0]);
    
        const scalar_t a11 = dm16[0];
        const scalar_t a12 = dm16[1];
        const scalar_t a13 = dm16[2];
        const scalar_t a14 = dm16[3];
    
        const scalar_t a21 = dm16[4];
        const scalar_t a22 = dm16[5];
        const scalar_t a23 = dm16[6];
        const scalar_t a24 = dm16[7];
    
        const scalar_t a31 = dm16[8];
        const scalar_t a32 = dm16[9];
        const scalar_t a33 = dm16[10];
        const scalar_t a34 = dm16[11];
    
        const scalar_t a41 = dm16[12];
        const scalar_t a42 = dm16[13];
        const scalar_t a43 = dm16[14];
        const scalar_t a44 = dm16[15];
    
    
        return a11 * a22* a33* a44 + a11 * a23 * a34 * a42 + a11 * a24 * a32 * a43
          + a12 * a21 * a34 * a43 + a12 * a23 * a31 * a44 + a12 * a24 * a33 * a41
          + a13 * a21 * a32 * a44 + a13 * a22 * a34 * a41 + a13 * a24 * a31 * a42
          + a14 * a21 * a33 * a42 + a14 * a22 * a31 * a43 + a14 * a23 * a32 * a41
          - a11 * a22 * a34 * a43 - a11 * a23 * a32 * a44 - a11 * a24 * a33 * a42
          - a12 * a21 * a33 * a44 - a12 * a23 * a34 * a41 - a12 * a24 * a31 * a43
          - a13 * a21 * a34 * a42 - a13 * a22 * a31 * a44 - a13 * a24 * a32 * a41
          - a14 * a21 * a32 * a43 - a14 * a22 * a33 * a41 - a14 * a23 * a31 * a42;
      }
    
          
      //! @brief 4x4行列の逆行列を取得
      //! @param [out] dst_dm16 結果を格納する一次元配列
      //! @param [in] src_dm16 元の行列
      //! @retval true 成功した
      //! @retval false 行列式の大きさが小さすぎて失敗した
      template<typename real_t>
      inline bool inverse_matrix44(real_t* dst_dm16, const real_t* src_dm16) {
    
        using s_scalar_t = decltype(src_dm16[0]);
        using d_scalar_t = decltype(dst_dm16[0]);
    
        real_t a11 = src_dm16[0];
        real_t a12 = src_dm16[1];
        real_t a13 = src_dm16[2];
        real_t a14 = src_dm16[3];
    
        real_t a21 = src_dm16[4];
        real_t a22 = src_dm16[5];
        real_t a23 = src_dm16[6];
        real_t a24 = src_dm16[7];
    
        real_t a31 = src_dm16[8];
        real_t a32 = src_dm16[9];
        real_t a33 = src_dm16[10];
        real_t a34 = src_dm16[11];
    
        real_t a41 = src_dm16[12];
        real_t a42 = src_dm16[13];
        real_t a43 = src_dm16[14];
        real_t a44 = src_dm16[15];
    
        real_t& b11 = dst_dm16[0];
        real_t& b12 = dst_dm16[1];
        real_t& b13 = dst_dm16[2];
        real_t& b14 = dst_dm16[3];
        real_t& b21 = dst_dm16[4];
        real_t& b22 = dst_dm16[5];
    
        real_t& b23 = dst_dm16[6];
        real_t& b24 = dst_dm16[7];
        real_t& b31 = dst_dm16[8];
        real_t& b32 = dst_dm16[9];
        real_t& b33 = dst_dm16[10];
        real_t& b34 = dst_dm16[11];
    
        real_t& b41 = dst_dm16[12];
        real_t& b42 = dst_dm16[13];
        real_t& b43 = dst_dm16[14];
        real_t& b44 = dst_dm16[15];
    
        b11 = a22 * a33 * a44 + a23 * a34 * a42 + a24 * a32 * a43 - a22 * a34 * a43 - a23 * a32 * a44 - a24 * a33 * a42;
        b12 = a12 * a34 * a43 + a13 * a32 * a44 + a14 * a33 * a42 - a12 * a33 * a44 - a13 * a34 * a42 - a14 * a32 * a43;
        b13 = a12 * a23 * a44 + a13 * a24 * a42 + a14 * a22 * a43 - a12 * a24 * a43 - a13 * a22 * a44 - a14 * a23 * a42;
        b14 = a12 * a24 * a33 + a13 * a22 * a34 + a14 * a23 * a32 - a12 * a23 * a34 - a13 * a24 * a32 - a14 * a22 * a33;
    
        b21 = a21 * a34 * a43 + a23 * a31 * a44 + a24 * a33 * a41 - a21 * a33 * a44 - a23 * a34 * a41 - a24 * a31 * a43;
        b22 = a11 * a33 * a44 + a13 * a34 * a41 + a14 * a31 * a43 - a11 * a34 * a43 - a13 * a31 * a44 - a14 * a33 * a41;
        b23 = a11 * a24 * a43 + a13 * a21 * a44 + a14 * a23 * a41 - a11 * a23 * a44 - a13 * a24 * a41 - a14 * a21 * a43;
        b24 = a11 * a23 * a34 + a13 * a24 * a31 + a14 * a21 * a33 - a11 * a24 * a33 - a13 * a21 * a34 - a14 * a23 * a31;
        b31 = a21 * a32 * a44 + a22 * a34 * a41 + a24 * a31 * a42 - a21 * a34 * a42 - a22 * a31 * a44 - a24 * a32 * a41;
        b32 = a11 * a34 * a42 + a12 * a31 * a44 + a14 * a32 * a41 - a11 * a32 * a44 - a12 * a34 * a41 - a14 * a31 * a42;
        b33 = a11 * a22 * a44 + a12 * a24 * a41 + a14 * a21 * a42 - a11 * a24 * a42 - a12 * a21 * a44 - a14 * a22 * a41;
        b34 = a11 * a24 * a32 + a12 * a21 * a34 + a14 * a22 * a31 - a11 * a22 * a34 - a12 * a24 * a31 - a14 * a21 * a32;
        b41 = a21 * a33 * a42 + a22 * a31 * a43 + a23 * a32 * a41 - a21 * a32 * a43 - a22 * a33 * a41 - a23 * a31 * a42;
        b42 = a11 * a32 * a43 + a12 * a33 * a41 + a13 * a31 * a42 - a11 * a33 * a42 - a12 * a31 * a43 - a13 * a32 * a41;
        b43 = a11 * a23 * a42 + a12 * a21 * a43 + a13 * a22 * a41 - a11 * a22 * a43 - a12 * a23 * a41 - a13 * a21 * a42;
        b44 = a11 * a22 * a33 + a12 * a23 * a31 + a13 * a21 * a32 - a11 * a23 * a32 - a12 * a21 * a33 - a13 * a22 * a31;
    
        auto detA = determinant44(src_dm16);
    
        if (is_error(detA) == true)
          return false;
    
        detA = static_cast<decltype(detA)>(1.0) / detA;
    
        for (int i = 0; i < 16; i++) {
          dst_dm16[i] *= detA;
        }
        return true;
      }
    }
    

    動作チェック

    #include<Eigen/Dense>
    #include "inversematrix.hpp" //自前実装版
    
    void inversematrix_check() {
    
      /////////////////////////////////////////////
      // 元データ
      double base[16];
      for (size_t i = 0; i < 16; i++) {
        base[i] = rand() % 1000 / 1000.0;
      }
    
    
      /////////////////////////////////////////////
      // Eigenで求める
      Eigen::MatrixXd eg(4, 4);
      eg <<
        base[0], base[1], base[2], base[3],
        base[4], base[5], base[6], base[7],
        base[8], base[9], base[10], base[11],
        base[12], base[13], base[14], base[15];
    
      auto inv = eg.inverse();
    
      std::cout << "eigen----------" << std::endl;
      std::cout << inv << std::endl;
      std::cout << "----------" << std::endl;
    
          
      /////////////////////////////////////////////
      //自前実装版
    
      double* A = new double[4 * 4]{
        base[0], base[1], base[2], base[3] ,
        base[4], base[5], base[6], base[7] ,
        base[8], base[9], base[10], base[11] ,
        base[12], base[13], base[14], base[15]
      };
    
      double* B = new double[4 * 4];
    
      matcalc::inverse_matrix44(B, A);
    
    #pragma warning(push)
    #pragma warning(disable:4996)
    
      std::cout << "自前----------" << std::endl;
    
      for (int i = 0; i < 4; i++) {
        for (int j = 0; j < 4; j++) {
          printf("%0.5lf", B[i*4+ j]);
          printf(" ");
        }
        puts("");
      }
      std::cout << "差--------" << std::endl;
      std::cout << "----------" << std::endl;
      for (int i = 0; i < 4; i++) {
        for (int j = 0; j < 4; j++) {
          printf("%+0.5lf", B[i * 4 + j]-inv(i,j));
          printf(" ");
        }
        puts("");
      }
      std::cout << std::endl << std::endl << std::endl;
    
    
    #pragma warning(pop)
    
    
    }
    

    テスト結果