Physics engine 2d: accelerate collision detection be sorting bboxes on x-coordinate
This commit is contained in:
parent
9c54f6e730
commit
2875cb468d
1 changed files with 126 additions and 48 deletions
|
|
@ -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)
|
||||
{
|
||||
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);
|
||||
}
|
||||
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++;
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue