FrameVectorPair.hpp

Go to the documentation of this file.

/*
 * RDL - Robot Dynamics Library
 * Copyright (c) 2018 Jordan Lack <jlack1987@gmail.com>
 *
 * Licensed under the zlib license. See LICENSE for more details.
 */

#ifndef __RDL_FRAME_VECTOR_PAIR_HPP__
#define __RDL_FRAME_VECTOR_PAIR_HPP__

#include "rdl_dynamics/FrameObject.hpp"
#include "rdl_dynamics/FrameVector.hpp"
#include "rdl_dynamics/SpatialMotion.hpp"

namespace RobotDynamics
{
namespace Math
{
class FrameVectorPair
{
  public:
    FrameVectorPair() : lin(nullptr, 0., 0., 0.), ang(nullptr, 0., 0., 0.)
    {
    }

    /*
     * @brief constructor
     * @param linear Linear part
     * @param angular Angular part
     */
    FrameVectorPair(const FrameVector& linear, const FrameVector& angular) : lin(linear), ang(angular)
    {
        linear.checkReferenceFramesMatch(&angular);
    }

    /*
     * @brief constructor
     * @param referenceFrame pointer to a reference frame
     */
    explicit FrameVectorPair(ReferenceFramePtr referenceFrame) : lin(referenceFrame), ang(referenceFrame)
    {
    }

    explicit FrameVectorPair(const SpatialMotion& v)
    {
        lin.setIncludingFrame(v.getLinearPart(), v.getReferenceFrame());
        ang.setIncludingFrame(v.getAngularPart(), v.getReferenceFrame());
    }

    FrameVectorPair(ReferenceFramePtr referenceFrame, const Vector3d& linear, const Vector3d& angular) : lin(referenceFrame, linear), ang(referenceFrame, angular)
    {
    }

    FrameVectorPair(ReferenceFramePtr referenceFrame, const SpatialVector& v) : lin(referenceFrame, v.getLinearPart()), ang(referenceFrame, v.getAngularPart())
    {
    }

    virtual ~FrameVectorPair()
    {
    }

    void changeFrame(ReferenceFramePtr referenceFrame)
    {
        assert(referenceFrame);
        lin.changeFrame(referenceFrame);
        ang.changeFrame(referenceFrame);
    }

    FrameVectorPair changeFrameAndCopy(ReferenceFramePtr referenceFrame) const
    {
        FrameVectorPair p = *this;
        p.changeFrame(referenceFrame);
        return p;
    }

    inline void setToZero()
    {
        lin.setZero();
        ang.setZero();
    }

    void setReferenceFrame(ReferenceFramePtr referenceFrame)
    {
        lin.setReferenceFrame(referenceFrame);
        ang.setReferenceFrame(referenceFrame);
    }

    inline void setIncludingFrame(ReferenceFramePtr referenceFrame, const Vector3d& linear, const Vector3d& angular)
    {
        assert(referenceFrame);

        lin.setIncludingFrame(linear, referenceFrame);
        ang.setIncludingFrame(angular, referenceFrame);
    }

    inline void setIncludingFrame(ReferenceFramePtr referenceFrame, const SpatialVector& v)
    {
        assert(referenceFrame);

        lin.setIncludingFrame(v.getLinearPart(), referenceFrame);
        ang.setIncludingFrame(v.getAngularPart(), referenceFrame);
    }

    inline void setIncludingFrame(ReferenceFramePtr referenceFrame, const MotionVector& v)
    {
        assert(referenceFrame);

        lin.setIncludingFrame(v.getLinearPart(), referenceFrame);
        ang.setIncludingFrame(v.getAngularPart(), referenceFrame);
    }

    inline void setIncludingFrame(const SpatialMotion& v)
    {
        lin.setIncludingFrame(v.getLinearPart(), v.getReferenceFrame());
        ang.setIncludingFrame(v.getAngularPart(), v.getReferenceFrame());
    }

    inline void setLinearPart(const Vector3d& v)
    {
        lin.set(v);
    }

    inline void setAngularPart(const Vector3d& v)
    {
        ang.set(v);
    }

    inline FrameVector linear() const
    {
        return lin;
    }

    inline FrameVector angular() const
    {
        return ang;
    }

    template <typename T>
    void operator*=(const T scale)
    {
        lin *= scale;
        ang *= scale;
    }

    void operator+=(const FrameVectorPair& v)
    {
        lin += v.linear();
        ang += v.angular();
    }

    void operator-=(const FrameVectorPair& v)
    {
        lin -= v.linear();
        ang -= v.angular();
    }

    FrameVector* linearPtr()
    {
        return &lin;
    }

    FrameVector* angularPtr()
    {
        return &ang;
    }

  protected:
    FrameVector lin; 
    FrameVector ang; 
};

template <typename T>
inline FrameVectorPair operator*(FrameVectorPair pair, const T scalar)
{
    pair *= scalar;
    return pair;
}

template <typename T>
inline FrameVectorPair operator*(const T scalar, FrameVectorPair pair)
{
    pair *= scalar;
    return pair;
}

// cppcheck-suppress passedByValue
inline FrameVectorPair operator+(FrameVectorPair v1, const FrameVectorPair v2)
{
    v1 += v2;
    return v1;
}

// cppcheck-suppress passedByValue
inline FrameVectorPair operator-(FrameVectorPair v1, const FrameVectorPair v2)
{
    v1 -= v2;
    return v1;
}
}  // namespace Math
}  // namespace RobotDynamics

#endif  // ifndef __RDL_FRAME_VECTOR_HPP__