KinematicTransform.hpp

浏览该文件的文档.

 
 
#pragma once
 
#include "AstGlobal.h"
#include "Transform.hpp"
#include "AstMath/KinematicRotation.hpp"
#include "AstCore/OrbitElement.hpp"   
 
AST_NAMESPACE_BEGIN
 
class KinematicTransform : protected Transform
{
public:
    KinematicTransform() = default;
 
    A_ALWAYS_INLINE
    KinematicTransform(const CartState& translate, const KinematicRotation& rot)
        : Transform(translate.pos(), rot.getRotation())
        , angvel_(rot.getRotationRate()) 
        , velocity_(translate.vel())
    {}
 
    A_ALWAYS_INLINE
    KinematicTransform(const Vector3d& translation, const Vector3d& velocity, const KinematicRotation& rot)
        : Transform(translation, rot.getRotation())
        , angvel_(rot.getRotationRate()) 
        , velocity_(velocity)
    {}
 
    static KinematicTransform Identity();
 
    using Transform::getTranslation;
    using Transform::setTranslation;
 
    const Transform& getTransform() const { return *this; }
 
    const Vector3d& getRotationRate() const { return angvel_; }
    Vector3d& getRotationRate() { return angvel_; }
 
    void setTransform(const Transform& transform) { (Transform&)*this = transform; }
 
    const KinematicRotation& getKinematicRotation() const { return kinematicRotation(); }
    KinematicRotation& getKinematicRotation() { return kinematicRotation(); }
 
    void setKinematicRotation(const KinematicRotation& rot) { kinematicRotation() = rot; }
 
    const Vector3d& getVelocity() const { return velocity_; }
    Vector3d& getVelocity() { return velocity_; }
 
    void setVelocity(const Vector3d& vel) { velocity_ = vel; }
 
    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() { Transform::setIdentity(); angvel_ = Vector3d::Zero(); velocity_ = Vector3d::Zero(); }
 
    void transformPositionVelocity(const Vector3d& position, const Vector3d& velocity, Vector3d& positionOut, Vector3d& velocityOut);
 
    CartState transformPositionVelocity(const CartState& state);
private:
    KinematicRotation& kinematicRotation() { return reinterpret_cast<KinematicRotation&>(rotation_); }
    const KinematicRotation& kinematicRotation() const { return reinterpret_cast<const KinematicRotation&>(rotation_); }
protected:
    Vector3d angvel_;
    Vector3d velocity_;
};
 
A_ALWAYS_INLINE KinematicTransform KinematicTransform::Identity()
{
    return KinematicTransform(CartState::Zero(), KinematicRotation::Identity());
}
 
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->getKinematicRotation().composed(next.getKinematicRotation());
    /*
    也可以通过 getRotation().transformVectorVelocityInv() 来实现。
    */
    Vector3d vector = next.getTranslation() * this->getMatrix();
 
    Vector3d translation = this->getTranslation() + vector;
    Vector3d velocity = this->getVelocity() + next.getVelocity() * this->getMatrix() 
                        + this->getRotationRate().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->getKinematicRotation().transformVectorVelocity(-this->getTranslation(), -this->getVelocity(), inversed.getTranslation(), inversed.getVelocity());
    this->getKinematicRotation().getInverse(inversed.getKinematicRotation());
}
 
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->getKinematicRotation().transformVectorVelocity(
        position - this->getTranslation(), 
        velocity - this->getVelocity(), 
        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