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