Physics engine: implement collision resolution in order of importance
This commit is contained in:
parent
a75b8710de
commit
9d1ba342fc
1 changed files with 345 additions and 64 deletions
|
|
@ -13,6 +13,7 @@
|
|||
#include <psemek/log/log.hpp>
|
||||
|
||||
#include <optional>
|
||||
#include <set>
|
||||
|
||||
namespace psemek::phys2d
|
||||
{
|
||||
|
|
@ -355,6 +356,37 @@ namespace psemek::phys2d
|
|||
return convex_collision(points1, normals1, points2, normals2);
|
||||
}
|
||||
|
||||
struct contact
|
||||
{
|
||||
std::size_t gi, i, gj, j;
|
||||
geom::vector<float, 2> ri, rj;
|
||||
geom::vector<float, 2> penetration;
|
||||
geom::vector<float, 2> normal;
|
||||
float penetration_depth;
|
||||
geom::vector<float, 2> velocity;
|
||||
float velocity_projection;
|
||||
};
|
||||
|
||||
struct velocity_comparator
|
||||
{
|
||||
bool operator()(contact const & c1, contact const & c2) const
|
||||
{
|
||||
if (c1.velocity_projection == c2.velocity_projection)
|
||||
return std::tie(c1.gi, c1.gj, c1.i, c1.j) < std::tie(c2.gi, c2.gj, c2.i, c2.j);
|
||||
return c1.velocity_projection < c2.velocity_projection;
|
||||
}
|
||||
};
|
||||
|
||||
struct penetration_comparator
|
||||
{
|
||||
bool operator()(contact const & c1, contact const & c2) const
|
||||
{
|
||||
if (c1.penetration_depth == c2.penetration_depth)
|
||||
return std::tie(c1.gi, c1.gj, c1.i, c1.j) < std::tie(c2.gi, c2.gj, c2.i, c2.j);
|
||||
return c1.penetration_depth > c2.penetration_depth;
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
struct engine::impl
|
||||
|
|
@ -375,9 +407,8 @@ namespace psemek::phys2d
|
|||
std::vector<static_state> static_states;
|
||||
std::vector<dynamic_state> dynamic_states;
|
||||
|
||||
std::vector<geom::vector<float, 2>> position_change;
|
||||
std::vector<geom::vector<float, 2>> velocity_change;
|
||||
std::vector<float> angular_velocity_change;
|
||||
std::vector<std::vector<std::set<contact, velocity_comparator>::iterator>> contacts_by_velocity;
|
||||
std::vector<std::vector<std::set<contact, penetration_comparator>::iterator>> contacts_by_penetration;
|
||||
};
|
||||
|
||||
std::vector<group> groups;
|
||||
|
|
@ -449,6 +480,19 @@ namespace psemek::phys2d
|
|||
|
||||
void engine::impl::resolve_collisions()
|
||||
{
|
||||
for (std::size_t gi = 0; gi < groups.size(); ++gi)
|
||||
{
|
||||
groups[gi].contacts_by_velocity.resize(groups[gi].infos.size());
|
||||
groups[gi].contacts_by_penetration.resize(groups[gi].infos.size());
|
||||
for (std::size_t i = 0; i < groups[gi].infos.size(); ++i)
|
||||
{
|
||||
groups[gi].contacts_by_velocity[i].clear();
|
||||
groups[gi].contacts_by_penetration[i].clear();
|
||||
}
|
||||
}
|
||||
|
||||
std::set<contact, velocity_comparator> contacts_by_velocity;
|
||||
|
||||
for (std::size_t gi = 0; gi < groups.size(); ++gi)
|
||||
{
|
||||
for (std::size_t gj = gi; gj < groups.size(); ++gj)
|
||||
|
|
@ -471,11 +515,6 @@ namespace psemek::phys2d
|
|||
|
||||
if (c->penetration == geom::vector<float, 2>::zero()) continue;
|
||||
|
||||
auto const n = geom::normalized(c->penetration);
|
||||
|
||||
auto const & infoi = groups[gi].infos[i];
|
||||
auto const & infoj = groups[gj].infos[j];
|
||||
|
||||
auto ri = c->position - sti.position;
|
||||
auto rj = c->position - stj.position;
|
||||
|
||||
|
|
@ -487,8 +526,50 @@ namespace psemek::phys2d
|
|||
|
||||
auto const u = uj - ui;
|
||||
|
||||
if (geom::dot(u, n) < 0.f)
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
std::size_t const contact_count = contacts_by_velocity.size();
|
||||
std::size_t const impulse_iterations = contact_count * 2;
|
||||
std::size_t const penetraion_iterations = contact_count * 2;
|
||||
|
||||
for (std::size_t iteration = 0; iteration < impulse_iterations; ++iteration)
|
||||
{
|
||||
contact const & c = *contacts_by_velocity.begin();
|
||||
|
||||
if (c.velocity_projection > 0.f)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
if (c.gi == 1 && c.gj == 1)
|
||||
{
|
||||
int fuck = 42;
|
||||
(void)fuck;
|
||||
}
|
||||
|
||||
auto const & infoi = groups[c.gi].infos[c.i];
|
||||
auto const & infoj = groups[c.gj].infos[c.j];
|
||||
|
||||
auto const & mati = materials[infoi.material];
|
||||
auto const & matj = materials[infoj.material];
|
||||
|
||||
|
|
@ -498,31 +579,29 @@ namespace psemek::phys2d
|
|||
K[0][1] = 0.f;
|
||||
K[1][0] = 0.f;
|
||||
|
||||
K[0][0] += ri[1] * ri[1] * infoi.inv_inertia;
|
||||
K[1][1] += ri[0] * ri[0] * infoi.inv_inertia;
|
||||
K[0][1] -= ri[0] * ri[1] * infoi.inv_inertia;
|
||||
K[0][0] += c.ri[1] * c.ri[1] * infoi.inv_inertia;
|
||||
K[1][1] += c.ri[0] * c.ri[0] * infoi.inv_inertia;
|
||||
K[0][1] -= c.ri[0] * c.ri[1] * infoi.inv_inertia;
|
||||
|
||||
K[0][0] += rj[1] * rj[1] * infoj.inv_inertia;
|
||||
K[1][1] += rj[0] * rj[0] * infoj.inv_inertia;
|
||||
K[0][1] -= rj[0] * rj[1] * infoj.inv_inertia;
|
||||
K[0][0] += c.rj[1] * c.rj[1] * infoj.inv_inertia;
|
||||
K[1][1] += c.rj[0] * c.rj[0] * infoj.inv_inertia;
|
||||
K[0][1] -= c.rj[0] * c.rj[1] * infoj.inv_inertia;
|
||||
|
||||
K[1][0] = K[0][1];
|
||||
|
||||
auto const & n = c.normal;
|
||||
|
||||
// Plastic sliding impulse
|
||||
// Normal relative velocity -> 0
|
||||
// Tangential relative velocity -> unchanged
|
||||
auto const J1 = - n * geom::dot(n, u) / geom::dot(n, K * n);
|
||||
auto const J1 = - n * c.velocity_projection / geom::dot(n, K * n);
|
||||
// Plastic sticking impulse
|
||||
// Normal relative velocity -> 0
|
||||
// Tangential relative velocity -> 0
|
||||
auto const J2 = - *geom::solve(K, u);
|
||||
auto const J2 = - *geom::solve(K, c.velocity);
|
||||
|
||||
// float const e = (mati.elasticity + matj.elasticity) / 2.f;
|
||||
float const e = std::sqrt(mati.elasticity * matj.elasticity);
|
||||
|
||||
// float const f = (mati.friction + matj.friction) / 2.f;
|
||||
float const f = std::sqrt(mati.friction * matj.friction);
|
||||
|
||||
float const mu = f;
|
||||
|
||||
auto J = (1.f + e) * J1 + f * (J2 - J1);
|
||||
|
|
@ -535,17 +614,219 @@ namespace psemek::phys2d
|
|||
J = (1.f + e) * J1 + k * (J2 - J1);
|
||||
}
|
||||
|
||||
groups[gi].dynamic_states[i].velocity -= J * infoi.inv_mass;
|
||||
groups[gj].dynamic_states[j].velocity += J * infoj.inv_mass;
|
||||
auto dvi = -J * infoi.inv_mass;
|
||||
auto dvj = J * infoj.inv_mass;
|
||||
|
||||
groups[gi].dynamic_states[i].angular_velocity -= geom::det(ri, J) * infoi.inv_inertia;
|
||||
groups[gj].dynamic_states[j].angular_velocity += geom::det(rj, J) * infoj.inv_inertia;
|
||||
auto dai = -geom::det(c.ri, J) * infoi.inv_inertia;
|
||||
auto daj = geom::det(c.rj, J) * infoj.inv_inertia;
|
||||
|
||||
groups[c.gi].dynamic_states[c.i].velocity += dvi;
|
||||
groups[c.gj].dynamic_states[c.j].velocity += dvj;
|
||||
|
||||
groups[c.gi].dynamic_states[c.i].angular_velocity += dai;
|
||||
groups[c.gj].dynamic_states[c.j].angular_velocity += daj;
|
||||
|
||||
auto ct = contacts_by_velocity.begin();
|
||||
|
||||
std::size_t cti = -1, ctj = -1;
|
||||
|
||||
auto update_contact = [&](auto old_it, std::size_t G, std::size_t I, std::size_t i, geom::vector<float, 2> const & dv, float da)
|
||||
{
|
||||
contact const & c = *old_it;
|
||||
|
||||
std::size_t OG, OI;
|
||||
bool flip;
|
||||
|
||||
if (c.gi == G && c.i == I)
|
||||
{
|
||||
OG = c.gj;
|
||||
OI = c.j;
|
||||
flip = false;
|
||||
}
|
||||
else
|
||||
{
|
||||
OG = c.gi;
|
||||
OI = c.i;
|
||||
flip = true;
|
||||
}
|
||||
|
||||
groups[gi].static_states[i].position -= c->penetration * infoi.inv_mass / (infoi.inv_mass + infoj.inv_mass);
|
||||
groups[gj].static_states[j].position += c->penetration * infoj.inv_mass / (infoi.inv_mass + infoj.inv_mass);
|
||||
std::size_t oi;
|
||||
for (oi = 0; oi < groups[OG].contacts_by_velocity[OI].size(); ++oi)
|
||||
{
|
||||
if (groups[OG].contacts_by_velocity[OI][oi] == old_it)
|
||||
break;
|
||||
}
|
||||
|
||||
auto node = contacts_by_velocity.extract(old_it);
|
||||
contact & cc = node.value();
|
||||
cc.velocity += (flip ? 1.f : -1.f) * (dv + geom::ort(flip ? cc.rj : cc.ri) * da);
|
||||
cc.velocity_projection = geom::dot(cc.velocity, cc.normal);
|
||||
|
||||
auto res = contacts_by_velocity.insert(std::move(node));
|
||||
assert(res.inserted);
|
||||
auto new_it = res.position;
|
||||
|
||||
groups[G].contacts_by_velocity[I][i] = new_it;
|
||||
groups[OG].contacts_by_velocity[OI][oi] = new_it;
|
||||
};
|
||||
|
||||
for (std::size_t i = 0; i < groups[c.gi].contacts_by_velocity[c.i].size(); ++i)
|
||||
{
|
||||
auto old_it = groups[c.gi].contacts_by_velocity[c.i][i];
|
||||
if (old_it == ct)
|
||||
{
|
||||
cti = i;
|
||||
continue;
|
||||
}
|
||||
|
||||
update_contact(old_it, c.gi, c.i, i, dvi, dai);
|
||||
}
|
||||
|
||||
for (std::size_t i = 0; i < groups[c.gj].contacts_by_velocity[c.j].size(); ++i)
|
||||
{
|
||||
auto old_it = groups[c.gj].contacts_by_velocity[c.j][i];
|
||||
if (old_it == ct)
|
||||
{
|
||||
ctj = i;
|
||||
continue;
|
||||
}
|
||||
|
||||
update_contact(old_it, c.gj, c.j, i, dvj, daj);
|
||||
}
|
||||
|
||||
{
|
||||
auto node = contacts_by_velocity.extract(ct);
|
||||
contact & cc = node.value();
|
||||
cc.velocity -= dvi + geom::ort(cc.ri) * dai;
|
||||
cc.velocity += dvj + geom::ort(cc.rj) * daj;
|
||||
cc.velocity_projection = geom::dot(cc.velocity, cc.normal);
|
||||
|
||||
auto res = contacts_by_velocity.insert(std::move(node));
|
||||
assert(res.inserted);
|
||||
auto new_it = res.position;
|
||||
|
||||
auto & ccc = *new_it;
|
||||
|
||||
groups[ccc.gi].contacts_by_velocity[ccc.i][cti] = new_it;
|
||||
groups[ccc.gj].contacts_by_velocity[ccc.j][ctj] = new_it;
|
||||
}
|
||||
}
|
||||
|
||||
std::set<contact, penetration_comparator> contacts_by_penetration;
|
||||
|
||||
for (auto it = contacts_by_velocity.begin(); it != contacts_by_velocity.end();)
|
||||
{
|
||||
auto jt = it++;
|
||||
auto node = contacts_by_velocity.extract(jt);
|
||||
auto res = contacts_by_penetration.insert(std::move(node));
|
||||
assert(res.inserted);
|
||||
auto new_it = res.position;
|
||||
contact const & c = *new_it;
|
||||
groups[c.gi].contacts_by_penetration[c.i].push_back(new_it);
|
||||
groups[c.gj].contacts_by_penetration[c.j].push_back(new_it);
|
||||
}
|
||||
|
||||
assert(contacts_by_penetration.size() == contact_count);
|
||||
|
||||
for (std::size_t iteration = 0; iteration < penetraion_iterations; ++iteration)
|
||||
{
|
||||
contact const & c = *contacts_by_penetration.begin();
|
||||
|
||||
if (c.penetration_depth < 0.f)
|
||||
break;
|
||||
|
||||
auto const & infoi = groups[c.gi].infos[c.i];
|
||||
auto const & infoj = groups[c.gj].infos[c.j];
|
||||
|
||||
auto di = - c.penetration * infoi.inv_mass / (infoi.inv_mass + infoj.inv_mass);
|
||||
auto dj = c.penetration * infoj.inv_mass / (infoi.inv_mass + infoj.inv_mass);
|
||||
|
||||
groups[c.gi].static_states[c.i].position += di;
|
||||
groups[c.gj].static_states[c.j].position += dj;
|
||||
|
||||
auto ct = contacts_by_penetration.begin();
|
||||
|
||||
std::size_t cti = -1, ctj = -1;
|
||||
|
||||
auto update_contact = [&](auto old_it, std::size_t G, std::size_t I, std::size_t i, geom::vector<float, 2> const & d)
|
||||
{
|
||||
contact const & c = *old_it;
|
||||
|
||||
std::size_t OG, OI;
|
||||
bool flip;
|
||||
|
||||
if (c.gi == G && c.i == I)
|
||||
{
|
||||
OG = c.gj;
|
||||
OI = c.j;
|
||||
flip = false;
|
||||
}
|
||||
else
|
||||
{
|
||||
OG = c.gi;
|
||||
OI = c.i;
|
||||
flip = true;
|
||||
}
|
||||
|
||||
std::size_t oi;
|
||||
for (oi = 0; oi < groups[OG].contacts_by_penetration[OI].size(); ++oi)
|
||||
{
|
||||
if (groups[OG].contacts_by_penetration[OI][oi] == old_it)
|
||||
break;
|
||||
}
|
||||
|
||||
auto node = contacts_by_penetration.extract(old_it);
|
||||
contact & cc = node.value();
|
||||
cc.penetration += (flip ? -1.f : 1.f) * d;
|
||||
cc.penetration_depth = geom::dot(cc.penetration, cc.normal);
|
||||
|
||||
auto res = contacts_by_penetration.insert(std::move(node));
|
||||
assert(res.inserted);
|
||||
auto new_it = res.position;
|
||||
|
||||
groups[G].contacts_by_penetration[I][i] = new_it;
|
||||
groups[OG].contacts_by_penetration[OI][oi] = new_it;
|
||||
};
|
||||
|
||||
for (std::size_t i = 0; i < groups[c.gi].contacts_by_penetration[c.i].size(); ++i)
|
||||
{
|
||||
auto old_it = groups[c.gi].contacts_by_penetration[c.i][i];
|
||||
if (old_it == ct)
|
||||
{
|
||||
cti = i;
|
||||
continue;
|
||||
}
|
||||
|
||||
update_contact(old_it, c.gi, c.i, i, di);
|
||||
}
|
||||
|
||||
for (std::size_t i = 0; i < groups[c.gj].contacts_by_penetration[c.j].size(); ++i)
|
||||
{
|
||||
auto old_it = groups[c.gj].contacts_by_penetration[c.j][i];
|
||||
if (old_it == ct)
|
||||
{
|
||||
ctj = i;
|
||||
continue;
|
||||
}
|
||||
|
||||
update_contact(old_it, c.gj, c.j, i, dj);
|
||||
}
|
||||
|
||||
{
|
||||
auto node = contacts_by_penetration.extract(ct);
|
||||
contact & cc = node.value();
|
||||
cc.penetration += di;
|
||||
cc.penetration -= dj;
|
||||
cc.penetration_depth = geom::dot(cc.penetration, cc.normal);
|
||||
|
||||
auto res = contacts_by_penetration.insert(std::move(node));
|
||||
assert(res.inserted);
|
||||
auto new_it = res.position;
|
||||
|
||||
auto & ccc = *new_it;
|
||||
|
||||
groups[ccc.gi].contacts_by_penetration[ccc.i][cti] = new_it;
|
||||
groups[ccc.gj].contacts_by_penetration[ccc.j][ctj] = new_it;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue