FrameVector.hpp

Go to the documentation of this file.

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

#ifndef __RDL_FRAME_VECTOR_HPP__
#define __RDL_FRAME_VECTOR_HPP__

#include "rdl_dynamics/FrameObject.hpp"

namespace RobotDynamics
{
namespace Math
{
class FrameVector : public FrameObject, public Math::Vector3d
{
  public:
    FrameVector() : FrameObject(nullptr), Math::Vector3d(0., 0., 0.)
    {
    }

    explicit FrameVector(ReferenceFramePtr referenceFrame) : FrameObject(referenceFrame), Math::Vector3d()
    {
    }

    FrameVector(ReferenceFramePtr referenceFrame, const double& x, const double& y, const double& z) : FrameObject(referenceFrame), Math::Vector3d(x, y, z)
    {
    }

    FrameVector(ReferenceFramePtr referenceFrame, const Eigen::Vector3d& vector) : FrameObject(referenceFrame), Math::Vector3d(vector[0], vector[1], vector[2])
    {
    }

    virtual ~FrameVector()
    {
    }

    Math::TransformableGeometricObject* getTransformableGeometricObject()
    {
        return this;
    }

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

    inline void setToZero()
    {
        set(0., 0., 0.);
    }

    inline void setIncludingFrame(const double x, const double y, const double z, ReferenceFramePtr referenceFrame)
    {
        if (!referenceFrame)
        {
            throw ReferenceFrameException("Reference frame cannot be nullptr!");
        }

        set(x, y, z);
        this->referenceFrame = referenceFrame;
    }

    inline void setIncludingFrame(const Eigen::Vector3d& vector, ReferenceFramePtr referenceFrame)
    {
        if (!referenceFrame)
        {
            throw ReferenceFrameException("Reference frame cannot be nullptr!");
        }

        set(vector[0], vector[1], vector[2]);
        this->referenceFrame = referenceFrame;
    }

    inline double dot(const FrameVector& frameVector) const
    {
        checkReferenceFramesMatch(&frameVector);

        return this->x() * frameVector.x() + this->y() * frameVector.y() + this->z() * frameVector.z();
    }

    inline FrameVector cross(const FrameVector& vector) const
    {
        checkReferenceFramesMatch(&vector);
        return FrameVector(this->referenceFrame, this->y() * vector.z() - this->z() * vector.y(), vector.x() * this->z() - vector.z() * this->x(),
                           this->x() * vector.y() - this->y() * vector.x());
    }

    inline Vector3d cross(const Vector3d& vector) const
    {
        return RobotDynamics::Math::Vector3d::cross(vector);
    }

    inline double getAngleBetweenVectors(const FrameVector& frameVector) const
    {
        checkReferenceFramesMatch(&frameVector);

        return acos(std::max(-1., std::min(this->dot(frameVector) / (this->norm() * frameVector.norm()), 1.)));
    }

    inline Vector3d vec() const
    {
        return Math::Vector3d(x(), y(), z());
    }

    void operator+=(const Vector3d& v)
    {
        this->x() += v.x();
        this->y() += v.y();
        this->z() += v.z();
    }

    void operator-=(const Vector3d& v)
    {
        this->x() -= v.x();
        this->y() -= v.y();
        this->z() -= v.z();
    }

    void operator+=(const FrameVector& v)
    {
        this->checkReferenceFramesMatch(&v);
        this->x() += v.x();
        this->y() += v.y();
        this->z() += v.z();
    }

    void operator-=(const FrameVector& v)
    {
        this->checkReferenceFramesMatch(&v);
        this->x() -= v.x();
        this->y() -= v.y();
        this->z() -= v.z();
    }

    template <typename T>
    void operator*=(const T scale)
    {
        this->x() *= scale;
        this->y() *= scale;
        this->z() *= scale;
    }
};

template <typename T>
inline FrameVector operator*(FrameVector v1, const T scale)
{
    v1 *= scale;
    return v1;
}

template <typename T>
inline FrameVector operator*(const T scale, FrameVector v1)
{
    v1 *= scale;
    return v1;
}

inline FrameVector operator+(FrameVector v1, const Vector3d v2)
{
    v1 += v2;
    return v1;
}

inline FrameVector operator-(FrameVector v1, const Vector3d v2)
{
    v1 -= v2;
    return v1;
}

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

// cppcheck-suppress passedByValue
inline FrameVector operator-(FrameVector v1, const FrameVector v2)
{
    v1 -= v2;
    return v1;
}
typedef std::vector<FrameVector, Eigen::aligned_allocator<FrameVector>> FrameVectorV;
}  // namespace Math
}  // namespace RobotDynamics

#endif  // ifndef __RDL_FRAME_VECTOR_HPP__