Physics engine 2d: accelerate collision detection be sorting bboxes on x-coordinate

libs/phys/source/engine_2d.cpp

@@ -1,6 +1,7 @@
 #include <psemek/phys/engine_2d.hpp>
 
 #include <psemek/util/heterogeneous_container.hpp>
+#include <psemek/util/profiler.hpp>
 
 #include <psemek/geom/constants.hpp>
 #include <psemek/geom/matrix.hpp>
@@ -86,6 +87,30 @@ namespace psemek::phys2d
 			return s.position + ex * x + ey * y;
 		}
 
+		geom::box<float, 2> shape_bbox(ball const & b, static_state const & s)
+		{
+			return {{{s.position[0] - b.radius, s.position[0] + b.radius}, {s.position[1] - b.radius, s.position[1] + b.radius}}};
+		}
+
+		geom::box<float, 2> shape_bbox(box const & b, static_state const & s)
+		{
+			float cc = std::abs(std::cos(s.rotation));
+			float ss = std::abs(std::sin(s.rotation));
+
+			float w = b.width / 2.f;
+			float h = b.height / 2.f;
+
+			float tx = w * cc + h * ss;
+			float ty = w * ss + h * cc;
+
+			return {{{s.position[0] - tx, s.position[0] + tx}, {s.position[1] - ty, s.position[1] + ty}}};
+		}
+
+		geom::box<float, 2> shape_bbox(half_space const &, static_state const &)
+		{
+			return geom::box<float, 2>::full();
+		}
+
 		template <typename Shape>
 		struct wrapped_shape
 		{
@@ -430,7 +455,6 @@ namespace psemek::phys2d
 			, wrapped_shape<ball>
 			, wrapped_shape<half_space>
 			, wrapped_shape<box>
-//			, box
 //			, convex_polygon
 			> shapes;
 
@@ -490,6 +514,8 @@ namespace psemek::phys2d
 
 	void engine::impl::update(float dt)
 	{
+		profile_function;
+
 		apply_gravity(dt);
 		integrate(dt);
 		gather_collisions();
@@ -528,6 +554,8 @@ namespace psemek::phys2d
 
 	void engine::impl::gather_collisions()
 	{
+		profile_function;
+
 		for (std::size_t gi = 0; gi < groups.size(); ++gi)
 		{
 			groups[gi].contacts_by_velocity.resize(groups[gi].infos.size());
@@ -542,64 +570,112 @@ namespace psemek::phys2d
 		contacts_by_velocity.clear();
 		contacts_by_penetration.clear();
 
+		auto test_objects = [&](std::size_t gi, std::size_t i, std::size_t gj, std::size_t j)
+		{
+			auto shi = groups[gi].infos[i].shape;
+			auto shj = groups[gj].infos[j].shape;
+
+			auto & sti = groups[gi].static_states[i];
+			auto & stj = groups[gj].static_states[j];
+
+			auto c = shapes.visit([&](auto const & si, auto const & sj){
+				return shape_collision(si.shape, sti, sj.shape, stj);
+			}, shi, shj);
+
+			if (!c) return;
+
+			if (c->penetration == geom::vector<float, 2>::zero()) return;
+
+			auto ri = c->position - sti.position;
+			auto rj = c->position - stj.position;
+
+			auto & dsi = groups[gi].dynamic_states[i];
+			auto & dsj = groups[gj].dynamic_states[j];
+
+			auto ui = dsi.velocity + geom::ort(ri) * dsi.angular_velocity;
+			auto uj = dsj.velocity + geom::ort(rj) * dsj.angular_velocity;
+
+			auto const u = uj - ui;
+
+			contact ct;
+			ct.gi = gi;
+			ct.gj = gj;
+			ct.i = i;
+			ct.j = j;
+			ct.ri = ri;
+			ct.rj = rj;
+			ct.penetration = c->penetration;
+			ct.normal = geom::normalized(ct.penetration);
+			ct.penetration_depth = geom::length(ct.penetration);
+			ct.velocity = u;
+			ct.velocity_projection = geom::dot(u, ct.normal);
+
+			auto res = contacts_by_velocity.insert(ct);
+			assert(res.second);
+			groups[gi].contacts_by_velocity[i].push_back(res.first);
+			groups[gj].contacts_by_velocity[j].push_back(res.first);
+		};
+
+		struct bbox_data
+		{
+			geom::box<float, 2> box;
+			std::size_t g, i;
+		};
+
+		std::vector<bbox_data> bboxes;
 		for (std::size_t gi = 0; gi < groups.size(); ++gi)
 		{
-			for (std::size_t gj = gi; gj < groups.size(); ++gj)
+			for (std::size_t i = 0; i < groups[gi].infos.size(); ++i)
 			{
-				for (std::size_t i = 0; i < groups[gi].infos.size(); ++i)
+				bbox_data data;
+				data.box = shapes.visit([&](auto const & s){ return shape_bbox(s.shape, groups[gi].static_states[i]); }, groups[gi].infos[i].shape);
+				data.g = gi;
+				data.i = i;
+				bboxes.push_back(data);
+			}
+		}
+
+		std::vector<std::size_t> min, max;
+		min.reserve(bboxes.size());
+		max.reserve(bboxes.size());
+
+		for (std::size_t i = 0; i < bboxes.size(); ++i)
+		{
+			min.push_back(i);
+			max.push_back(i);
+		}
+
+		std::sort(min.begin(), min.end(), [&](auto i, auto j){ return bboxes[i].box[0].min < bboxes[j].box[0].min; });
+		std::sort(max.begin(), max.end(), [&](auto i, auto j){ return bboxes[i].box[0].max < bboxes[j].box[0].max; });
+
+		std::set<std::size_t> current;
+
+		for (std::size_t mini = 0, maxi = 0; mini < bboxes.size();)
+		{
+			if (bboxes[min[mini]].box[0].min < bboxes[max[maxi]].box[0].max)
+			{
+				std::size_t i = min[mini];
+				for (auto j : current)
 				{
-					for (std::size_t j = (gi == gj) ? i + 1 : 0; j < groups[gj].infos.size(); ++j)
+					if (!(bboxes[i].box[1] & bboxes[j].box[1]).empty())
-					{
+						test_objects(bboxes[i].g, bboxes[i].i, bboxes[j].g, bboxes[j].i);
-						auto shi = groups[gi].infos[i].shape;
-						auto shj = groups[gj].infos[j].shape;
-
-						auto & sti = groups[gi].static_states[i];
-						auto & stj = groups[gj].static_states[j];
-
-						auto c = shapes.visit([&](auto const & si, auto const & sj){
-							return shape_collision(si.shape, sti, sj.shape, stj);
-						}, shi, shj);
-
-						if (!c) continue;
-
-						if (c->penetration == geom::vector<float, 2>::zero()) continue;
-
-						auto ri = c->position - sti.position;
-						auto rj = c->position - stj.position;
-
-						auto & dsi = groups[gi].dynamic_states[i];
-						auto & dsj = groups[gj].dynamic_states[j];
-
-						auto ui = dsi.velocity + geom::ort(ri) * dsi.angular_velocity;
-						auto uj = dsj.velocity + geom::ort(rj) * dsj.angular_velocity;
-
-						auto const u = uj - ui;
-
-						contact ct;
-						ct.gi = gi;
-						ct.gj = gj;
-						ct.i = i;
-						ct.j = j;
-						ct.ri = ri;
-						ct.rj = rj;
-						ct.penetration = c->penetration;
-						ct.normal = geom::normalized(ct.penetration);
-						ct.penetration_depth = geom::length(ct.penetration);
-						ct.velocity = u;
-						ct.velocity_projection = geom::dot(u, ct.normal);
-
-						auto res = contacts_by_velocity.insert(ct);
-						assert(res.second);
-						groups[gi].contacts_by_velocity[i].push_back(res.first);
-						groups[gj].contacts_by_velocity[j].push_back(res.first);
-					}
 				}
+				current.insert(i);
+
+				++mini;
+			}
+			else
+			{
+				current.erase(max[maxi]);
+				++maxi;
 			}
 		}
 	}
 
 	void engine::impl::resolve_impulses()
 	{
+		profile_function;
+
 		std::size_t const iterations = contacts_by_velocity.size() * 2;
 
 		for (std::size_t iteration = 0; iteration < iterations; ++iteration)
@@ -760,6 +836,8 @@ namespace psemek::phys2d
 
 	void engine::impl::resolve_penetrations()
 	{
+		profile_function;
+
 		for (auto it = contacts_by_velocity.begin(); it != contacts_by_velocity.end();)
 		{
 			auto jt = it++;