KinematicTransform.hpp

浏览该文件的文档.

 
 
#pragma once
 
#include "AstGlobal.h"
#include "Transform.hpp"
#include "AstMath/KinematicRotation.hpp"
#include "AstCore/OrbitElement.hpp"   
 
AST_NAMESPACE_BEGIN
 
 
#define _AST_DEF_KINEMATICTRANSFORM_PROPERTIES\
    _AST_DEF_TRANSFORM_PROPERTIES\
    const Vector3d& velocity() const { return velocity_; }\
    Vector3d& velocity() { return velocity_; }\
    const Vector3d& getVelocity() const { return velocity_; }\
    Vector3d& getVelocity() { return velocity_; }\
    void setVelocity(const Vector3d& velocity) { velocity_ = velocity; }\
    const Transform& transform() const { return reinterpret_cast<const Transform&>(translation_); }\
    Transform& transform() { return reinterpret_cast<Transform&>(translation_); }\
    const Transform& getTransform() const { return transform(); }\
    Transform& getTransform() { return transform(); }\
    void setTransform(const Transform& xform) { transform() = xform; }
 
 
 
class KinematicTransform
{
public:
    KinematicTransform() = default;
 
    A_ALWAYS_INLINE
    KinematicTransform(const CartState& translate, const KinematicRotation& rot)
        : velocity_(translate.vel())
        , translation_(translate.pos())
        , rotation_(rot.rotation())
        , angvel_(rot.rotationRate()) 
    {}
 
    A_ALWAYS_INLINE
    KinematicTransform(const Vector3d& translation, const Vector3d& velocity, const KinematicRotation& rot)
        : velocity_(velocity)
        , translation_(translation)
        , rotation_(rot.rotation())
        , angvel_(rot.rotationRate()) 
    {}
 
    static KinematicTransform Identity();
 
 
public:
    _AST_DEF_KINEMATICTRANSFORM_PROPERTIES
    _AST_DEF_KINEMATICROTATION_PROPERTIES
 
    KinematicTransform& compose(const KinematicTransform& next);
 
    KinematicTransform composed(const KinematicTransform& next) const;
 
    KinematicTransform operator*(const KinematicTransform& next) const;
 
    KinematicTransform& operator*=(const KinematicTransform& next);
 
 
    KinematicTransform inverse() const;
 
    void getInverse(KinematicTransform& inversed) const;
    
    void setIdentity();
 
    void transformPositionVelocity(const Vector3d& position, const Vector3d& velocity, Vector3d& positionOut, Vector3d& velocityOut);
 
    CartState transformPositionVelocity(const CartState& state);
protected:
    Vector3d velocity_{};     
    Vector3d translation_{};  
    Rotation rotation_{};     
    Vector3d angvel_{};       
};
 
A_ALWAYS_INLINE KinematicTransform KinematicTransform::Identity()
{
    return KinematicTransform(CartState::Zero(), KinematicRotation::Identity());
}
 
A_ALWAYS_INLINE void KinematicTransform::setIdentity()
{
    velocity_ = Vector3d::Zero();
    translation_ = Vector3d::Zero();
    rotation_ = Rotation::Identity();
    angvel_ = Vector3d::Zero();
}
 
A_ALWAYS_INLINE KinematicTransform &KinematicTransform::compose(const KinematicTransform &next)
{
    *this = this->composed(next);
    return *this;
}
 
A_ALWAYS_INLINE KinematicTransform KinematicTransform::composed(const KinematicTransform &next) const
{
    KinematicRotation rotation = this->kinematicRotation().composed(next.kinematicRotation());
    /*
    也可以通过 getRotation().transformVectorVelocityInv() 来实现。
    */
    Vector3d vector = next.translation() * this->matrix();
 
    Vector3d translation = this->translation() + vector;
    Vector3d velocity = this->velocity() + next.velocity() * this->matrix() 
                        + this->rotationRate().cross(vector);
    return KinematicTransform(translation, velocity, rotation);
}
 
A_ALWAYS_INLINE KinematicTransform KinematicTransform::operator*(const KinematicTransform &next) const
{
    return composed(next);
}
 
A_ALWAYS_INLINE KinematicTransform &KinematicTransform::operator*=(const KinematicTransform &next)
{
    return compose(next);
}
 
A_ALWAYS_INLINE KinematicTransform KinematicTransform::inverse() const
{
    KinematicTransform retval;
    this->getInverse(retval);
    return retval;
}
 
A_ALWAYS_INLINE void KinematicTransform::getInverse(KinematicTransform &inversed) const
{
    this->kinematicRotation().transformVectorVelocity(-this->translation(), -this->velocity(), inversed.translation(), inversed.velocity());
    this->kinematicRotation().getInverse(inversed.kinematicRotation());
}
 
A_ALWAYS_INLINE void KinematicTransform::transformPositionVelocity(const Vector3d &position, const Vector3d &velocity, Vector3d &positionOut, Vector3d &velocityOut)
{
    #if 0
    // 先计算相对位置向量
    Vector3d relPos = position - this->getTranslation();
    // 旋转位置
    this->getRotation().transformVector(relPos, positionOut);
    // 计算速度：绝对速度 - 平动速度 - 角速度 × 相对位置向量
    Vector3d velDiff = velocity - this->getVelocity() - this->getRotationRate().cross(relPos);
    // 旋转速度
    this->getRotation().transformVector(velDiff, velocityOut);
    #else
    this->kinematicRotation().transformVectorVelocity(
        position - this->translation(), 
        velocity - this->velocity(), 
        positionOut, 
        velocityOut
    );
    #endif
}
 
A_ALWAYS_INLINE CartState KinematicTransform::transformPositionVelocity(const CartState &state)
{
    CartState stateOut;
    this->transformPositionVelocity(state.pos(), state.vel(), stateOut.pos(), stateOut.vel());
    return stateOut;
}
 
 
AST_NAMESPACE_END