diff --git a/VirtualRobot/math/OrientedBox.h b/VirtualRobot/math/OrientedBox.h index 85486bf8d840db761a2896e3a36c18bc6752404d..0b67c5438d2b3e176d3c9257304b701beb406d0e 100644 --- a/VirtualRobot/math/OrientedBox.h +++ b/VirtualRobot/math/OrientedBox.h @@ -295,7 +295,7 @@ namespace VirtualRobot bool contains(const vector_t& p) { const vector_t b = to_box_frame(p); - static const auto check_dim = [&](int i) + const auto check_dim = [&](int i) { return _d(i) < 0 ? (b(i) <= 0 && b(i) >= _d(i)) : @@ -304,6 +304,18 @@ namespace VirtualRobot return check_dim(0) && check_dim(1) && check_dim(2); } + bool contains_by(const vector_t& p, float_t thresh = 0) + { + const vector_t b = to_box_frame(p); + const auto check_dim = [&](int i) + { + return _d(i) < 0 ? + (b(i) <= -thresh && b(i) >= _d(i) + thresh) : + (b(i) >= +thresh && b(i) <= _d(i) - thresh); + }; + return check_dim(0) && check_dim(1) && check_dim(2); + } + template<class T, class...Other> std::vector<Eigen::Matrix<T, 3, 1>, Other...> contained_points(const std::vector<Eigen::Matrix<T, 3, 1>, Other...>& ps) {