FramePoint.hpp

Go to the documentation of this file.

// Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
// RDL - Robot Dynamics Library
// 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& operator=(const FramePoint& p)
    {
        this->referenceFrame = p.getReferenceFrame();
        this->x() = p.x();
        this->y() = p.y();
        this->z() = p.z();
        return *this;
    }
 
    ~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 distance2DSquared(const FramePoint& point, int plane = 2) const
    {
        checkReferenceFramesMatch(&point);
        if (plane < 0 || plane > 2)
        {
            throw std::runtime_error("FramePoint.distance2DSquared: Plane argument must be either 0, 1, or 2");
        }
 
        double dx = plane == 0 ? 0. : this->x() - point.x();
        double dy = plane == 1 ? 0. : this->y() - point.y();
        double dz = plane == 2 ? 0. : this->z() - point.z();
        return dx * dx + dy * dy + dz * dz;
    }
 
    double distance2DSquared(const Point3d& point, int plane = 2) const
    {
        if (plane < 0 || plane > 2)
        {
            throw std::runtime_error("FramePoint.distance2DSquared: Plane argument must be either 0, 1, or 2");
        }
 
        double dx = plane == 0 ? 0. : this->x() - point.x();
        double dy = plane == 1 ? 0. : this->y() - point.y();
        double dz = plane == 2 ? 0. : this->z() - point.z();
        return dx * dx + dy * dy + dz * dz;
    }
 
    double distanceSquared(const Point3d& point) const
    {
        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 distance2D(const FramePoint& point, int plane = 2) const
    {
        checkReferenceFramesMatch(&point);
 
        return sqrt(distance2DSquared(point, plane));
    }
 
    double distance(const FramePoint& point) const
    {
        checkReferenceFramesMatch(&point);
 
        return sqrt(distanceSquared(point));
    }
 
    double distance2D(const Point3d& point, int plane = 2) const
    {
        return sqrt(distance2DSquared(point, plane));
    }
 
    double distance(const Point3d& point) const
    {
        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);
    }
 
    [[deprecated("Use isApprox(FramePoint, double) instead.")]] bool epsilonEquals(const FramePoint& point, const double epsilon) const
    {
        checkReferenceFramesMatch(&point);
        return std::abs(this->x() - point.x()) < epsilon && std::abs(this->y() - point.y()) < epsilon && std::abs(this->z() - point.z()) < epsilon;
    }
 
    bool isApprox(const FramePoint& point, const double epsilon) const
    {
        return referenceFrame == point.getReferenceFrame() && std::abs(this->x() - point.x()) < epsilon && std::abs(this->y() - point.y()) < epsilon &&
               std::abs(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 FrameVector(p.getReferenceFrame(), p.x(), p.y(), p.z());
}
 
template <typename T>
inline FrameVector operator*(FramePoint p, const T scale)
{
    p *= scale;
    return FrameVector(p.getReferenceFrame(), p.x(), p.y(), p.z());
}
 
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__