From 9d1ba342fc4ff95ac50cd17f439a6bb15298e3fa Mon Sep 17 00:00:00 2001 From: lisyarus Date: Thu, 3 Dec 2020 22:57:15 +0300 Subject: [PATCH] Physics engine: implement collision resolution in order of importance --- libs/phys/source/engine_2d.cpp | 409 +++++++++++++++++++++++++++------ 1 file changed, 345 insertions(+), 64 deletions(-) diff --git a/libs/phys/source/engine_2d.cpp b/libs/phys/source/engine_2d.cpp index 2da9a851..f0daf0fe 100644 --- a/libs/phys/source/engine_2d.cpp +++ b/libs/phys/source/engine_2d.cpp @@ -13,6 +13,7 @@ #include #include +#include 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 ri, rj; + geom::vector penetration; + geom::vector normal; + float penetration_depth; + geom::vector 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_states; std::vector dynamic_states; - std::vector> position_change; - std::vector> velocity_change; - std::vector angular_velocity_change; + std::vector::iterator>> contacts_by_velocity; + std::vector::iterator>> contacts_by_penetration; }; std::vector 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 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::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,67 +526,309 @@ namespace psemek::phys2d auto const u = uj - ui; - if (geom::dot(u, n) < 0.f) - { - auto const & mati = materials[infoi.material]; - auto const & matj = materials[infoj.material]; + 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); - 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].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); + 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]; + + 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] += 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] += 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 * 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, c.velocity); + + float const e = std::sqrt(mati.elasticity * matj.elasticity); + 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); + } + + auto dvi = -J * infoi.inv_mass; + auto dvj = J * infoj.inv_mass; + + 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 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; + } + + 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 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 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; + } + } } float engine::impl::energy()