三次元オブジェクトをマウス回転(glut使用)

#pragma once
#include <windows.h>
#include <GL/gl.h>
#include <GL/freeglut.h>
#include "nurotate.h"
#include "cube.h"
int width, height;
//回転オブジェクト定義
nu::mrotate camr;
void disp(void) {
  glClear(GL_COLOR_BUFFER_BIT);
  glViewport(0, 0, width, height);

  glPushMatrix();

  //原点を視線方向に0.5ずらす
  glTranslated(0, 0, -0.5);

  //回転行列を適用
  double mat[16];
  glMultMatrixd(camr.getmatrix(mat));

  //一辺0.7のキューブを描画
  cube(0.7);

  glPopMatrix();

  glFlush();
}
void reshape(int w, int h) {
  width = w; height = h;

  //クライアント領域のサイズを指定
  //Win32APIの場合はGetClientRectで求めたrightとbottomだがglutではwとh
  camr.setWindowSize(width, height);

  //クライアント領域のどこの範囲に描画しているかを指定。
  //glViewportで一画面丸々描画しているので、クライアント領域の(0,0)-(width,height)に書いていることになる
  camr.setRenderRect(0, 0, width, height);
  disp();
}
//マウスクリック時のイベント
void mouse(int button, int state, int x, int y) {
  if (button == GLUT_LEFT_BUTTON && state == GLUT_DOWN) {
    camr.dragstart(x, y);//最初の回転軸を決定
  }
  else if (state == GLUT_UP) {
    camr.dragend();//ドラッグ終了
  }
  glutPostRedisplay();
}
//ドラッグ時のイベント
void motion(int x, int y) {
  camr.dragto(x, y);//移動中の回転軸を更新
  disp();
}
//エントリポイント
int main(int argc, char ** argv) {
  glutInit(&argc, argv);
  glutInitWindowPosition(100, 50);
  glutInitWindowSize(400, 300);
  glutInitDisplayMode(GLUT_SINGLE | GLUT_RGBA);

  glutCreateWindow("MouseRotateSample");
  glutDisplayFunc(disp);
  glutReshapeFunc(reshape);
  glutMouseFunc(mouse);
  glutMotionFunc(motion);
  glutMainLoop();
  return 0;
}

#pragma once
#include <cmath>
namespace nu {

  ////////////////////////////////////
  //! @brief マウス回転用クラス
  class mrotate
  {
    double m_cam_matrix[16]; //!< @brief 現在の行列
    double m_tmp_matrix[16]; //!< @brief マウス回転中に使用する一時的な行列


    double m_vstart[3]; //!< @brief 回転スタート時の、「球の中心→"クリック位置のレイと球の交点"」　のベクトル
    double m_vend[3]; //!< @brief マウスが移動中の、〃
    double m_raxis[3]; //vstartとvendの外積から算出した、回転中の回転軸(ベクトル)

    //viewportで指定した表示範囲をウィンドウ座標系で指定
    int m_rectLTx, m_rectLTy;
    int m_rectRBx, m_rectRBy;

    //ウィンドウのサイズ
    int windowSizeX, windowSizeY;

    //マウスボタンが押されている間はtrueになり、trueの間は回転処理を行う
    bool m_fmousepush;
  public:
    mrotate();
    ~mrotate();

    //! @brief クライアント領域のサイズを指定する
    //! @param [in] width ウィンドウのクライアント領域の幅(ピクセル)
    //! @param [in] height ウィンドウのクライアント領域の高さ(ピクセル)
    void setWindowSize(const int width, const int height) {
      windowSizeX = width;
      windowSizeY = height;
    }

    //! @brief クライアント領域内にOpenGLで描画する範囲を矩形で指定。glviewportと同じだがこちらはクライアント座標系で矩形の四隅の座標を指定する
    void setRenderRect(int LeftTopX, int LeftTopY, int RightBottomX, int RightBottomY) {
      m_rectLTx = LeftTopX;
      m_rectLTy = LeftTopY;
      m_rectRBx = RightBottomX;
      m_rectRBy = RightBottomY;
    }

    //! @brief ドラッグ開始時に呼び出す
    //! @param [in] x クライアント座標系のマウスのx座標
    //! @param [in] y クライアント座標系のマウスのy座標
    //! @return なし
    void dragstart(int curx, int cury);


    //! @brief ドラッグ中に呼び出す
    //! @param [in] curx クライアント座標系のマウスのx座標
    //! @param [in] cury クライアント座標系のマウスのy座標
    //! @return なし
    void dragto(int curx, int cury);


    //! @brief ドラッグ終了時に呼び出す
    //! @return なし
    void dragend();

    //! @brief 現在の回転行列を取得する
    const double* getmatrix(double* const dst)const;
  };
  ////////////////////////////////////

  /////////////////////////////////////////////   
  //以下ベクトルと行列演算用の汎用的な関数////////

  //三次元ベクトルの長さを求める
  inline double length3(const double* const vec3) {
    return std::sqrt(vec3[0] * vec3[0] + vec3[1] * vec3[1] + vec3[2] * vec3[2]);
  }

  //三次元ベクトルの外積を求める
  inline double inner3(const double * const vec1, const double * const vec2) {
    return ((vec1[0])*(vec2[0]) + (vec1[1])*(vec2[1]) + (vec1[2])*(vec2[2]));
  }

  inline void outer3(double * const dvec, double const * const svec1, double const * const svec2) {

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

    dvec[0] = static_cast<double>(y1 * z2 - z1 * y2);
    dvec[1] = static_cast<double>(z1 * x2 - x1 * z2);
    dvec[2] = static_cast<double>(x1 * y2 - y1 * x2);
  }
  inline double vangle3(double const * const vec1, double const * const vec2) {
    return acos(inner3(vec1, vec2) / (length3(vec1) * length3(vec2)));

  }
  inline void GetRotateMatrix(double* const m, const double radian, const double * const axis) {
    double s = sin(radian);
    double c = cos(radian);
    double len = length3(axis);
    double x = axis[0] / len;
    double y = axis[1] / len;
    double z = axis[2] / len;

    m[0] = x * x*(1 - c) + c;  m[4] = x * y*(1 - c) - z * s;  m[8] = x * z*(1 - c) + y * s;  m[12] = 0;
    m[1] = x * y*(1 - c) + z * s;  m[5] = y * y*(1 - c) + c;  m[9] = y * z*(1 - c) - x * s;  m[13] = 0;
    m[2] = x * z*(1 - c) - y * s;  m[6] = y * z*(1 - c) + x * s;  m[10] = z * z*(1 - c) + c;  m[14] = 0;
    m[3] = 0;              m[7] = 0;              m[11] = 0;              m[15] = 1;
  }


  inline bool normalize3(double * const pV)
  {
    double len;
    double& x = pV[0];
    double& y = pV[1];
    double& z = pV[2];

    len = sqrt(x * x + y * y + z * z);

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

    len = 1.0 / len;
    x *= len;
    y *= len;
    z *= len;

    return true;
  }

  inline double* CopyMat44(double* const dst16, const double* const src16) {
    for (int i = 0; i < 16; i++) {
      dst16[i] = src16[i];
    }
    return dst16;
  }

  inline double* MultMatrix(double* const dst, const double* const m1, const double* const m2) {
    for (int i = 0; i < 16; i++)
      dst[i] = 0.0;

    double d1, d2;
    for (size_t j = 0; j < 4; j++)
      for (size_t k = 0; k < 4; k++)
        for (size_t c = 0; c < 4; c++) {
          d1 = m1[4 * j + c];

          d2 = m2[4 * c + k];

          dst[4 * j + k] += d1 * d2;

        }
    return dst;
  }

  inline double* MultMatrix(double* dst, const double* const src) {
    double m1[16];
    for (int i = 0; i < 16; i++) {
      m1[i] = dst[i];
    }
    MultMatrix(dst, m1, src);
    return dst;
  }
  inline void loadIdentity16(double * const mat) {
    for (int i = 0; i < 16; i++) {
      mat[i] = 0;
    }
    mat[0] = 1;
    mat[5] = 1;
    mat[10] = 1;
    mat[15] = 1;
  }
}

#include "nurotate.h"
#include <cstdio>
namespace nu {

  mrotate::mrotate() {
    loadIdentity16(m_cam_matrix);
    loadIdentity16(m_tmp_matrix);

    m_fmousepush = false;
  }

  mrotate::~mrotate()
  {
  }

  const double* mrotate::getmatrix(double* const dst)const {
    CopyMat44(dst, m_cam_matrix);
    MultMatrix(dst, m_tmp_matrix);

    return dst;
  }


  void mrotate::dragstart(int curx, int cury) {

    //本来はマウスが押されている間に呼び出されることはない
    //もしそういうことが起こったら何もせずに抜ける
    if (m_fmousepush == true)
      return;

    //ドラッグ中であることを示す
    m_fmousepush = true;

    printf(" dragstart\n");

    const long cx = (m_rectRBx - m_rectLTx) / 2;//開始点を左上と考えた時の、画面中央の座標
    const long cy = (m_rectRBy - m_rectLTy) / 2;//

    const long x = curx - m_rectLTx - cx;//cx,cyを0と考えた時のマウスの座標(左下原点)
    const long y = (windowSizeY - cury) - m_rectLTy - cy;


    //球の半径
    double r = std::fmin((m_rectRBx - m_rectLTx), (m_rectRBy - m_rectLTy));

    //レイの方程式 p = s + td
    double s[3] = { (double)x,(double)y,-1000 };
    double d[3] = { 0,0,1 };

    double A = std::pow(length3(d), 2);
    double B = 2 * inner3(s, d);
    double C = std::pow(length3(s), 2) - r * r;
    double t1 = (-B + std::sqrt(B*B - 4 * A * C)) / (2 * A);
    double t2 = (-B - std::sqrt(B*B - 4 * A * C)) / (2 * A);

    //交点
    double p[3] = {
      s[0] + t1 * d[0],
      s[1] + t1 * d[1],
      s[2] + t1 * d[2]
    };
    normalize3(p);

    //中心→交点のベクトル
    m_vstart[0] = p[0];
    m_vstart[1] = p[1];
    m_vstart[2] = p[2];

  }
  void mrotate::dragto(int curx, int cury) {

    //マウスドラッグ中でないなら何もしない
    if (m_fmousepush == false)
      return;

    printf(" dragend\n");

    const long cx = (m_rectRBx - m_rectLTx) / 2;//開始点を左上と考えた時の、画面中央の座標
    const long cy = (m_rectRBy - m_rectLTy) / 2;//

    const long x = curx - m_rectLTx - cx;//cx,cyを0と考えた時のマウスの座標(左下原点)
    const long y = (windowSizeY - cury) - m_rectLTy - cy;


    //球の半径
    double r = std::fmin((m_rectRBx - m_rectLTx), (m_rectRBy - m_rectLTy));

    //レイの方程式 p = s + td
    double s[3] = { (double)x,(double)y,-1000 };
    double d[3] = { 0,0,1 };

    double A = std::pow(length3(d), 2);
    double B = 2 * inner3(s, d);
    double C = std::pow(length3(s), 2) - r * r;
    double t1 = (-B + std::sqrt(B*B - 4 * A * C)) / (2 * A);
    double t2 = (-B - std::sqrt(B*B - 4 * A * C)) / (2 * A);

    //交点
    double p[3] = {
      s[0] + t1 * d[0],
      s[1] + t1 * d[1],
      s[2] + t1 * d[2]
    };
    normalize3(p);

    //中心→交点のベクトル
    m_vend[0] = p[0];
    m_vend[1] = p[1];
    m_vend[2] = p[2];


    outer3(m_raxis, m_vstart, m_vend);
    normalize3(m_raxis);
    double angle = vangle3(m_vstart, m_vend) * 3;

    if (abs(angle) < 1e-6)
      return;

    GetRotateMatrix(m_tmp_matrix, angle, m_raxis);

  }

  void mrotate::dragend() {

    //マウスが押されている間しか処理しない
    if (m_fmousepush == false)
      return;

    //マウスドラッグが終わったことを示す
    m_fmousepush = false;


    MultMatrix(m_cam_matrix, m_tmp_matrix);
    loadIdentity16(m_tmp_matrix);
  }
}