Rotation.hpp

浏览该文件的文档.

 
 
#pragma once
 
#include "AstGlobal.h"
#include "AstMath/Quaternion.hpp"
#include "AstMath/Matrix.hpp"
#include "AstMath/Vector.hpp"
#include "AstMath/AngleAxis.hpp"
#include "AstMath/AttitudeConvert.hpp"
 
 
AST_NAMESPACE_BEGIN
 
 
#define _AST_DEF_ROTATION_PROPERTIES\
    const Rotation& rotation() const { return rotation_; }\
    Rotation& rotation() { return rotation_; }\
    const Rotation& getRotation() const { return rotation_; }\
    Rotation& getRotation() { return rotation_; }\
    void setRotation(const Rotation& rotation) { rotation_ = rotation; }\
    const Matrix3d& matrix() const { return rotation_.matrix(); }\
    Matrix3d& matrix() { return rotation_.matrix(); }\
    const Matrix3d& getMatrix() const { return rotation_.matrix(); }\
    Matrix3d& getMatrix() { return rotation_.matrix(); }\
    void setMatrix(const Matrix3d& mat) { rotation_.setMatrix(mat); }\
    Quaternion quaternion() const { return rotation_.getQuaternion(); }\
    Quaternion getQuaternion() const { return rotation_.getQuaternion(); }\
 
 
    
class Rotation
{
public:
    static Rotation Identity();
 
    static Rotation& CastFrom(Matrix3d& mat)
    {
        return *reinterpret_cast<Rotation*>(&mat);
    }
 
    static const Rotation& CastFrom(const Matrix3d& mat)
    {
        return *reinterpret_cast<const Rotation*>(&mat);
    }
 
    static Rotation FromMatrix(const Matrix3d& mat)
    {
        return Rotation(mat);
    }
 
    Rotation() = default;
 
    Rotation(const Rotation& other) = default;
 
    explicit Rotation(const Quaternion& quat);
 
    explicit Rotation(const Matrix3d& mat);
 
    Rotation(double angle, const Vector3d& axis);
 
 
    explicit Rotation(const AngleAxis& aa);
 
    const Matrix3d& getMatrix() const { return matrix_; }
    Matrix3d& getMatrix() { return matrix_; }
    const Matrix3d& matrix() const { return matrix_; }
    Matrix3d& matrix() { return matrix_; }
 
    void setMatrix(const Matrix3d& mat) { matrix_ = mat; }
 
    Quaternion getQuaternion() const;
 
    void setQuaternion(const Quaternion& quat);
 
    Vector3d getAxis() const;
 
    double getAngle() const;
 
 
    void getAngleAxis(double& angle, Vector3d& axis) const;
 
    
    void setAngleAxis(double angle, const Vector3d& axis);
 
    void getAngleAxis(AngleAxis& aa) const;
 
    void setAngleAxis(const AngleAxis& aa);
 
    Rotation& compose(const Rotation& next);
 
    Rotation composed(const Rotation& next) const;
 
    Rotation operator*(const Rotation& next) const;
    
    Rotation& operator*=(const Rotation& next);
 
    void getInverse(Rotation& inversed) const;
 
    Rotation inverse() const;
 
    void transformVector(const Vector3d& vector, Vector3d& vectorOut) const;
 
 
    void transformVectorInv(const Vector3d& vector, Vector3d& vectorOut) const;
 
    Vector3d transformVector(const Vector3d& vector) const;
 
 
    Vector3d transformVectorInv(const Vector3d& vector) const;
 
protected:
    Matrix3d matrix_{};
};
 
 
 
A_ALWAYS_INLINE Rotation::Rotation(const Quaternion& quat)
{
    aQuatToMatrix(quat, matrix_);
}
 
 
A_ALWAYS_INLINE Rotation::Rotation(const Matrix3d& mat)
    : matrix_(mat)
{
}
 
A_ALWAYS_INLINE Rotation::Rotation(double angle, const Vector3d &axis)
    : Rotation(AngleAxis(angle, axis))
{
}
 
A_ALWAYS_INLINE Rotation::Rotation(const AngleAxis &aa)
    : matrix_(aa.toRotationMatrix())
{
}
 
A_ALWAYS_INLINE Rotation Rotation::Identity()
{
    return Rotation(Matrix3d::Identity());
}
 
A_ALWAYS_INLINE Quaternion Rotation::getQuaternion() const
{
    return aMatrixToQuat(matrix_);
}
 
A_ALWAYS_INLINE void Rotation::setQuaternion(const Quaternion& quat)
{
    aQuatToMatrix(quat, matrix_);
}
 
A_ALWAYS_INLINE Vector3d Rotation::getAxis() const
{
    AngleAxis aa;
    aMatrixToAngleAxis(matrix_, aa);
    return aa.axis();
}
 
A_ALWAYS_INLINE double Rotation::getAngle() const
{
    AngleAxis aa;
    aMatrixToAngleAxis(matrix_, aa);
    return aa.angle();
}
 
A_ALWAYS_INLINE void Rotation::getAngleAxis(double &angle, Vector3d &axis) const
{
    AngleAxis aa;
    getAngleAxis(aa);
    axis = aa.axis();
    angle = aa.angle();
}
 
A_ALWAYS_INLINE void Rotation::setAngleAxis(double angle, const Vector3d &axis)
{
    setAngleAxis(AngleAxis(angle, axis));
}
 
A_ALWAYS_INLINE void Rotation::getAngleAxis(AngleAxis &aa) const
{
    aMatrixToAngleAxis(matrix_, aa);
}
 
A_ALWAYS_INLINE void Rotation::setAngleAxis(const AngleAxis &aa)
{
    matrix_ = aa.toRotationMatrix();
}
 
A_ALWAYS_INLINE Rotation& Rotation::compose(const Rotation &next)
{
    matrix_ = next.matrix_ * matrix_;
    return *this;
}
 
A_ALWAYS_INLINE Rotation Rotation::composed(const Rotation &next) const
{
    return Rotation(next.matrix_ * matrix_);
}
 
A_ALWAYS_INLINE Rotation Rotation::operator*(const Rotation &next) const
{
    return composed(next);
}
 
A_ALWAYS_INLINE Rotation &Rotation::operator*=(const Rotation &next)
{
    return compose(next);
}
 
A_ALWAYS_INLINE void Rotation::getInverse(Rotation &inversed) const
{
    inversed.matrix_ = this->matrix_.transpose();
}
 
A_ALWAYS_INLINE Rotation Rotation::inverse() const
{
    Rotation retval;
    this->getInverse(retval);
    return retval;
}
 
A_ALWAYS_INLINE void Rotation::transformVector(const Vector3d &vector, Vector3d &vectorOut) const
{
    vectorOut = this->matrix_ * vector;
}
 
A_ALWAYS_INLINE void Rotation::transformVectorInv(const Vector3d &vector, Vector3d &vectorOut) const
{
    vectorOut = vector * this->matrix_;
}
 
A_ALWAYS_INLINE Vector3d Rotation::transformVector(const Vector3d& vector) const
{
    Vector3d retval;
    this->transformVector(vector, retval);
    return retval;
}
 
A_ALWAYS_INLINE Vector3d Rotation::transformVectorInv(const Vector3d &vector) const
{
    Vector3d retval;
    this->transformVectorInv(vector, retval);
    return retval;
}
 
AST_NAMESPACE_END