hpm6750/controller_yy_app/matrix/quaternion.h

#pragma once

#include "matrix.h"

/// 四元数的简单构造
void Quaternion(float q[4]);

/// 四元数归一化
void Quaternion_Normalize(float Q[4]);

/// 四元数规范化
void Quaternion_Canonicalize(float Q[4]);

/// 使用旋转矩阵构造四元数
void Quaternion_ByDcm(const float dcm[3][3], float Q[4]);

/// 使用欧拉角构造四元数
void Quaternion_ByEuler(float yaw, float pitch, float roll, float Q[4]);

/// 使用旋转矩阵构造四元数
void Quaternion_ByDcm(const float R[3][3], float Q[4]);

/// 使用欧拉角构造四元数
void Quaternion_ByEuler(float yaw, float pitch, float roll, float Q[4]);

/// 使用轴角构造四元数
void Quaternion_ByAxisAngle(const float anxisAngle[3], float Q[4]);

/// 使用向量, 构造由 Vsrc->Vdst 的最短旋转四元数
void Quaternion_ByTwoVectors(const float Vsrc[3], const float Vdst[3], float Q[4]);

/// 四元数乘法
void Quaternion_Multiplication(const float Q1[4], const float Q2[4], float Q[4]);

/// 四元数求逆
void Quaternion_Inversed(const float Q[4], float Q_inv[4]);

/// 将四元数求逆
void Quaternion_Invert(float Q[4]);

/// 通过 frame1 的角速率求四元数 q21 的微分
void Quaternion_Derivative1(const float q21[4], const float angle_rate1[3], float q21_deriv[4]);

/// 通过 frame2 的角速率求四元数 q21 的微分
void Quaternion_Derivative2(const float q21[4], const float angle_rate2[3], float q21_deriv[4]);

/// 四元数通过轴角进行旋转
void Quaternion_RotateByAxisAngle(const float axisAngle[3], float Q[4]);

/// 通过四元数将 frame1 下的 V1 转到 frame2 下的 V2
void Quaternion_Conj(const float Q[4], const float V1[3], float V2[3]);

/// 通过四元数将 frame2 下的 V2 转到 frame1 下的 V1
void Quaternion_ConjInv(const float Q[4], const float V2[3], float V1[3]);

/// 通过四元数求其对应的旋转矩阵的 Z 轴
void Quaternion_GetDcmZ(const float Q[4], float dcm_z[3]);