TransformのLookAtをUnity APIを使わずに書く

概要

仕事でLookAtを、Unity APIを使わずに書くことがあったのでメモ。

実行した結果。ちゃんとCubeを注視し続けます。
f:id:edo_m18:20180418015908g:plain

サンプルコード

実装したサンプルコードは以下。

このスクリプトをカメラに貼り付けて、ターゲットを指定すると常にそれを視界に捉えた状態になります。

using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class LookAtSample : MonoBehaviour
{
    [SerializeField]
    private Transform _target;
    
    private void Update()
    {
        LookAt(_target.position);
    }

    private void LookAt(Vector3 target)
    {
        Vector3 z = (target - transform.position).normalized;
        Vector3 x = Vector3.Cross(Vector3.up, z).normalized;
        Vector3 y = Vector3.Cross(z, x).normalized;

        Matrix4x4 m = Matrix4x4.identity;
        m[0, 0] = x.x; m[0, 1] = y.x; m[0, 2] = z.x;
        m[1, 0] = x.y; m[1, 1] = y.y; m[1, 2] = z.y;
        m[2, 0] = x.z; m[2, 1] = y.z; m[2, 2] = z.z;

        Quaternion rot = GetRotation(m);
        transform.rotation = rot;
    }

    private Quaternion GetRotation(Matrix4x4 m)
    {
        float[] elem = new float[4];
        elem[0] = m.m00 - m.m11 - m.m22 + 1.0f;
        elem[1] = -m.m00 + m.m11 - m.m22 + 1.0f;
        elem[2] = -m.m00 - m.m11 + m.m22 + 1.0f;
        elem[3] = m.m00 + m.m11 + m.m22 + 1.0f;

        int biggestIdx = 0;
        for (int i = 0; i < elem.Length; i++)
        {
            if (elem[i] > elem[biggestIdx])
            {
                biggestIdx = i;
            }
        }

        if (elem[biggestIdx] < 0)
        {
            Debug.Log("Wrong matrix.");
            return new Quaternion();
        }

        float[] q = new float[4];
        float v = Mathf.Sqrt(elem[biggestIdx]) * 0.5f;
        q[biggestIdx] = v;
        float mult = 0.25f / v;

        switch (biggestIdx)
        {
            case 0:
                q[1] = (m.m10 + m.m01) * mult;
                q[2] = (m.m02 + m.m20) * mult;
                q[3] = (m.m21 - m.m12) * mult;
                break;
            case 1:
                q[0] = (m.m10 + m.m01) * mult;
                q[2] = (m.m21 + m.m12) * mult;
                q[3] = (m.m02 - m.m20) * mult;
                break;
            case 2:
                q[0] = (m.m02 + m.m20) * mult;
                q[1] = (m.m21 + m.m12) * mult;
                q[3] = (m.m10 - m.m01) * mult;
                break;
            case 3:
                q[0] = (m.m21 - m.m12) * mult;
                q[1] = (m.m02 - m.m20) * mult;
                q[2] = (m.m10 - m.m01) * mult;
                break;
        }

        return new Quaternion(q[0], q[1], q[2], q[3]);
    }
}

LookAtの行列を作る

LookAtは結局のところ、ターゲットに向く「回転」を作ることと同義です。

上のコードでやっていることはシンプルです。
「向かせたい方向のベクトル」と「空方向のベクトル」から、外積を使ってX軸を割り出します。
（空方向と垂直なベクトルはX軸ですよね）

そして向かせたい方向ベクトルがZ軸となるわけなので、結果として、割り出したX軸と向かせたいベクトルとの外積を取ることで最終的な「上方向」のベクトルを得ることができます。
（「上」が空方向とは限らない）

そうやって求めた3軸の各要素を元に行列を作ります。
なぜこれが「LookAt」に相当するのか。

(x, y, z)が(1, 1, 1)のときを考えてみましょう。
該当のコードは以下です。

private void LookAt(Vector3 target)
{
    Vector3 z = (target - transform.position).normalized;
    Vector3 x = Vector3.Cross(Vector3.up, z).normalized;
    Vector3 y = Vector3.Cross(z, x).normalized;

    Matrix4x4 m = Matrix4x4.identity;
    m[0, 0] = x.x; m[0, 1] = y.x; m[0, 2] = z.x;
    m[1, 0] = x.y; m[1, 1] = y.y; m[1, 2] = z.y;
    m[2, 0] = x.z; m[2, 1] = y.z; m[2, 2] = z.z;

    Quaternion rot = GetRotation(m);
    transform.rotation = rot;
}

そして今作った行列をそのベクトル(1, 1, 1)に適用してみます。
Unityの行列は列オーダーなので、ベクトルを右から掛けることで計算します。
つまり、

$\begin{vmatrix} x.x &y.x &z.x \\ x.y &y.y &z.y \\ x.z &y.z &z.z \\ \end{vmatrix}\times \begin{vmatrix} 1 \\ 1 \\ 1 \\ \end{vmatrix}= \begin{vmatrix} x.x &y.x & z.x \end{vmatrix}$

となって、回転後の値になっているのが分かるかと思います。

回転行列からクォータニオンを計算する

さて、無事に回転行列が作れましたが、Unityでは回転をクォータニオンで扱っています。
そのため、求めた回転行列はそのまま使えません。なので、クォータニオンに変換する必要があります。

回転行列からクォータニオンへの変換は、マルペケさんのこちらの記事（その58 やっぱり欲しい回転行列⇔クォータニオン相互変換）を参考にさせていただきました。

コード部分は以下です。

private Quaternion GetRotation(Matrix4x4 m)
{
    float[] elem = new float[4];
    elem[0] = m.m00 - m.m11 - m.m22 + 1.0f;
    elem[1] = -m.m00 + m.m11 - m.m22 + 1.0f;
    elem[2] = -m.m00 - m.m11 + m.m22 + 1.0f;
    elem[3] = m.m00 + m.m11 + m.m22 + 1.0f;

    int biggestIdx = 0;
    for (int i = 0; i < elem.Length; i++)
    {
        if (elem[i] > elem[biggestIdx])
        {
            biggestIdx = i;
        }
    }

    if (elem[biggestIdx] < 0)
    {
        Debug.Log("Wrong matrix.");
        return new Quaternion();
    }

    float[] q = new float[4];
    float v = Mathf.Sqrt(elem[biggestIdx]) * 0.5f;
    q[biggestIdx] = v;
    float mult = 0.25f / v;

    switch (biggestIdx)
    {
        case 0:
            q[1] = (m.m10 + m.m01) * mult;
            q[2] = (m.m02 + m.m20) * mult;
            q[3] = (m.m21 - m.m12) * mult;
            break;
        case 1:
            q[0] = (m.m10 + m.m01) * mult;
            q[2] = (m.m21 + m.m12) * mult;
            q[3] = (m.m02 - m.m20) * mult;
            break;
        case 2:
            q[0] = (m.m02 + m.m20) * mult;
            q[1] = (m.m21 + m.m12) * mult;
            q[3] = (m.m10 - m.m01) * mult;
            break;
        case 3:
            q[0] = (m.m21 - m.m12) * mult;
            q[1] = (m.m02 - m.m20) * mult;
            q[2] = (m.m10 - m.m01) * mult;
            break;
    }

    return new Quaternion(q[0], q[1], q[2], q[3]);
}

詳細についてはマルペケさんの記事をご覧ください。

ざっくり概要だけを書くと、回転行列の各要素はクォータニオンから回転行列に変換する際に「どの要素を使うか」で求めることができます。
マルペケさんで書かれている行列を引用させていただくと、以下のようになります。

$\begin{vmatrix} 1 - 2y^2 - 2z^2 &2xy + 2wz &2xz - 2wy &0 \\ 2xy - 2wz &1 - 2x^2 - 2z^2 &2yz + 2wx &0 \\ 2xz + 2wy &2yz - 2wx &1 - 2x^2 - 2y^2 &0 \\ 0 &0 &0 &1 \end{vmatrix}$