template<
  typename real_tC,
  typename real_tA,
  typename real_tB>
real_tC* mult_matrix44(real_tC* C, const real_tA* A, const real_tB* B) {
  return mult_matrixIJK(C, A, B, 4, 4, 4);
}
template<
  typename real_tC,
  typename real_tA,
  typename real_tB>
  real_tC* mult_matrix_vec41(real_tC* C, const real_tA* A, const real_tB* B) {
  return mult_matrixIJK(C, A, B, 4, 4, 1);
}

///////////////////////////////////////
// gluUnProject
// https://www.khronos.org/registry/OpenGL-Refpages/gl2.1/xhtml/gluUnProject.xml
//! @brief gluUnProjectの自前実装版
//! @param [in] winX 画面上のX座標
//! @param [in] winY 画面上のY座標
//! @param [in] winZ 画面上のZ座標
//! @param [in] model モデルビュー行列
//! @param [in] proj プロジェクション行列
//! @param [in] ビューポート x,y,width,height
//! @param [out] objX 空間座標のX
//! @param [out] objY 空間座標のY
//! @param [out] objZ 空間座標のZ
//! @retval true 計算成功
//! @retval false 計算失敗
template<typename real_t>
bool myUnProject(
  real_t winX,
  real_t winY,
  real_t winZ,
  const real_t* model,
  const real_t* proj,
  const int* view,
  real_t* objX,
  real_t* objY,
  real_t* objZ
) {

  real_t pm[16];
  real_t invpm[16];

  mult_matrix44(pm, proj, model);//pm = P×M
  bool ret = inverse_matrix44(invpm, pm);// pmの逆行列

  if (ret == false)
    return false;

  real_t v[4] = {
    2 * (winX - view[0]) / view[2] - 1.0 ,
    2 * (winY - view[1]) / view[3] - 1.0 ,
    2 * winZ - 1.0,
    1
  };

  real_t vd[4];
  mult_matrix_vec41(vd, invpm, v); // vd = invpm × v

  *objX = vd[0] / vd[3];
  *objY = vd[1] / vd[3];
  *objZ = vd[2] / vd[3];

  return true;
}

int WIDTH = 300;
int HEIGHT = 400;
void disp(void) {

  glViewport(0, 0, WIDTH, HEIGHT);


  glMatrixMode(GL_PROJECTION);
  glPushMatrix();
  glLoadIdentity();
  gluPerspective(45, 0.5, 0.1, 2.5);

  glMatrixMode(GL_MODELVIEW);
  glPushMatrix();
  glLoadIdentity();
  glTranslated(-1, 0, -2);
  glRotated(1.5, 1.0, 1.0, 1.0);

  ////作成した変換行列を取得する
  GLdouble  modeld[16];
  glGetDoublev(GL_MODELVIEW_MATRIX, modeld);

  GLdouble  projd[16];
  glGetDoublev(GL_PROJECTION_MATRIX, projd);

  GLint view[4];
  glGetIntegerv(GL_VIEWPORT, view);

  // 三次元座標（出力）
  double
    objx,
    objy,
    objz;

  // 二次元ピクセル座標（入力）
  double 
    winx = (rand() % 1000) / 100.0,
    winy = (rand() % 1000) / 100.0,
    winz = (rand() % 1000) / 100.0;

  gluUnProject(
    winx, winy, winz,
    modeld, projd, view,
    &objx, &objy, &objz);

  printf("glu %lf %lf %lf\n", objx, objy, objz);

  myUnProject(
    winx, winy, winz,
    modeld, projd, view,
    &objx, &objy, &objz
  );

  printf("my  %lf %lf %lf\n", objx, objy, objz);

  puts("-----------");

  glMatrixMode(GL_PROJECTION);
  glPopMatrix();

  glMatrixMode(GL_MODELVIEW);
  glPopMatrix();

}

  //誤差の定義
  template<typename T>struct real_error;
  template<>struct real_error<double> { static constexpr double value = 1e-15; };
  template<>struct real_error<float> { static constexpr float value = (float)1e-7; };
  //! @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 IxJ行列の i,j の要素にアクセスする
  //! @param [in] i 行番号
  //! @param [in] j 列番号
  //! @param [in] I 行数
  inline int matijI(const int i, const int j, const int I) {
    return i + I * j;
  }

  //! @brief IxJ行列 × JxK行列 の乗算関数
  //https://suzulang.com/cpp-multmatrix44/
  //! @param [out] C Cik 計算結果格納先
  //! @param [in] A Aijの行列
  //! @param [in] B Bjkの行列
  //! @param [in] I Aの行列の行数
  //! @param [in] J Aの行列の列数
  //! @param [in] K Bの行列の列数
  //! @return C
  template<
    typename real_tC,
    typename real_tA,
    typename real_tB>
  real_tC* mult_matrixIJK(real_tC* C, const real_tA* A, const real_tB* B, const int I, const int J, const int K) {

    for (int i = 0; i < I; i++)
      for (int k = 0; k < K; k++) {
        C[matijI(i, k, I)] = 0.0;
        for (int j = 0; j < J; j++) {
          C[matijI(i, k, I)] += A[matijI(i, j, I)] * B[matijI(j, k, J)];
        }
      }

    return C;
  }

  template<
    typename real_tC,
    typename real_tA,
    typename real_tB>
  real_tC* mult_matrix_vec41(real_tC* C, const real_tA* A, const real_tB* B) {
    return mult_matrixIJK(C, A, B, 4, 4, 1);
  }

  // gluProject 
  // https://www.khronos.org/registry/OpenGL-Refpages/gl2.1/xhtml/gluProject.xml
  // http://collagefactory.blogspot.com/2010/03/gluproject-source-code.html

  //! @brief gluProjectの自前実装版
  //! @param [in] wldX 空間座標のX
  //! @param [in] wldY 空間座標のY
  //! @param [in] wldZ 空間座標のZ
  //! @param [in] model モデルビュー行列
  //! @param [in] proj プロジェクション行列
  //! @param [in] ビューポート x,y,width,height
  //! @param [out] pixX 画面上のX座標
  //! @param [out] pixY 画面上のY座標
  //! @param [out] pixZ 画面上のZ座標　※計算上は存在する
  //! @retval true 計算に成功
  //! @retval false 計算に失敗
  template<typename real_t>
  inline bool myProject(
    real_t wldX,
    real_t wldY,
    real_t wldZ,
    const real_t* model,
    const real_t* proj,
    const int* view,
    real_t* pixX,
    real_t* pixY,
    real_t* pixZ
  ) {

    real_t world[4] = { wldX,wldY,wldZ,1.0 };
    real_t tmp[4];
    real_t vdd[4];

    mult_matrix_vec41(tmp, model, world); // tmp = M×v
    mult_matrix_vec41(
      vdd, 
      proj, tmp
    ); // v'' = P×tmp
  
    if (is_error(vdd[3]) == true)
      return false;

    vdd[0] /= vdd[3];
    vdd[1] /= vdd[3];
    vdd[2] /= vdd[3]; //v''' = v'' / v''[3]

    *pixX = view[0] + view[2] * (vdd[0] + 1) / 2.0;
    *pixY = view[1] + view[3] * (vdd[1] + 1) / 2.0;
    *pixZ = (vdd[2] + 1) / 2.0;

    return true;

  }

int WIDTH = 300;
int HEIGHT = 400;

void disp(void) {

  glViewport(0, 0, WIDTH, HEIGHT);


  glMatrixMode(GL_PROJECTION);
  glPushMatrix();
  glLoadIdentity();
  gluPerspective(45, 0.5, 0.1, 2.5);

  glMatrixMode(GL_MODELVIEW);
  glPushMatrix();
  glLoadIdentity();
  glTranslated(-1, 0, -2);
  glRotated(1.5, 1.0, 1.0, 1.0);