Feature/test geometry

common/math/geometry/test/CMakeLists.txt

@@ -1,18 +1,26 @@
+add_subdirectory(vector3)
+
 ament_add_gtest(test_bounding_box test_bounding_box.cpp)
 ament_add_gtest(test_catmull_rom_spline test_catmull_rom_spline.cpp)
+ament_add_gtest(test_catmull_rom_subspline test_catmull_rom_subspline.cpp)
 ament_add_gtest(test_collision test_collision.cpp)
 ament_add_gtest(test_distance test_distance.cpp)
 ament_add_gtest(test_hermite_curve test_hermite_curve.cpp)
-ament_add_gtest(test_line_segment test_line_segment.cpp)
 ament_add_gtest(test_linear_algebra test_linear_algebra.cpp)
 ament_add_gtest(test_polygon test_polygon.cpp)
 ament_add_gtest(test_polynomial_solver test_polynomial_solver.cpp)
+ament_add_gtest(test_transform test_transform.cpp)
+ament_add_gtest(test_intersection test_intersection.cpp)
+ament_add_gtest(test_line_segment test_line_segment.cpp)
 target_link_libraries(test_bounding_box geometry)
 target_link_libraries(test_catmull_rom_spline geometry)
+target_link_libraries(test_catmull_rom_subspline geometry)
 target_link_libraries(test_collision geometry)
 target_link_libraries(test_distance geometry)
 target_link_libraries(test_hermite_curve geometry)
-target_link_libraries(test_line_segment geometry)
 target_link_libraries(test_linear_algebra geometry)
 target_link_libraries(test_polygon geometry)
 target_link_libraries(test_polynomial_solver geometry)
+target_link_libraries(test_transform geometry)
+target_link_libraries(test_intersection geometry)
+target_link_libraries(test_line_segment geometry)

common/math/geometry/test/expect_eq_macros.hpp

@@ -28,21 +28,50 @@
   EXPECT_DOUBLE_EQ(DATA0.y, DATA1.y); \
   EXPECT_DOUBLE_EQ(DATA0.z, DATA1.z);
 
+#define EXPECT_BOOST_POINT_2D_AND_POINT_EQ(DATA0, DATA1) \
+  EXPECT_DOUBLE_EQ(DATA0.x(), DATA1.x);                  \
+  EXPECT_DOUBLE_EQ(DATA0.y(), DATA1.y);
+
+#define EXPECT_POINT_NEAR(DATA0, DATA1, TOLERANCE) \
+  EXPECT_NEAR(DATA0.x, DATA1.x, TOLERANCE);        \
+  EXPECT_NEAR(DATA0.y, DATA1.y, TOLERANCE);        \
+  EXPECT_NEAR(DATA0.z, DATA1.z, TOLERANCE);
+
+#define EXPECT_POINT_NAN(DATA)     \
+  EXPECT_TRUE(std::isnan(DATA.x)); \
+  EXPECT_TRUE(std::isnan(DATA.y)); \
+  EXPECT_TRUE(std::isnan(DATA.z));
+
 #define EXPECT_VECTOR3_EQ(DATA0, DATA1) \
   EXPECT_DOUBLE_EQ(DATA0.x, DATA1.x);   \
   EXPECT_DOUBLE_EQ(DATA0.y, DATA1.y);   \
   EXPECT_DOUBLE_EQ(DATA0.z, DATA1.z);
 
+#define EXPECT_VECTOR3_NEAR(DATA0, DATA1, TOLERANCE) \
+  EXPECT_NEAR(DATA0.x, DATA1.x, TOLERANCE);          \
+  EXPECT_NEAR(DATA0.y, DATA1.y, TOLERANCE);          \
+  EXPECT_NEAR(DATA0.z, DATA1.z, TOLERANCE);
+
 #define EXPECT_QUATERNION_EQ(DATA0, DATA1) \
   EXPECT_DOUBLE_EQ(DATA0.x, DATA1.x);      \
   EXPECT_DOUBLE_EQ(DATA0.y, DATA1.y);      \
   EXPECT_DOUBLE_EQ(DATA0.z, DATA1.z);      \
   EXPECT_DOUBLE_EQ(DATA0.w, DATA1.w);
 
+#define EXPECT_QUATERNION_NEAR(DATA0, DATA1, TOLERANCE) \
+  EXPECT_NEAR(DATA0.x, DATA1.x, TOLERANCE);             \
+  EXPECT_NEAR(DATA0.y, DATA1.y, TOLERANCE);             \
+  EXPECT_NEAR(DATA0.z, DATA1.z, TOLERANCE);             \
+  EXPECT_NEAR(DATA0.w, DATA1.w, TOLERANCE);
+
 #define EXPECT_POSE_EQ(DATA0, DATA1)               \
   EXPECT_POINT_EQ(DATA0.position, DATA1.position); \
   EXPECT_QUATERNION_EQ(DATA0.orientation, DATA1.orientation);
 
+#define EXPECT_POSE_NEAR(DATA0, DATA1, TOLERANCE)               \
+  EXPECT_POINT_NEAR(DATA0.position, DATA1.position, TOLERANCE); \
+  EXPECT_QUATERNION_NEAR(DATA0.orientation, DATA1.orientation, TOLERANCE);
+
 #define EXPECT_LANELET_POSE_EQ(DATA0, DATA1)     \
   EXPECT_EQ(DATA0.lanelet_id, DATA1.lanelet_id); \
   EXPECT_DOUBLE_EQ(DATA0.s, DATA1.s);            \

common/math/geometry/test/test_bounding_box.cpp

@@ -13,40 +13,115 @@
 // limitations under the License.
 
 #include <gtest/gtest.h>
+#include <quaternion_operation/quaternion_operation.h>
 
 #include <geometry/bounding_box.hpp>
 #include <scenario_simulator_exception/exception.hpp>
 
-TEST(BoundingBox, GetPolygonDistanceWithCollision)
+#include "expect_eq_macros.hpp"
+#include "test_utils.hpp"
+
+TEST(BoundingBox, getPointsFromBboxDefault)
+{
+  geometry_msgs::msg::Pose pose;
+  traffic_simulator_msgs::msg::BoundingBox bbox;
+  std::vector<geometry_msgs::msg::Point> points = math::geometry::getPointsFromBbox(bbox);
+  EXPECT_EQ(points.size(), size_t(4));
+  EXPECT_POINT_EQ(points[0], makePoint(0.0, 0.0, 0.0));
+  EXPECT_POINT_EQ(points[1], makePoint(0.0, 0.0, 0.0));
+  EXPECT_POINT_EQ(points[2], makePoint(0.0, 0.0, 0.0));
+  EXPECT_POINT_EQ(points[3], makePoint(0.0, 0.0, 0.0));
+}
+
+TEST(BoundingBox, getPointsFromBboxCustom)
+{
+  geometry_msgs::msg::Pose pose;
+  traffic_simulator_msgs::msg::BoundingBox bbox = makeBbox(5.0, 2.0, 2.0, 1.0);
+  std::vector<geometry_msgs::msg::Point> points =
+    math::geometry::getPointsFromBbox(bbox, 1.0, 2.0, 3.0, 4.0);
+  EXPECT_EQ(points.size(), size_t(4));
+  EXPECT_POINT_EQ(points[0], makePoint(6.5, 3.0, 1.0));
+  EXPECT_POINT_EQ(points[1], makePoint(-5.5, 3.0, 1.0));
+  EXPECT_POINT_EQ(points[2], makePoint(-5.5, -2.0, 1.0));
+  EXPECT_POINT_EQ(points[3], makePoint(6.5, -2.0, 1.0));
+}
+
+TEST(BoundingBox, get2DPolygonZeroPose)
+{
+  geometry_msgs::msg::Pose pose;
+  traffic_simulator_msgs::msg::BoundingBox bbox = makeBbox(2.0, 2.0, 2.0);
+  boost::geometry::model::polygon<boost::geometry::model::d2::point_xy<double>> poly =
+    math::geometry::get2DPolygon(pose, bbox);
+  EXPECT_EQ(poly.inners().size(), size_t(0));
+  EXPECT_EQ(poly.outer().size(), size_t(5));
+  EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[0], makePoint(1.0, 1.0));
+  EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[1], makePoint(-1.0, 1.0));
+  EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[2], makePoint(-1.0, -1.0));
+  EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[3], makePoint(1.0, -1.0));
+  EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[4], makePoint(1.0, 1.0));
+}
+
+TEST(BoundingBox, get2DPolygonOnlyTranslation)
+{
+  geometry_msgs::msg::Pose pose = makePose(1.0, 2.0);
+  traffic_simulator_msgs::msg::BoundingBox bbox = makeBbox(2.0, 2.0, 2.0);
+  boost::geometry::model::polygon<boost::geometry::model::d2::point_xy<double>> poly =
+    math::geometry::get2DPolygon(pose, bbox);
+  EXPECT_EQ(poly.inners().size(), size_t(0));
+  EXPECT_EQ(poly.outer().size(), size_t(5));
+  EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[0], makePoint(2.0, 3.0));
+  EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[1], makePoint(0.0, 3.0));
+  EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[2], makePoint(0.0, 1.0));
+  EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[3], makePoint(2.0, 1.0));
+  EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[4], makePoint(2.0, 3.0));
+}
+
+TEST(BoundingBox, get2DPolygonFullPose)
+{
+  geometry_msgs::msg::Pose pose = makePose(1.0, 2.0);
+  pose.orientation = quaternion_operation::convertEulerAngleToQuaternion(
+    geometry_msgs::build<geometry_msgs::msg::Vector3>().x(0.0).y(0.0).z(30.0 * M_PI / 180.0));
+  traffic_simulator_msgs::msg::BoundingBox bbox = makeBbox(2.0, 2.0, 2.0);
+  boost::geometry::model::polygon<boost::geometry::model::d2::point_xy<double>> poly =
+    math::geometry::get2DPolygon(pose, bbox);
+  EXPECT_EQ(poly.inners().size(), size_t(0));
+  EXPECT_EQ(poly.outer().size(), size_t(5));
+  const double x = std::sqrt(2.0) * std::cos((30.0 + 45.0) * M_PI / 180.0);
+  const double y = std::sqrt(2.0) * std::sin((30.0 + 45.0) * M_PI / 180.0);
+  EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[0], makePoint(x + 1.0, y + 2.0));
+  EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[1], makePoint(-y + 1.0, x + 2.0));
+  EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[2], makePoint(-x + 1.0, -y + 2.0));
+  EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[3], makePoint(y + 1.0, -x + 2.0));
+  EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[4], makePoint(x + 1.0, y + 2.0));
+}
+
+TEST(BoundingBox, getPolygonDistanceWithCollision)
 {
   geometry_msgs::msg::Pose pose0;
-  traffic_simulator_msgs::msg::BoundingBox bbox0;
-  bbox0.dimensions.x = 3;
-  bbox0.dimensions.y = 3;
-  bbox0.dimensions.z = 3;
+  traffic_simulator_msgs::msg::BoundingBox bbox0 = makeBbox(3.0, 3.0, 3.0);
   geometry_msgs::msg::Pose pose1;
-  traffic_simulator_msgs::msg::BoundingBox bbox1;
-  bbox1.dimensions.x = 1;
-  bbox1.dimensions.y = 1;
-  bbox1.dimensions.z = 1;
+  traffic_simulator_msgs::msg::BoundingBox bbox1 = makeBbox(1.0, 1.0, 1.0);
   EXPECT_EQ(math::geometry::getPolygonDistance(pose0, bbox0, pose1, bbox1), std::nullopt);
 }
 
-TEST(BoundingBox, GetPolygonDistanceWithoutCollision)
+TEST(BoundingBox, getPolygonDistanceTouch)
 {
   geometry_msgs::msg::Pose pose0;
-  traffic_simulator_msgs::msg::BoundingBox bbox0;
-  bbox0.dimensions.x = 3;
-  bbox0.dimensions.y = 3;
-  bbox0.dimensions.z = 3;
-  geometry_msgs::msg::Pose pose1;
-  pose1.position.y = 5;
-  traffic_simulator_msgs::msg::BoundingBox bbox1;
-  bbox1.dimensions.x = 1;
-  bbox1.dimensions.y = 1;
-  bbox1.dimensions.z = 1;
-  EXPECT_TRUE(math::geometry::getPolygonDistance(pose0, bbox0, pose1, bbox1));
-  EXPECT_DOUBLE_EQ(math::geometry::getPolygonDistance(pose0, bbox0, pose1, bbox1).value(), 3.0);
+  traffic_simulator_msgs::msg::BoundingBox bbox0 = makeBbox(4.0, 4.0, 4.0);
+  geometry_msgs::msg::Pose pose1 = makePose(3.0, 3.0, 3.0);
+  traffic_simulator_msgs::msg::BoundingBox bbox1 = makeBbox(2.0, 2.0, 2.0);
+  EXPECT_EQ(math::geometry::getPolygonDistance(pose0, bbox0, pose1, bbox1), std::nullopt);
+}
+
+TEST(BoundingBox, getPolygonDistanceWithoutCollision)
+{
+  geometry_msgs::msg::Pose pose0;
+  traffic_simulator_msgs::msg::BoundingBox bbox0 = makeBbox(3.0, 3.0, 3.0);
+  geometry_msgs::msg::Pose pose1 = makePose(0.0, 5.0);
+  traffic_simulator_msgs::msg::BoundingBox bbox1 = makeBbox(1.0, 1.0, 1.0);
+  const auto ans = math::geometry::getPolygonDistance(pose0, bbox0, pose1, bbox1);
+  EXPECT_TRUE(ans);
+  EXPECT_DOUBLE_EQ(ans.value(), 3.0);
 }
 
 int main(int argc, char ** argv)