add method for checking if 3DPoint is inside of bounding box

VirtualRobot/BoundingBox.cpp

@@ -216,4 +216,35 @@ namespace VirtualRobot
 
             return ss.str();
     }
+
+    bool BoundingBox::isInside(Eigen::Vector3f point) const
+    {
+        Eigen::Vector3f p0(min(0), min(1), min(2));
+        Eigen::Vector3f p1(min(0), min(1), max(2));
+        Eigen::Vector3f p2(min(0), max(1), min(2));
+        Eigen::Vector3f p4(max(0), min(1), min(2));
+
+        Eigen::Vector3f v01 = p0 - p1;
+        Eigen::Vector3f v02 = p0 - p2;
+        Eigen::Vector3f v04 = p0 - p4;
+
+        point(0) = std::isnan(point(0)) ? (max(0) + min(0)) / 2 : point(0);
+        point(1) = std::isnan(point(1)) ? (max(1) + min(1)) / 2 : point(1);
+        point(2) = std::isnan(point(2)) ? (max(2) + min(2)) / 2 : point(2);
+
+        auto isBetween = [](float x, float a, float b) {
+            if (a <= b)
+            {
+                return a <= x && x <= b;
+            }
+            else
+            {
+                return b <= x && x <= a;
+            }
+        };
+
+        return isBetween(v01.dot(point), v01.dot(p0), v01.dot(p1))
+                && isBetween(v02.dot(point), v02.dot(p0), v02.dot(p2))
+                && isBetween(v04.dot(point), v04.dot(p0), v04.dot(p4));
+    }
 }

VirtualRobot/BoundingBox.h

@@ -92,6 +92,15 @@ namespace VirtualRobot
 
         std::string toXML(int tabs = 2, bool skipMatrixTag = false);
 
+        /*!
+         * \brief isInside Checks whether the given point lies within the bounding box. The given vector can contain
+         * entries that are none, if this is the case the respective dimension is ignored and the point is only tested
+         * for the remaining non-nan dimensions.
+         * \param point the point to test
+         * \return True if the point lies within the bounding box. False if not.
+         */
+        bool isInside(Eigen::Vector3f point) const;
+
     protected:
         Eigen::Vector3f min;
         Eigen::Vector3f max;