OpenGL/1.x/Quaternionによる回転 - bambooflow Note

Quaternionによる回転

• GLUT (freeglut3.7)
• GLM 0.9.0.6

四元数について

四元数の表現

四元数Qは、

Q = (t; x, y, z)

と表すことができます。
このとき、tは実数部、x,y,zは虚数部です。

四元数の掛け算

A = (a; U)
B = (b; V)
AB = (ab - U・V; aV + bU + U×V)

ただし、「・」は内積、「×」は外積を表します。

三次元座標の四元数による表現

たとえばx,y,zで示された座標を四元数で表現すると、

P = (0; x,y,z)

となります。このとき実数部はとりあえず0にしておきます。

四元数での回転表現

原点を中心とした回転軸を(α,γ,β)としたとき、回転をθとした四元数は、

Q = (cos(θ/2); αsin(θ/2), βsin(θ/2), γsin(θ/2)
R = (cos(θ/2);-αsin(θ/2),-βsin(θ/2),-γsin(θ/2)

となります。
このとき、RはQの共役四元数といいます。

回転の実行は、

RPQ = (0; x,y,z)   #x,y,zが回転した結果

Pに対してRとQを挟み込むことで座標Pに対する任意の回転座標が得られます。

c実装メモ

//////////////////////////////////////////////
// クォータニオンの積 r <- p x q
static void qmul(double r[], const double p[], const double q[])
{
 r[0] = p[0] * q[0] - p[1] * q[1] - p[2] * q[2] - p[3] * q[3];
 r[1] = p[0] * q[1] + p[1] * q[0] + p[2] * q[3] - p[3] * q[2];
 r[2] = p[0] * q[2] - p[1] * q[3] + p[2] * q[0] + p[3] * q[1];
 r[3] = p[0] * q[3] + p[1] * q[2] - p[2] * q[1] + p[3] * q[0];
}
/////////////////////////////////////////////
// 回転の変換行列 r <- クォータニオン q
static void qrot(double r[], double q[]){
 double x2 = q[1] * q[1] * 2.0;
 double y2 = q[2] * q[2] * 2.0;
 double z2 = q[3] * q[3] * 2.0;
 double xy = q[1] * q[2] * 2.0;
 double yz = q[2] * q[3] * 2.0;
 double zx = q[3] * q[1] * 2.0;
 double xw = q[1] * q[0] * 2.0;
 double yw = q[2] * q[0] * 2.0;
 double zw = q[3] * q[0] * 2.0;
 
 r[ 0] = 1.0 - y2 - z2;
 r[ 1] = xy + zw;
 r[ 2] = zx - yw;
 r[ 4] = xy - zw;
 r[ 5] = 1.0 - z2 - x2;
 r[ 6] = yz + xw;
 r[ 8] = zx + yw;
 r[ 9] = yz - xw;
 r[10] = 1.0 - x2 - y2;
 r[ 3] = r[ 7] = r[11] = r[12] = r[13] = r[14] = 0.0;
 r[15] = 1.0;
}
 

GLMを使ったサンプルコード

GLMを使うことで簡単に記述できます。

マウスでくるくる。。

#include <math.h>
#include <GL/glut.h>
 
#include <glm/glm.hpp>
#include <glm/gtc/type_ptr.hpp>
#include <glm/gtc/quaternion.hpp>
 
static glm::vec4 red = glm::vec4( 0.8, 0.2, 0.2, 1.0 ); // 物体の色
static glm::vec4 lightPos = glm::vec4( 3.0, 4.0, 5.0, 1.0 ); // 光源の位置
 
static glm::vec3 eyePos(0.0, 0.0, 10.0); // 視点
static glm::vec3 ctrPos(0.0, 0.0, 0.0);  // 目標点
 
static glm::dvec2 s;                     // 係数
static glm::ivec2 st_pos;                // マウスクリック位置
 
static glm::quat cq(1.0, 0.0, 0.0, 0.0); // 回転初期値 (クォータニオン)
static glm::quat tq;                     // ドラッグ中の回転 (クォータニオン)
static glm::mat4 rt;                     // 回転の変換行列
 
void display(void)
{
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
 
  glLoadIdentity();
 
  gluLookAt(
      eyePos.x, eyePos.y, eyePos.z,
      ctrPos.x, ctrPos.y, ctrPos.z,
      0.0, 1.0, 0.0
  );
 
  glLightfv(GL_LIGHT0, GL_POSITION, glm::value_ptr(lightPos));
 
  glPushMatrix();
  {
    glEnable(GL_LIGHTING);
    glMaterialfv(GL_FRONT, GL_DIFFUSE, glm::value_ptr(red));
    glMultMatrixf(glm::value_ptr(rt)); // 回転
    glutSolidTeapot(1.5);
  }
  glPopMatrix();
 
  glutSwapBuffers();
}
 
void resize(int w, int h)
{
  // ドラッグ用係数 - ウィンドウサイズに依存
  s.x = 1.0 / (double)w;
  s.y = 1.0 / (double)h;
 
  glViewport(0, 0, w, h);
 
  glMatrixMode(GL_PROJECTION);
 
  glLoadIdentity();
  gluPerspective(30.0, (double)w / (double)h, 1.0, 100.0);
 
  glMatrixMode(GL_MODELVIEW);
}
 
void idle(void)
{
  glutPostRedisplay();
}
 
void mouse(int button, int state, int x, int y)
{
  switch (button) {
  case GLUT_LEFT_BUTTON:
    switch (state) {
    case GLUT_DOWN:
      st_pos = glm::ivec2( x, y );
      glutIdleFunc(idle);
      break;
    case GLUT_UP:
      glutIdleFunc(0);
      cq = tq; // 回転保持
      break;
    default:
      break;
    }
    break;
  default:
    break;
  }
}
 
void motion(int x, int y)
{
  static const double scale = (2.0 * M_PI);
  glm::dvec2 d;
  double a;
 
  d.x = double(x - st_pos.x) * s.x;
  d.y = double(y - st_pos.y) * s.y;
 
  a = glm::length(d);
 
  if (a != 0.0) {
    double ar = a * scale * 0.5;
    double as = sin(ar) / a;
    glm::quat dq(cos(ar), as*d.y, as*d.x, 0.0);
    tq = glm::cross(dq, cq); // 回転合成
    rt = glm::mat4_cast(tq); // Quotanion->回転の変換行列
  }
}
 
void init(void)
{
  glClearColor(0.2, 0.2, 0.2, 1.0);
 
  glEnable(GL_DEPTH_TEST);
  glEnable(GL_CULL_FACE);
  glEnable(GL_LIGHTING);
  glEnable(GL_LIGHT0);
 
  glCullFace(GL_FRONT);
 
  rt = glm::mat4_cast(cq); // 回転行列初期化
}
 
int main(int argc, char *argv[])
{
  glutInit(&argc, argv);
  glutInitWindowSize(600, 400);
  glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE | GLUT_DEPTH);
  glutCreateWindow("glm quatanion test");
  glutDisplayFunc(display);
  glutReshapeFunc(resize);
  glutMouseFunc(mouse);
  glutMotionFunc(motion);
 
  init();
 
  glutMainLoop();
 
  return 0;
}