クォータニオンのお勉強のためのPythonスクリプト

クォータニオンの利点

１．コンパクトな表現

回転行列だと９つの要素．それに対して，クォータニオンは，３次元空間における回転を４要素で実現．

２．ジンバルロック回避

特定の回転軸が他の軸と一致する際に，自由度が失われる現象（ジンバルロック）が発生し，特定の回転を表現できなくなることを防ぐことができる．

３．スムーズな補間

2つのクォータニオンの間をスムーズに補間するSlerp（球面線形補間）と呼ばれる手法が非常にシンプルで効率的に実装できる．（アニメーションやシミュレーション時に有効）

４．正規化が容易

連続的な回転を扱う際に，浮動小数点演算の誤差により正規直交性が失われることがある．これを補正するための再正規化処理が必要．

５．回転の連結が高速

2つのクォータニオンを合成することで，2つの回転を連結する操作が回転行列の乗算よりも高速．

quaternion_input_gif.py

# coding: utf-8
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from matplotlib.animation import FuncAnimation
import tkinter as tk
from tkinter import simpledialog

# --- 1. クォータニオンの基本操作を定義するクラス ---
class Quaternion:
    def __init__(self, w, x, y, z):
        """
        クォータニオンを初期化します。
        q = w + xi + yj + zk
        """
        self.w = w
        self.x = x
        self.y = y
        self.z = z

    def normalize(self):
        """クォータニオンを正規化します（単位クォータニオンにする）。"""
        norm = np.sqrt(self.w**2 + self.x**2 + self.y**2 + self.z**2)
        if norm == 0:
            return Quaternion(0, 0, 0, 0) # ゼロクォータニオンの場合
        return Quaternion(self.w / norm, self.x / norm, self.y / norm, self.z / norm)

    def conjugate(self):
        """共役クォータニオンを返します。"""
        return Quaternion(self.w, -self.x, -self.y, -self.z)

    def multiply(self, other):
        """
        このクォータニオンと別のクォータニオンを乗算します (q1 * q2)。
        """
        w1, x1, y1, z1 = self.w, self.x, self.y, self.z
        w2, x2, y2, z2 = other.w, other.x, other.y, other.z

        w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
        x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
        y = w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2
        z = w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2
        return Quaternion(w, x, y, z)

    @staticmethod
    def from_axis_angle(axis, angle_rad):
        """
        回転軸と角度（ラジアン）からクォータニオンを作成します。
        axis: numpy配列 (x, y, z)
        angle_rad: ラジアン
        """
        axis = np.array(axis) / np.linalg.norm(axis) # 軸を正規化
        half_angle = angle_rad / 2
        sin_half_angle = np.sin(half_angle)
        w = np.cos(half_angle)
        x = axis[0] * sin_half_angle
        y = axis[1] * sin_half_angle
        z = axis[2] * sin_half_angle
        return Quaternion(w, x, y, z).normalize()

    def rotate_vector(self, vector):
        """
        このクォータニオンを使用してベクトルを回転させます。
        v_rotated = q * v * q_conjugate
        vector: numpy配列 (x, y, z)
        """
        p = Quaternion(0, vector[0], vector[1], vector[2]) # ベクトルを純粋クォータニオンとして表現
        rotated_p = self.multiply(p).multiply(self.conjugate())
        return np.array([rotated_p.x, rotated_p.y, rotated_p.z])

# --- 2. 立方体の頂点データの定義 ---
# 立方体の各頂点の座標
# 中心を原点(0,0,0)とする
half_side = 0.5
cube_vertices = np.array([
    [-half_side, -half_side, -half_side],
    [ half_side, -half_side, -half_side],
    [ half_side,  half_side, -half_side],
    [-half_side,  half_side, -half_side],
    [-half_side, -half_side,  half_side],
    [ half_side, -half_side,  half_side],
    [ half_side,  half_side,  half_side],
    [-half_side,  half_side,  half_side]
])

# 立方体の辺の接続定義
cube_edges = [
    (0, 1), (1, 2), (2, 3), (3, 0), # 底面
    (4, 5), (5, 6), (6, 7), (7, 4), # 天面
    (0, 4), (1, 5), (2, 6), (3, 7)  # 側面
]

# --- 3. ユーザー入力ダイアログを表示 ---
def get_rotation_angles():
    root = tk.Tk()
    root.withdraw() # メインウィンドウを非表示にする

    try:
        angle_x = simpledialog.askfloat("Rotation Angle", "Enter rotation angle around X-axis (degrees):", initialvalue=0.0)
        if angle_x is None: return None, None, None # キャンセルされた場合

        angle_y = simpledialog.askfloat("Rotation Angle", "Enter rotation angle around Y-axis (degrees):", initialvalue=0.0)
        if angle_y is None: return None, None, None

        angle_z = simpledialog.askfloat("Rotation Angle", "Enter rotation angle around Z-axis (degrees):", initialvalue=0.0)
        if angle_z is None: return None, None, None

        return angle_x, angle_y, angle_z
    except ValueError:
        print("Invalid input. Please enter a number.")
        return None, None, None
    finally:
        root.destroy() # Tkinterのルートウィンドウを破棄

# --- グローバル変数 ---
current_rotation_q = Quaternion(1, 0, 0, 0) # 現在の回転クォータニオン
total_frames = 90 # アニメーションの総フレーム数 (ここでは30FPSで3秒間)

# --- 4. アニメーションの更新関数 ---
def update(frame):
    global current_rotation_q

    if frame == 0:
        # アニメーション開始時に現在の回転をリセット
        current_rotation_q.w = 1.0
        current_rotation_q.x = 0.0
        current_rotation_q.y = 0.0
        current_rotation_q.z = 0.0

    # 毎フレームの微小回転角度
    delta_angle_x = np.deg2rad(INPUT_ANGLE_X) / total_frames
    delta_angle_y = np.deg2rad(INPUT_ANGLE_Y) / total_frames
    delta_angle_z = np.deg2rad(INPUT_ANGLE_Z) / total_frames

    # 各軸の微小回転クォータニオンを生成
    delta_q_x = Quaternion.from_axis_angle(np.array([1, 0, 0]), delta_angle_x)
    delta_q_y = Quaternion.from_axis_angle(np.array([0, 1, 0]), delta_angle_y)
    delta_q_z = Quaternion.from_axis_angle(np.array([0, 0, 1]), delta_angle_z)

    # 現在の回転クォータニオンに、各軸の微小回転を適用
    # グローバル座標系におけるZ-Y-X順での回転の累積
    current_rotation_q = delta_q_x.multiply(current_rotation_q).normalize()
    current_rotation_q = delta_q_y.multiply(current_rotation_q).normalize()
    current_rotation_q = delta_q_z.multiply(current_rotation_q).normalize()


    # 各頂点を回転させる
    rotated_vertices = np.zeros_like(cube_vertices)
    rotation_origin = np.array([0.0, 0.0, 0.0]) # 回転の中心（原点）

    for i, vertex in enumerate(cube_vertices):
        relative_vertex = vertex - rotation_origin
        rotated_relative_vertex = current_rotation_q.rotate_vector(relative_vertex)
        rotated_vertices[i] = rotated_relative_vertex + rotation_origin

    # 更新された頂点データで線を描画
    for i, (p1_idx, p2_idx) in enumerate(cube_edges):
        x_data = [rotated_vertices[p1_idx, 0], rotated_vertices[p2_idx, 0]]
        y_data = [rotated_vertices[p1_idx, 1], rotated_vertices[p2_idx, 1]]
        z_data = [rotated_vertices[p1_idx, 2], rotated_vertices[p2_idx, 2]]
        lines[i].set_data(x_data, y_data)
        lines[i].set_3d_properties(z_data)

    return lines

# --- メイン処理 ---
if __name__ == "__main__":
    # 1. ユーザーから回転角度を取得
    INPUT_ANGLE_X, INPUT_ANGLE_Y, INPUT_ANGLE_Z = get_rotation_angles()

    if INPUT_ANGLE_X is None:
        print("アニメーションがユーザーによってキャンセルされました。")
    else:
        # 2. アニメーションの初期化
        fig = plt.figure(figsize=(8, 8))
        ax = fig.add_subplot(111, projection='3d')
        ax.set_title(f"Cube Rotation: X={INPUT_ANGLE_X}°, Y={INPUT_ANGLE_Y}°, Z={INPUT_ANGLE_Z}°")
        ax.set_xlim([-1, 1])
        ax.set_ylim([-1, 1])
        ax.set_zlim([-1, 1])
        ax.set_xlabel("X-axis")
        ax.set_ylabel("Y-axis")
        ax.set_zlabel("Z-axis")
        ax.set_aspect('auto')

        lines = [ax.plot([], [], [], 'b-')[0] for _ in cube_edges]

        # 3. アニメーションの実行
        # interval=33ms は約30FPSに相当 (1000ms / 30フレーム = 33.3ms)
        # total_framesを90に設定することで、全体で3秒のアニメーション (90フレーム / 30FPS = 3秒)
        ani = FuncAnimation(fig, update, frames=total_frames, interval=33, blit=True, repeat=False)

        # 4. GIFファイルとして保存
        gif_filename = f"cube_rotation_X{INPUT_ANGLE_X}_Y{INPUT_ANGLE_Y}_Z{INPUT_ANGLE_Z}.gif"
        print(f"アニメーションを '{gif_filename}' に保存中...")
        ani.save(gif_filename, writer='pillow', fps=30) # fpsはintervalと合わせる
        print("保存が完了しました！")

        plt.show()