PyOpenGLでクオータニオンによる物体の回転を試してみた

http://staff.aist.go.jp/toru-nakata/quaternion.html

ここに書いてある内容を読んで、pythonで４元数クラスを素直に実装してみた。そしてその実装で物体を回転させるプログラムをPyOpenGLで書いてみた。

Quaternionクラスは単項+,単項-,+,-,*,abs,reprをオーバーロードしていて、conj()で共役４元数、inverse()で逆４元数が得られる。

演算子を適切にオーバーロードしたので座標を回転するコードはごく素直で読みやすく実装できた。

[,h300,left]
[,h300]

# coding: utf-8
# クオータニオンクラス

from numpy import dot, cross, array
from math import pi, sin, cos

class Quaternion(object):
    def __init__(self, t, v):
        self.t = float(t)
        self.v = array(map(float,v))
    def __pos__(p): # +p
        return p
    def __neg__(p): # -p
        return Quaternion(-p.t, -p.v)
    def __add__(p, q): # p+q
        return Quaternion(p.t + q.t, p.v + q.v)
    def __sub__(p, q): # p-q
        return Quaternion(p.t - q.t, p.v - q.v)
    def __mul__(p, q): # p*q
        return Quaternion(p.t*q.t - dot(p.v, q.v),
                          p.t*q.v + q.t*p.v + cross(p.v, q.v)
                         )
    def inverse(p): # p*p_ = 1
        # 逆４元数
        n = p.abs2()
        p_= p.conj()
        return Quaternion(p_.t / n, p_.v / n)
    def abs2(p):
        return p.t**2 + sum(p.v**2)
    def __abs__(p):
        return p.abs2()**.5
    def conj(p):
        # 共役４元数
        return Quaternion(p.t, -p.v)
    def __repr__(p):
        return "(%f; %f, %f, %f)" % (p.t, p.x, p.y, p.z)
    @property
    def x(self): return self.v[0]
    @property
    def y(self): return self.v[1]
    @property
    def z(self): return self.v[2]
   
def rotation(v, theta, r):
    # v: 回転対象のベクトル 
    # theta: 回転角度(radian)
    # r: 回転軸(リスト)
    
    # 回転軸の正規化
    n = sum(map(lambda x: x**2, r))**.5
    r[0] /= n; r[1] /= n; r[2] /= n

    w = theta / 2.
    a, b = cos(w), sin(w)
    p = Quaternion(a, map(lambda x:x*b, r))
    p_= p.conj()
    q = Quaternion(0, v)
    q = p * q * p_ # 回転
    return (q.x, q.y, q.z)