hpm6750/controller_yy_app/matrix/axis_angle.h

#pragma once
#include "helpler_funtions.h"
#include "math.h"

/**
 * @brief 使用四元数构造轴角向量
 *
 * @param Q 四元数
 * @param axisAngle 轴角
 */
static inline void AxisAngle_ByQuaternion(const float Q[4], float axisAngle[3])
{
    float q_imag[3] = {Q[1], Q[2], Q[3]};
    float q_imag_mag = sqrtf(q_imag[0] * q_imag[0] + q_imag[1] * q_imag[1] +
                             q_imag[2] * q_imag[2]);
    float r;
    if (q_imag_mag > 1e-10)
    {
        r = 2 * atan2f(q_imag_mag, Q[0]) / q_imag_mag;
    }
    else
    {
        r = 2 * sign(Q[0]);
    }

    for (int i = 0; i < 3; ++i)
    {
        axisAngle[i] = r * q_imag[i];
    }
}