transform.hpp

Go to the documentation of this file.

/*****************************************************************************/
/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/
/*****************************************************************************/
 
/*
 * Copyright (c) 2022-2024 Advanced Robotics at the University of Washington <robomstr@uw.edu>
 *
 * This file is part of Taproot.
 *
 * Taproot is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Taproot is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Taproot.  If not, see <https://www.gnu.org/licenses/>.
 */
 
#ifndef TAPROOT_TRANSFORM_HPP_
#define TAPROOT_TRANSFORM_HPP_
 
#include "tap/algorithms/cmsis_mat.hpp"
#include "tap/algorithms/math_user_utils.hpp"
 
#include "angular_velocity.hpp"
#include "dynamic_orientation.hpp"
#include "dynamic_position.hpp"
#include "orientation.hpp"
#include "position.hpp"
#include "vector.hpp"
 
namespace tap::algorithms::transforms
{
class Transform
{
public:
    Transform(const Position& translation, const Orientation& rotation);
    Transform(Position&& translation, Orientation&& rotation);
 
    Transform(const CMSISMat<3, 1>& translation, const CMSISMat<3, 3>& rotation);
    Transform(CMSISMat<3, 1>&& translation, CMSISMat<3, 3>&& rotation);
 
    Transform(float x, float y, float z, float rx, float ry, float rz);
 
    Transform(
        const Position& translation,
        const Orientation& rotation,
        const Vector& velocity,
        const Vector& acceleration,
        const Vector& angularVelocity);
 
    Transform(
        Position&& translation,
        Orientation&& rotation,
        Vector&& velocity,
        Vector&& acceleration,
        Vector&& angularVelocity);
 
    Transform(const DynamicPosition& dynamicPosition, const DynamicOrientation& dynamicOrientation);
    Transform(DynamicPosition&& dynamicPosition, DynamicOrientation&& dynamicOrientation);
 
    Transform(
        const CMSISMat<3, 1>& translation,
        const CMSISMat<3, 3>& rotation,
        const CMSISMat<3, 1>& velocity,
        const CMSISMat<3, 1>& acceleration,
        const CMSISMat<3, 3>& angularVelocity);
 
    Transform(
        CMSISMat<3, 1>&& translation,
        CMSISMat<3, 3>&& rotation,
        CMSISMat<3, 1>&& velocity,
        CMSISMat<3, 1>&& acceleration,
        CMSISMat<3, 3>&& angularVelocity);
 
    Transform(
        float x,
        float y,
        float z,
        float vx,
        float vy,
        float vz,
        float ax,
        float ay,
        float az,
        float rx,
        float ry,
        float rz,
        float wx,
        float wy,
        float wz);
 
    static inline Transform identity() { return Transform(0., 0., 0., 0., 0., 0.); }
 
    Position apply(const Position& position) const;
 
    Vector apply(const Vector& vector) const;
 
    DynamicPosition apply(const DynamicPosition& dynamicPosition) const;
 
    Orientation apply(const Orientation& orientation) const;
 
    DynamicOrientation apply(const DynamicOrientation& dynamicOrientation) const;
 
    inline void updateTranslation(const Position& newTranslation)
    {
        this->translation = newTranslation.coordinates();
    }
 
    inline void updateTranslation(Position&& newTranslation)
    {
        this->translation = std::move(newTranslation.coordinates());
    }
 
    inline void updateTranslation(float x, float y, float z)
    {
        this->translation = CMSISMat<3, 1>({x, y, z});
    }
 
    inline void updateTranslation(const DynamicPosition& newTranslation)
    {
        this->translation = newTranslation.position;
        this->transVel = newTranslation.velocity;
        this->transAcc = newTranslation.acceleration;
    }
 
    inline void updateTranslation(DynamicPosition&& newTranslation)
    {
        this->translation = std::move(newTranslation.position);
        this->transVel = std::move(newTranslation.velocity);
        this->transAcc = std::move(newTranslation.acceleration);
    }
 
    inline void updateRotation(const Orientation& newRotation)
    {
        this->rotation = newRotation.matrix();
        this->tRotation = this->rotation.transpose();
    }
 
    inline void updateRotation(Orientation&& newRotation)
    {
        this->rotation = std::move(newRotation.matrix());
        this->tRotation = this->rotation.transpose();
    }
 
    void updateRotation(float roll, float pitch, float yaw)
    {
        this->rotation = Orientation(roll, pitch, yaw).matrix();
        this->tRotation = this->rotation.transpose();
    }
 
    inline void updateRotation(const DynamicOrientation& newRotation)
    {
        this->rotation = newRotation.orientation;
        this->tRotation = this->rotation.transpose();
        this->angVel = newRotation.angularVelocity;
    }
 
    inline void updateRotation(DynamicOrientation&& newRotation)
    {
        this->rotation = std::move(newRotation.orientation);
        this->tRotation = this->rotation.transpose();
        this->angVel = std::move(newRotation.angularVelocity);
    }
 
    inline void updateVelocity(const Vector& newVelocity)
    {
        this->transVel = newVelocity.coordinates();
        checkDynamic();
    }
 
    inline void updateVelocity(Vector&& newVelocity)
    {
        this->transVel = std::move(newVelocity.coordinates());
        checkDynamic();
    }
 
    inline void updateVelocity(float vx, float vy, float vz)
    {
        this->transVel = CMSISMat<3, 1>({vx, vy, vz});
        checkDynamic();
    }
 
    inline void updateAcceleration(const Vector& newAcceleration)
    {
        this->transVel = newAcceleration.coordinates();
        checkDynamic();
    }
 
    inline void updateAcceleration(Vector&& newAcceleration)
    {
        this->transVel = std::move(newAcceleration.coordinates());
        checkDynamic();
    }
 
    inline void updateAcceleration(float ax, float ay, float az)
    {
        this->transAcc = CMSISMat<3, 1>({ax, ay, az});
        checkDynamic();
    }
 
    inline void updateAngularVelocity(const Vector& newAngularVelocity)
    {
        this->angVel = AngularVelocity::skewMatFromAngVel(
            newAngularVelocity.x(),
            newAngularVelocity.y(),
            newAngularVelocity.z());
        checkDynamic();
    }
 
    inline void updateAngularVelocity(Position&& newAngularVelocity)
    {
        this->angVel = AngularVelocity::skewMatFromAngVel(
            newAngularVelocity.x(),
            newAngularVelocity.y(),
            newAngularVelocity.z());
        checkDynamic();
    }
 
    inline void updateAngularVelocity(float vr, float vp, float vy)
    {
        this->angVel = AngularVelocity::skewMatFromAngVel(vr, vp, vy);
        checkDynamic();
    }
 
    Transform getInverse() const;
 
    Transform compose(const Transform& second) const;
 
    Transform composeStatic(const Transform& second) const;
 
    Transform projectForward(float dt) const;
 
    /* Getters */
    inline Position getTranslation() const { return Position(translation); };
 
    inline Vector getVelocity() const { return Vector(transVel); };
 
    inline Vector getAcceleration() const { return Vector(transAcc); };
 
    inline DynamicPosition getDynamicTranslation() const
    {
        return DynamicPosition(translation, transVel, transAcc);
    };
 
    inline Orientation getRotation() const { return Orientation(rotation); }
 
    inline Vector getAngularVel() const
    {
        return Vector(getRollVelocity(), getPitchVelocity(), getYawVelocity());
    }
 
    inline DynamicOrientation getDynamicOrientation() const
    {
        return DynamicOrientation(rotation, angVel);
    };
 
    float getRoll() const;
 
    float getPitch() const;
 
    float getYaw() const;
 
    float getRollVelocity() const;
 
    float getPitchVelocity() const;
 
    float getYawVelocity() const;
 
    inline float getX() const { return this->translation.data[0]; }
 
    inline float getY() const { return this->translation.data[1]; }
 
    inline float getZ() const { return this->translation.data[2]; }
 
    inline float getXVel() const { return this->transVel.data[0]; }
 
    inline float getYVel() const { return this->transVel.data[1]; }
 
    inline float getZVel() const { return this->transVel.data[2]; }
 
    inline float getXAcc() const { return this->transAcc.data[0]; }
 
    inline float getYAcc() const { return this->transAcc.data[1]; }
 
    inline float getZAcc() const { return this->transAcc.data[2]; }
 
    inline bool isDynamic() const { return dynamic; }
 
private:
    bool dynamic{true};
 
    CMSISMat<3, 1> translation;
 
    CMSISMat<3, 1> transVel;
 
    CMSISMat<3, 1> transAcc;
 
    CMSISMat<3, 3> rotation;
 
    CMSISMat<3, 3> tRotation;
 
    CMSISMat<3, 3> angVel;
 
    inline void checkDynamic()
    {
        dynamic = false;
 
        dynamic |=
            !(compareFloatClose(getXVel(), 0, 1e-5) && compareFloatClose(getYVel(), 0, 1e-5) &&
              compareFloatClose(getZVel(), 0, 1e-5));
 
        dynamic |=
            !(compareFloatClose(getXAcc(), 0, 1e-5) && compareFloatClose(getYAcc(), 0, 1e-5) &&
              compareFloatClose(getZAcc(), 0, 1e-5));
 
        dynamic |=
            !(compareFloatClose(getRollVelocity(), 0, 1e-5) &&
              compareFloatClose(getPitchVelocity(), 0, 1e-5) &&
              compareFloatClose(getYawVelocity(), 0, 1e-5));
    }
};  // class Transform
}  // namespace tap::algorithms::transforms
 
#endif  // TAPROOT_TRANSFORM_HPP_