From 2e81763d5a5a1d39c746ee9e8b27b7031aea4bc4 Mon Sep 17 00:00:00 2001
From: lisyarus <lisyarus@gmail.com>
Date: Fri, 4 Dec 2020 14:52:19 +0300
Subject: [PATCH] Physics engine 2d: properly apply explosion impulse

---
 libs/phys/source/engine_2d.cpp | 45 +++++++++++++++++++++++++++++++++-
 1 file changed, 44 insertions(+), 1 deletion(-)

diff --git a/libs/phys/source/engine_2d.cpp b/libs/phys/source/engine_2d.cpp
index b15af8d9..751d443c 100644
--- a/libs/phys/source/engine_2d.cpp
+++ b/libs/phys/source/engine_2d.cpp
@@ -7,6 +7,7 @@
 #include <psemek/geom/gauss.hpp>
 #include <psemek/geom/rotation.hpp>
 #include <psemek/geom/math.hpp>
+#include <psemek/geom/interval.hpp>
 
 #include <psemek/log/log.hpp>
 
@@ -49,6 +50,42 @@ namespace psemek::phys2d
 			return shape_area(b) * (b.width * b.width + b.height * b.height) / 12.f;
 		}
 
+		geom::point<float, 2> shape_closest(ball const & b, static_state const & s, geom::point<float, 2> const & p)
+		{
+			auto d = p - s.position;
+			auto l = geom::length(d);
+			if (l <= b.radius)
+				return p;
+			return s.position + (d / l) * b.radius;
+		}
+
+		geom::point<float, 2> shape_closest(half_space const & b, static_state const &, geom::point<float, 2> const & p)
+		{
+			auto v = geom::dot(b.normal, p - geom::point<float, 2>::zero()) - b.value;
+			if (v <= 0.f)
+				return p;
+			return p - b.normal * v;
+		}
+
+		geom::point<float, 2> shape_closest(box const & b, static_state const & s, geom::point<float, 2> const & p)
+		{
+			geom::vector ex{std::cos(s.rotation), std::sin(s.rotation)};
+			geom::vector ey{-std::sin(s.rotation), std::cos(s.rotation)};
+
+			float w = b.width / 2.f;
+			float h = b.height / 2.f;
+
+			auto d = p - s.position;
+
+			float x = geom::dot(d, ex);
+			float y = geom::dot(d, ey);
+
+			x = geom::clamp(x, {-w, w});
+			y = geom::clamp(y, {-h, h});
+
+			return s.position + ex * x + ey * y;
+		}
+
 		template <typename Shape>
 		struct wrapped_shape
 		{
@@ -436,11 +473,17 @@ namespace psemek::phys2d
 		{
 			for (std::size_t i = 0; i < g.infos.size(); ++i)
 			{
-				auto r = g.static_states[i].position - center;
+				if (g.infos[i].inv_mass == 0.f) continue;
+
+				auto const closest = shapes.visit([&](auto const & s) -> geom::point<float, 2> { return shape_closest(s.shape, g.static_states[i], center); }, g.infos[i].shape);
+
+				auto r = closest - center;
 				float l = geom::length(r);
 
 				auto J = (r / l) * strength / (1.f + l * l * attenuation);
+
 				g.dynamic_states[i].velocity += J * g.infos[i].inv_mass;
+				g.dynamic_states[i].angular_velocity += geom::det(closest - g.static_states[i].position, J) * g.infos[i].inv_inertia;
 			}
 		}
 	}