Point3d.h

Go to the documentation of this file.

 
#pragma once
 
#include <Eigen/Core>
#include <cassert>
#include <limits>
 
#include "mathlib_export.h"
 
namespace MathLib
{
class Point3d
{
public:
    Point3d();
 
    explicit Point3d(std::array<double, 3> x);
 
    virtual ~Point3d() = default;
 
    Point3d(Point3d const&) = default;
    Point3d& operator=(Point3d const&) = default;
 
    const double& operator[](std::size_t idx) const
    {
        assert(idx < 3);
        return x_[idx];
    }
    double& operator[](std::size_t idx)
    {
        return const_cast<double&>(static_cast<const Point3d&>(*this)[idx]);
    }
 
    const double* data() const { return x_.data(); }
 
    double* data() { return x_.data(); }
 
    Eigen::Vector3d const& asEigenVector3d() const { return x_; }
    Eigen::Vector3d& asEigenVector3d() { return x_; }
 
private:
    Eigen::Vector3d x_;
};
 
inline bool operator<(Point3d const& a, Point3d const& b)
{
    return std::lexicographical_compare(a.data(), a.data() + 3, b.data(),
                                        b.data() + 3);
}
 
bool lessEq(Point3d const& a, Point3d const& b,
            double eps = std::numeric_limits<double>::epsilon());
 
inline std::ostream& operator<<(std::ostream& os, const Point3d& p)
{
    os << p[0] << " " << p[1] << " " << p[2];
    return os;
}
 
extern MATHLIB_EXPORT const Point3d ORIGIN;
MathLib::Point3d operator*(Eigen::Matrix3d const& mat,
                           MathLib::Point3d const& p);
 
double sqrDist(MathLib::Point3d const& p0, MathLib::Point3d const& p1);
 
inline bool operator==(Point3d const& a, Point3d const& b)
{
    auto const sqr_dist(sqrDist(a, b));
    auto const eps = std::numeric_limits<double>::epsilon();
    return (sqr_dist < eps * eps);
}
 
inline double sqrDist2d(MathLib::Point3d const& p0, MathLib::Point3d const& p1)
{
    return (p0[0] - p1[0]) * (p0[0] - p1[0]) +
           (p0[1] - p1[1]) * (p0[1] - p1[1]);
}
 
}  // end namespace MathLib