AccelerationTransform.hpp

浏览该文件的文档.

 
 
#pragma once
 
#include "AstGlobal.h"
#include "KinematicTransform.hpp"
#include "AccelerationRotation.hpp"
 
AST_NAMESPACE_BEGIN
 
 
#define _AST_DEF_ACCELERATIONTRANSFORM_PROPERTIES\
    _AST_DEF_KINEMATICTRANSFORM_PROPERTIES\
    const Vector3d& acceleration() const { return acceleration_; }\
    Vector3d& acceleration() { return acceleration_; }\
    const Vector3d& getAcceleration() const { return acceleration_; }\
    Vector3d& getAcceleration() { return acceleration_; }\
    void setAcceleration(const Vector3d& acceleration) { acceleration_ = acceleration; }\
    const KinematicTransform& kinematicTransform() const { return reinterpret_cast<const KinematicTransform&>(velocity_); }\
    KinematicTransform& kinematicTransform() { return reinterpret_cast<KinematicTransform&>(velocity_); }\
    const KinematicTransform& getKinematicTransform() const { return kinematicTransform(); }\
    KinematicTransform& getKinematicTransform() { return kinematicTransform(); }\
    void setKinematicTransform(const KinematicTransform& transform) { kinematicTransform() = transform; }\
 
class AccelerationTransform
{
public:
    AccelerationTransform() = default;
 
    AccelerationTransform(const Vector3d& translation, const Vector3d& velocity,
                          const Vector3d& acceleration, const AccelerationRotation& rot);
 
    AccelerationTransform(const KinematicTransform& transform, const Vector3d& acceleration,
                          const Vector3d& angvelDot);
 
    static AccelerationTransform Identity();
 
    _AST_DEF_ACCELERATIONTRANSFORM_PROPERTIES
    _AST_DEF_ACCELERATIONROTATION_PROPERTIES
 
 
    AccelerationTransform& compose(const AccelerationTransform& next);
 
    AccelerationTransform composed(const AccelerationTransform& next) const;
 
    AccelerationTransform operator*(const AccelerationTransform& next) const;
 
    AccelerationTransform& operator*=(const AccelerationTransform& next);
 
    void getInverse(AccelerationTransform& inversed) const;
 
    AccelerationTransform inverse() const;
 
    void setIdentity()
    {
        acceleration_ = Vector3d::Zero();
        velocity_ = Vector3d::Zero();
        translation_ = Vector3d::Zero();
        rotation_ = Rotation::Identity();
        angvel_ = Vector3d::Zero();
        angvelDot_ = Vector3d::Zero();
    }
 
    void transformPosVelAcc(
        const Vector3d& position,
        const Vector3d& velocity,
        const Vector3d& acceleration,
        Vector3d& positionOut,
        Vector3d& velocityOut,
        Vector3d& accelerationOut) const;
 
    void transformPosVelAccInv(
        const Vector3d& position,
        const Vector3d& velocity,
        const Vector3d& acceleration,
        Vector3d& positionOut,
        Vector3d& velocityOut,
        Vector3d& accelerationOut) const;
 
protected:
    Vector3d acceleration_{};  
    Vector3d velocity_{};      
    Vector3d translation_{};   
    Rotation rotation_{};      
    Vector3d angvel_{};        
    Vector3d angvelDot_{};     
};
 
A_ALWAYS_INLINE AccelerationTransform::AccelerationTransform(const Vector3d &translation, const Vector3d &velocity, const Vector3d &acceleration, const AccelerationRotation &rot)
    : acceleration_(acceleration)
    , velocity_(velocity)
    , translation_(translation)
    , rotation_(rot.rotation())
    , angvel_(rot.rotationRate())
    , angvelDot_(rot.rotationRateDot())
{
}
 
A_ALWAYS_INLINE AccelerationTransform::AccelerationTransform(const KinematicTransform &transform, const Vector3d &acceleration, const Vector3d &angvelDot)
    : acceleration_(acceleration)
    , velocity_(transform.velocity())
    , translation_(transform.translation())
    , rotation_(transform.rotation())
    , angvel_(transform.rotationRate())
    , angvelDot_(angvelDot)
{
}
 
A_ALWAYS_INLINE AccelerationTransform AccelerationTransform::Identity()
{
    return AccelerationTransform(Vector3d::Zero(), Vector3d::Zero(), Vector3d::Zero(), AccelerationRotation::Identity());
}
 
 
 
A_ALWAYS_INLINE AccelerationTransform AccelerationTransform::composed(const AccelerationTransform &next) const
{
    // 先取到局部量，再构造返回值，避免原地赋值
    Matrix3d mat1 = this->matrix();
    Vector3d angvel1 = this->rotationRate();
    Vector3d angvelDot1 = this->rotationRateDot();
    Vector3d tau = next.translation() * mat1;             // 即 M₁ᵀ t₂
    Vector3d velocity2 = next.velocity() * mat1;          // 即 M₁ᵀ V₂
    Vector3d acceleration2 = next.acceleration() * mat1;  // 即 M₁ᵀ A₂
 
    Vector3d translation = this->translation() + tau;
    Vector3d velocity = this->velocity() + velocity2 + angvel1.cross(tau);
    Vector3d acceleration = this->acceleration() + acceleration2
        + angvel1.cross(velocity2) * 2.0
        + angvel1.cross(angvel1.cross(tau))
        + angvelDot1.cross(tau);
 
    AccelerationRotation rotation = AccelerationRotation(mat1, angvel1, angvelDot1)
        .composed(next.accelerationRotation());
 
    return AccelerationTransform(translation, velocity, acceleration, rotation);
}
 
A_ALWAYS_INLINE AccelerationTransform &AccelerationTransform::compose(const AccelerationTransform &next)
{
    *this = this->composed(next);
    return *this;
}
 
A_ALWAYS_INLINE AccelerationTransform AccelerationTransform::operator*(const AccelerationTransform &next) const
{
    return composed(next);
}
 
A_ALWAYS_INLINE AccelerationTransform &AccelerationTransform::operator*=(const AccelerationTransform &next)
{
    return compose(next);
}
 
A_ALWAYS_INLINE void AccelerationTransform::getInverse(AccelerationTransform &inversed) const
{
    // 先全部取到局部量，允许 inversed 与 *this 为同一对象
    AccelerationRotation rotation = this->accelerationRotation();
    AccelerationRotation inversedRotation;
    rotation.getInverse(inversedRotation);
 
    rotation.transformVecVelAcc(
        -this->translation(),
        -this->velocity(),
        -this->acceleration(),
        inversed.translation(),
        inversed.velocity(),
        inversed.acceleration_);
 
    inversed.setAccelerationRotation(inversedRotation);
}
 
A_ALWAYS_INLINE AccelerationTransform AccelerationTransform::inverse() const
{
    AccelerationTransform retval;
    this->getInverse(retval);
    return retval;
}
 
A_ALWAYS_INLINE void AccelerationTransform::transformPosVelAcc(
    const Vector3d &position, const Vector3d &velocity, const Vector3d &acceleration,
    Vector3d &positionOut, Vector3d &velocityOut, Vector3d &accelerationOut) const
{
    this->accelerationRotation().transformVecVelAcc(
        position - this->translation(),
        velocity - this->velocity(),
        acceleration - this->acceleration(),
        positionOut,
        velocityOut,
        accelerationOut);
}
 
A_ALWAYS_INLINE void AccelerationTransform::transformPosVelAccInv(
    const Vector3d &position, const Vector3d &velocity, const Vector3d &acceleration,
    Vector3d &positionOut, Vector3d &velocityOut, Vector3d &accelerationOut) const
{
    this->accelerationRotation().transformVecVelAccInv(
        position,
        velocity,
        acceleration,
        positionOut,
        velocityOut,
        accelerationOut);
 
    positionOut += this->translation();
    velocityOut += this->velocity();
    accelerationOut += this->acceleration();
}
 
AST_NAMESPACE_END