diff --git a/src/generic/CMakeLists.txt b/src/generic/CMakeLists.txt index 9d664b4..3d50212 100644 --- a/src/generic/CMakeLists.txt +++ b/src/generic/CMakeLists.txt @@ -1,3 +1,4 @@ add_subdirectory(common) +add_subdirectory(convex_hull) add_subdirectory(digital_filter) add_subdirectory(polynomial) \ No newline at end of file diff --git a/src/generic/convex_hull/CMakeLists.txt b/src/generic/convex_hull/CMakeLists.txt new file mode 100644 index 0000000..7375377 --- /dev/null +++ b/src/generic/convex_hull/CMakeLists.txt @@ -0,0 +1 @@ +add_executable(test_convex_hull convex_hull.cpp) \ No newline at end of file diff --git a/src/generic/convex_hull/convex_hull.cpp b/src/generic/convex_hull/convex_hull.cpp new file mode 100644 index 0000000..d67c489 --- /dev/null +++ b/src/generic/convex_hull/convex_hull.cpp @@ -0,0 +1,183 @@ +#include +#include +#include + +template +struct Point { + T x; + T y; +}; + +/** + * @brief Get the Cross Product of segment (a, b) and (a, c) + * + * Example: + * W(3, 4) + * V(5, 2) + * (0,0) + * + * V x W = VxWy - WxVy + * = | 5 3 | + * | 2 4 | + * = (5 * 4) - (2 * 3) = 14 (counter-clockwise) + * + * @tparam T + * @param a + * @param b + * @param c + * @return T positive for counter-clockwise, negative for clockwise, zero for + * collinear + */ +template +T GetCrossProduct(const Point &a, const Point &b, const Point &c) { + return ((b.x - a.x) * (c.y - a.y) - (b.y - a.y) * (c.x - a.x)); +} + +template +Point SwapAndGetFirstPoint(std::vector> &points) { + auto iterator = std::min_element( + points.begin(), points.end(), [](const Point a, const Point b) { + return ((a.y < b.y) || ((a.y == b.y) && (a.x < b.x))); + }); + auto old_first_point = points[0]; + points[0] = *iterator; + *iterator = old_first_point; + return points[0]; +} + +template +std::vector> GetConvexHull(std::vector> &points) { + if (points.size() < 3) { + return std::vector>{}; + } + auto first_point = SwapAndGetFirstPoint(points); + std::sort(points.begin() + 1, points.end(), + [&](const Point &b, const Point &c) { + return (GetCrossProduct(first_point, b, c) < 0); + }); + + std::vector> result; + auto it = points.begin(); + + auto value = *it++; + std::cout << "Adding... " << value.x << " " << value.y << std::endl; + + result.push_back(value); + + value = *it++; + std::cout << "Adding... " << value.x << " " << value.y << std::endl; + + result.push_back(value); + + value = *it++; + std::cout << "Adding... " << value.x << " " << value.y << std::endl; + + result.push_back(value); + + while (it != points.end()) { + // StepConverge + // Pop off any points that make a convex angle with *it + while (GetCrossProduct(*(result.rbegin() + 1), *(result.rbegin()), *it) >= + 0) { + std::cout << "Popping out: " << result.back().x << " " << result.back().y + << std::endl; + result.pop_back(); + } + + auto value = *it++; + std::cout << "Adding... " << value.x << " " << value.y << std::endl; + result.push_back(value); + } + + return result; +} + +template +class ConvexHull { + public: + using Callback = std::function &)>; + explicit ConvexHull(const std::vector> &points) + : points_(points), current_index_(0) { + auto first_point = SwapAndGetFirstPoint(points_); + std::sort(points_.begin() + 1, points_.end(), + [&](const Point &b, const Point &c) { + return (GetCrossProduct(first_point, b, c) < 0); + }); + initialize_first_three_points_ = true; + } + + std::optional>> StepForward(Callback insert_callback, + Callback pop_callback) { + if (points_.size() < 3) { + return std::nullopt; + } else { + if (initialize_first_three_points_) { + for (auto i = 0; i < 3; ++i) { + InsertCurrentValueWithCallback(insert_callback); + } + initialize_first_three_points_ = false; + return std::nullopt; + } + } + + if (current_index_ < points_.size()) { + if (GetCrossProduct(*(results_.rbegin() + 1), *(results_.rbegin()), + points_.at(current_index_)) >= 0) { + PopCurrentValueWithCallback(pop_callback); + } else { + InsertCurrentValueWithCallback(insert_callback); + } + } else { + return results_; + } + + return std::nullopt; + } + + private: + void InsertCurrentValueWithCallback(Callback insert_callback) { + const auto value = points_.at(current_index_); + insert_callback(value); + results_.push_back(value); + current_index_++; + } + + void PopCurrentValueWithCallback(Callback pop_callback) { + pop_callback(results_.back()); + results_.pop_back(); + } + + std::vector> points_; + std::vector> results_; + size_t current_index_; + bool initialize_first_three_points_; +}; + +int main() { + auto points = std::vector>{{0, 3}, {1, 1}, {2, 2}, {4, 4}, + {0, 0}, {1, 2}, {3, 1}, {3, 3}}; + auto result = GetConvexHull(points); + std::cout << "Results" << std::endl; + for (const auto value : result) { + std::cout << value.x << " " << value.y << std::endl; + } + + std::cout << "-------------------" << std::endl; + auto convex_hull = ConvexHull(points); + while (convex_hull.StepForward( + [](const Point &point) { + std::cout << "Adding... " << point.x << " " << point.y + << std::endl; + }, + [](const Point &point) { + std::cout << "Popping out: " << point.x << " " << point.y + << std::endl; + }) == std::nullopt) { + } + auto new_result = convex_hull.StepForward([](const Point &) {}, + [](const Point &) {}); + std::cout << "Results" << std::endl; + for (const auto value : *new_result) { + std::cout << value.x << " " << value.y << std::endl; + } +} \ No newline at end of file