Add OrientedBox::contains_by

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)
         {