【Unity】オブジェクトの角速度を計算（推定）する

はじめに

Transformやアニメーションで動かしている（Rigidbody不使用の）オブジェクトで角速度を取得したい。
Transform.rotationの変化を追って推定することくらいなら難しくないんじゃないかと甘く見ていたのですが、結構苦戦してしまいました。

「rotationがどれくらい変わったかを刻々と求めればいいはず」ということで、とりあえずクォータニオンの引き算っぽいことをすればいいんだろうとは思ったのですが…。
肝心の計算方法がすぐに分からず、色々試す羽目に。

ソース

速度の推定もついでに入れて、GitHubにアップロードしました。
unity-kinematic-estimator

コード内容

先に結論から。

1. 回転の変化（前フレームの回転の逆クォータニオン×現在の回転）から、Quaternion.ToAngleAxisで角度と回転軸を得ます。
2. 得られた角度値は [deg/frame] なので [rad/s] に換算します。
3. それを（Rigidbody.angularVelocityの形になるように）軸ベクトルにかけて完了。

AngularVelocityEstimator.cs

using UnityEngine;

/// <summary>
/// 角速度推定コンポーネント
/// </summary>
public class AngularVelocityEstimator : MonoBehaviour
{
    /// <summary>
    /// 推定角速度 [rad/s]
    /// </summary>
    [SerializeField]
    private Vector3 _estimatedAngularVelocity = Vector3.zero;

    /// <summary>
    /// 回転前回値
    /// </summary>
    private Quaternion _rotationPrevious = Quaternion.identity;

    public Vector3 EstimatedAngularVelocity
    {
        get { return _estimatedAngularVelocity; }
    }

    private void FixedUpdate()
    {
        // 回転変化量を計算
        Quaternion deltaRotation = Quaternion.Inverse(_rotationPrevious) * transform.rotation;

        // 角度と回転軸に変換
        deltaRotation.ToAngleAxis(out float angle, out Vector3 axis);

        // 角速度 [rad/s] を算出
        float angularSpeed = (angle * Mathf.Deg2Rad) / Time.deltaTime;
        _estimatedAngularVelocity = axis * angularSpeed;

        // 今回値を覚えておく
        _rotationPrevious = transform.rotation;
    }
}

試しにこれをRigidbody付きのオブジェクトに付けてみて、
適当なトルクを加えたときのRigidbody.angularVelocityと比較してみます。

多少の誤差はありますが、angularDrag（というか角加速度）を考慮していないのでこんなものでしょうか。
（一応、angularDragが小さいほど誤差が小さくなりました。）

補足

以下は蛇足的情報です。スルーで構いません。

最初私は考えました。
そもそもオイラー角の引き算じゃダメなの？
要はこういうことですよね。

NG

        // 回転変化量を計算
        _estimatedAngularVelocity = transform.rotation.eulerAngles - _rotationPrevious.eulerAngles;
        _estimatedAngularVelocity = (_estimatedAngularVelocity * Mathf.Deg2Rad) / Time.deltaTime;

        // 今回値を覚えておく
        _rotationPrevious = transform.rotation;

ダメでした。
これについてはオイラー角の差分を見たとして、それはオイラー角変化量とでもいうべきものであり、角速度ベクトルとは別のものである、という自己解釈で納得しました。

おわりに

浅学ゆえ正しく理解できているか不安ですが、今のところこれで欲しい結果を得ることができているので共有します。
間違いや誤解があるようでしたらご指摘頂けますと幸いです。