Added random color generation to the polygons, also made so that the

color of the vector indicating speed is the same as the color of the
polygon, but a little lighter

.gitignore

@@ -2,3 +2,4 @@ balls
 *.o
 compile_commands.json
 .cache/
+.vimspector.json

Makefile

@@ -11,7 +11,7 @@ CXXFLAGS=-Wall -g -O2 $(PROFILING_CFLAGS) $(GTK_CFLAGS)
 LIBS=$(GTK_LIBS) -lm
 
 PROGS=balls
-OBJS=balls.o c_index.o game.o gravity.o spaceship.o main.o polygons.o polygon_generator.o collisions.o
+OBJS=balls.o c_index.o game.o gravity.o spaceship.o main.o polygons.o polygon_generator.o collisions.o color.o
 
 # dependencies (gcc -MM *.cc)
 balls.o: balls.cc game.h balls.h vec2d.h gravity.h
@@ -21,14 +21,15 @@ gravity.o: gravity.cc gravity.h balls.h vec2d.h game.h
 main.o: main.cc game.h balls.h vec2d.h c_index.h gravity.h spaceship.h
 spaceship.o: spaceship.cc balls.h vec2d.h game.h
 stats.o: stats.cc
-polygons.o: polygons.cc polygons.h vec2d.h polygon_generator.h
+polygons.o: polygons.cc polygons.h vec2d.h polygon_generator.h color.h
 polygon_generator.o: polygon_generator.cc polygon_generator.h
 collisions.o: collisions.cc collisions.h vec2d.h
+color.o: color.cc color.h
 
 
 .PHONY: run
 run: balls
-	./balls n=0
+	./balls polygons=1
 
 .PHONY: all
 all: $(PROGS)

collisions.cc

@@ -1,5 +1,7 @@
 #include "collisions.h"
 
+#include "game.h"
+
 #include <algorithm>
 #include <cassert>
 #include <iostream>
@@ -35,10 +37,10 @@ static std::vector<vertex> vertices_of(polygon& p) {
 // Given three collinear points p, q, r, the function checks if
 // point q lies on line segment 'pr'
 static bool on_segment(vec2d& q, segment& pr) {
-    return q.x <= std::max(pr.first.x, pr.second.x) &&
-           q.x >= std::min(pr.first.x, pr.second.x) &&
-           q.y <= std::max(pr.first.y, pr.second.y) &&
-           q.y >= std::min(pr.first.y, pr.second.y);
+    return q.x <= std::max(pr.first.x, pr.second.x)
+        && q.x >= std::min(pr.first.x, pr.second.x)
+        && q.y <= std::max(pr.first.y, pr.second.y)
+        && q.y >= std::min(pr.first.y, pr.second.y);
 }
 
 static Orientation orientation(vec2d& p, vec2d& q, vec2d& r) {
@@ -85,7 +87,8 @@ static std::vector<segment> edges_of(polygon& p) {
     ret.reserve(p.points.size());
     for (uint i = 0; i < p.points.size(); ++i)
         ret.push_back(
-            {p.global_points[i], p.global_points[(i + 1) % p.points.size()]});
+            {p.global_points[i], p.global_points[(i + 1) % p.points.size()]}
+        );
     return ret;
 }
 
@@ -96,10 +99,15 @@ static collision penetration(segment& edge, vertex& vertex, vec2d& d) {
     vec2d n = (edge.second - edge.first).orthogonal();
     ret.n = vec2d::normalize(n);
 
-    // if (vec2d::dot(n, d) > 0)
-    //     ret.n *= -1;
-    std::cout << "-------------- Impact: penetration --------------"
-              << std::endl;
+
+    if (vec2d::dot(n, d) > 0)
+        ret.n *= -1;
+
+    vec2d temp = vertex.v - edge.first;
+    ret.overlap = vec2d::dot(temp, ret.n) * -ret.n;
+    ret.overlap += .1 * delta * -ret.n;
+    // std::cout << "-------------- Impact: penetration --------------"
+    //           << std::endl;
     return ret;
 }
 
@@ -139,13 +147,19 @@ static collision parallel(segment edge_p, segment edge_q, vec2d d) {
     ret.n = base.orthogonal();
     if (vec2d::dot(ret.n, d) > 0)
         ret.n *= -1;
-    std::cout << "-------------- Impact: parallel --------------" << std::endl;
+
+    vec2d temp = ret.impact_point - edge_p.first;
+    ret.overlap = vec2d::dot(temp, ret.n) * -ret.n;
+    ret.overlap += .1 * delta * -ret.n;
+
+    // std::cout << "-------------- Impact: parallel --------------" <<
+    // std::endl;
     return ret;
 }
 
 static bool are_vecs_parallel(vec2d s1, vec2d s2) {
-    return std::abs(vec2d::dot(vec2d::normalize(s1), vec2d::normalize(s2))) >
-           .99;
+    return std::abs(vec2d::dot(vec2d::normalize(s1), vec2d::normalize(s2)))
+         > .99;
 }
 
 static double distance_between_parallel_segments(segment s1, segment s2) {
@@ -159,8 +173,8 @@ static double distance_between_parallel_segments(segment s1, segment s2) {
 static bool are_edges_colinear(segment& e1, segment& e2) {
     vec2d e1_vec = e1.second - e1.first;
     vec2d e2_vec = e2.second - e2.first;
-    return are_vecs_parallel(e1_vec, e2_vec) &&
-           distance_between_parallel_segments(e1, e2) < SMALLEST_DIST;
+    return are_vecs_parallel(e1_vec, e2_vec)
+        && distance_between_parallel_segments(e1, e2) < SMALLEST_DIST;
 }
 
 static collision vertex_edge_collision(polygon& p, polygon& q) {
@@ -205,10 +219,14 @@ static collision vertex_vertex_collision(polygon& p, polygon& q) {
                 if (vec2d::dot(n, d) > 0)
                     n *= -1;
 
-                std::cout
-                    << "-------------- Impact: angle in angle --------------"
-                    << std::endl;
-                return {true, n, vertex.v};
+                vec2d temp = vertex.v - edge_q.first;
+                vec2d overlap = vec2d::dot(temp, n) * -n;
+                overlap += .1 * delta * -n;
+
+                // std::cout
+                //     << "-------------- Impact: angle in angle --------------"
+                //     << std::endl;
+                return {true, n, vertex.v, overlap};
             }
         }
     return {false};
@@ -222,14 +240,17 @@ static collision convex_collides(polygon& p, polygon& q) {
 
     if ((ret = vertex_edge_collision(q, p)).collides) {
         ret.n *= -1;
+        ret.overlap *= -1;
         return ret;
     }
 
     if ((ret = vertex_vertex_collision(p, q)).collides)
         return ret;
 
-    if ((ret = vertex_vertex_collision(q, p)).collides)
+    if ((ret = vertex_vertex_collision(q, p)).collides) {
         ret.n *= -1;
+        ret.overlap *= -1;
+    }
     return ret;
 }
 

collisions.h

@@ -6,8 +6,9 @@
 
 struct collision {
     bool collides = false;
-    vec2d n; // minimum push vector
+    vec2d n;
     vec2d impact_point;
+    vec2d overlap; // minimum push vector
 };
 
 extern collision collides(polygon& p, polygon& q);

color.cc

@@ -0,0 +1,102 @@
+#include "color.h"
+
+// taken from
+// https://stackoverflow.com/questions/3018313/algorithm-to-convert-rgb-to-hsv-and-hsv-to-rgb-in-range-0-255-for-both
+hsv_t rgb2hsv(color_t in) {
+    hsv_t out;
+    double min, max, delta;
+
+    min = in.red < in.green ? in.red : in.green;
+    min = min < in.blue ? min : in.blue;
+
+    max = in.red > in.green ? in.red : in.green;
+    max = max > in.blue ? max : in.blue;
+
+    out.val = max; // v
+    delta = max - min;
+    if (delta < 0.00001) {
+        out.sat = 0;
+        out.hue = 0; // undefined, maybe nan?
+        return out;
+    }
+    if (max > 0.0) { // NOTE: if Max is == 0, this divide would cause a crash
+        out.sat = (delta / max); // s
+    } else {
+        // if max is 0, then r = g = b = 0
+        // s = 0, h is undefined
+        out.sat = 0.0;
+        out.hue = NAN; // its now undefined
+        return out;
+    }
+    if (in.red >= max) // > is bogus, just keeps compilor happy
+        out.hue = (in.green - in.blue) / delta; // between yellow & magenta
+    else if (in.green >= max)
+        out.hue = 2.0 + (in.blue - in.red) / delta; // between cyan & yellow
+    else
+        out.hue = 4.0 + (in.red - in.green) / delta; // between magenta & cyan
+
+    out.hue *= 60.0; // degrees
+
+    if (out.hue < 0.0)
+        out.hue += 360.0;
+
+    return out;
+}
+
+color_t hsv2rgb(hsv_t in) {
+    double hh, p, q, t, ff;
+    long i;
+    color_t out;
+
+    if (in.sat <= 0.0) { // < is bogus, just shuts up warnings
+        out.red = in.val;
+        out.green = in.val;
+        out.blue = in.val;
+        return out;
+    }
+    hh = in.hue;
+    if (hh >= 360.0)
+        hh = 0.0;
+    hh /= 60.0;
+    i = (long) hh;
+    ff = hh - i;
+    p = in.val * (1.0 - in.sat);
+    q = in.val * (1.0 - (in.sat * ff));
+    t = in.val * (1.0 - (in.sat * (1.0 - ff)));
+
+    switch (i) {
+    case 0:
+        out.red = in.val;
+        out.green = t;
+        out.blue = p;
+        break;
+    case 1:
+        out.red = q;
+        out.green = in.val;
+        out.blue = p;
+        break;
+    case 2:
+        out.red = p;
+        out.green = in.val;
+        out.blue = t;
+        break;
+
+    case 3:
+        out.red = p;
+        out.green = q;
+        out.blue = in.val;
+        break;
+    case 4:
+        out.red = t;
+        out.green = p;
+        out.blue = in.val;
+        break;
+    case 5:
+    default:
+        out.red = in.val;
+        out.green = p;
+        out.blue = q;
+        break;
+    }
+    return out;
+}

color.h

@@ -0,0 +1,17 @@
+#ifndef COLOR_H_INCLUDED
+#define COLOR_H_INCLUDED
+
+#include <cmath>
+
+struct color_t {
+    double red, green, blue;
+};
+
+struct hsv_t {
+    double hue, sat, val;
+};
+
+extern hsv_t rgb2hsv(color_t in);
+extern color_t hsv2rgb(hsv_t in);
+
+#endif

game.h

@@ -8,7 +8,7 @@
 
 extern double delta; /* simulation time delta in seconds */
 
-#define DEFAULT_DELTA 0.001
+#define DEFAULT_DELTA 0.01
 
 extern int width;  /* game canvas width */
 extern int height; /* game canvas height */

main.cc

@@ -39,13 +39,15 @@ void check_collisions_with_index() {
 
 void (*check_collisions)() = check_collisions_simple;
 
+unsigned int polys = 0;
+
 void update_state() {
     if (check_collisions)
         check_collisions();
     for (unsigned int i = 0; i < n_balls; ++i)
         ball_update_state(balls + i);
-    polygons_update_state();
     spaceship_update_state();
+    polygons_update_state();
 }
 
 /* Graphics System
@@ -55,13 +57,17 @@ void game_init() {
     srand(time(NULL));
     balls_init();
     assert(c_index_init());
+
+    if (polys)
         polygons_init_state();
-    // spaceship_init_state();
+    else
+        spaceship_init_state();
 }
 
 void game_destroy() {
     c_index_destroy();
     balls_destroy();
+    polygons_destroy();
 }
 
 static guint animation_timeout_id = 0;
@@ -207,6 +213,10 @@ gint keyboard_input(GtkWidget* widget, GdkEventKey* event) {
     case GDK_KEY_p:
         game_animation_on_off();
         return TRUE;
+    case GDK_KEY_S:
+    case GDK_KEY_s:
+        draw_speed = !draw_speed;
+        return TRUE;
     case GDK_KEY_Q:
     case GDK_KEY_q:
         gtk_main_quit();
@@ -241,6 +251,7 @@ void print_usage(const char* progname) {
         "usage: %s [options...]\n"
         "options:\n"
         "\t<width>x<height>\n"
+        "\tpolygons=<0 | 1> :: boolean\n"
         "\tn=<number of balls>\n"
         "\tfconst=<x-force>,<y-force> :: constant force field\n"
         "\tfnewt=<radius>,<g> :: radial, Newtonian force field\n"
@@ -310,6 +321,11 @@ int main(int argc, const char* argv[]) {
     for (int i = 1; i < argc; ++i) {
         if (sscanf(argv[i], "%dx%d", &w, &h) == 2)
             continue;
+        if (sscanf(argv[i], "polygons=%u", &polys) == 1) {
+            if (polys)
+                n_balls = 0;
+            continue;
+        }
         if (sscanf(argv[i], "n=%u", &n_balls) == 1)
             continue;
         if (sscanf(argv[i], "fconst=%lf,%lf", &fa, &fb) == 2) {

polygon_generator.cc

@@ -3,17 +3,23 @@
 #include <cassert>
 #include <cmath>
 
+#define MASS_COEF .1
+
 static double to_rad(double angle_in_deg) {
     static double PI_180 = M_PI / 180;
     return angle_in_deg * PI_180;
 }
 
-polygon poly_generate::rectangle(
-    double width, double height, double mass, std::string label) {
+polygon
+poly_generate::rectangle(double width, double height, std::string label) {
     assert(width > 0);
     assert(height > 0);
     static const double one_over_twelve = 1. / 12;
-    return polygon{{0, 0},
+
+    double mass = MASS_COEF * width * height;
+
+    return polygon{
+        {0, 0},
         0,
         {{-width / 2, -height / 2},
          {-width / 2, height / 2},
@@ -25,7 +31,8 @@ polygon poly_generate::rectangle(
 }
 
 polygon poly_generate::triangle(
-    double side1, double side2, double angle, double mass, std::string label) {
+    double side1, double side2, double angle, std::string label
+) {
     assert(side1 > 0);
     assert(side2 > 0);
     static const double one_over_36 = 1. / 36;
@@ -33,16 +40,21 @@ polygon poly_generate::triangle(
     base = side1;
     height = side2 * std::sin(to_rad(angle));
     vec2d points[] = {
-        {0, 0}, {side1, 0}, {side2 * std::cos(to_rad(angle)), height}};
+        {0, 0},
+        {side1, 0},
+        {side2 * std::cos(to_rad(angle)), height}};
 
     vec2d barycenter = {
         (points[0].x + points[1].x + points[2].x) / 3,
         (points[0].y + points[1].y + points[2].y) / 3,
     };
 
+    double mass = MASS_COEF * base * height * .5;
+
     for (unsigned int i = 0; i < 3; ++i)
         points[i] -= barycenter;
-    return polygon{{0, 0},
+    return polygon{
+        {0, 0},
         0,
         {std::begin(points), std::end(points)},
         one_over_36 * mass * base * std::pow(height, 3),
@@ -50,28 +62,6 @@ polygon poly_generate::triangle(
         label};
 }
 
-polygon poly_generate::regular(
-    double radius, uint n_sides, double mass, std::string label) {
-    assert(n_sides > 2);
-    std::vector<vec2d> points;
-    label += " ";
-    label += n_sides;
-    points.reserve(n_sides);
-    double theta = 2 * M_PI / n_sides;
-
-    for (uint i = 0; i < n_sides; i++)
-        points.push_back({radius * cos(i * theta), radius * sin(i * theta)});
-
-    double l = vec2d::norm(points[1] - points[0]);
-
-    double sin_, cos_;
-    sincos(M_PI / n_sides, &sin_, &cos_);
-    double cot = cos_ / sin_;
-
-    double inertia = mass * l * l / 24 * (1 + 3 * cot * cot);
-    return polygon{{0, 0}, 0, points, inertia, mass, label};
-}
-
 static double area_of_triangle(vec2d& a, vec2d& b, vec2d& c) {
     return std::abs(vec2d::cross(c - a, b - a)) / 2;
 }
@@ -80,23 +70,46 @@ 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()]);
+            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) {
+polygon poly_generate::regular(double radius, uint n_sides, std::string label) {
+    assert(n_sides > 2);
+    std::vector<vec2d> points;
+    points.reserve(n_sides);
+    double theta = 2 * M_PI / n_sides;
+
+
+    for (uint i = 0; i < n_sides; i++)
+        points.push_back({radius * cos(i * theta), radius * sin(i * theta)});
+
+    vec2d centroid = {0, 0};
+    double mass = MASS_COEF * area_of_poly(points, centroid);
+    double l = vec2d::norm(points[1] - points[0]);
+
+    double sin_, cos_;
+    sincos(M_PI / n_sides, &sin_, &cos_);
+    double cot = cos_ / sin_;
+
+    double inertia = mass * l * l / 24 * (1 + 3 * cot * cot);
+    return polygon{centroid, 0, points, inertia, mass, label};
+}
+
+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);
     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));
+    return partial_mass / 2
+         * (vec2d::norm2(AB) / 3 + vec2d::dot(AB, CA) + vec2d::norm2(CA));
 }
 
 static vec2d centroid(std::vector<vec2d>& points) {
@@ -107,15 +120,20 @@ static vec2d centroid(std::vector<vec2d>& points) {
     return vec2d{x, y} / points.size();
 }
 
-polygon poly_generate::general(
-    std::vector<vec2d> points, double mass, std::string label) {
+polygon poly_generate::general(std::vector<vec2d> points, std::string label) {
     double intertia = 0;
     vec2d c = centroid(points);
     double area = area_of_poly(points, c);
+    double mass = MASS_COEF * area;
 
     for (int i = 0; i < points.size(); ++i)
         intertia += intertia_of_polygon_subtriangle(
-            mass, area, c, points[i], points[(i + 1) % points.size()]);
+            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;

polygon_generator.h

@@ -7,31 +7,21 @@
 
 namespace poly_generate {
 
-    polygon rectangle(double width,
-        double height,
-        double mass = 1,
-        std::string label = "rectangle");
+    polygon
+    rectangle(double width, double height, std::string label = "rectangle");
 
-    inline polygon square(
-        double width, double mass = 1, std::string label = "square") {
+    inline polygon square(double width, std::string label = "square") {
         assert(width > 0);
-        return rectangle(width, width, mass, label);
+        return rectangle(width, width, label);
     };
 
-    polygon triangle(double side1,
-        double side2,
-        double angle,
-        double mass = 1,
-        std::string label = "triangle");
+    polygon triangle(
+        double side1, double side2, double angle, std::string label = "triangle"
+    );
 
-    polygon regular(double radius,
-        uint n_sides,
-        double mass = 1,
-        std::string label = "regular");
+    polygon regular(double radius, uint n_sides, std::string label = "regular");
 
-    polygon general(std::vector<vec2d> points,
-        double mass = 1,
-        std::string label = "general");
+    polygon general(std::vector<vec2d> points, std::string label = "general");
 
 }; // namespace poly_generate
 #endif

polygons.cc

@@ -13,74 +13,13 @@
 #include <iostream>
 #include <utility>
 
+#define ARROW_VAL_RATIO 1.7
+
+
+bool draw_speed = true;
+
 polygon* polygons = nullptr;
-uint n_polygons = 11;
-
-void polygons_init_state() {
-    polygons = new polygon[n_polygons];
-    int wall_thickness = 50;
-    // north wall
-    polygons[0] = poly_generate::rectangle(width, wall_thickness, INFINITY)
-                      .set_center({width / 2., -wall_thickness / 2.});
-    // south wall
-    polygons[1] = poly_generate::rectangle(width, wall_thickness, INFINITY)
-                      .set_center({width / 2., height + wall_thickness / 2.});
-    // west wall
-    polygons[2] = poly_generate::rectangle(wall_thickness, height, INFINITY)
-                      .set_center({-wall_thickness / 2., height / 2.});
-    // east wall
-    polygons[3] = poly_generate::rectangle(wall_thickness, height, INFINITY)
-                      .set_center({width + wall_thickness / 2., height / 2.});
-    polygons[4] = poly_generate::rectangle(50, height / 2., INFINITY)
-                      .set_center({25 + width * 1. / 2, height / 2.})
-                      .set_angle(0);
-
-#define PROBLEMATIC_COUPLES 0
-#if PROBLEMATIC_COUPLES == 0
-    polygons[5] = poly_generate::regular(100, 3)
-                      .set_center({100, 400})
-                      .set_angle(0)
-                      .set_speed({200, -10});
-
-    polygons[6] = poly_generate::square(100)
-                      .set_center({600, 400})
-                      .set_angle(45)
-                      .set_speed({-200, -10});
-
-    polygons[7] = poly_generate::general(
-        {{0, 0}, {100, 0}, {100, 100}, {50, 150}, {0, 100}}, 3)
-                      .set_center({200, 600});
-
-    polygons[8] = poly_generate::rectangle(100, 150).set_center({600, 200});
-    polygons[9] = poly_generate::regular(50, 5).set_center({150, 150});
-    polygons[10] =
-        poly_generate::general({{0, 0}, {50, 80}, {0, 160}, {-50, 80}})
-            .set_center({700, 700})
-            .set_speed({0, -100});
-#elif PROBLEMATIC_COUPLES == 1
-    // for this problematic couple to work, remove the gravity
-    n_polygons = 7;
-    polygons[4] = poly_generate::regular(100, 3)
-                      .set_center({103.91, 684.587})
-                      .set_angle(280.108)
-                      .set_speed({190.114, 131.983})
-                      .set_angular_speed(32.6448);
-
-    polygons[5] = poly_generate::square(100)
-                      .set_center({614.338, 514.889})
-                      .set_angle(-54.3526)
-                      .set_speed({-70.0347, -62.4788})
-                      .set_angular_speed(-39.3846);
-
-    polygons[6] = poly_generate::general(
-        {{0, 0}, {100, 0}, {100, 100}, {50, 150}, {0, 100}}, 3)
-                      .set_center({261.425, 556.613})
-                      .set_angle(-122.59)
-                      .set_speed({-46.9522, 48.5392})
-                      .set_angular_speed(-35.5983);
-
-#endif
-}
+uint n_polygons = 0;
 
 static double to_rad(double angle_in_deg) {
     static double PI_180 = M_PI / 180.;
@@ -92,6 +31,98 @@ static double to_deg(double angle_in_rad) {
     return angle_in_rad * PI_180;
 }
 
+static double random_color_component() {
+    return 1.0 * (rand() % 200 + 56) / 255;
+};
+
+void polygons_init_state() {
+    n_polygons = 20;
+    polygons = new polygon[n_polygons];
+    int wall_thickness = 50;
+    uint n = 0;
+    // north wall
+    polygons[n++] = poly_generate::rectangle(width, wall_thickness)
+                        .set_mass(INFINITY)
+                        .set_center({width / 2., -wall_thickness / 2.});
+    // south wall
+    polygons[n++] = poly_generate::rectangle(width, wall_thickness)
+                        .set_mass(INFINITY)
+                        .set_center({width / 2., height + wall_thickness / 2.});
+    // west wall
+    polygons[n++] = poly_generate::rectangle(wall_thickness, height)
+                        .set_mass(INFINITY)
+                        .set_center({-wall_thickness / 2., height / 2.});
+    // east wall
+    polygons[n++] = poly_generate::rectangle(wall_thickness, height)
+                        .set_mass(INFINITY)
+                        .set_center({width + wall_thickness / 2., height / 2.});
+
+    // top triangle wall
+    polygons[n++] = poly_generate::triangle(height / 4., height / 4., 90)
+                        .set_mass(INFINITY)
+                        .set_center({width / 2., height / 17.})
+                        .set_angle(-135);
+    // bottom triangle wall
+    polygons[n++] = poly_generate::triangle(height / 4., height / 4., 90)
+                        .set_mass(INFINITY)
+                        .set_center({width / 2., height - height / 17.})
+                        .set_angle(45);
+
+
+    // ---------- Shapes flying around start here ----------
+    polygons[n++] = poly_generate::regular(100, 3)
+                        .set_center({100, 400})
+                        .set_angle(0)
+                        .set_speed({200, -10});
+
+    polygons[n++] = poly_generate::square(100)
+                        .set_center({600, 400})
+                        .set_angle(45)
+                        .set_speed({-200, -10});
+
+    polygons[n++] = poly_generate::general(
+                        {{0, 0}, {100, 0}, {100, 100}, {50, 150}, {0, 100}}
+    )
+                        .set_center({200, 600})
+                        .set_angle(45)
+                        .set_speed({10, 0});
+
+    polygons[n++] = poly_generate::rectangle(100, 150).set_center({600, 200});
+    polygons[n++] = poly_generate::regular(50, 5)
+                        .set_center({150, 150})
+                        .set_speed({100, 0});
+
+    polygons[n++] = poly_generate::general({{40, 20},
+                                            {40, 40},
+                                            {80, 40},
+                                            {80, -40},
+                                            {40, -40},
+                                            {40, -20},
+                                            {-40, -20},
+                                            {-40, -40},
+                                            {-80, -40},
+                                            {-80, 40},
+                                            {-40, 40},
+                                            {-40, 20}})
+                        .set_center({700, 700})
+                        .set_speed({0, -100});
+
+    assert(n <= n_polygons);
+    n_polygons = n;
+
+    // Set the color of the polygons to random
+    for (polygon* p = polygons; p != polygons + n_polygons; ++p) {
+        p->color = {
+            random_color_component(),
+            random_color_component(),
+            random_color_component()};
+        hsv_t hsv = rgb2hsv(p->color);
+        if (hsv.val > 0.95)
+            hsv.val *= .7;
+        p->color = hsv2rgb(hsv);
+    }
+}
+
 static bool is_point_inside_rect(rect rect, vec2d point) {
     vec2d tl = rect.first, br = rect.second;
     return point.x > tl.x && point.x < br.x && point.y > tl.y && point.y < br.y;
@@ -99,13 +130,14 @@ static bool is_point_inside_rect(rect rect, vec2d point) {
 
 static bool bounding_rects_collide(rect cur_bound, rect other_bound) {
     vec2d other_tl = other_bound.first, other_br = other_bound.second;
-    return is_point_inside_rect(cur_bound, other_tl) ||
-           is_point_inside_rect(cur_bound, {other_tl.x, other_br.y}) ||
-           is_point_inside_rect(cur_bound, {other_br.x, other_tl.y}) ||
-           is_point_inside_rect(cur_bound, other_br);
+    return is_point_inside_rect(cur_bound, other_tl)
+        || is_point_inside_rect(cur_bound, {other_tl.x, other_br.y})
+        || is_point_inside_rect(cur_bound, {other_br.x, other_tl.y})
+        || is_point_inside_rect(cur_bound, other_br);
 }
 
-static double impulse_parameter(vec2d v_ab1,
+static double impulse_parameter(
+    vec2d v_ab1,
     vec2d n,
     double m_a,
     double m_b,
@@ -113,13 +145,14 @@ static double impulse_parameter(vec2d v_ab1,
     vec2d r_bp,
     double I_a,
     double I_b,
-    double e) {
+    double e
+) {
     double nominator = -(1 + e) * vec2d::dot(v_ab1, n);
 
     double r_ap_cross_n = vec2d::cross(r_ap, n);
     double r_bp_cross_n = vec2d::cross(r_bp, n);
-    double denominator = 1 / m_a + 1 / m_b + r_ap_cross_n * r_ap_cross_n / I_a +
-                         r_bp_cross_n * r_bp_cross_n / I_b;
+    double denominator = 1 / m_a + 1 / m_b + r_ap_cross_n * r_ap_cross_n / I_a
+                       + r_bp_cross_n * r_bp_cross_n / I_b;
 
     return nominator / denominator;
 }
@@ -138,18 +171,26 @@ static void handle_collision(collision& c, polygon* a, polygon* b) {
 
     vec2d v_ab1 = v_ap1 - v_bp1;
 
-    // avoid the polygons getting stuck if, on the frame after the impact,
-    // the polygon a is still inside of b
-    if (a->collided_with.find(b) != a->collided_with.end())
-        return;
-
-    a->collided_with.insert(b);
-    b->collided_with.insert(a);
+    if (vec2d::norm(c.overlap) > 10)
+        c.overlap = -.1 * vec2d::normalize(c.overlap);
+    // std::cout << c.overlap << std::endl;
+    if (b->mass == INFINITY)
+        a->translate(c.overlap);
+    else {
+        double ma = a->mass;
+        double mb = b->mass;
+        double m_total = ma + mb;
 
+        // If b is wall, then mb / m_total = INFINITY / INFINITY = -nan,
+        // so we need the if statement above
+        a->translate(c.overlap * mb / m_total);
+        b->translate(-c.overlap * ma / m_total);
+    }
 
     double I_a = a->inertia, I_b = b->inertia;
 
-    double j = impulse_parameter(v_ab1,
+    double j = impulse_parameter(
+        v_ab1,
         c.n,
         a->mass,
         b->mass,
@@ -157,7 +198,8 @@ static void handle_collision(collision& c, polygon* a, polygon* b) {
         r_bp,
         I_a,
         I_b,
-        restitution_coefficient_get());
+        restitution_coefficient_get()
+    );
 
     vec2d v_a2 = a->speed + j * c.n / a->mass;
     vec2d v_b2 = b->speed - j * c.n / b->mass;
@@ -172,45 +214,27 @@ static void handle_collision(collision& c, polygon* a, polygon* b) {
     b->angular_speed = to_deg(omega_b2);
 }
 
-collision col; // tbd
-
 static void check_collisions(polygon* current_p) {
     rect cur_bound = current_p->get_bounding_box();
 
-    bool collided_with_something = false;
-    for (polygon* other_p = polygons; other_p != polygons + n_polygons;
-         ++other_p) {
+    collision c;
+    polygon* other_p;
+    for (other_p = polygons; other_p != polygons + n_polygons; ++other_p) {
         if (other_p == current_p) // polygons don't collide with themselves
             continue;
-        if (vec2d::norm(current_p->speed) == 0 &&
-            vec2d::norm(other_p->speed) == 0)
-            // if both are not moving, no need to go through the whole
-            // collision detection thing
-            continue;
         rect other_bound = other_p->get_bounding_box();
-        if (bounding_rects_collide(cur_bound, other_bound) ||
-            bounding_rects_collide(other_bound, cur_bound)) {
-            collision c = collides(*current_p, *other_p);
-            if (c.collides) {
-                collided_with_something = true;
-                col = c;
+        if ((bounding_rects_collide(cur_bound, other_bound)
+             || bounding_rects_collide(other_bound, cur_bound))
+            && (c = collides(*current_p, *other_p)).collides) {
             handle_collision(c, current_p, other_p);
-            } else if (current_p->collided_with.find(other_p) !=
-                       current_p->collided_with.end()) {
-                current_p->collided_with.erase(other_p);
-                other_p->collided_with.erase(current_p);
         }
     }
 }
-    if (!collided_with_something && current_p->collided_with.size() > 0)
-        current_p->collided_with.clear();
-}
 
 void polygons_update_state() {
     for (polygon* p = polygons; p != polygons + n_polygons; ++p) {
         if (p->mass == INFINITY) // immovable objects don't need to be updated
             continue;
-        // check_border_collision(p);
         check_collisions(p);
 
         p->rotate(delta * p->angular_speed);
@@ -221,7 +245,6 @@ void polygons_update_state() {
         vec2d g = gravity_vector(p);
         p->translate(.5 * delta * delta * g);
         p->speed += delta * g;
-        // std::cout << *p << std::endl;
     }
 }
 
@@ -268,21 +291,38 @@ void polygon::draw(cairo_t* cr) const {
         cairo_line_to(cr, point.x, point.y);
 
     cairo_line_to(cr, this->global_points[0].x, this->global_points[0].y);
-    cairo_stroke(cr);
-
+    if (mass == INFINITY) {
+        cairo_set_source_rgb(cr, .5, .5, .5);
+        cairo_fill(cr);
+    } else {
+        cairo_set_source_rgb(
+            cr,
+            this->color.red,
+            this->color.green,
+            this->color.blue
+        );
+        cairo_fill(cr);
+    }
 
     // draw centroid
     vec2d centroid = this->centroid();
     draw_circle(cr, centroid, 1);
 
+
     // draw speed
-    (10 * delta * this->speed).draw(cr, centroid);
+    if (draw_speed && this->mass != INFINITY) {
+        hsv_t hsv = rgb2hsv(this->color);
+        hsv.val = hsv.val * ARROW_VAL_RATIO;
+        color_t arrow_color = hsv2rgb(hsv);
+        (delta * this->speed).draw(cr, centroid, arrow_color);
+    }
 }
 
 void polygons_draw(cairo_t* cr) {
-    draw_circle(cr, col.impact_point, 3); // tbd
-    col.n.draw(cr, col.impact_point);     // tbd
-
     for (const polygon* p = polygons; p != polygons + n_polygons; ++p)
         p->draw(cr);
 }
+
+void polygons_destroy() {
+    delete[] (polygons);
+}

polygons.h

@@ -1,11 +1,11 @@
 #ifndef POLYGONS_H_INCLUDED
 #define POLYGONS_H_INCLUDED
 
+#include "color.h"
 #include "vec2d.h"
 
 #include <cmath>
 #include <gtk/gtk.h>
-#include <unordered_set>
 #include <utility>
 #include <vector>
 
@@ -23,11 +23,11 @@ class polygon {
     double mass;
     std::string label;
 
+    color_t color;
     std::vector<vec2d> global_points = points;
 
     vec2d speed;
     double angular_speed;
-    std::unordered_set<polygon*> collided_with;
 
     void draw(cairo_t* cr) const;
     void draw_bounding_rect(cairo_t* cr) const;
@@ -73,6 +73,14 @@ class polygon {
 
     polygon& set_mass(double m) {
         mass = m;
+        // TODO: This is pretty shit, but it's good to make stuff work now,
+        // since the only time we change the mass of an object is the wall,
+        // otherwise we take the area and the polygon_generator module handles
+        // that. But what's to be done is to extract the intertia calcualtion to
+        // a new module so that we can import it here, and recalculate the
+        // intertia of the polygon if the mass changes.
+        if (m == INFINITY)
+            inertia = INFINITY;
         return *this;
     }
 
@@ -104,6 +112,7 @@ class polygon {
         os << "    angle: " << p.angle << std::endl;
         os << "    speed: " << p.speed << std::endl;
         os << "    angular speed: " << p.angular_speed << std::endl;
+        os << "    intertia: " << p.inertia << std::endl;
         return os;
     }
 };
@@ -111,8 +120,11 @@ class polygon {
 extern polygon* polygons;
 extern uint n_polygons;
 
+extern bool draw_speed;
+
 extern void polygons_init_state();
 extern void polygons_update_state();
 extern void polygons_draw(cairo_t* cr);
+extern void polygons_destroy();
 
 #endif

vec2d.h

@@ -2,6 +2,7 @@
 #define VEC2D_H_INCLUDED
 
 #include "cairo.h"
+#include "color.h"
 
 #include <cmath>
 #include <iostream>
@@ -74,7 +75,7 @@ class vec2d {
         return {-y, x};
     }
 
-    void draw(cairo_t* cr, vec2d p) const {
+    void draw(cairo_t* cr, vec2d p, color_t color) const {
         double arrow_lenght_ = 10 * vec2d::norm(*this);
         double arrow_degrees_ = .5;
 
@@ -84,12 +85,13 @@ class vec2d {
 
         vec2d end = p + (*this) * 30;
 
-        double x1 = end.x + arrow_lenght_ * cos(angle - arrow_degrees_);
-        double y1 = end.y + arrow_lenght_ * sin(angle - arrow_degrees_);
-        double x2 = end.x + arrow_lenght_ * cos(angle + arrow_degrees_);
-        double y2 = end.y + arrow_lenght_ * sin(angle + arrow_degrees_);
+        double head_length = fmin(10, arrow_lenght_);
+        double x1 = end.x + head_length * cos(angle - arrow_degrees_);
+        double y1 = end.y + head_length * sin(angle - arrow_degrees_);
+        double x2 = end.x + head_length * cos(angle + arrow_degrees_);
+        double y2 = end.y + head_length * sin(angle + arrow_degrees_);
 
-        cairo_set_source_rgb(cr, 255, 0, 0);
+        cairo_set_source_rgb(cr, color.red, color.green, color.blue);
 
         cairo_move_to(cr, p.x, p.y);
         cairo_line_to(cr, end.x, end.y);