diff --git a/libs/cg/include/psemek/cg/area.hpp b/libs/cg/include/psemek/cg/area.hpp index a4d19f41..4062a313 100644 --- a/libs/cg/include/psemek/cg/area.hpp +++ b/libs/cg/include/psemek/cg/area.hpp @@ -40,4 +40,47 @@ namespace psemek::cg return result / scalar_type{12}; } + template + auto polygon_center(Iterator begin, Iterator end) + { + auto const first = *begin; + + auto offset = decltype(*begin - *begin)::zero(); + + std::size_t count = 0; + + for (; begin != end; ++begin) + { + offset += *begin - first; + ++count; + } + + using scalar_type = std::remove_cvref_t; + return first + offset / static_cast(count); + } + + template + auto polygon_mass_center(Iterator begin, Iterator end) + { + using scalar_type = std::remove_cvref_t; + using result_type = std::remove_cvref_t; + + auto result = result_type::zero(); + + for (auto it = begin, prev = std::prev(end); it != end; prev = it++) + { + auto const v0 = *prev; + auto const v1 = *it; + result[0] += (v1[1] - v0[1]) * (v0[0] * v0[0] + v0[0] * v1[0] + v1[0] * v1[0]); + result[1] -= (v1[0] - v0[0]) * (v0[1] * v0[1] + v0[1] * v1[1] + v1[1] * v1[1]); + } + + scalar_type a = polygon_area(begin, end) * 6; + + result[0] /= a; + result[1] /= a; + + return result; + } + }