KinematicRotation.hpp

浏览该文件的文档.

 
 
#pragma once
 
#include "AstGlobal.h"
#include "Rotation.hpp"
#include "AstCore/OrbitElement.hpp"    
 
AST_NAMESPACE_BEGIN
 
class KinematicRotation: protected Rotation
{
public:
    using Rotation::getMatrix;
    using Rotation::transformVector;
    using Rotation::getQuaternion;
 
    static KinematicRotation Identity();
 
    KinematicRotation() = default;
 
    KinematicRotation(const Rotation& rot, const Vector3d& angvel);
 
 
    KinematicRotation(const Matrix3d& mat, const Vector3d& angvel);
 
    const Vector3d& getRotationRate() const { return angvel_; }
 
    void setRotationRate(const Vector3d& angvel) { angvel_ = angvel; }
    
    const Rotation& getRotation() const { return *this; }
    Rotation& getRotation() { return *this; }
 
 
    void setRotation(const Rotation& rot) { (Rotation&)*this = rot; }
 
    KinematicRotation& compose(const KinematicRotation& next);
 
    KinematicRotation composed(const KinematicRotation& next) const;
 
    KinematicRotation operator*(const KinematicRotation& next) const;
 
    KinematicRotation& operator*=(const KinematicRotation& next);
 
    void getInverse(KinematicRotation& inversed) const;
 
    KinematicRotation inverse() const;
 
    void transformVectorVelocity(const Vector3d& vector, const Vector3d& velocity, Vector3d& vectorOut, Vector3d& velocityOut) const;
 
    CartState transformCartState(const CartState& state) const;
 
    void transformVectorVelocityInv(const Vector3d& vector, const Vector3d& velocity, Vector3d& vectorOut, Vector3d& velocityOut) const;
protected:
    Vector3d angvel_;       
};
 
A_ALWAYS_INLINE KinematicRotation KinematicRotation::Identity()
{
    return KinematicRotation(Matrix3d::Identity(), Vector3d::Zero());
}
 
A_ALWAYS_INLINE KinematicRotation::KinematicRotation(const Matrix3d &mat, const Vector3d &angvel)
    : Rotation(mat)
    , angvel_(angvel)
{
}
 
A_ALWAYS_INLINE KinematicRotation::KinematicRotation(const Rotation &rot, const Vector3d &angvel)
    : Rotation(rot)
    , angvel_(angvel)
{
}
 
A_ALWAYS_INLINE KinematicRotation &KinematicRotation::compose(const KinematicRotation &next)
{
    angvel_ = this->angvel_ + next.angvel_ * this->matrix_;
    matrix_ = next.matrix_ * this->matrix_;
    return *this;
}
 
A_ALWAYS_INLINE KinematicRotation KinematicRotation::composed(const KinematicRotation &next) const
{
    return KinematicRotation(next.matrix_ * this->matrix_, this->angvel_ + next.angvel_ * this->matrix_);
}
 
A_ALWAYS_INLINE KinematicRotation KinematicRotation::operator*(const KinematicRotation &next) const
{
    return composed(next);
}
 
A_ALWAYS_INLINE KinematicRotation &KinematicRotation::operator*=(const KinematicRotation &next)
{
    return compose(next);
}
 
A_ALWAYS_INLINE void KinematicRotation::getInverse(KinematicRotation &inversed) const
{
    inversed.setRotationRate(-(this->matrix_*angvel_));
    this->Rotation::getInverse(inversed);
}
 
A_ALWAYS_INLINE KinematicRotation KinematicRotation::inverse() const
{
    KinematicRotation retval;
    this->getInverse(retval);
    return retval;
}
 
A_ALWAYS_INLINE void KinematicRotation::transformVectorVelocity(const Vector3d &vector, const Vector3d &velocity, Vector3d& vectorOut, Vector3d &velocityOut) const
{
    // 注意：这里要先计算velocityOut，防止vector和vectorOut地址相同时值被覆盖
    velocityOut = this->matrix_ * (velocity - this->angvel_.cross(vector));
    vectorOut = this->matrix_ * vector;
}
 
A_ALWAYS_INLINE CartState KinematicRotation::transformCartState(const CartState &state) const
{
    CartState stateOut;
    this->transformVectorVelocity(state.pos(), state.vel(), stateOut.pos(), stateOut.vel());
    return stateOut;
}
 
A_ALWAYS_INLINE void KinematicRotation::transformVectorVelocityInv(const Vector3d &vector, const Vector3d &velocity, Vector3d &vectorOut, Vector3d &velocityOut) const
{
    vectorOut = vector * this->matrix_;
    velocityOut = velocity* this->matrix_ + this->angvel_.cross(vectorOut);
}
 
AST_NAMESPACE_END