diff --git a/VirtualRobot/MathTools.cpp b/VirtualRobot/MathTools.cpp
index 60100a95b081217550e860ce6f17a6a0ad65b242..2a09eb60ae17c988ed5e0d99721180e2a8f3c23b 100644
--- a/VirtualRobot/MathTools.cpp
+++ b/VirtualRobot/MathTools.cpp
@@ -12,6 +12,7 @@
#include <boost/math/special_functions/fpclassify.hpp>
+#include <Eigen/Dense>
#include <Eigen/Geometry>
#define MIN_TO_ZERO (1e-6f)
@@ -2147,6 +2148,117 @@ namespace VirtualRobot
return area;
}
+ MathTools::OOBB::OOBB()
+ {
+ minBB.setZero();
+ maxBB.setZero();
+ pose.setIdentity();
+ }
+
+ MathTools::OOBB::OOBB(const Eigen::Vector3f& minLocal, const Eigen::Vector3f& maxLocal, const Eigen::Matrix4f& globalPose)
+ {
+ minBB = minLocal;
+ maxBB = maxLocal;
+ pose = globalPose;
+ }
+
+ std::vector<Eigen::Vector3f> MathTools::OOBB::getOOBBPoints() const
+ {
+ Eigen::Vector3f result[8];
+ std::vector<Eigen::Vector3f> result3;
+ result[0] << minBB(0), minBB(1), minBB(2);
+ result[1] << maxBB(0), minBB(1), minBB(2);
+ result[2] << minBB(0), maxBB(1), minBB(2);
+ result[3] << maxBB(0), maxBB(1), minBB(2);
+ result[4] << minBB(0), minBB(1), maxBB(2);
+ result[5] << maxBB(0), minBB(1), maxBB(2);
+ result[6] << minBB(0), maxBB(1), maxBB(2);
+ result[7] << maxBB(0), maxBB(1), maxBB(2);
+ Eigen::Matrix4f m;
+ m.setIdentity();
+
+ for (int i = 0; i < 8; i++)
+ {
+ m.block(0, 3, 3, 1) = result[i];
+ m = pose * m;
+ result3.push_back(m.block(0, 3, 3, 1));
+ }
+
+ return result3;
+ }
+
+ std::vector<MathTools::Segment> MathTools::OOBB::getSegments() const
+ {
+ std::vector<Eigen::Vector3f> oobbPoint = getOOBBPoints();
+ std::vector<Segment> result;
+
+ result.push_back(Segment(oobbPoint[0], oobbPoint[1]));
+ result.push_back(Segment(oobbPoint[0], oobbPoint[2]));
+ result.push_back(Segment(oobbPoint[2], oobbPoint[3]));
+ result.push_back(Segment(oobbPoint[1], oobbPoint[3]));
+
+ result.push_back(Segment(oobbPoint[4], oobbPoint[5]));
+ result.push_back(Segment(oobbPoint[4], oobbPoint[6]));
+ result.push_back(Segment(oobbPoint[5], oobbPoint[7]));
+ result.push_back(Segment(oobbPoint[6], oobbPoint[7]));
+
+ result.push_back(Segment(oobbPoint[2], oobbPoint[6]));
+ result.push_back(Segment(oobbPoint[0], oobbPoint[4]));
+ result.push_back(Segment(oobbPoint[1], oobbPoint[5]));
+ result.push_back(Segment(oobbPoint[3], oobbPoint[7]));
+
+ return result;
+ }
+
+ void MathTools::OOBB::changeCoordSystem(const Eigen::Matrix4f& newGlobalPose)
+ {
+ Eigen::Vector3f result[8];
+ std::vector<Eigen::Vector3f> result3;
+ result[0] << minBB(0), minBB(1), minBB(2);
+ result[1] << maxBB(0), minBB(1), minBB(2);
+ result[2] << minBB(0), maxBB(1), minBB(2);
+ result[3] << maxBB(0), maxBB(1), minBB(2);
+ result[4] << minBB(0), minBB(1), maxBB(2);
+ result[5] << maxBB(0), minBB(1), maxBB(2);
+ result[6] << minBB(0), maxBB(1), maxBB(2);
+ result[7] << maxBB(0), maxBB(1), maxBB(2);
+ Eigen::Matrix4f m;
+
+ for (std::size_t i = 0; i < 8; i++)
+ {
+ m.setIdentity();
+ m.block(0, 3, 3, 1) = result[i];
+ // to global
+ m = pose * m;
+ // to new coord system (local in new coord system)
+ m = newGlobalPose.inverse() * m;
+
+ result3.push_back(m.block(0, 3, 3, 1));
+ }
+
+ // now find min max values
+ minBB << FLT_MAX, FLT_MAX, FLT_MAX;
+ maxBB << -FLT_MAX, -FLT_MAX, -FLT_MAX;
+
+ for (std::uint8_t i = 0; i < 8; i++)
+ {
+ for (std::uint8_t j = 0; j < 3; j++)
+ {
+ if (result3[i](j) < minBB(j))
+ {
+ minBB(j) = result3[i](j);
+ }
+
+ if (result3[i](j) > maxBB(j))
+ {
+ maxBB(j) = result3[i](j);
+ }
+ }
+ }
+
+ pose = newGlobalPose;
+ }
+
} // namespace
diff --git a/VirtualRobot/MathTools.h b/VirtualRobot/MathTools.h
index 674994fbe06f1d3842b028c2f58e7519889c3470..e2d37fb588f1e861dda21321498365ecfa2cec3b 100644
--- a/VirtualRobot/MathTools.h
+++ b/VirtualRobot/MathTools.h
@@ -269,115 +269,16 @@ namespace VirtualRobot
struct OOBB
{
- OOBB()
- {
- minBB.setZero();
- maxBB.setZero();
- pose.setIdentity();
- }
- OOBB(const Eigen::Vector3f& minLocal, const Eigen::Vector3f& maxLocal, const Eigen::Matrix4f& globalPose)
- {
- minBB = minLocal;
- maxBB = maxLocal;
- pose = globalPose;
- }
- //! Returns the 8 bounding box points transformed to global frame
- std::vector<Eigen::Vector3f> getOOBBPoints() const
- {
- Eigen::Vector3f result[8];
- std::vector<Eigen::Vector3f> result3;
- result[0] << minBB(0), minBB(1), minBB(2);
- result[1] << maxBB(0), minBB(1), minBB(2);
- result[2] << minBB(0), maxBB(1), minBB(2);
- result[3] << maxBB(0), maxBB(1), minBB(2);
- result[4] << minBB(0), minBB(1), maxBB(2);
- result[5] << maxBB(0), minBB(1), maxBB(2);
- result[6] << minBB(0), maxBB(1), maxBB(2);
- result[7] << maxBB(0), maxBB(1), maxBB(2);
- Eigen::Matrix4f m;
- m.setIdentity();
-
- for (int i = 0; i < 8; i++)
- {
- m.block(0, 3, 3, 1) = result[i];
- m = pose * m;
- result3.push_back(m.block(0, 3, 3, 1));
- }
-
- return result3;
- }
- //! Returns the 12 segments of the bounding box (in global frame)
- std::vector<Segment> getSegments() const
- {
- std::vector<Eigen::Vector3f> oobbPoint = getOOBBPoints();
- std::vector<Segment> result;
-
- result.push_back(Segment(oobbPoint[0], oobbPoint[1]));
- result.push_back(Segment(oobbPoint[0], oobbPoint[2]));
- result.push_back(Segment(oobbPoint[2], oobbPoint[3]));
- result.push_back(Segment(oobbPoint[1], oobbPoint[3]));
+ OOBB();
- result.push_back(Segment(oobbPoint[4], oobbPoint[5]));
- result.push_back(Segment(oobbPoint[4], oobbPoint[6]));
- result.push_back(Segment(oobbPoint[5], oobbPoint[7]));
- result.push_back(Segment(oobbPoint[6], oobbPoint[7]));
+ OOBB(const Eigen::Vector3f& minLocal, const Eigen::Vector3f& maxLocal, const Eigen::Matrix4f& globalPose);
- result.push_back(Segment(oobbPoint[2], oobbPoint[6]));
- result.push_back(Segment(oobbPoint[0], oobbPoint[4]));
- result.push_back(Segment(oobbPoint[1], oobbPoint[5]));
- result.push_back(Segment(oobbPoint[3], oobbPoint[7]));
-
- return result;
- }
-
- void changeCoordSystem(const Eigen::Matrix4f& newGlobalPose)
- {
- Eigen::Vector3f result[8];
- std::vector<Eigen::Vector3f> result3;
- result[0] << minBB(0), minBB(1), minBB(2);
- result[1] << maxBB(0), minBB(1), minBB(2);
- result[2] << minBB(0), maxBB(1), minBB(2);
- result[3] << maxBB(0), maxBB(1), minBB(2);
- result[4] << minBB(0), minBB(1), maxBB(2);
- result[5] << maxBB(0), minBB(1), maxBB(2);
- result[6] << minBB(0), maxBB(1), maxBB(2);
- result[7] << maxBB(0), maxBB(1), maxBB(2);
- Eigen::Matrix4f m;
-
- for (std::size_t i = 0; i < 8; i++)
- {
- m.setIdentity();
- m.block(0, 3, 3, 1) = result[i];
- // to global
- m = pose * m;
- // to new coord system (local in new coord system)
- m = newGlobalPose.inverse() * m;
-
- result3.push_back(m.block(0, 3, 3, 1));
- }
-
- // now find min max values
- minBB << FLT_MAX, FLT_MAX, FLT_MAX;
- maxBB << -FLT_MAX, -FLT_MAX, -FLT_MAX;
-
- for (std::uint8_t i = 0; i < 8; i++)
- {
- for (std::uint8_t j = 0; j < 3; j++)
- {
- if (result3[i](j) < minBB(j))
- {
- minBB(j) = result3[i](j);
- }
-
- if (result3[i](j) > maxBB(j))
- {
- maxBB(j) = result3[i](j);
- }
- }
- }
+ //! Returns the 8 bounding box points transformed to global frame
+ std::vector<Eigen::Vector3f> getOOBBPoints() const;
+ //! Returns the 12 segments of the bounding box (in global frame)
+ std::vector<Segment> getSegments() const;
- pose = newGlobalPose;
- }
+ void changeCoordSystem(const Eigen::Matrix4f& newGlobalPose);
// the bounding box is defined via min and max values (in local frame)
Eigen::Vector3f minBB;