AccelerationRotation.hpp

浏览该文件的文档.

 
 
#pragma once
 
#include "AstGlobal.h"
#include "KinematicRotation.hpp"
 
AST_NAMESPACE_BEGIN
 
 
#define _AST_DEF_ACCELERATIONROTATION_PROPERTIES\
    _AST_DEF_KINEMATICROTATION_PROPERTIES\
    const Vector3d& rotationRateDot() const { return angvelDot_; }\
    Vector3d& rotationRateDot() { return angvelDot_; }\
    const Vector3d& getRotationRateDot() const { return rotationRateDot(); }\
    Vector3d& getRotationRateDot() { return rotationRateDot(); }\
    void setRotationRateDot(const Vector3d& angvelDot) { angvelDot_ = angvelDot; }\
    const AccelerationRotation& accelerationRotation() const { return reinterpret_cast<const AccelerationRotation&>(rotation_); }\
    AccelerationRotation& accelerationRotation() { return reinterpret_cast<AccelerationRotation&>(rotation_); }\
    const AccelerationRotation& getAccelerationRotation() const { return accelerationRotation(); }\
    AccelerationRotation& getAccelerationRotation() { return accelerationRotation(); }\
    void setAccelerationRotation(const AccelerationRotation& rot) { accelerationRotation() = rot; }\
 
 
class AccelerationRotation
{
public:
 
    static AccelerationRotation Identity();
 
    AccelerationRotation() = default;
 
    AccelerationRotation(const Matrix3d& mat, const Vector3d& angvel, const Vector3d& angvelDot);
 
    AccelerationRotation(const Rotation& rot, const Vector3d& angvel, const Vector3d& angvelDot);
 
    AccelerationRotation(const KinematicRotation& rot, const Vector3d& angvelDot);
 
    _AST_DEF_ACCELERATIONROTATION_PROPERTIES
 
    AccelerationRotation& compose(const AccelerationRotation& next);
 
    AccelerationRotation composed(const AccelerationRotation& next) const;
 
    AccelerationRotation operator*(const AccelerationRotation& next) const;
 
    AccelerationRotation& operator*=(const AccelerationRotation& next);
 
    void getInverse(AccelerationRotation& inversed) const;
 
    AccelerationRotation inverse() const;
 
    void transformVecVelAcc(
        const Vector3d& position,
        const Vector3d& velocity,
        const Vector3d& acceleration,
        Vector3d& positionOut,
        Vector3d& velocityOut,
        Vector3d& accelerationOut) const;
 
    void transformVecVelAccInv(
        const Vector3d& position,
        const Vector3d& velocity,
        const Vector3d& acceleration,
        Vector3d& positionOut,
        Vector3d& velocityOut,
        Vector3d& accelerationOut) const;
protected:
    Rotation rotation_{};     
    Vector3d angvel_{};       
    Vector3d angvelDot_{};    
};
 
A_ALWAYS_INLINE AccelerationRotation AccelerationRotation::Identity()
{
    return AccelerationRotation(Matrix3d::Identity(), Vector3d::Zero(), Vector3d::Zero());
}
 
A_ALWAYS_INLINE AccelerationRotation::AccelerationRotation(const Matrix3d &mat, const Vector3d &angvel, const Vector3d &angvelDot)
    : rotation_(mat)
    , angvel_(angvel)
    , angvelDot_(angvelDot)
{
}
 
A_ALWAYS_INLINE AccelerationRotation::AccelerationRotation(const Rotation &rot, const Vector3d &angvel, const Vector3d &angvelDot)
    : rotation_(rot)
    , angvel_(angvel)
    , angvelDot_(angvelDot)
{
}
 
A_ALWAYS_INLINE AccelerationRotation::AccelerationRotation(const KinematicRotation &rot, const Vector3d &angvelDot)
    : rotation_(rot.rotation())
    , angvel_(rot.rotationRate())
    , angvelDot_(angvelDot)
{
}
 
A_ALWAYS_INLINE AccelerationRotation AccelerationRotation::composed(const AccelerationRotation &next) const
{
    Matrix3d mat1 = this->matrix();
    Vector3d angvel1 = this->angvel_;
    Vector3d angvel2InSource = next.angvel_ * mat1;      // 即 M1ᵀ ω2
    Vector3d angvel = angvel1 + angvel2InSource;
    Vector3d angvelDot = this->angvelDot_ + angvel1.cross(angvel2InSource) + next.angvelDot_ * mat1;
    return AccelerationRotation(next.matrix() * mat1, angvel, angvelDot);
}
 
A_ALWAYS_INLINE AccelerationRotation &AccelerationRotation::compose(const AccelerationRotation &next)
{
    *this = this->composed(next);
    return *this;
}
 
A_ALWAYS_INLINE AccelerationRotation AccelerationRotation::operator*(const AccelerationRotation &next) const
{
    return composed(next);
}
 
A_ALWAYS_INLINE AccelerationRotation &AccelerationRotation::operator*=(const AccelerationRotation &next)
{
    return compose(next);
}
 
A_ALWAYS_INLINE void AccelerationRotation::getInverse(AccelerationRotation &inversed) const
{
    // 先取出matrix_，允许inversed与*this为同一对象
    Matrix3d mat = this->matrix();
    Vector3d angvel = this->angvel_;
    Vector3d angvelDot = this->angvelDot_;
    inversed.matrix() = mat.transpose();
    inversed.angvel_ = -(mat * angvel);
    inversed.angvelDot_ = -(mat * angvelDot);   // ω×ω = 0，故角加速度没有附加项
}
 
A_ALWAYS_INLINE AccelerationRotation AccelerationRotation::inverse() const
{
    AccelerationRotation retval;
    this->getInverse(retval);
    return retval;
}
 
A_ALWAYS_INLINE void AccelerationRotation::transformVecVelAcc(
    const Vector3d &position, const Vector3d &velocity, const Vector3d &acceleration,
    Vector3d &positionOut, Vector3d &velocityOut, Vector3d &accelerationOut) const
{
    // 注意：这里要先计算accelerationOut，再计算velocityOut，最后计算positionOut，
    //       防止入参与出参地址相同时值被覆盖
    accelerationOut = this->matrix() * (acceleration
        - this->angvel_.cross(velocity) * 2.0
        - this->angvelDot_.cross(position)
        + this->angvel_.cross(this->angvel_.cross(position)));
    velocityOut = this->matrix() * (velocity - this->angvel_.cross(position));
    positionOut = this->matrix() * position;
}
 
A_ALWAYS_INLINE void AccelerationRotation::transformVecVelAccInv(
    const Vector3d &position, const Vector3d &velocity, const Vector3d &acceleration,
    Vector3d &positionOut, Vector3d &velocityOut, Vector3d &accelerationOut) const
{
    // 注意：这里要先计算positionOut，再计算velocityOut，最后计算accelerationOut
    positionOut = position * this->matrix();
    velocityOut = velocity * this->matrix() + this->angvel_.cross(positionOut);
    accelerationOut = acceleration * this->matrix()
        + this->angvelDot_.cross(positionOut)
        + this->angvel_.cross(velocityOut) * 2.0
        - this->angvel_.cross(this->angvel_.cross(positionOut));
}
 
AST_NAMESPACE_END