gluLookAtの自前実装

#include <gl/GLU.h>

#pragma comment(lib,"glu32.lib")

#pragma warning(disable:4996)

//! @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;
}

//! @brief 4x4行列の行列の積を計算する
//! @param [out] C 結果の格納先
//! @param [in] A 行列A
//! @param [in] B 行列B
//! @return Cへのポインタ
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);
}

//! @brief 移動行列を作成する
//! @param [out] m 結果の格納先
//! @param [in] x ベクトルのx成分
//! @param [in] y ベクトルのy成分
//! @param [in] z ベクトルのz成分
//! @return なし
template<typename real_t>
void mytranslate(
  real_t* m,
  real_t x,
  real_t y,
  real_t z
) {
  m[0] = 1.0;
  m[1] = 0.0;
  m[2] = 0.0;
  m[3] = 0.0;

  m[4] = 0.0;
  m[5] = 1.0;
  m[6] = 0.0;
  m[7] = 0.0;

  m[8] = 0.0;
  m[9] = 0.0;
  m[10] = 1.0;
  m[11] = 0.0;

  m[12] = x;
  m[13] = y;
  m[14] = z;
  m[15] = 1.0;

}

//! @brief 単位行列を作成する
//! @param [out] m 結果の格納先
//! @return なし
template<typename real_t>
inline void loadidentity3(real_t* m){
  m[0] = 1.0;
  m[1] = 0.0;
  m[2] = 0.0;
  m[3] = 0.0;

  m[4] = 0.0;
  m[5] = 1.0;
  m[6] = 0.0;
  m[7] = 0.0;

  m[8] = 0.0;
  m[9] = 0.0;
  m[10] = 1.0;
  m[11] = 0.0;

  m[12] = 0;
  m[13] = 0;
  m[14] = 0;
  m[15] = 1.0;
}

//! @brief 配列にNANが入っているか確認
//! @param [in] m 評価する配列
//! @param [in] count 行列の要素数
template<typename real_t>
bool chek_array_nan(const real_t* m, const size_t count) {
  for (size_t i = 0; i < count; i++) {
    if (isnan(m[i]) == true)
      return true;
  }
  return false;
}

template<typename real_t>
inline void Outer3(real_t* const dvec, real_t const* const svec1, real_t const* const svec2) {

  const real_t& x1 = svec1[0];
  const real_t& y1 = svec1[1];
  const real_t& z1 = svec1[2];
  const real_t& x2 = svec2[0];
  const real_t& y2 = svec2[1];
  const real_t& z2 = svec2[2];

  dvec[0] = static_cast<real_t>(y1 * z2 - z1 * y2);
  dvec[1] = static_cast<real_t>(z1 * x2 - x1 * z2);
  dvec[2] = static_cast<real_t>(x1 * y2 - y1 * x2);
}

template<typename real_t>
real_t Inner3(real_t const* const vec1, real_t const* const vec2) {
  const int X = 0;
  const int Y = 1;
  const int Z = 2;
  return ((vec1[X]) * (vec2[X]) + (vec1[Y]) * (vec2[Y]) + (vec1[Z]) * (vec2[Z]));
}

template<typename real_t>
real_t Length3(const real_t* const vec) {
  const int X = 0;
  const int Y = 1;
  const int Z = 2;
  return sqrt(vec[X] * vec[X] + vec[Y] * vec[Y] + vec[Z] * vec[Z]);
}

template<typename real_t>
bool Normalize3(real_t * const pV)
{

  const int X = 0;
  const int Y = 1;
  const int Z = 2;

  real_t len;
  real_t& x = pV[X];
  real_t& y = pV[Y];
  real_t& z = pV[Z];

  len = static_cast<real_t>(sqrt(x * x + y * y + z * z));

  if (len < static_cast<real_t>(1e-6)) return false;

  len = static_cast<real_t>(1.0) / len;
  x *= len;
  y *= len;
  z *= len;

  return true;
}

//! @brief gluLookAtの自前実装版
//! @param [out] M 結果の格納先
//! @param [out] eyeX カメラ位置
//! @param [out] eyeY カメラ位置
//! @param [out] eyeZ カメラ位置
//! @param [out] centerX 
//! @param [out] centerY
//! @param [out] centerZ
//! @param [out] upX
//! @param [out] upY
//! @param [out] upZ
//! @return なし
void myglulookat(
  GLdouble* M,
  GLdouble eyeX,
  GLdouble eyeY,
  GLdouble eyeZ,
  GLdouble centerX,
  GLdouble centerY,
  GLdouble centerZ,
  GLdouble upX,
  GLdouble upY,
  GLdouble upZ)
{
  GLdouble f[3] = {
    centerX - eyeX,
    centerY - eyeY,
    centerZ - eyeZ
  };
  Normalize3(f);

  GLdouble UP[3] = {upX,upY,upZ};
  Normalize3(UP);

  GLdouble s[3];
  Outer3(s, f, UP);

  Normalize3(s);
  GLdouble u[3];
  Outer3(u, s, f);

  GLdouble M0[16];
  M0[ 0] = s[0];
  M0[ 1] = u[0];
  M0[ 2] = -f[0];
  M0[ 3] = 0;

  M0[ 4] = s[1];
  M0[ 5] = u[1];
  M0[ 6] = -f[1];
  M0[ 7] = 0;

  M0[ 8] = s[2];
  M0[ 9] = u[2];
  M0[10] = -f[2];
  M0[11] = 0;

  M0[12] = 0;
  M0[13] = 0;
  M0[14] = 0;
  M0[15] = 1;


  /////////////////////////////
  /////////////////////////////
  GLdouble E[16];

  mytranslate(E, -eyeX, -eyeY, -eyeZ);

  mult_matrix44(M, M0, E);

  if (chek_array_nan(M, 16) == true)
    loadidentity3(M);

}

void test() {
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();

  GLdouble ex = (rand() % 1000) / 1000.0 - 0.5;
  GLdouble ey = (rand() % 1000) / 1000.0 - 0.5;
  GLdouble ez = (rand() % 1000) / 1000.0 - 0.5;
  GLdouble cx = (rand() % 1000) / 1000.0 - 0.5;
  GLdouble cy = (rand() % 1000) / 1000.0 - 0.5;
  GLdouble cz = (rand() % 1000) / 1000.0 - 0.5;
  GLdouble ux = (rand() % 1000) / 1000.0 - 0.5;
  GLdouble uy = (rand() % 1000) / 1000.0 - 0.5;
  GLdouble uz = (rand() % 1000) / 1000.0 - 0.5;