primitives/BoundingBox.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/Geometry>

#include "lanelet2_core/primitives/Point.h"
#include "lanelet2_core/primitives/Traits.h"
#pragma GCC diagnostic pop

namespace lanelet {

class BoundingBox2d {
 public:
  enum { AmbientDimAtCompileTime = 2 };
  using Scalar = double;
  using ScalarTraits = Eigen::NumTraits<Scalar>;
  using Index = Eigen::Index;
  using RealScalar = ScalarTraits::Real;
  using NonInteger = ScalarTraits::NonInteger;
  using VectorType = BasicPoint2d;

  enum CornerType {
    Min = 0,
    Max = 1,
    BottomLeft = 0,
    BottomRight = 1,
    TopLeft = 2,
    TopRight = 3,
    BottomLeftFloor = 0,
    BottomRightFloor = 1,
    TopLeftFloor = 2,
    TopRightFloor = 3,
    BottomLeftCeil = 4,
    BottomRightCeil = 5,
    TopLeftCeil = 6,
    TopRightCeil = 7
  };

  inline BoundingBox2d() {
    if (AmbientDimAtCompileTime != Eigen::Dynamic) {
      setEmpty();
    }
  }

  inline explicit BoundingBox2d(Index dim) : min_(dim), max_(dim) { setEmpty(); }

  template <typename OtherVectorType1, typename OtherVectorType2>
  inline BoundingBox2d(OtherVectorType1 min, OtherVectorType2 max) : min_(std::move(min)), max_(std::move(max)) {}

  template <typename Derived>
  inline explicit BoundingBox2d(const Eigen::MatrixBase<Derived>& p) : min_(p), max_(min_) {}

  inline BoundingBox2d(const Eigen::AlignedBox<double, 2>& other) : min_(other.min()), max_(other.max()) {}  // NOLINT

  inline Index dim() const {
    return AmbientDimAtCompileTime == Eigen::Dynamic ? min_.size() : Index(AmbientDimAtCompileTime);
  }

  inline bool isNull() const { return isEmpty(); }

  inline void setNull() { setEmpty(); }

  inline bool isEmpty() const { return (min_.array() > max_.array()).any(); }

  inline void setEmpty() {
    min_.setConstant(ScalarTraits::highest());
    max_.setConstant(ScalarTraits::lowest());
  }

  inline const VectorType&(min)() const { return min_; }
  inline VectorType&(min)() { return min_; }
  inline const VectorType&(max)() const { return max_; }
  inline VectorType&(max)() { return max_; }

  inline BasicPoint2d center() const { return (min_ + max_) / 2; }

  inline BasicPoint2d sizes() const { return max_ - min_; }

  inline Scalar volume() const { return sizes().prod(); }

  inline BasicPoint2d diagonal() const { return sizes(); }

  inline VectorType corner(CornerType corner) const {
    VectorType res;

    Index mult = 1;
    for (Index d = 0; d < dim(); ++d) {
      if ((mult & corner) != 0) {
        res[d] = max_[d];
      } else {
        res[d] = min_[d];
      }
      mult *= 2;
    }
    return res;
  }

  inline VectorType sample() const { return Eigen::AlignedBox<double, 2>(min_, max_).sample(); }

  template <typename Derived>
  inline bool contains(const Eigen::MatrixBase<Derived>& p) const {
    return Eigen::AlignedBox<double, 2>(min_, max_).contains(p);
  }

  inline bool contains(const BoundingBox2d& b) const {
    return (min_.array() <= (b.min)().array()).all() && ((b.max)().array() <= max_.array()).all();
  }

  inline bool intersects(const BoundingBox2d& b) const {
    return (min_.array() <= (b.max)().array()).all() && ((b.min)().array() <= max_.array()).all();
  }

  template <typename Derived>
  inline BoundingBox2d& extend(const Eigen::MatrixBase<Derived>& p) {
    auto pN(p.derived().eval());
    min_ = min_.cwiseMin(pN);
    max_ = max_.cwiseMax(pN);
    return *this;
  }

  inline BoundingBox2d& extend(const BoundingBox2d& b) {
    min_ = min_.cwiseMin(b.min_);
    max_ = max_.cwiseMax(b.max_);
    return *this;
  }

  inline BoundingBox2d& clamp(const BoundingBox2d& b) {
    min_ = min_.cwiseMax(b.min_);
    max_ = max_.cwiseMin(b.max_);
    return *this;
  }

  inline BoundingBox2d intersection(const BoundingBox2d& b) const {
    return {min_.cwiseMax(b.min_), max_.cwiseMin(b.max_)};
  }

  inline BoundingBox2d merged(const BoundingBox2d& b) const { return {min_.cwiseMin(b.min_), max_.cwiseMax(b.max_)}; }

  template <typename Derived>
  inline BoundingBox2d& translate(const Eigen::MatrixBase<Derived>& aT) {
    const auto t(aT.derived().eval());
    min_ += t;
    max_ += t;
    return *this;
  }

  template <typename Derived>
  inline Scalar squaredExteriorDistance(const Eigen::MatrixBase<Derived>& p) const {
    return Eigen::AlignedBox<double, 2>(min_, max_).squaredExteriorDistance(p);
  }

  inline Scalar squaredExteriorDistance(const BoundingBox2d& b) const {
    using EigenBox = Eigen::AlignedBox<double, 2>;
    return EigenBox(min_, max_).squaredExteriorDistance(EigenBox(b.min_, b.max_));
  }

  template <typename Derived>
  inline NonInteger exteriorDistance(const Eigen::MatrixBase<Derived>& p) const {
    return std::sqrt(NonInteger(squaredExteriorDistance(p)));
  }

  inline NonInteger exteriorDistance(const BoundingBox2d& b) const {
    using std::sqrt;
    return sqrt(NonInteger(squaredExteriorDistance(b)));
  }

  template <typename OtherScalarType>
  inline explicit BoundingBox2d(const Eigen::AlignedBox<OtherScalarType, AmbientDimAtCompileTime>& other) {
    min_ = (other.min)().template cast<Scalar>();
    max_ = (other.max)().template cast<Scalar>();
  }

  bool isApprox(const BoundingBox2d& other, const RealScalar& prec = ScalarTraits::dummy_precision()) const {
    return min_.isApprox(other.min_, prec) && max_.isApprox(other.max_, prec);
  }

 private:
  VectorType min_, max_;
};

using BoundingBox3d = Eigen::AlignedBox3d;

namespace traits {
template <>
struct PrimitiveTraits<BoundingBox2d> {
  using ConstType = BoundingBox2d;
  using MutableType = BoundingBox2d;
  using TwoDType = BoundingBox2d;
  using ThreeDType = BoundingBox3d;
  using Category = BoundingBoxTag;
};
template <>
struct PrimitiveTraits<BoundingBox3d> {
  using ConstType = BoundingBox3d;
  using MutableType = BoundingBox3d;
  using TwoDType = BoundingBox2d;
  using ThreeDType = BoundingBox3d;
  using Category = BoundingBoxTag;
};

inline BoundingBox3d to3D(const BoundingBox2d& primitive) { return {to3D(primitive.min()), to3D(primitive.max())}; }
inline BoundingBox2d to2D(const BoundingBox3d& primitive) { return {to2D(primitive.min()), to2D(primitive.max())}; }
}  // namespace traits
}  // namespace lanelet