diff --git a/libs/phys/source/engine_2d.cpp b/libs/phys/source/engine_2d.cpp index 13902dda..a8c98424 100644 --- a/libs/phys/source/engine_2d.cpp +++ b/libs/phys/source/engine_2d.cpp @@ -1,6 +1,7 @@ #include #include +#include #include #include @@ -86,6 +87,30 @@ namespace psemek::phys2d return s.position + ex * x + ey * y; } + geom::box 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 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 shape_bbox(half_space const &, static_state const &) + { + return geom::box::full(); + } + template struct wrapped_shape { @@ -430,7 +455,6 @@ namespace psemek::phys2d , wrapped_shape , wrapped_shape , wrapped_shape -// , 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::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 box; + std::size_t g, i; + }; + + std::vector 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 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 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) - { - 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::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); - } + if (!(bboxes[i].box[1] & bboxes[j].box[1]).empty()) + test_objects(bboxes[i].g, bboxes[i].i, bboxes[j].g, bboxes[j].i); } + 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++;