Quaternion.hpp

浏览该文件的文档.

 
 
#pragma once
 
#include "AstGlobal.h"
#include "AstMath/MathOperator.hpp"
#include "AstMath/Matrix.hpp"
#include "AstMath/AttitudeConvertProto.hpp"
 
AST_NAMESPACE_BEGIN
 
class Quaternion;
 
Quaternion aQuatProduct(const Quaternion& q1, const Quaternion& q2);
 
 
class Quaternion
{
public:
    static Quaternion Identity(){return Quaternion{1,0,0,0};}
 
    static Quaternion FromRotationMatrix(const Matrix3d& mtx){return aMatrixToQuat(mtx);}
 
    double norm() const{return _ASTMATH norm(*this); }
    void normalize(){ _ASTMATH normalize(*this); }
    Quaternion normalized() const{return _ASTMATH normalized(*this);}
    void setIdentity(){ *this = {1,0,0,0};}
    double qs() const{return m_qs;}
    double qx() const{return m_qx;}
    double qy() const{return m_qy;}
    double qz() const{return m_qz;}
    double& qs(){return m_qs;}
    double& qx(){return m_qx;}
    double& qy(){return m_qy;}
    double& qz(){return m_qz;}
    const Vector3d& vec() const {return (const Vector3d&)m_qx;}
 
public:
    // 兼容Eigen接口
 
    double x() const{return m_qx;}
    double y() const{return m_qy;}
    double z() const{return m_qz;}
    double w() const{return m_qs;}
 
    double& x(){return m_qx;}
    double& y(){return m_qy;}
    double& z(){return m_qz;}
    double& w(){return m_qs;}
 
    A_DEF_POD_ITERABLE(double)
public:
    double squaredNorm() const{return _ASTMATH squaredNorm(*this);}
 
 
    Quaternion conjugate() const{return {qs(), -qx(), -qy(), -qz()};}
 
    Quaternion inverse() const{return conjugate() / squaredNorm();}
 
 
    Quaternion operator*(const Quaternion& q2) const{return aQuatProduct(*this, q2);}
 
    Quaternion& operator*=(const Quaternion& q2){return *this = aQuatProduct(*this, q2);}
    
 
    Quaternion operator*(double val) const{ Quaternion v{*this}; v *= val; return v;}
 
    Quaternion& operator*=(double val){qs() *= val; qx() *= val; qy() *= val; qz() *= val; return *this;}
 
 
    Quaternion operator/(double val) const{ Quaternion v{*this}; v /= val; return v;}
 
    Quaternion& operator/=(double val){qs() /= val; qx() /= val; qy() /= val; qz() /= val; return *this;}
public:
    Matrix3d toRotationMatrix() const{
        Matrix3d mtx;
        aQuatToMatrix(*this, mtx);
        return mtx;
    }
 
public:
    // 设置为public仅为实现聚合初始化，不要直接访问成员变量
    double m_qs;
    double m_qx;
    double m_qy;
    double m_qz;
};
 
 
inline Quaternion aQuatProduct(const Quaternion& q1, const Quaternion& q2)
{
    return {
        q1.qs() * q2.qs() - q1.qx() * q2.qx() - q1.qy() * q2.qy() - q1.qz() * q2.qz(),
        q1.qs() * q2.qx() + q1.qx() * q2.qs() + q1.qy() * q2.qz() - q1.qz() * q2.qy(),
        q1.qs() * q2.qy() - q1.qx() * q2.qz() + q1.qy() * q2.qs() + q1.qz() * q2.qx(),
        q1.qs() * q2.qz() + q1.qx() * q2.qy() - q1.qy() * q2.qx() + q1.qz() * q2.qs(),
    };
}
 
 
 
AST_NAMESPACE_END
 
#include "AttitudeConvertInline.hpp"
 
 
AST_DECL_TYPE_ALIAS(Quaternion)