与方向相关的计算代码备忘

1. 计算两个3D方向之间的四元数

计算从一个3D方向最短旋转到另一个3D方向的四元数的Python代码如下：

import numpy as np
from pyrr import Quaternion, Vector3


def rotation_to(src: Vector3, dst: Vector3) -> Quaternion:
    """Get a quaternion representing the rotation from src to dst.

    Args:
        src (Vector3): source direction.
        dst (Vector3): destination direction.

    Returns:
        Quaternion: the rotation from src to dst.

    Reference:
        https://stackoverflow.com/questions/1171849/finding-quaternion-representing-the-rotation-from-one-vector-to-another
        https://github.com/toji/gl-matrix/blob/f0583ef53e94bc7e78b78c8a24f09ed5e2f7a20c/src/gl-matrix/quat.js#L54

    >>> v1 = Vector3([1, 2, 3]).normalized
    >>> v2 = Vector3([4, 5, 6]).normalized
    >>> q = rotation_to(v1, v2)
    >>> v3 = q * v1
    >>> np.isclose(v2, v3).all()
    Vector3(True)
    """

    src = src.normalized
    dst = dst.normalized
    dot = src | dst
    if dot < -0.999999:
        tmp = Vector3([1, 0, 0]) ^ src
        if tmp.length < 0.000001:
            tmp = Vector3([0, 1, 0]) ^ src
        tmp = tmp.normalized
        return Quaternion(tmp.tolist() + [np.pi]).normalized
    elif dot > 0.999909:
        return Quaternion([0, 0, 0, 1])
    else:
        return Quaternion((src ^ dst).tolist() + [1 + dot]).normalized

2. 计算两个2D方向的顺时针旋转度数及逆运算

import numpy as np

def clockwise_rotation(src: np.ndarray, dst: np.ndarray) -> float:
    """get clockwise rotation from src to dst in degree.

    Args:
        src (np.ndarray): source direction.
        dst (np.ndarray): destination direction.

    Returns:
        float: clockwise rotation in degree.
    """

    ang1 = np.arctan2(*src[::-1])
    ang2 = np.arctan2(*dst[::-1])
    return np.rad2deg((ang1 - ang2) % (2 * np.pi))


def ratate_clockwise(src: np.ndarray, degree: float) -> np.ndarray:
    """rotate src with degree in clockwise

    Args:
        src (np.ndarray): source direction.
        degree (float): clockwise rotation in degree.

    Returns:
        np.ndarray: ratated direction.
    """

    radian = np.deg2rad(degree)
    s = np.sin(radian)
    c = np.cos(radian)
    x = src[0] * c + src[1] * s
    y = -src[0] * s + src[1] * c
    return np.array([x, y])