lisyarus/psemek
1
0
Fork 1
Code Issues Pull requests 1 Projects Releases Packages Wiki Activity Actions

Physics engine: always resolve penetration, but apply impulse only on approach velocity

Browse source
This commit is contained in:
Nikita Lisitsa 2020-12-03 20:04:53 +03:00
parent b2b6d18dd0
commit a75b8710de
1 changed files with 54 additions and 61 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

115
libs/phys/source/engine_2d.cpp
View file

@ -390,7 +390,7 @@ namespace psemek::phys2d
void apply_gravity(float dt); void apply_gravity(float dt);
void integrate(float dt); void integrate(float dt);
bool resolve_collisions(); void resolve_collisions();
void log_energy(); void log_energy();
float energy(); float energy();
@ -447,10 +447,8 @@ namespace psemek::phys2d
} }
} }
bool engine::impl::resolve_collisions() void engine::impl::resolve_collisions()
{ {
bool had_collision = false;
for (std::size_t gi = 0; gi < groups.size(); ++gi) for (std::size_t gi = 0; gi < groups.size(); ++gi)
{ {
for (std::size_t gj = gi; gj < groups.size(); ++gj) for (std::size_t gj = gi; gj < groups.size(); ++gj)
@ -473,8 +471,6 @@ namespace psemek::phys2d
if (c->penetration == geom::vector<float, 2>::zero()) continue; if (c->penetration == geom::vector<float, 2>::zero()) continue;
// TODO: handle inv_mass == 0
auto const n = geom::normalized(c->penetration); auto const n = geom::normalized(c->penetration);
auto const & infoi = groups[gi].infos[i]; auto const & infoi = groups[gi].infos[i];
@ -491,70 +487,67 @@ namespace psemek::phys2d
auto const u = uj - ui; auto const u = uj - ui;
if (geom::dot(u, n) > 0.f) continue; if (geom::dot(u, n) < 0.f)
had_collision = true;
auto const & mati = materials[infoi.material];
auto const & matj = materials[infoj.material];
geom::matrix<float, 2, 2> K;
K[0][0] = infoi.inv_mass + infoj.inv_mass;
K[1][1] = K[0][0];
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] += 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[1][0] = K[0][1];
// Plastic sliding impulse
// Normal relative velocity -> 0
// Tangential relative velocity -> unchanged
auto const J1 = - n * geom::dot(n, u) / geom::dot(n, K * n);
// Plastic sticking impulse
// Normal relative velocity -> 0
// Tangential relative velocity -> 0
auto const J2 = - *geom::solve(K, u);
// 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);
// Test for friction cone
float q = geom::length(J - n * geom::dot(J, n));
if (q > mu * geom::dot(J, n))
{ {
float k = (mu * (1.f + e) * geom::dot(J1, n)) / (geom::length(J2 - n * geom::dot(J2, n)) - mu * geom::dot(n, J2 - J1)); auto const & mati = materials[infoi.material];
J = (1.f + e) * J1 + k * (J2 - J1); auto const & matj = materials[infoj.material];
geom::matrix<float, 2, 2> K;
K[0][0] = infoi.inv_mass + infoj.inv_mass;
K[1][1] = K[0][0];
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] += 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[1][0] = K[0][1];
// Plastic sliding impulse
// Normal relative velocity -> 0
// Tangential relative velocity -> unchanged
auto const J1 = - n * geom::dot(n, u) / geom::dot(n, K * n);
// Plastic sticking impulse
// Normal relative velocity -> 0
// Tangential relative velocity -> 0
auto const J2 = - *geom::solve(K, u);
// 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);
// Test for friction cone
float q = geom::length(J - n * geom::dot(J, n));
if (q > mu * geom::dot(J, n))
{
float k = (mu * (1.f + e) * geom::dot(J1, n)) / (geom::length(J2 - n * geom::dot(J2, n)) - mu * geom::dot(n, J2 - J1));
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;
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;
} }
groups[gi].dynamic_states[i].velocity -= J * infoi.inv_mass;
groups[gj].dynamic_states[j].velocity += 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;
groups[gi].static_states[i].position -= c->penetration * infoi.inv_mass / (infoi.inv_mass + infoj.inv_mass); 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); groups[gj].static_states[j].position += c->penetration * infoj.inv_mass / (infoi.inv_mass + infoj.inv_mass);
} }
} }
} }
} }
return !had_collision;
} }
float engine::impl::energy() float engine::impl::energy()