KinematicRotation.hpp

浏览该文件的文档.

 
 
#pragma once
 
#include "AstGlobal.h"
#include "Rotation.hpp"
#include "AstCore/OrbitElement.hpp"    
 
AST_NAMESPACE_BEGIN
 
#define _AST_DEF_KINEMATICROTATION_PROPERTIES\
    _AST_DEF_ROTATION_PROPERTIES\
    const Vector3d& rotationRate() const { return angvel_; }\
    Vector3d& rotationRate() { return angvel_; }\
    const Vector3d& getRotationRate() const { return angvel_; }\
    Vector3d& getRotationRate() { return angvel_; }\
    void setRotationRate(const Vector3d& angvel) { angvel_ = angvel; }\
    KinematicRotation& kinematicRotation() { return reinterpret_cast<KinematicRotation&>(rotation_); }\
    const KinematicRotation& kinematicRotation() const { return reinterpret_cast<const KinematicRotation&>(rotation_); }\
    KinematicRotation& getKinematicRotation() { return kinematicRotation(); }\
    const KinematicRotation& getKinematicRotation() const { return kinematicRotation(); }\
    void setKinematicRotation(const KinematicRotation& rot) { kinematicRotation() = rot; }\
 
 
class KinematicRotation
{
public:
    static KinematicRotation Identity();
 
    KinematicRotation() = default;
 
    KinematicRotation(const Rotation& rot, const Vector3d& angvel);
 
 
    KinematicRotation(const Matrix3d& mat, const Vector3d& angvel);
 
    _AST_DEF_KINEMATICROTATION_PROPERTIES
 
    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:
    Rotation rotation_{};     
    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.rotation());
}
 
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