TTwist3D.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */
#pragma once

#include <mrpt/math/TPoseOrPoint.h>

namespace mrpt::math
{
/** 3D twist: 3D velocity vector (vx,vy,vz) + angular velocity (wx,wy,wz)
 * \sa mrpt::math::TTwist2D, mrpt::math::TPose3D
 */
struct TTwist3D : public internal::ProvideStaticResize<TTwist3D>
{
    enum
    {
        static_size = 6
    };

    /** Velocity components: X,Y (m/s) */
    double vx{.0}, vy{.0}, vz{.0};
    /** Angular velocity (rad/s) */
    double wx{.0}, wy{.0}, wz{.0};

    /** Constructor from components */
    constexpr TTwist3D(
        double vx_, double vy_, double vz_, double wx_, double wy_, double wz_)
        : vx(vx_), vy(vy_), vz(vz_), wx(wx_), wy(wy_), wz(wz_)
    {
    }
    /** Default fast constructor. Initializes to zeros  */
    TTwist3D() = default;
    /** Coordinate access using operator[]. Order: vx,vy,vz, wx, wy, wz */
    double& operator[](size_t i)
    {
        switch (i)
        {
            case 0:
                return vx;
            case 1:
                return vy;
            case 2:
                return vz;
            case 3:
                return wx;
            case 4:
                return wy;
            case 5:
                return wz;
            default:
                throw std::out_of_range("index out of range");
        }
    }
    /// \overload
    constexpr double operator[](size_t i) const
    {
        switch (i)
        {
            case 0:
                return vx;
            case 1:
                return vy;
            case 2:
                return vz;
            case 3:
                return wx;
            case 4:
                return wy;
            case 5:
                return wz;
            default:
                throw std::out_of_range("index out of range");
        }
    }

    /** (i,0) access operator (provided for API compatibility with matrices).
     * \sa operator[] */
    double& operator()(int row, int col)
    {
        ASSERT_EQUAL_(col, 0);
        return (*this)[row];
    }
    /// \overload
    constexpr double operator()(int row, int col) const
    {
        ASSERT_EQUAL_(col, 0);
        return (*this)[row];
    }

    /** Scale factor */
    void operator*=(const double k)
    {
        vx *= k;
        vy *= k;
        vz *= k;
        wx *= k;
        wy *= k;
        wz *= k;
    }

    /** Transformation into vector [vx vy vz wx wy wz] */
    template <typename VECTORLIKE>
    void asVector(VECTORLIKE& v) const
    {
        v.resize(6);
        for (int i = 0; i < 6; i++) v[i] = (*this)[i];
    }
    template <typename VECTORLIKE>
    VECTORLIKE asVector() const
    {
        VECTORLIKE v;
        asVector(v);
        return v;
    }

    /** Sets from a vector [vx vy vz wx wy wz] */
    template <typename VECTORLIKE>
    void fromVector(const VECTORLIKE& v)
    {
        ASSERT_EQUAL_(v.size(), 6);
        for (int i = 0; i < 6; i++) (*this)[i] = v[i];
    }
    bool operator==(const TTwist3D& o) const;
    bool operator!=(const TTwist3D& o) const;
    /** Returns a human-readable textual representation of the object (eg: "[vx
     * vy vz wx wy wz]", omegas in deg/s)
     * \sa fromString
     */
    void asString(std::string& s) const;
    std::string asString() const
    {
        std::string s;
        asString(s);
        return s;
    }

    /** Transform all 6 components for a change of reference frame from "A" to
     * another frame "B" whose rotation with respect to "A" is given by `rot`.
     * The translational part of the pose is ignored */
    void rotate(const mrpt::math::TPose3D& rot);

    /** Set the current object value from a string generated by 'asString' (eg:
     * "[vx vy vz wx wy wz]" )
     * \sa asString
     * \exception std::exception On invalid format
     */
    void fromString(const std::string& s);

    static TTwist3D FromString(const std::string& s)
    {
        TTwist3D o;
        o.fromString(s);
        return o;
    }
};

mrpt::serialization::CArchive& operator>>(
    mrpt::serialization::CArchive& in, mrpt::math::TTwist3D& o);
mrpt::serialization::CArchive& operator<<(
    mrpt::serialization::CArchive& out, const mrpt::math::TTwist3D& o);

}  // namespace mrpt::math

namespace mrpt::typemeta
{
// Specialization must occur in the same namespace
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TTwist3D, mrpt::math)

}  // namespace mrpt::typemeta