primitives/Point.h

Go to the documentation of this file.

#pragma once
#pragma GCC diagnostic push
#if defined __GNUC__ && (__GNUC__ >= 6)
#pragma GCC diagnostic ignored "-Wignored-attributes"
#pragma GCC diagnostic ignored "-Wint-in-bool-context"
#endif
#include <Eigen/Core>
#pragma GCC diagnostic pop
#include <utility>
 
#include "lanelet2_core/Forward.h"
#include "lanelet2_core/primitives/Primitive.h"
#include "lanelet2_core/primitives/Traits.h"
#include "lanelet2_core/utility/TransformIterator.h"
#include "lanelet2_core/utility/Utilities.h"
 
namespace lanelet {
 
using BasicPoint3d = Eigen::Vector3d;                                
using BasicPoint2d = Eigen::Matrix<double, 2, 1, Eigen::DontAlign>;  
using BasicPoints2d = std::vector<Eigen::Vector2d,
                                  Eigen::aligned_allocator<Eigen::Vector2d>>;  
using BasicPoints3d = std::vector<BasicPoint3d>;                               
 
namespace internal {
template <>
class Converter<const BasicPoint2d> {
 public:
  template <typename InType>
  const BasicPoint2d& convert(const InType& t) const {
    return t.basicPoint2d();
  }
};
}  // namespace internal
 
namespace traits {
template <>
struct PrimitiveTraits<BasicPoint3d> {
  using ConstType = BasicPoint3d;
  using MutableType = BasicPoint3d;
  using TwoDType = BasicPoint2d;
  using ThreeDType = BasicPoint3d;
  using Category = PointTag;
};
template <>
struct PrimitiveTraits<BasicPoint2d> {
  using ConstType = BasicPoint2d;
  using MutableType = BasicPoint2d;
  using TwoDType = BasicPoint2d;
  using ThreeDType = BasicPoint3d;
  using Category = PointTag;
};
template <>
struct PrimitiveTraits<Eigen::Vector2d> {
  using ConstType = BasicPoint2d;
  using MutableType = BasicPoint2d;
  using TwoDType = BasicPoint2d;
  using ThreeDType = BasicPoint3d;
  using Category = PointTag;
};
template <>
struct PointTraits<BasicPoint3d> : PrimitiveTraits<BasicPoint3d> {
  using BasicPoint = BasicPoint3d;
  using ConstPoint = typename PrimitiveTraits<BasicPoint3d>::ConstType;
  using MutablePoint = typename PrimitiveTraits<BasicPoint3d>::MutableType;
  static constexpr bool IsPrimitive = false;
  static constexpr Dimensions Dimension = Dimensions::Three;
};
template <>
struct PointTraits<BasicPoint2d> : PrimitiveTraits<BasicPoint2d> {
  using BasicPoint = BasicPoint2d;
  using ConstPoint = typename PrimitiveTraits<BasicPoint2d>::ConstType;
  using MutablePoint = typename PrimitiveTraits<BasicPoint2d>::MutableType;
  static constexpr bool IsPrimitive = false;
  static constexpr Dimensions Dimension = Dimensions::Two;
};
template <>
struct PointTraits<Eigen::Vector2d> : PrimitiveTraits<Eigen::Vector2d> {
  using BasicPoint = BasicPoint2d;
  using ConstPoint = typename PrimitiveTraits<BasicPoint2d>::ConstType;
  using MutablePoint = typename PrimitiveTraits<BasicPoint2d>::MutableType;
  static constexpr bool IsPrimitive = false;
  static constexpr Dimensions Dimension = Dimensions::Two;
};
 
template <>
inline BasicPoint3d to3D(const BasicPoint2d& primitive) {
  return {primitive.x(), primitive.y(), 0};
}
template <>
inline BasicPoint2d to2D(const BasicPoint3d& primitive) {
  return {primitive.x(), primitive.y()};
}
}  // namespace traits
 
struct ArcCoordinates {
  double length{0.};    
  double distance{0.};  
};
 
class PointData : public PrimitiveData {  // NOLINT
 public:
  PointData(Id id, BasicPoint3d point, const AttributeMap& attributes = AttributeMap())
      : PrimitiveData(id, attributes), point(point), point2d_{point.x(), point.y()} {}
  PointData(const PointData&) = delete;
  PointData& operator=(const LineStringData&) = delete;
  PointData(PointData&&) = default;
  PointData& operator=(PointData&&) = default;
  ~PointData() = default;
  const BasicPoint2d& point2d() const {
    if (BOOST_UNLIKELY(point.head<2>() != point2d_)) {
      point2d_ = point.head<2>();
    }
    return point2d_;
  }
 
  BasicPoint3d point;  // NOLINT(cppcoreguidelines-non-private-member-variables-in-classes)
 
 private:
  mutable BasicPoint2d point2d_;
};
 
 
class ConstPoint2d : public ConstPrimitive<PointData> {
 public:
  using BasicPoint = BasicPoint2d;  
  using ConstType = ConstPoint2d;
  using MutableType = Point2d;
  using TwoDType = ConstPoint2d;
  using ThreeDType = ConstPoint3d;
  using Category = traits::PointTag;
  static constexpr traits::Dimensions Dimension = traits::Dimensions::Two;
 
  using ConstPrimitive::ConstPrimitive;
 
  ConstPoint2d(Id id, const BasicPoint3d& point, const AttributeMap& attributes = AttributeMap())
      : ConstPrimitive<PointData>{std::make_shared<PointData>(id, point, attributes)} {}  // NOLINT
 
 
  explicit ConstPoint2d(Id id = InvalId, double x = 0., double y = 0., double z = 0.,
                        const AttributeMap& attributes = AttributeMap())
      : ConstPrimitive<PointData>{std::make_shared<PointData>(  // NOLINT
            id, BasicPoint3d(x, y, z), attributes)} {}
 
  operator BasicPoint2d() const noexcept {  // NOLINT
    return point2d();
  }
 
  const BasicPoint& basicPoint() const noexcept { return point2d(); }
 
  const BasicPoint2d& basicPoint2d() const noexcept { return point2d(); }
 
  double x() const noexcept { return point().x(); }
 
  double y() const noexcept { return point().y(); }
 
 protected:
  const BasicPoint3d& point() const { return constData()->point; }
  const BasicPoint2d& point2d() const { return constData()->point2d(); }
};
 
class ConstPoint3d : public ConstPoint2d {
 public:
  using BasicPoint = BasicPoint3d;
  using ConstType = ConstPoint3d;
  using MutableType = Point3d;
  static constexpr traits::Dimensions Dimension = traits::Dimensions::Three;
 
  using ConstPoint2d::ConstPoint2d;
  ConstPoint3d() = default;
  explicit ConstPoint3d(const ConstPoint2d& other) : ConstPoint2d(other) {}
 
  operator BasicPoint2d() const noexcept = delete;  // NOLINT
 
  operator const BasicPoint3d&() const noexcept {  // NOLINT
    return point();
  }
 
  const BasicPoint& basicPoint() const noexcept { return point(); }
 
  double z() const noexcept { return point().z(); }
};
 
class Point2d : public Primitive<ConstPoint3d> {
 public:
  using BasicPoint = BasicPoint2d;
  using ConstType = ConstPoint2d;
  using MutableType = Point2d;
  using TwoDType = Point2d;
  using ThreeDType = Point3d;
  static constexpr traits::Dimensions Dimension = traits::Dimensions::Two;
 
  using Primitive::Primitive;
  using Primitive::x;
  using Primitive::y;
  Point2d() = default;
  operator const BasicPoint3d&() const = delete;
 
  operator BasicPoint2d() const noexcept {  // NOLINT
    return point().head<2>();
  }
 
  auto basicPoint() noexcept { return point().head<2>(); }
 
  const BasicPoint2d& basicPoint() const noexcept { return point2d(); }
 
  double& x() noexcept { return point().x(); }
 
  double& y() noexcept { return point().y(); }
  double z() const noexcept = delete;
 
 protected:
  using Primitive::point;
  BasicPoint3d& point() { return data()->point; }
};
 
class Point3d : public Point2d {
 public:
  using BasicPoint = BasicPoint3d;
  using ConstType = ConstPoint3d;
  using MutableType = Point3d;
  Point3d() = default;
  static constexpr traits::Dimensions Dimension = traits::Dimensions::Three;
 
  using Point2d::Point2d;
 
  explicit Point3d(const Point2d& other) : Point2d(other) {}
 
  using Primitive::x;
  using Primitive::y;
  using Primitive::z;
 
  operator BasicPoint2d() const noexcept = delete;
 
  operator BasicPoint3d&() noexcept { return point(); }  // NOLINT
 
  operator const BasicPoint3d&() const noexcept { return point(); }  // NOLINT
 
  BasicPoint& basicPoint() noexcept { return point(); }
 
  const BasicPoint& basicPoint() const noexcept { return point(); }
 
  double& x() noexcept { return point().x(); }
 
  double& y() noexcept { return point().y(); }
 
  double& z() noexcept { return point().z(); }
};
 
inline std::ostream& operator<<(std::ostream& stream, const ConstPoint2d& obj) {
  return stream << "[id: " << obj.id() << " x: " << obj.x() << " y: " << obj.y() << "]";
}
 
inline std::ostream& operator<<(std::ostream& stream, const ConstPoint3d& obj) {
  return stream << "[id: " << obj.id() << " x: " << obj.x() << " y: " << obj.y() << " z: " << obj.z() << "]";
}
 
inline std::ostream& operator<<(std::ostream& stream, const Point2d& obj) {
  return stream << "[id: " << obj.id() << " x: " << obj.x() << " y: " << obj.y() << "]";
}
 
inline std::ostream& operator<<(std::ostream& stream, const Point3d& obj) {
  return stream << "[id: " << obj.id() << " x: " << obj.x() << " y: " << obj.y() << " z: " << obj.z() << "]";
}
 
namespace traits {
template <typename T>
constexpr bool isPointT() {
  return isCategory<T, traits::PointTag>();
}
}  // namespace traits
template <typename T, typename RetT>
using IfPT = std::enable_if_t<traits::isPointT<T>(), RetT>;
 
}  // namespace lanelet
 
// Hash function for usage in containers
namespace std {
template <>
struct hash<lanelet::Point3d> : public lanelet::HashBase<lanelet::Point3d> {};
template <>
struct hash<lanelet::ConstPoint3d> : public lanelet::HashBase<lanelet::ConstPoint3d> {};
template <>
struct hash<lanelet::Point2d> : public lanelet::HashBase<lanelet::Point2d> {};
template <>
struct hash<lanelet::ConstPoint2d> : public lanelet::HashBase<lanelet::ConstPoint2d> {};
}  // namespace std