From a75b8710de84e91515d87f46784f508115e6c8c3 Mon Sep 17 00:00:00 2001 From: lisyarus Date: Thu, 3 Dec 2020 20:04:53 +0300 Subject: [PATCH] Physics engine: always resolve penetration, but apply impulse only on approach velocity --- libs/phys/source/engine_2d.cpp | 115 ++++++++++++++++----------------- 1 file changed, 54 insertions(+), 61 deletions(-) diff --git a/libs/phys/source/engine_2d.cpp b/libs/phys/source/engine_2d.cpp index a4df452a..2da9a851 100644 --- a/libs/phys/source/engine_2d.cpp +++ b/libs/phys/source/engine_2d.cpp @@ -390,7 +390,7 @@ namespace psemek::phys2d void apply_gravity(float dt); void integrate(float dt); - bool resolve_collisions(); + void resolve_collisions(); void log_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 gj = gi; gj < groups.size(); ++gj) @@ -473,8 +471,6 @@ namespace psemek::phys2d if (c->penetration == geom::vector::zero()) continue; - // TODO: handle inv_mass == 0 - auto const n = geom::normalized(c->penetration); auto const & infoi = groups[gi].infos[i]; @@ -491,70 +487,67 @@ namespace psemek::phys2d auto const u = uj - ui; - if (geom::dot(u, n) > 0.f) continue; - - had_collision = true; - - auto const & mati = materials[infoi.material]; - auto const & matj = materials[infoj.material]; - - geom::matrix 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)) + if (geom::dot(u, n) < 0.f) { - 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); + auto const & mati = materials[infoi.material]; + auto const & matj = materials[infoj.material]; + + geom::matrix 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[gj].static_states[j].position += c->penetration * infoj.inv_mass / (infoi.inv_mass + infoj.inv_mass); } } } } - - return !had_collision; } float engine::impl::energy()