Collision detection

packages/geom/CMakeLists.txt

@@ -18,6 +18,7 @@ add_library(
   src/angle_vector.cpp
   src/arc.cpp
   src/bezier.cpp
+  src/bounding_box.cpp
   src/circle.cpp
   src/complex_polygon.cpp
   src/distance.cpp
@@ -27,10 +28,10 @@ add_library(
   src/polygon.cpp
   src/polyline.cpp
   src/pose.cpp
-  src/intersection.cpp
   src/segment.cpp
   src/transform.cpp
   src/vector.cpp
+  src/vector3.cpp
 )
 
 ament_target_dependencies(

packages/geom/include/geom/bounding_box.h

@@ -0,0 +1,32 @@
+#pragma once
+
+#include "geom/vector.h"
+
+#include <algorithm>
+
+namespace truck::geom {
+
+struct BoundingBox {
+    BoundingBox(const Vec2& v) {
+        min = v;
+        max = v;
+    }
+
+    BoundingBox(const Vec2& a, const Vec2& b) {
+        min.x = std::min(a.x, b.x);
+        min.y = std::min(a.y, b.y);
+        max.x = std::max(a.x, b.x);
+        max.y = std::max(a.y, b.y);
+    }
+
+    BoundingBox& extend(const Vec2& v) noexcept;
+    BoundingBox& extend(double margin) noexcept;
+
+    Vec2 min, max;
+};
+
+inline BoundingBox extend(BoundingBox& box, const Vec2& v) noexcept { return box.extend(v); }
+
+inline BoundingBox extend(BoundingBox& box, double margin) noexcept { return box.extend(margin); }
+
+}  // namespace truck::geom

packages/geom/include/geom/polygon.h

@@ -1,8 +1,9 @@
 #pragma once
 
-#include "geom/vector.h"
-#include "geom/triangle.h"
+#include "geom/bounding_box.h"
 #include "geom/segment.h"
+#include "geom/triangle.h"
+#include "geom/vector.h"
 
 #include <vector>
 
@@ -36,4 +37,6 @@ Polygon clip(
     const Polygon& boundary_polygon, const Polygon& clipped_polygon,
     const double eps = 1e-4) noexcept;
 
+BoundingBox makeBoundingBox(const Polygon& polygon) noexcept;
+
 }  // namespace truck::geom

packages/geom/include/geom/vector.h

@@ -23,6 +23,16 @@ struct Vec2 {
 
     static Vec2 fromAngle(Angle a) noexcept { return {cos(a), sin(a)}; }
 
+    Vec2& operator=(const Vec2& other) noexcept {
+        if (this == &other) {
+            return *this;
+        }
+
+        x = other.x;
+        y = other.y;
+        return *this;
+    }
+
     Vec2& operator+=(const Vec2& other) noexcept {
         x += other.x;
         y += other.y;

packages/geom/include/geom/vector3.h

@@ -1,5 +1,7 @@
 #pragma once
 
+#include "geom/vector.h"
+
 #include <cmath>
 #include <cmath>
 #include <ostream>

packages/geom/src/bounding_box.cpp

@@ -0,0 +1,21 @@
+#include "geom/bounding_box.h"
+
+namespace truck::geom {
+
+BoundingBox& BoundingBox::extend(const Vec2& v) noexcept {
+    min.x = std::min(min.x, v.x);
+    min.y = std::min(min.y, v.y);
+    max.x = std::max(max.x, v.x);
+    max.y = std::max(max.y, v.y);
+    return *this;
+}
+
+BoundingBox& BoundingBox::extend(double margin) noexcept {
+    min.x -= margin;
+    min.y -= margin;
+    max.x += margin;
+    max.y += margin;
+    return *this;
+}
+
+}  // namespace truck::geom

packages/geom/src/polygon.cpp

@@ -12,6 +12,7 @@
 #include <CGAL/mark_domain_in_triangulation.h>
 #include <CGAL/Polygon_2.h>
 
+#include <algorithm>
 #include <unordered_map>
 #include <boost/property_map/property_map.hpp>
 
@@ -86,6 +87,23 @@ Orientation Polygon::orientation() const noexcept {
     return (orientation_sign > 0 ? Orientation::COUNTERCLOCKWISE : Orientation::CLOCKWISE);
 }
 
+Segments Polygon::segments() const noexcept {
+    const auto& points = *this;
+    Segments segments;
+    segments.reserve(points.size());
+
+    segments[0].begin = {points.back().x, points.back().y};
+    segments[0].end = {points[0].x, points[0].y};
+
+    for (size_t i = 1; i < points.size(); ++i) {
+        Vec2 begin = {points[i - 1].x, points[i - 1].y};
+        Vec2 end = {points[i].x, points[i].y};
+        segments.emplace_back(begin, end);
+    }
+
+    return segments;
+}
+
 /** Implementation of Sutherland–Hodgman algorithm
  *
  * See https://en.wikipedia.org/wiki/Sutherland–Hodgman_algorithm
@@ -108,13 +126,13 @@ Polygon clip(
             return orientation_sign * cross(clip_edge, point - clip_vertex_2) >= eps;
         };
 
-        geom::Polygon current_polygon = std::move(clipped_polygon);
+        Polygon current_polygon = std::move(clipped_polygon);
         clipped_polygon.clear();
         auto current_vertex_1 = current_polygon.back();
         for (const auto& current_vertex_2 : current_polygon) {
-            auto intersection_point = geom::intersect(
-                geom::Line::fromTwoPoints(current_vertex_1, current_vertex_2),
-                geom::Line::fromTwoPoints(clip_vertex_1, clip_vertex_2));
+            auto intersection_point = intersect(
+                Line::fromTwoPoints(current_vertex_1, current_vertex_2),
+                Line::fromTwoPoints(clip_vertex_1, clip_vertex_2));
             if (is_inside_clip_edge(current_vertex_2)) {
                 if (!is_inside_clip_edge(current_vertex_1)) {
                     clipped_polygon.push_back(*intersection_point);
@@ -131,21 +149,15 @@ Polygon clip(
     return clipped_polygon;
 }
 
-Segments Polygon::segments() const noexcept {
-    const auto points = *this;
-    Segments segments;
-    segments.reserve(points.size());
+BoundingBox makeBoundingBox(const Polygon& polygon) noexcept {
+    VERIFY(!polygon.empty());
 
-    segments[0].begin = {points.back().x, points.back().y};
-    segments[0].end = {points[0].x, points[0].y};
-
-    for (size_t i = 1; i < points.size(); ++i) {
-        Vec2 begin = {points[i - 1].x, points[i - 1].y};
-        Vec2 end = {points[i].x, points[i].y};
-        segments.emplace_back(begin, end);
+    BoundingBox box(polygon[0]);
+    for (auto i = 1; i < polygon.size(); ++i) {
+        box.extend(polygon[i]);
     }
 
-    return segments;
+    return box;
 }
 
 }  // namespace truck::geom

packages/geom/test/CMakeLists.txt

@@ -1,4 +1,17 @@
-ament_add_gtest(${PROJECT_NAME}_tests main.cpp)
+ament_add_gtest(
+  ${PROJECT_NAME}_tests
+  angle_tests.cpp
+  angle_vector_tests.cpp
+  arc_tests.cpp
+  distance_tests.cpp
+  line_tests.cpp
+  main.cpp
+  polygon_tests.cpp
+  ray_tests.cpp
+  segment_tests.cpp
+  vector_tests.cpp
+  vector3_tests.cpp
+)
 target_link_libraries(${PROJECT_NAME}_tests ${PROJECT_NAME})
 
 set_tests_properties(${Tests} PROPERTIES TIMEOUT 1)

packages/geom/test/angle_tests.cpp

@@ -0,0 +1,61 @@
+#include <gtest/gtest.h>
+
+#include "geom/test/equal_assert.h"
+#include "geom/angle.h"
+
+using namespace truck::geom;
+
+TEST(Angle, constructor) {
+    const auto a = Angle::fromRadians(0);
+    const auto b = Angle::fromDegrees(90);
+    const auto c = Angle::fromVector(-1, 0);
+
+    constexpr double eps = 1e-9;
+
+    ASSERT_GEOM_EQUAL(a, PI0, eps);
+    ASSERT_GEOM_EQUAL(b, PI_2, eps);
+    ASSERT_GEOM_EQUAL(c, PI, eps);
+}
+
+TEST(Angle, coversion) {
+    const auto a = PI_2;
+
+    constexpr double eps = 1e-9;
+
+    ASSERT_GEOM_EQUAL(a.radians(), M_PI / 2, eps);
+    ASSERT_GEOM_EQUAL(a.degrees(), 90.0, eps);
+}
+
+TEST(Angle, literals) {
+    using namespace truck::geom::literals;
+
+    const auto a = 0_rad;
+    const auto b = 90_deg;
+
+    constexpr double eps = 1e-9;
+
+    ASSERT_GEOM_EQUAL(a, PI0, eps);
+    ASSERT_GEOM_EQUAL(b, PI_2, eps);
+}
+
+TEST(Angle, print) {
+    std::stringstream ss;
+    ss << Angle::fromDegrees(90);
+    ASSERT_EQ(ss.str(), "90.00'");
+}
+
+TEST(Angle, normalization) {
+    const auto a = Angle::fromDegrees(90);
+    const auto b = Angle::fromDegrees(360 + 45);
+    const auto c = Angle::fromDegrees(-2 * 360 - 45);
+
+    constexpr double eps = 1e-9;
+
+    ASSERT_GEOM_EQUAL(a._0_2PI(), PI_2, eps);
+    ASSERT_GEOM_EQUAL(b._0_2PI(), PI_4, eps);
+    ASSERT_GEOM_EQUAL(c._0_2PI(), 7 * PI_4, eps);
+
+    ASSERT_GEOM_EQUAL(a._mPI_PI(), PI_2, eps);
+    ASSERT_GEOM_EQUAL(b._mPI_PI(), PI_4, eps);
+    ASSERT_GEOM_EQUAL(c._mPI_PI(), -PI_4, eps);
+}

packages/geom/test/angle_vector_tests.cpp

@@ -0,0 +1,66 @@
+#include <gtest/gtest.h>
+
+#include "geom/test/equal_assert.h"
+#include "geom/angle_vector.h"
+
+using namespace truck::geom;
+
+TEST(AngleVec2, constructor) {
+    const auto a = AngleVec2::fromVector(1, 1);
+    const auto b = AngleVec2(PI_6);
+
+    constexpr double eps = 1e-9;
+
+    ASSERT_GEOM_EQUAL(a.angle(), PI_4, eps);
+    ASSERT_GEOM_EQUAL(a.x(), std::sqrt(2) / 2, eps);
+    ASSERT_GEOM_EQUAL(a.y(), std::sqrt(2) / 2, eps);
+
+    ASSERT_GEOM_EQUAL(b.angle(), PI_6, eps);
+    ASSERT_GEOM_EQUAL(b.x(), std::sqrt(3) / 2, eps);
+    ASSERT_GEOM_EQUAL(b.y(), 0.5, eps);
+}
+
+TEST(AngleVec2, operation) {
+    const auto a = AngleVec2(PI_2);
+    const auto b = AngleVec2(PI_2);
+
+    const auto sum = a + b;
+    const auto diff = a - b;
+    const auto inv = a.inv();
+
+    constexpr double eps = 1e-9;
+
+    ASSERT_GEOM_EQUAL(sum.angle(), PI, eps);
+    ASSERT_GEOM_EQUAL(sum.x(), -1., eps);
+    ASSERT_GEOM_EQUAL(sum.y(), 0., eps);
+
+    ASSERT_GEOM_EQUAL(diff.angle(), PI0, eps);
+    ASSERT_GEOM_EQUAL(diff.x(), 1., eps);
+    ASSERT_GEOM_EQUAL(diff.y(), 0., eps);
+
+    ASSERT_GEOM_EQUAL(inv.angle(), -PI_2, eps);
+    ASSERT_GEOM_EQUAL(inv.x(), 0., eps);
+    ASSERT_GEOM_EQUAL(inv.y(), -1., eps);
+}
+
+TEST(AngleVec2, rotate) {
+    const auto a = AngleVec2(Angle::zero());
+
+    const auto left = a.left();
+    const auto right = a.right();
+    const auto inv = AngleVec2(-PI).apply(a);
+
+    constexpr double eps = 1e-9;
+
+    ASSERT_GEOM_EQUAL(left.angle(), PI_2, eps);
+    ASSERT_GEOM_EQUAL(left.x(), 0., eps);
+    ASSERT_GEOM_EQUAL(left.y(), 1., eps);
+
+    ASSERT_GEOM_EQUAL(right.angle(), -PI_2, eps);
+    ASSERT_GEOM_EQUAL(right.x(), 0., eps);
+    ASSERT_GEOM_EQUAL(right.y(), -1., eps);
+
+    ASSERT_GEOM_EQUAL(inv.angle(), -PI, eps);
+    ASSERT_GEOM_EQUAL(inv.x(), -1., eps);
+    ASSERT_GEOM_EQUAL(inv.y(), 0., eps);
+}