From d0de911344d731c5b9e366d015bb1a808d8cfe26 Mon Sep 17 00:00:00 2001 From: Karma Riuk Date: Sun, 2 Apr 2023 21:53:21 +0200 Subject: [PATCH] got the speed working (yay) but the angular velocity that results is superrrr low, thus not creating any rotation after impact, wierd... --- Makefile | 3 +- collisions.cc | 123 ++++++++++++++++++++++++++ collisions.h | 15 ++++ polygon_generator.cc | 27 ++++-- polygon_generator.h | 8 +- polygons.cc | 203 +++++++++++++++++++++++++++++++++++++------ polygons.h | 41 ++++++--- vec2d.h | 22 ++++- 8 files changed, 390 insertions(+), 52 deletions(-) create mode 100644 collisions.cc create mode 100644 collisions.h diff --git a/Makefile b/Makefile index 8f8e715..cb2aed7 100644 --- a/Makefile +++ b/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 +OBJS=balls.o c_index.o game.o gravity.o spaceship.o main.o polygons.o polygon_generator.o collisions.o # dependencies (gcc -MM *.cc) balls.o: balls.cc game.h balls.h vec2d.h gravity.h @@ -23,6 +23,7 @@ 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 polygon_generator.o: polygon_generator.cc polygon_generator.h +collisions.o: collisions.cc collisions.h vec2d.h .PHONY: run diff --git a/collisions.cc b/collisions.cc new file mode 100644 index 0000000..3203099 --- /dev/null +++ b/collisions.cc @@ -0,0 +1,123 @@ +#include "collisions.h" + +#include +#include +#include + +static std::vector edges_of(polygon& p) { + std::vector edges; + edges.reserve(p.global_points.size()); + + for (uint i = 0; i < p.global_points.size(); ++i) + edges.push_back(p.global_points[(i + 1) % p.global_points.size()] - + p.global_points[i]); + return edges; +} + +static bool separating_axis( + vec2d& axis, polygon& p, polygon& q, vec2d* pv, vec2d* impact_point) { + double min_p, max_p, min_q, max_q; + min_p = min_q = INFINITY; + max_p = max_q = -INFINITY; + + double projection; + vec2d min_p_point, max_p_point; + for (auto& point : p.global_points) { + projection = vec2d::dot(point, axis); + + if (projection < min_p) { + min_p = projection; + min_p_point = point; + } + + if (projection > max_p) { + max_p = projection; + max_p_point = point; + } + } + + for (auto& point : q.global_points) { + projection = vec2d::dot(point, axis); + + min_q = std::min(min_q, projection); + max_q = std::max(max_q, projection); + } + + if (max_p >= min_q && max_q >= min_p) { + double d; + if (max_q - min_p < max_p - min_q) { + d = max_q - min_p; + *impact_point = min_p_point; + } else { + d = max_p - min_q; + *impact_point = max_p_point; + } + // push a bit more than needed so the shapes do not overlap in future + // tests due to float precision + double d_over_o_squared = d / vec2d::dot(axis, axis) + 1e-10; + *pv = d_over_o_squared * axis; + return false; + } + + return true; +} + +static uint get_smallest_vec_index(std::vector vs) { + uint ret, i = 0; + double min = INFINITY; + for (auto& v : vs) { + double dot = vec2d::dot(v, v); + if (dot < min) { + ret = i; + min = dot; + } + i++; + } + return ret; +} + +static collision convex_collides(polygon& p, polygon& q) { + collision ret{false}; + + std::vector edges_p = edges_of(p); + std::vector edges_q = edges_of(q); + std::vector edges; + edges.reserve(edges_p.size() + edges_q.size()); + edges.insert(edges.end(), edges_p.begin(), edges_p.end()); + edges.insert(edges.end(), edges_q.begin(), edges_q.end()); + std::vector orthogonals; + orthogonals.reserve(edges.size()); + for (auto& v : edges) + orthogonals.push_back(v.orthogonal()); + + std::vector push_vectors; + push_vectors.reserve(orthogonals.size()); + std::vector impact_points; + push_vectors.reserve(orthogonals.size()); + + vec2d push_vector; + vec2d impact_point; + for (auto& o : orthogonals) { + if (separating_axis(o, p, q, &push_vector, &impact_point)) + // the axis is separating (the projections don't overlap) + return ret; + push_vectors.push_back(push_vector); + impact_points.push_back(impact_point); + } + // if no axis is sperating, then they must be colliding + ret.collides = true; + uint i = get_smallest_vec_index(push_vectors); + ret.n = vec2d::normalize(push_vectors[i]); + ret.impact_point = impact_points[i]; + + // assert that p pushes away from q + vec2d d = q.centroid() - p.centroid(); + if (vec2d::dot(ret.n, d) > 0) + ret.n *= -1; + + return ret; +} + +collision collides(polygon& p, polygon& q) { + return convex_collides(p, q); +} diff --git a/collisions.h b/collisions.h new file mode 100644 index 0000000..10b8aa2 --- /dev/null +++ b/collisions.h @@ -0,0 +1,15 @@ +#ifndef COLLISIONS_H_INCLUDED +#define COLLISIONS_H_INCLUDED + +#include "polygons.h" +#include "vec2d.h" + +struct collision { + bool collides; + vec2d n; // minimum push vector + vec2d impact_point; +}; + +extern collision collides(polygon& p, polygon& q); + +#endif diff --git a/polygon_generator.cc b/polygon_generator.cc index fad40e5..4d7e1a2 100644 --- a/polygon_generator.cc +++ b/polygon_generator.cc @@ -8,23 +8,30 @@ static double to_rad(double angle_in_deg) { return angle_in_deg * PI_180; } -polygon poly_generate::rectangle(double width, double height) { +polygon poly_generate::rectangle(double width, double height, double mass) { assert(width > 0); assert(height > 0); + static const double one_over_twelve = 1. / 12; return polygon{{0, 0}, 0, {{-width / 2, -height / 2}, - {width / 2, -height / 2}, + {-width / 2, height / 2}, {width / 2, height / 2}, - {-width / 2, height / 2}}}; + {width / 2, -height / 2}}, + one_over_twelve * mass * width * std::pow(height, 3), + mass}; } -polygon poly_generate::triangle(double side1, double side2, double angle) { +polygon poly_generate::triangle( + double side1, double side2, double angle, double mass) { assert(side1 > 0); assert(side2 > 0); - vec2d points[] = {{0, 0}, - {side1, 0}, - {side2 * std::cos(to_rad(angle)), side2 * std::sin(to_rad(angle))}}; + static const double one_over_36 = 1. / 36; + double base, height; + base = side1; + height = side2 * std::sin(to_rad(angle)); + vec2d points[] = { + {0, 0}, {side1, 0}, {side2 * std::cos(to_rad(angle)), height}}; vec2d barycenter = { (points[0].x + points[1].x + points[2].x) / 3, @@ -33,5 +40,9 @@ polygon poly_generate::triangle(double side1, double side2, double angle) { for (unsigned int i = 0; i < 3; ++i) points[i] -= barycenter; - return polygon{{0, 0}, 0, {std::begin(points), std::end(points)}}; + return polygon{{0, 0}, + 0, + {std::begin(points), std::end(points)}, + one_over_36 * mass * base * std::pow(height, 3), + mass}; } diff --git a/polygon_generator.h b/polygon_generator.h index 11dd5f4..f92655f 100644 --- a/polygon_generator.h +++ b/polygon_generator.h @@ -7,14 +7,14 @@ namespace poly_generate { - polygon rectangle(double width, double height); + polygon rectangle(double width, double height, double mass = 1); - inline polygon square(double width) { + inline polygon square(double width, double mass = 1) { assert(width > 0); - return rectangle(width, width); + return rectangle(width, width, mass); }; - polygon triangle(double side1, double side2, double angle); + polygon triangle(double side1, double side2, double angle, double mass = 1); }; // namespace poly_generate #endif diff --git a/polygons.cc b/polygons.cc index 2b23d49..1b7d9bc 100644 --- a/polygons.cc +++ b/polygons.cc @@ -1,43 +1,184 @@ #include "polygons.h" #include "cairo.h" +#include "collisions.h" #include "game.h" #include "matrix.h" #include "polygon_generator.h" #include +#include polygon* polygons = nullptr; -uint n_polygons = 1; +uint n_polygons = 2; void polygons_init_state() { polygons = new polygon[n_polygons]; - std::cout << "width" << width << std::endl; - // polygons[0] = poly_generate::square(200) - // .set_center({400, 600}) - // .set_angle(37) - // .set_speed({1, -1}); - polygons[0] = poly_generate::triangle(150, 150, 30) - .set_center({400, 300}) - .set_angle(45) - .set_speed({3, 2}); - // polygons[2] = poly_generate::rectangle(200, 100).set_center({600, 600}); + polygons[0] = poly_generate::square(100, 1) + .set_center({200, 530}) + .set_angle(-30) + .set_angular_speed(0) + .set_speed({200, -200}); + // polygons[0] = poly_generate::triangle(150, 150, 30) + // .set_center({200, 400}) + // .set_angle(13) + // .set_angular_speed(10) + // .set_speed({200, 50}); + polygons[1] = poly_generate::rectangle(50, height / 2., INFINITY) + .set_center({25 + width * 1. / 2, height / 2.}) + .set_angle(0); } static double to_rad(double angle_in_deg) { - static double PI_180 = M_PI / 180; + static double PI_180 = M_PI / 180.; return angle_in_deg * PI_180; } +static double to_deg(double angle_in_rad) { + static double PI_180 = 180. / M_PI; + return angle_in_rad * PI_180; +} + +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; +} + +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); +} + +static double impulse_parameter(vec2d v_ab1, + vec2d n, + double m_a, + double m_b, + vec2d r_ap, + vec2d r_bp, + double I_a, + double I_b, + double e) { + double nominator = -(1 + e) * vec2d::dot(v_ab1, n); + double denominator = 1 / m_a + 1 / m_b + + std::pow(vec2d::cross(r_ap, n), 2) / I_a + + std::pow(vec2d::cross(r_bp, n), 2) / I_b; + return nominator / denominator; +} + +static void handle_collision(collision& c, polygon* a, polygon* b) { + // see https://www.myphysicslab.com/engine2D/collision-en.html for the + // formulas + + double omega_a1 = to_rad(a->angular_speed); + double omega_b1 = to_rad(b->angular_speed); + + vec2d r_ap = c.impact_point - a->centroid(); + vec2d v_ap1 = a->speed + vec2d::cross(omega_a1, r_ap); + std::cout << " " + << "r_ap = " << r_ap << std::endl; + std::cout << " " + << "v_ap1 = " << v_ap1 << std::endl; + + vec2d r_bp = c.impact_point - b->centroid(); + vec2d v_bp1 = b->speed + vec2d::cross(omega_b1, r_bp); + std::cout << " " + << "r_bp = " << r_bp << std::endl; + std::cout << " " + << "v_bp1 = " << v_bp1 << std::endl; + + vec2d v_ab1 = v_ap1 - v_bp1; + std::cout << " " + << "v_ab1 = " << v_ab1 << std::endl; + + double I_a = a->inertia, I_b = b->inertia; + std::cout << " Parameters for j: " << std::endl; + std::cout << " " + << "v_ab1 = " << v_ab1 << std::endl; + std::cout << " " + << "n = " << c.n << std::endl; + std::cout << " " + << "m_a = " << a->mass << std::endl; + std::cout << " " + << "m_b = " << b->mass << std::endl; + std::cout << " " + << "r_ap = " << r_ap << std::endl; + std::cout << " " + << "r_bp = " << r_bp << std::endl; + std::cout << " " + << "I_a = " << I_a << std::endl; + std::cout << " " + << "I_b = " << I_b << std::endl; + + double j = impulse_parameter( + v_ab1, c.n, a->mass, b->mass, r_ap, r_bp, I_a, I_b, 1); + std::cout << "====> j = " << j << std::endl; + + vec2d v_a2 = a->speed + j * c.n / a->mass; + vec2d v_b2 = b->speed - j * c.n / b->mass; + std::cout << " " + << "v_a2 = " << v_a2 << std::endl; + std::cout << " " + << "v_b2 = " << v_b2 << std::endl; + + double omega_a2 = omega_a1 + vec2d::cross(r_ap, j * c.n) / I_a; + double omega_b2 = omega_b1 - vec2d::cross(r_bp, j * c.n) / I_b; + std::cout << " " + << "omega_a2 = " << omega_a2 << std::endl; + std::cout << " " + << "omega_b2 = " << omega_b2 << std::endl; + + a->speed = v_a2; + a->angular_speed = to_deg(omega_a2); + + b->speed = v_b2; + b->angular_speed = to_deg(omega_b2); +} + +static void check_collisions(polygon* current_p) { + rect cur_bound = current_p->get_bounding_box(); + + for (polygon* other_p = polygons; other_p != polygons + n_polygons; + ++other_p) { + if (other_p == current_p) // polygons don't collide with themselves + continue; + rect other_bound = other_p->get_bounding_box(); + if (bounding_rects_collide(cur_bound, other_bound) || + bounding_rects_collide(other_bound, cur_bound)) { + // std::cout << "Bounding boxes do collide" << std::endl; + collision c = collides(*current_p, *other_p); + if (c.collides) { + std::cout << "colliding" << std::endl; + std::cout << "speed before: " << current_p->speed << std::endl; + std::cout << "angular speed before: " + << current_p->angular_speed << std::endl; + handle_collision(c, current_p, other_p); + std::cout << "speed after: " << current_p->speed << std::endl; + std::cout << "angular speed after: " << current_p->angular_speed + << std::endl; + std::cout << std::endl; + } + } + } +} + static void check_border_collision(polygon* p) { for (auto& point : p->global_points) { - if (point.x <= 0 || point.x >= width) { - p->speed.x *= -1; - break; - } - if (point.y <= 0 || point.y >= height) { - p->speed.y *= -1; + bool hit_vert_wall = point.x <= 0 || point.x >= width; + bool hit_hori_wall = point.y <= 0 || point.y >= width; + + if (hit_vert_wall || hit_hori_wall) { + p->set_angular_speed(-p->angular_speed); // this is not accurate, + // but avoids bugs for now + + if (hit_vert_wall) + p->speed.x *= -1; + if (hit_hori_wall) + p->speed.y *= -1; + break; } } @@ -45,9 +186,12 @@ static void check_border_collision(polygon* p) { void polygons_update_state() { for (polygon* p = polygons; p != polygons + n_polygons; ++p) { - p->rotate(1); + if (p->mass == INFINITY) // immovable objects don't need to be updated + continue; check_border_collision(p); - p->translate(p->speed); + check_collisions(p); + p->rotate(delta * p->angular_speed); + p->translate(delta * p->speed); } } @@ -67,13 +211,13 @@ void polygon::draw_bounding_rect(cairo_t* cr) const { cairo_set_dash(cr, dashes, 2, 0); auto bb = this->get_bounding_box(); - vec2d min = bb.first, max = bb.second; + vec2d tl = bb.first, br = bb.second; - cairo_line_to(cr, min.x, min.y); - cairo_line_to(cr, min.x, max.y); - cairo_line_to(cr, max.x, max.y); - cairo_line_to(cr, max.x, min.y); - cairo_line_to(cr, min.x, min.y); + cairo_line_to(cr, tl.x, tl.y); + cairo_line_to(cr, tl.x, br.y); + cairo_line_to(cr, br.x, br.y); + cairo_line_to(cr, br.x, tl.y); + cairo_line_to(cr, tl.x, tl.y); cairo_stroke(cr); cairo_set_dash(cr, 0, 0, 0); // disable dashes } @@ -88,6 +232,13 @@ void polygon::draw(cairo_t* cr) const { cairo_line_to(cr, this->global_points[0].x, this->global_points[0].y); cairo_stroke(cr); + + // draw centroid + vec2d centroid = this->centroid(); + cairo_translate(cr, centroid.x, centroid.y); + cairo_arc(cr, 0, 0, 1, 0, 2 * M_PI); + cairo_stroke(cr); + cairo_translate(cr, -centroid.x, -centroid.y); } void polygons_draw(cairo_t* cr) { diff --git a/polygons.h b/polygons.h index afa3298..edc3c59 100644 --- a/polygons.h +++ b/polygons.h @@ -8,6 +8,8 @@ #include #include +typedef std::pair rect; + class polygon { private: void update_global_points(); @@ -16,30 +18,32 @@ class polygon { vec2d center; double angle; std::vector points; + double inertia; + double mass; + std::vector global_points = points; vec2d speed; double angular_speed; - double mass = 1; void draw(cairo_t* cr) const; void draw_bounding_rect(cairo_t* cr) const; - std::pair get_bounding_box() const { - vec2d min{INFINITY, INFINITY}, max{-INFINITY, -INFINITY}; + rect get_bounding_box() const { + vec2d tl{INFINITY, INFINITY}, tr{-INFINITY, -INFINITY}; for (auto& point : this->global_points) { - if (point.x < min.x) - min.x = point.x; - if (point.y < min.y) - min.y = point.y; + if (point.x < tl.x) + tl.x = point.x; + if (point.y < tl.y) + tl.y = point.y; - if (point.x > max.x) - max.x = point.x; - if (point.y > max.y) - max.y = point.y; + if (point.x > tr.x) + tr.x = point.x; + if (point.y > tr.y) + tr.y = point.y; } - return {min, max}; + return {tl, tr}; } polygon& set_center(vec2d c) { @@ -64,6 +68,11 @@ class polygon { return *this; } + polygon& set_mass(double m) { + mass = m; + return *this; + } + polygon& translate(vec2d delta) { center += delta; update_global_points(); @@ -75,6 +84,14 @@ class polygon { update_global_points(); return *this; } + + vec2d centroid() const { + double x = 0, y = 0; + for (auto& p : global_points) + x += p.x, y += p.y; + + return vec2d{x, y} / points.size(); + } }; extern polygon* polygons; diff --git a/vec2d.h b/vec2d.h index b5bbc79..1bca361 100644 --- a/vec2d.h +++ b/vec2d.h @@ -41,6 +41,10 @@ class vec2d { return vec2d{x - other.x, y - other.y}; } + vec2d operator-() const { + return vec2d{-x, -y}; + } + vec2d operator*(double l) const { return vec2d{x * l, y * l}; } @@ -59,12 +63,28 @@ class vec2d { return *this; } + vec2d orthogonal() { + return {-y, x}; + } + static double dot(const vec2d& a, const vec2d& b) { return a.x * b.x + a.y * b.y; } static double cross(const vec2d& a, const vec2d& b) { - return a.x * b.x + a.y * b.y; + return a.x * b.y - a.y * b.x; + } + + static vec2d cross(const double omega, const vec2d& v) { + return {-omega * v.y, omega * v.x}; + } + + static double norm(const vec2d& v) { + return std::sqrt(dot(v, v)); + } + + static vec2d normalize(const vec2d& v) { + return v / norm(v); } };