primitives/LineString.h

Go to the documentation of this file.

#pragma once
#include <utility>

#include "lanelet2_core/Forward.h"
#include "lanelet2_core/primitives/BoundingBox.h"
#include "lanelet2_core/primitives/Point.h"
#include "lanelet2_core/primitives/Primitive.h"
#include "lanelet2_core/utility/ReverseAndForwardIterator.h"
#include "lanelet2_core/utility/TransformIterator.h"
#include "lanelet2_core/utility/Utilities.h"

namespace lanelet {

using BasicLineString2d = BasicPoints2d;
using BasicLineString3d = BasicPoints3d;

template <typename PointT>
using Segment = std::pair<PointT, PointT>;

using Segment2d = Segment<Point2d>;
using ConstSegment2d = Segment<ConstPoint2d>;
using Segment3d = Segment<Point3d>;
using ConstSegment3d = Segment<ConstPoint3d>;
using BasicSegment2d = Segment<BasicPoint2d>;
using BasicSegment3d = Segment<BasicPoint3d>;

namespace traits {
template <>
struct PrimitiveTraits<BasicLineString2d> {
  using ConstType = BasicLineString2d;
  using MutableType = BasicLineString2d;
  using TwoDType = BasicLineString2d;
  using ThreeDType = BasicLineString3d;
  using Category = LineStringTag;
};
template <>
struct LineStringTraits<BasicLineString2d> : public PrimitiveTraits<BasicLineString2d> {
  using PointType = BasicPoint2d;
  using HybridType = BasicLineString2d;
};
template <>
struct PrimitiveTraits<BasicLineString3d> {
  using ConstType = BasicLineString3d;
  using MutableType = BasicLineString3d;
  using TwoDType = BasicLineString2d;
  using ThreeDType = BasicLineString3d;
  using Category = LineStringTag;
};
template <>
struct LineStringTraits<BasicLineString3d> : public PrimitiveTraits<BasicLineString3d> {
  using PointType = BasicPoint3d;
  using HybridType = BasicLineString3d;
};
template <typename PointT>
struct PrimitiveTraits<Segment<PointT>> {
  using ConstType = Segment<ConstPrimitiveType<PointT>>;
  using MutableType = Segment<MutablePrimitiveType<PointT>>;
  using TwoDType = Segment<TwoD<PointT>>;
  using ThreeDType = Segment<ThreeD<PointT>>;
  using Category = LineStringTag;
};
template <typename PointT>
struct LineStringTraits<Segment<PointT>> : public PrimitiveTraits<Segment<PointT>> {
  using PointType = PointT;
  using HybridType = Segment<traits::BasicPointT<PointT>>;
};
template <>
inline BasicLineString2d to2D<BasicLineString3d>(const BasicLineString3d& primitive) {
  BasicLineString2d ls2d(primitive.size());
  std::transform(primitive.begin(), primitive.end(), ls2d.begin(), utils::to2D<BasicPoint3d>);
  return ls2d;
}

template <typename PointT>
auto toBasicSegment(const Segment<PointT>& s) {
  return std::make_pair(toBasicPoint(s.first), toBasicPoint(s.second));
}

}  // namespace traits

namespace utils {
using traits::toBasicSegment;
}

namespace internal {
template <typename PointT>
struct SelectLsIterator {
  using Iterator = internal::TransformIterator<internal::ReverseAndForwardIterator<Points3d::iterator>, PointT>;
};
template <typename PointT>
struct SelectLsIterator<const PointT> {
  using Iterator =
      internal::TransformIterator<internal::ReverseAndForwardIterator<Points3d::const_iterator>, const PointT>;
};
template <>
struct SelectLsIterator<Point3d> {
  using Iterator = internal::ReverseAndForwardIterator<Points3d::iterator>;
};
template <>
struct SelectLsIterator<const Point3d> {
  using Iterator = internal::ReverseAndForwardIterator<Points3d::const_iterator>;
};

template <typename T>
using SelectLsIteratorT = typename SelectLsIterator<T>::Iterator;

template <typename PointT>
struct SelectBasicLineString {};

template <>
struct SelectBasicLineString<BasicPoint2d> {
  using Type = BasicLineString2d;
};

template <>
struct SelectBasicLineString<BasicPoint3d> {
  using Type = BasicLineString3d;
};

template <typename T>
using SelectBasicLineStringT = typename SelectBasicLineString<T>::Type;

inline Points3d::const_iterator pointIter(internal::ReverseAndForwardIterator<Points3d::iterator> it) {
  return Points3d::iterator(it);
}
template <typename T>
struct SelectInsertIterator {
  using Type = internal::TransformIterator<T, Point3d>;
};
template <>
struct SelectInsertIterator<typename SelectLsIterator<Point2d>::Iterator> {
  using Type = internal::ReverseAndForwardIterator<Points3d::iterator>;
};

}  // namespace internal

class LineStringData : public PrimitiveData {
 public:
  explicit LineStringData(Id id) : PrimitiveData(id) {}
  LineStringData(Id id, Points3d points, AttributeMap attributes)
      : PrimitiveData(id, std::move(attributes)), points_(std::move(points)) {}
  // NOLINTNEXTLINE
  using iterator = internal::ReverseAndForwardIterator<Points3d::iterator>;
  // NOLINTNEXTLINE
  using const_iterator = internal::ReverseAndForwardIterator<Points3d::const_iterator>;

  Points3d& points() { return points_; }

  template <typename IteratorT = const_iterator>
  IteratorT begin(bool inverted) const noexcept {
    return inverted ? IteratorT(const_iterator(points_.rbegin())) : IteratorT(const_iterator(points_.begin()));
  }

  template <typename IteratorT = iterator>
  IteratorT begin(bool inverted) noexcept {
    return inverted ? IteratorT(iterator(points_.rbegin())) : IteratorT(iterator(points_.begin()));
  }

  template <typename IteratorT = const_iterator>
  IteratorT end(bool inverted) const noexcept {
    return inverted ? IteratorT(const_iterator(points_.rend())) : IteratorT(const_iterator(points_.end()));
  }
  template <typename IteratorT = iterator>
  IteratorT end(bool inverted) noexcept {
    return inverted ? IteratorT(iterator(points_.rend())) : IteratorT(iterator(points_.end()));
  }
  auto size() const noexcept { return points_.size(); }
  auto empty() const noexcept { return points_.empty(); }
  const ConstPoint3d& at(bool inverted, size_t idx) const noexcept {
    return inverted ? points_[points_.size() - 1 - idx] : points_[idx];
  }
  const ConstPoint3d& front(bool inverted) const noexcept { return inverted ? points_.back() : points_.front(); }
  const ConstPoint3d& back(bool inverted) const noexcept { return inverted ? points_.front() : points_.back(); }

 private:
  Points3d points_;
};  // class LineStringData


template <typename PointT>
class ConstLineStringImpl : public ConstPrimitive<LineStringData> {
 public:
  // using declarations
  using BasicPointType = traits::BasicPointT<PointT>;
  using ConstPointType = traits::ConstPointT<PointT>;
  using PointType = ConstPointType;  
  using MutablePointType = traits::MutablePointT<PointT>;
  using Category = traits::LineStringTag;

  // NOLINTNEXTLINE
  using const_iterator = internal::SelectLsIteratorT<const ConstPointType>;
  using iterator = const_iterator;                             // NOLINT
  using value_type = ConstPointType;                           // NOLINT
  using size_type = size_t;                                    // NOLINT
  using difference_type = typename iterator::difference_type;  // NOLINT

  using BasicIterator = internal::TransformIterator<const_iterator, const BasicPointType>;
  using BasicLineString = internal::SelectBasicLineStringT<BasicPointType>;
  using SegmentType = Segment<PointT>;
  using ConstSegmentType = traits::ConstPrimitiveType<SegmentType>;
  static constexpr traits::Dimensions Dimension = traits::PointTraits<PointT>::Dimension;

  explicit ConstLineStringImpl(Id id = InvalId, Points3d points = Points3d(),
                               const AttributeMap& attributes = AttributeMap())
      : ConstPrimitive{std::make_shared<LineStringData>(id, std::move(points), attributes)} {}

  explicit ConstLineStringImpl(const std::shared_ptr<const LineStringData>& data, bool inverted = false)
      : ConstPrimitive(data), inverted_{inverted} {}

  template <typename OtherT>
  explicit ConstLineStringImpl(const ConstLineStringImpl<OtherT>& other)
      : ConstPrimitive(other.constData()), inverted_{other.inverted()} {}

  bool inverted() const noexcept { return inverted_; }

  size_t size() const noexcept { return constData()->size(); }

  bool empty() const noexcept { return constData()->empty(); }


  const_iterator begin() const noexcept { return constData()->template begin<const_iterator>(inverted()); }

  const_iterator end() const noexcept { return constData()->template end<const_iterator>(inverted()); }


  const ConstPointType& front() const noexcept { return constData()->front(inverted()); }

  const ConstPointType& back() const noexcept { return constData()->back(inverted()); }

  const ConstPointType& operator[](size_t idx) const noexcept {
    assert(idx < size());
    return constData()->at(inverted(), idx);
  }

  BasicIterator basicBegin() const noexcept { return constData()->template begin<BasicIterator>(inverted_); }

  BasicIterator basicEnd() const noexcept { return constData()->template end<BasicIterator>(inverted_); }

  ConstSegmentType segment(size_t idx) const noexcept {
    assert(idx < size());
    auto first = begin() + idx;
    auto second = idx + 1 == size() ? begin() : first + 1;
    return ConstSegmentType(*first, *second);
  }

  size_t numSegments() const noexcept { return size() <= 1 ? 0 : size() - 1; }

  BasicLineString basicLineString() const noexcept { return {basicBegin(), basicEnd()}; }

  template <typename LineStringT>
  class CompoundLineString;

 protected:
  // This class is only an implementation class and should never be instanciated
  ConstLineStringImpl(ConstLineStringImpl&& rhs) noexcept = default;
  ConstLineStringImpl& operator=(ConstLineStringImpl&& rhs) noexcept = default;
  ConstLineStringImpl(const ConstLineStringImpl& rhs) = default;
  ConstLineStringImpl& operator=(const ConstLineStringImpl& rhs) = default;
  ~ConstLineStringImpl() noexcept = default;

 private:
  bool inverted_{false};
};  // class ConstLineStringImpl

template <typename ConstLineStringT>
class LineStringImpl : public Primitive<ConstLineStringT> {
 protected:
  using Base = Primitive<ConstLineStringT>;
  using Base::data;

 public:
  using PointType = typename Base::MutablePointType;
  using iterator = internal::SelectLsIteratorT<PointType>;  // NOLINT
  using Base::Base;
  using Base::inverted;
  using Base::size;
  using SegmentType = typename Base::SegmentType;
  LineStringImpl() = default;  // Not inherited from base class

  template <typename OtherT>
  explicit LineStringImpl(const LineStringImpl<OtherT>& other) : Base(other) {}

  explicit LineStringImpl(const std::shared_ptr<const LineStringData>&, bool inverted) = delete;

  explicit LineStringImpl(const std::shared_ptr<LineStringData>& data, bool inverted) : Base(data, inverted) {}

  LineStringImpl& operator=(std::vector<PointType> rhs) {
    if (inverted()) {
      points() = utils::transform(rhs.rbegin(), rhs.rend(), [](const auto& v) { return Point3d(v); });
    } else {
      points() = utils::transform(rhs.begin(), rhs.end(), [](const auto& v) { return Point3d(v); });
    }
    return *this;
  }

  LineStringImpl& operator=(std::vector<Point3d>&& rhs) {
    if (inverted()) {
      utils::invert(rhs);
    }
    points() = std::move(rhs);
    return *this;
  }

  iterator insert(iterator position, const PointType& point) {
    using Iter = LineStringData::iterator;
    auto fwIter = points().insert(internal::pointIter(position), Point3d(point));
    return inverted() ? Iter::reversed(fwIter + 1) : Iter(fwIter);
  }

  template <typename InIter>
  iterator insert(iterator position, InIter start, InIter end) {
    using Viter = typename internal::SelectInsertIterator<InIter>::Type;
    using RIter = LineStringData::iterator;
    if (inverted()) {
      using RViter = std::reverse_iterator<Viter>;
      auto fwIter = points().insert(internal::pointIter(position), RViter(end), RViter(start));
      return RIter(Points3d::reverse_iterator(fwIter + 1));
    }
    auto fwIter = points().insert(internal::pointIter(position), Viter(start), Viter(end));
    return RIter(fwIter);
  }

  void push_back(const PointType& point) {  // NOLINT
    inverted() ? void(points().insert(points().begin(), Point3d(point))) : points().push_back(Point3d(point));
  }

  iterator erase(iterator position) {  // NOLINT
    if (inverted()) {
      position += 1;
    }
    using Iter = LineStringData::iterator;
    auto it = points().erase(internal::pointIter(position));
    return inverted() ? Iter::reversed(it) : Iter(it);
  }

  void pop_back() {  // NOLINT
    inverted() ? void(points().erase(points().begin())) : points().pop_back();
  }

  void reserve(size_t num) { points().reserve(num); }

  void resize(size_t num) {
    if (inverted()) {
      std::reverse(begin(), end());
    }
    points().resize(num);
    if (inverted()) {
      std::reverse(begin(), end());
    }
  }

  void clear() { points().clear(); }

  // inherit the const versions
  using Base::back;
  using Base::begin;
  using Base::end;
  using Base::front;
  using Base::operator[];
  using Base::segment;

  iterator begin() { return inverted() ? iterator(points().rbegin()) : iterator(points().begin()); }

  iterator end() { return inverted() ? iterator(points().rend()) : iterator(points().end()); }

  PointType& front() { return inverted() ? points().back() : points().front(); }

  PointType& back() { return inverted() ? points().front() : points().back(); }

  PointType& operator[](size_t idx) {
    assert(idx < size());
    return points()[inverted() ? points().size() - 1 - idx : idx];
  }

  SegmentType segment(size_t idx) noexcept {
    assert(idx < size());
    const auto snd = idx + 1;
    return SegmentType(operator[](idx), operator[](snd == size() ? 0 : snd));
  }

 protected:
  // This class is only an implementation class and should never be instanciated
  LineStringImpl(LineStringImpl&& rhs) noexcept = default;
  LineStringImpl& operator=(LineStringImpl&& rhs) noexcept = default;
  LineStringImpl(const LineStringImpl& rhs) = default;
  LineStringImpl& operator=(const LineStringImpl& rhs) = default;
  ~LineStringImpl() noexcept = default;
  Points3d& points() { return data()->points(); }
};  // class LineStringImpl

class ConstLineString3d : public ConstLineStringImpl<Point3d> {
 public:
  using ConstLineStringImpl::ConstLineStringImpl;
  using ConstType = ConstLineString3d;
  using MutableType = LineString3d;
  using TwoDType = ConstLineString2d;
  using ThreeDType = ConstLineString3d;
  using HybridType = ConstHybridLineString3d;
  ConstLineString3d() = default;  // Not inherited from base class

  ConstLineString3d invert() const noexcept { return ConstLineString3d{constData(), !inverted()}; }
};

class LineString3d : public LineStringImpl<ConstLineString3d> {
 public:
  using LineStringImpl::LineStringImpl;
  using TwoDType = LineString2d;
  using ThreeDType = LineString3d;
  LineString3d() = default;  // Not inherited from base class

  LineString3d invert() const noexcept { return LineString3d{data(), !inverted()}; }
};

class ConstLineString2d : public ConstLineStringImpl<Point2d> {
 public:
  using ConstLineStringImpl::ConstLineStringImpl;
  using ConstType = ConstLineString2d;
  using MutableType = LineString2d;
  using TwoDType = ConstLineString2d;
  using ThreeDType = ConstLineString3d;
  using HybridType = ConstHybridLineString2d;
  ConstLineString2d() = default;  // Not inherited from base class

  ConstLineString2d invert() const noexcept { return ConstLineString2d{constData(), !inverted()}; }
};

class LineString2d : public LineStringImpl<ConstLineString2d> {
 public:
  using LineStringImpl::LineStringImpl;
  using TwoDType = LineString2d;
  using ThreeDType = LineString3d;
  LineString2d() = default;  // Not inherited from base class

  LineString2d invert() const noexcept { return LineString2d{data(), !inverted()}; }
};

class ConstHybridLineString3d : public ConstLineString3d {
 public:
  using PointType = BasicPointType;
  using const_iterator = BasicIterator;  // NOLINT
  using iterator = BasicIterator;        // NOLINT
  using ConstType = ConstHybridLineString3d;
  using TwoDType = ConstHybridLineString2d;
  using ThreeDType = ConstHybridLineString3d;
  using SegmentType = Segment<BasicPointType>;

  using ConstLineString3d::ConstLineString3d;
  ConstHybridLineString3d() = default;  // Not inherited from base class
  explicit ConstHybridLineString3d(const ConstLineString3d& ls) : ConstLineString3d(ls) {}
  explicit ConstHybridLineString3d(const LineString3d& ls) : ConstLineString3d(ls) {}

  ConstHybridLineString3d invert() const noexcept { return ConstHybridLineString3d{constData(), !inverted()}; }

  BasicIterator begin() const noexcept { return basicBegin(); }

  BasicIterator end() const noexcept { return basicEnd(); }

  const BasicPointType& front() const noexcept { return ConstLineString3d::front().basicPoint(); }

  const BasicPointType& back() const noexcept { return ConstLineString3d::back().basicPoint(); }

  const BasicPointType& operator[](size_t idx) const noexcept {
    return ConstLineString3d::operator[](idx).basicPoint();
  }

  SegmentType segment(size_t idx) const noexcept {
    assert(idx < size());
    const auto snd = idx + 1;
    return {operator[](idx), operator[](snd == size() ? 0 : snd)};
  }
};

class ConstHybridLineString2d : public ConstLineString2d {
 public:
  using PointType = BasicPointType;
  using const_iterator = BasicIterator;  // NOLINT
  using iterator = BasicIterator;        // NOLINT
  using ConstType = ConstHybridLineString2d;
  using TwoDType = ConstHybridLineString2d;
  using ThreeDType = ConstHybridLineString3d;
  using SegmentType = Segment<BasicPointType>;

  using ConstLineString2d::ConstLineString2d;
  ConstHybridLineString2d() = default;  // Not inherited from base class
  explicit ConstHybridLineString2d(const ConstLineString2d& ls) : ConstLineString2d(ls) {}
  explicit ConstHybridLineString2d(const LineString2d& ls) : ConstLineString2d(ls) {}

  ConstHybridLineString2d invert() const noexcept { return ConstHybridLineString2d{constData(), !inverted()}; }

  BasicIterator begin() const noexcept { return basicBegin(); }

  BasicIterator end() const noexcept { return basicEnd(); }

  const BasicPointType& front() const noexcept { return ConstLineString2d::front().basicPoint(); }

  const BasicPointType& back() const noexcept { return ConstLineString2d::back().basicPoint(); }

  const BasicPointType& operator[](size_t idx) const noexcept {
    return ConstLineString2d::operator[](idx).basicPoint();
  }

  SegmentType segment(size_t idx) const noexcept {
    assert(idx < size());
    const auto snd = idx + 1;
    return {operator[](idx), operator[](snd == size() ? 0 : snd)};
  }
};

inline std::ostream& operator<<(std::ostream& stream, const ConstLineString2d& obj) {
  stream << "[id: " << obj.id();
  if (obj.inverted()) {
    stream << ", inverted";
  }
  stream << " point ids: ";
  for (auto it = obj.begin(); it != obj.end(); ++it) {
    stream << it->id();
    if (std::next(it) != obj.end()) {
      stream << ", ";
    }
  }
  return stream << "]";
}

inline std::ostream& operator<<(std::ostream& stream, const ConstLineString3d& obj) {
  stream << "[id: " << obj.id();
  if (obj.inverted()) {
    stream << ", inverted";
  }
  stream << " point ids: ";
  for (auto it = obj.begin(); it != obj.end(); ++it) {
    stream << it->id();
    if (std::next(it) != obj.end()) {
      stream << ", ";
    }
  }
  return stream << "]";
}

// comparison
template <typename LhsPointT, typename RhsPointT>
bool operator==(const ConstLineStringImpl<LhsPointT>& lhs, const ConstLineStringImpl<RhsPointT>& rhs) {
  return lhs.constData() == rhs.constData() && lhs.inverted() == rhs.inverted();
}
template <typename LhsPointT, typename RhsPointT>
bool operator!=(const ConstLineStringImpl<LhsPointT>& lhs, const ConstLineStringImpl<RhsPointT>& rhs) {
  return !(lhs == rhs);
}

template <typename PointT>
bool operator==(const ConstLineStringImpl<PointT>& lhs, const std::vector<PointT>& rhs) {
  return lhs.size() == rhs.size() && std::equal(lhs.begin(), lhs.end(), rhs.begin());
}

template <typename PointT>
bool operator==(const std::vector<PointT>& lhs, const ConstLineStringImpl<PointT>& rhs) {
  return rhs == lhs;
}

template <typename PointT>
bool operator!=(const ConstLineStringImpl<PointT>& lhs, const std::vector<PointT>& rhs) {
  return !(lhs == rhs);
}

template <typename PointT>
bool operator!=(const std::vector<PointT>& lhs, const ConstLineStringImpl<PointT>& rhs) {
  return !(lhs == rhs);
}

namespace traits {
template <typename T>
constexpr bool isLinestringT() {
  return isCategory<T, traits::LineStringTag>();
}

namespace detail {
template <typename T, typename Enable = void>
struct HybridType {};

template <typename T>
struct HybridType<T, std::enable_if_t<traits::isLinestringT<T>(), void>> {
  using Type = typename LineStringTraits<T>::HybridType;
};
template <typename T>
struct HybridType<T, std::enable_if_t<!traits::isLinestringT<T>(), void>> {
  using Type = typename LineStringTraits<T>::HybridType;
};
}  // namespace detail
template <typename LineStringT>
using HybridT = typename detail::HybridType<LineStringT>::Type;

template <typename LineStringT>
constexpr auto toHybrid(const LineStringT ls) {
  return HybridT<LineStringT>(ls);
}
}  // namespace traits

template <typename T, typename RetT>
using IfLS = std::enable_if_t<traits::isLinestringT<T>(), RetT>;

template <typename T1, typename T2, typename RetT>
using IfLS2 = std::enable_if_t<traits::isLinestringT<T1>() && traits::isLinestringT<T2>(), RetT>;

namespace utils {
using traits::toHybrid;

template <typename PointT>
bool has(const ConstLineStringImpl<PointT>& ls, Id id) {
  return std::any_of(ls.begin(), ls.end(), [&id](const auto& elem) { return elem.id() == id; });
}
}  // namespace utils

}  // namespace lanelet

// Hash function for usage in containers
namespace std {
template <>
struct hash<lanelet::LineString3d> : public lanelet::HashBase<lanelet::LineString3d> {};
template <>
struct hash<lanelet::ConstLineString3d> : public lanelet::HashBase<lanelet::ConstLineString3d> {};
template <>
struct hash<lanelet::LineString2d> : public lanelet::HashBase<lanelet::LineString2d> {};
template <>
struct hash<lanelet::ConstLineString2d> : public lanelet::HashBase<lanelet::ConstLineString2d> {};
template <>
struct hash<lanelet::ConstHybridLineString2d> : public lanelet::HashBase<lanelet::ConstHybridLineString2d> {};
template <>
struct hash<lanelet::ConstHybridLineString3d> : public lanelet::HashBase<lanelet::ConstHybridLineString3d> {};
}  // namespace std