From c0d4343b77ecdf4cf65c3c40d0ab6f0d0134ad6f Mon Sep 17 00:00:00 2001
From: Karma Riuk <riukkarma@gmail.com>
Date: Sat, 6 May 2023 14:04:58 +0200
Subject: [PATCH] Added general polygon generation WITH CORRECT INERTIA YUHUU

---
 polygon_generator.cc | 53 +++++++++++++++++++++++++++++++++++++++-----
 polygon_generator.h  |  2 +-
 vec2d.h              |  4 ++++
 3 files changed, 52 insertions(+), 7 deletions(-)

diff --git a/polygon_generator.cc b/polygon_generator.cc
index e55a32d..e94912c 100644
--- a/polygon_generator.cc
+++ b/polygon_generator.cc
@@ -66,14 +66,55 @@ polygon poly_generate::regular(double radius, uint n_sides, double mass) {
     return polygon{{0, 0}, 0, points, inertia, mass};
 }
 
-static double intertia_of_polygon_subtriangle(
-    vec2d& centroid, vec2d& p1, vec2d& p2) {
-    double base, height;
-    if (vec2d::norm(p1 - centroid) > vec2d::norm(p2 - centroid))
-        base = vec2d::norm(p1 - centroid);
+static double area_of_triangle(vec2d& a, vec2d& b, vec2d& c) {
+    return std::abs(vec2d::cross(c - a, b - a)) / 2;
 }
 
-polygon poly_generate::general(std::vector<vec2d>& points, double mass) {
+static double area_of_poly(std::vector<vec2d>& points, vec2d& centroid) {
+    double area = 0;
+    for (int i = 0; i < points.size(); ++i)
+        area += area_of_triangle(
+            centroid, points[i], points[(i + 1) % points.size()]);
+    return area;
+}
+
+static double intertia_of_polygon_subtriangle(double total_mass,
+    double total_area,
+    vec2d& centroid,
+    vec2d& p1,
+    vec2d& p2) {
+    double partial_area = area_of_triangle(centroid, p1, p2);
+    std::cout << "partial area: " << partial_area << std::endl;
+    double partial_mass = total_mass * partial_area / total_area;
+
+    vec2d CA = p1 - centroid;
+    vec2d AB = p2 - p1;
+
+    return partial_mass / 2 *
+           (vec2d::norm2(AB) / 3 + vec2d::dot(AB, CA) + vec2d::norm2(CA));
+}
+
+static vec2d centroid(std::vector<vec2d>& points) {
+    double x = 0, y = 0;
+    for (auto& p : points)
+        x += p.x, y += p.y;
+
+    return vec2d{x, y} / points.size();
+}
+
+polygon poly_generate::general(std::vector<vec2d> points, double mass) {
     double intertia = 0;
+    vec2d c = centroid(points);
+    double area = area_of_poly(points, c);
+    std::cout << "area: " << area << std::endl;
+
+    std::cout << "centroid: " << c << std::endl;
+    for (int i = 0; i < points.size(); ++i)
+        intertia += intertia_of_polygon_subtriangle(
+            mass, area, c, points[i], points[(i + 1) % points.size()]);
+
+    for (auto& p : points) // set the center of the polygon to it's centroid
+        p -= c;
+
     return polygon{{0, 0}, 0, points, intertia, mass};
 }
diff --git a/polygon_generator.h b/polygon_generator.h
index c223c23..2e75972 100644
--- a/polygon_generator.h
+++ b/polygon_generator.h
@@ -18,7 +18,7 @@ namespace poly_generate {
 
     polygon regular(double radius, uint n_sides, double mass = 1);
 
-    polygon general(std::vector<vec2d>& points, double mass = 1);
+    polygon general(std::vector<vec2d> points, double mass = 1);
 
 }; // namespace poly_generate
 #endif
diff --git a/vec2d.h b/vec2d.h
index 319295c..924c801 100644
--- a/vec2d.h
+++ b/vec2d.h
@@ -116,6 +116,10 @@ class vec2d {
         return {-omega * v.y, omega * v.x};
     }
 
+    static double norm2(const vec2d& v) {
+        return vec2d::dot(v, v);
+    }
+
     static double norm(const vec2d& v) {
         return std::sqrt(dot(v, v));
     }