Program Listing for File transform.hpp

↰ Return to documentation for file (src/tap/algorithms/transforms/transform.hpp)

/*
 * Copyright (c) 2022-2023 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/math_user_utils.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 roll, float pitch, float yaw);

    // TODO: template specialization for transform between identical frames??
    static inline Transform identity() { return Transform(0., 0., 0., 0., 0., 0.); }

    Position apply(const Position& position) const;

    Vector apply(const Vector& vector) const;

    Orientation apply(const Orientation& orientation) 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 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();
    }

    Transform getInverse() const;

    Transform compose(const Transform& second) const;

    /* Getters */
    inline Position getTranslation() const { return Position(translation); };

    inline Orientation getRotation() const { return Orientation(rotation); }

    float getRoll() const;

    float getPitch() const;

    float getYaw() 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]; }

private:
    CMSISMat<3, 1> translation;

    CMSISMat<3, 3> rotation;

    CMSISMat<3, 3> tRotation;
};  // class Transform
}  // namespace tap::algorithms::transforms

#endif  // TAPROOT_TRANSFORM_HPP_