FramePoint.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_POINT_HPP__
#define __RDL_FRAME_POINT_HPP__

#include "rdl_dynamics/FrameObject.hpp"
#include "rdl_dynamics/FrameVector.hpp"
#include "rdl_dynamics/Point3.hpp"
#include "rdl_dynamics/FrameExceptions.hpp"

namespace RobotDynamics
{
namespace Math
{
class FramePoint : public FrameObject, public Math::Point3d
{
  public:
    FramePoint(ReferenceFramePtr referenceFrame, const double x, const double y, const double z) : FrameObject(referenceFrame), Math::Point3d(x, y, z)
    {
    }

    FramePoint(ReferenceFramePtr referenceFrame, Math::Vector3d v) : FrameObject(referenceFrame), Math::Point3d(v[0], v[1], v[2])
    {
    }

    FramePoint(ReferenceFramePtr referenceFrame, const Math::Point3d& point) : FrameObject(referenceFrame), Math::Point3d(point)
    {
    }

    FramePoint(const FramePoint& framePoint) : FrameObject(framePoint.getReferenceFrame()), Math::Point3d(framePoint.x(), framePoint.y(), framePoint.z())
    {
    }

    explicit FramePoint(ReferenceFramePtr referenceFrame) : FrameObject(referenceFrame), Math::Point3d()
    {
    }

    FramePoint() : FrameObject(nullptr), Math::Point3d()
    {
    }

    ~FramePoint()
    {
    }

    inline Math::Point3d point() const
    {
        return Math::Point3d(x(), y(), z());
    }

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

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

    EIGEN_STRONG_INLINE void setIncludingFrame(const Math::Vector3d& v, ReferenceFramePtr referenceFrame)
    {
        setIncludingFrame(v(0), v(1), v(2), referenceFrame);
    }

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

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

    void setIncludingFrame(const Math::Point3d& point, ReferenceFramePtr referenceFrame)
    {
        if (!referenceFrame)
        {
            throw ReferenceFrameException("Reference frame cannot be nullptr!");
        }

        this->x() = point.x();
        this->y() = point.y();
        this->z() = point.z();
        this->referenceFrame = referenceFrame;
    }

    double distanceSquared(const FramePoint& point) const
    {
        checkReferenceFramesMatch(&point);

        double dx = this->x() - point.x();
        double dy = this->y() - point.y();
        double dz = this->z() - point.z();
        return dx * dx + dy * dy + dz * dz;
    }

    double distance(const FramePoint& point) const
    {
        checkReferenceFramesMatch(&point);

        return sqrt(distanceSquared(point));
    }

    double distanceL1(const FramePoint& point) const
    {
        checkReferenceFramesMatch(&point);

        return fabs(this->x() - point.x()) + fabs(this->y() - point.y()) + fabs(this->z() - point.z());
    }

    double distanceLinf(const FramePoint& point) const
    {
        checkReferenceFramesMatch(&point);

        double dx = this->x() - point.x();
        double dy = this->y() - point.y();
        double dz = this->z() - point.z();

        double tmp = fabs(dx) > fabs(dy) ? fabs(dx) : fabs(dy);

        return tmp > fabs(dz) ? tmp : fabs(dz);
    }

    bool epsilonEquals(const FramePoint& point, const double epsilon) const
    {
        checkReferenceFramesMatch(&point);
        return fabs(this->x() - point.x()) < epsilon && fabs(this->y() - point.y()) < epsilon && fabs(this->z() - point.z()) < epsilon;
    }

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

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

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

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

inline FramePoint operator+(FramePoint p, const FrameVector& v)
{
    p += v;
    return p;
}

inline FramePoint operator-(FramePoint p, const FrameVector& v)
{
    p -= v;
    return p;
}

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

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

inline bool operator==(const FramePoint& lhs, const FramePoint& rhs)
{
    lhs.checkReferenceFramesMatch(&rhs);

    if (lhs.x() != rhs.x())
    {
        return false;
    }

    if (lhs.y() != rhs.y())
    {
        return false;
    }

    if (lhs.z() != rhs.z())
    {
        return false;
    }

    return true;
}

inline FrameVector operator-(FramePoint p1, const FramePoint& p2)
{
    p1.getReferenceFrame()->checkReferenceFramesMatch(p2.getReferenceFrame());
    return FrameVector(p1.getReferenceFrame(), p1.x() - p2.x(), p1.y() - p2.y(), p1.z() - p2.z());
}

inline bool operator!=(const FramePoint& lhs, const FramePoint& rhs)
{
    return !operator==(lhs, rhs);
}

inline std::ostream& operator<<(std::ostream& output, const FramePoint& framePoint)
{
    output << "ReferenceFrame = " << framePoint.getReferenceFrame()->getName() << std::endl;
    output << "x = " << framePoint.x() << " y = " << framePoint.y() << " z = " << framePoint.z() << std::endl;
    return output;
}
typedef std::vector<FramePoint, Eigen::aligned_allocator<FramePoint>> FramePointV;
}  // namespace Math
}  // namespace RobotDynamics
#endif  // ifndef __RDL_FRAME_POINT_HPP__