From 62e010094b7cf75b3dca90f1f937cfe312aff1b1 Mon Sep 17 00:00:00 2001 From: lisyarus Date: Sat, 8 May 2021 22:02:09 +0300 Subject: [PATCH] Try to implement constraints in 2d physics engine --- libs/phys/include/psemek/phys/common_2d.hpp | 10 + libs/phys/include/psemek/phys/engine_2d.hpp | 10 + libs/phys/source/engine_2d.cpp | 202 +++++++++++++++++--- 3 files changed, 192 insertions(+), 30 deletions(-) diff --git a/libs/phys/include/psemek/phys/common_2d.hpp b/libs/phys/include/psemek/phys/common_2d.hpp index c5cea3e1..4aa49401 100644 --- a/libs/phys/include/psemek/phys/common_2d.hpp +++ b/libs/phys/include/psemek/phys/common_2d.hpp @@ -50,4 +50,14 @@ namespace psemek::phys2d float angular_velocity = 0.f; }; + struct fixed_constraint + { + geom::point point, target; + }; + + struct joint_constraint + { + geom::point point1, point2; + }; + } diff --git a/libs/phys/include/psemek/phys/engine_2d.hpp b/libs/phys/include/psemek/phys/engine_2d.hpp index 496a7de8..dfc6f6d3 100644 --- a/libs/phys/include/psemek/phys/engine_2d.hpp +++ b/libs/phys/include/psemek/phys/engine_2d.hpp @@ -66,9 +66,19 @@ namespace psemek::phys2d object_info const & get_object(group_handle group, std::size_t index); + // Constraint management + + using constraint_handle = std::uint32_t; + + constraint_handle add_constraint(group_handle g, std::size_t index, fixed_constraint const & c); + constraint_handle add_constraint(group_handle g1, std::size_t index1, group_handle g2, std::size_t index2, joint_constraint const & c); + + void remove_constraint(constraint_handle handle); + // Global system properties void set_gravity(geom::vector const & g); + void set_air_friction(float friction); // Effects diff --git a/libs/phys/source/engine_2d.cpp b/libs/phys/source/engine_2d.cpp index cb59bbb9..e8452b27 100644 --- a/libs/phys/source/engine_2d.cpp +++ b/libs/phys/source/engine_2d.cpp @@ -460,10 +460,21 @@ namespace psemek::phys2d util::flat_list materials; + struct constraint + { + std::function resolve; + std::vector> objects; + }; + + util::flat_list constraints; + std::vector constraint_handles; + struct group { std::vector infos; + std::vector> constraints; std::vector static_states; + std::vector static_states_temp; std::vector dynamic_states; std::vector::iterator>> contacts_by_velocity; @@ -472,7 +483,8 @@ namespace psemek::phys2d std::vector groups; - std::optional> gravity; + geom::vector gravity{0.f, 0.f}; + float air_friction = 0.f; std::set contacts_by_velocity; std::set contacts_by_penetration; @@ -483,9 +495,11 @@ namespace psemek::phys2d void apply_gravity(float dt); void integrate(float dt); + void resolve_constraints(float dt); void gather_collisions(); void resolve_impulses(); void resolve_penetrations(); + void apply_air_friction(float dt); void log_energy(); float energy(); @@ -518,15 +532,16 @@ namespace psemek::phys2d apply_gravity(dt); integrate(dt); + resolve_constraints(dt); gather_collisions(); resolve_impulses(); resolve_penetrations(); -// log_energy(); + apply_air_friction(dt); } void engine::impl::apply_gravity(float dt) { - if (gravity) + if (gravity != geom::vector{0.f, 0.f}) { for (auto & g : groups) { @@ -534,7 +549,7 @@ namespace psemek::phys2d { if (g.infos[i].inv_mass == 0.f) continue; - g.dynamic_states[i].velocity += *gravity * dt; + g.dynamic_states[i].velocity += gravity * dt; } } } @@ -552,6 +567,31 @@ namespace psemek::phys2d } } + void engine::impl::resolve_constraints(float dt) + { + for (std::size_t gi = 0; gi < groups.size(); ++gi) + for (std::size_t i = 0; i < groups[gi].infos.size(); ++i) + groups[gi].static_states_temp[i] = groups[gi].static_states[i]; + (void)dt; + + for (int i = 0; i < 4; ++i) + { + for (auto h : constraint_handles) + { + constraints[h].resolve(); + } + } + + for (std::size_t gi = 0; gi < groups.size(); ++gi) + { + for (std::size_t i = 0; i < groups[gi].infos.size(); ++i) + { + groups[gi].dynamic_states[i].velocity += (groups[gi].static_states[i].position - groups[gi].static_states_temp[i].position) / dt; + groups[gi].dynamic_states[i].angular_velocity += (groups[gi].static_states[i].rotation - groups[gi].static_states_temp[i].rotation) / dt; + } + } + } + void engine::impl::gather_collisions() { profile_function; @@ -833,7 +873,6 @@ namespace psemek::phys2d } } - void engine::impl::resolve_penetrations() { profile_function; @@ -955,6 +994,25 @@ namespace psemek::phys2d } } + void engine::impl::apply_air_friction(float dt) + { + if (air_friction != 0.f) + { + float const factor = 1.f - std::min(1.f, air_friction * dt); + + for (auto & g : groups) + { + for (std::size_t i = 0; i < g.infos.size(); ++i) + { + if (g.infos[i].inv_mass == 0.f) continue; + + g.dynamic_states[i].velocity *= factor; + g.dynamic_states[i].angular_velocity *= factor; + } + } + } + } + float engine::impl::energy() { float Eg = 0.f; @@ -967,8 +1025,7 @@ namespace psemek::phys2d { if (g.infos[i].inv_mass == 0.f) continue; - if (gravity) - Eg -= geom::dot(*gravity, g.static_states[i].position - geom::point::zero()) / g.infos[i].inv_mass; + Eg -= geom::dot(gravity, g.static_states[i].position - geom::point::zero()) / g.infos[i].inv_mass; Ek += geom::length_sqr(g.dynamic_states[i].velocity) / g.infos[i].inv_mass / 2.f; Er += geom::sqr(g.dynamic_states[i].angular_velocity) / g.infos[i].inv_inertia / 2.f; @@ -978,29 +1035,6 @@ namespace psemek::phys2d return Eg + Ek + Er; } - void engine::impl::log_energy() - { - float Eg = 0.f; - float Er = 0.f; - float Ek = 0.f; - - for (auto & g : groups) - { - for (std::size_t i = 0; i < g.infos.size(); ++i) - { - if (g.infos[i].inv_mass == 0.f) continue; - - if (gravity) - Eg -= geom::dot(*gravity, g.static_states[i].position - geom::point::zero()) / g.infos[i].inv_mass; - - Ek += geom::length_sqr(g.dynamic_states[i].velocity) / g.infos[i].inv_mass / 2.f; - Er += geom::sqr(g.dynamic_states[i].angular_velocity) / g.infos[i].inv_inertia / 2.f; - } - } - - log::info() << "G " << Eg << " K " << Ek << " R " << Er; - } - engine::engine() : pimpl_{make_impl()} {} @@ -1103,7 +1137,9 @@ namespace psemek::phys2d auto & g = impl().groups[group]; g.infos.push_back({shape, material, 1.f / (area * density), 1.f / (inertia * density)}); + g.constraints.emplace_back(); g.static_states.push_back(ss); + g.static_states_temp.emplace_back(); g.dynamic_states.push_back(ds); return g.infos.size() - 1; @@ -1112,8 +1148,14 @@ namespace psemek::phys2d void engine::remove_object(group_handle group, std::size_t index) { auto & g = impl().groups[group]; + + for (auto h : g.constraints[index]) + remove_constraint(h); + g.infos.erase(g.infos.begin() + index); + g.constraints.erase(g.constraints.begin() + index); g.static_states.erase(g.static_states.begin() + index); + g.static_states_temp.erase(g.static_states_temp.begin() + index); g.dynamic_states.erase(g.dynamic_states.begin() + index); } @@ -1122,11 +1164,111 @@ namespace psemek::phys2d return impl().groups[group].infos[index]; } + engine::constraint_handle engine::add_constraint(group_handle g, std::size_t index, fixed_constraint const & c) + { + impl::constraint constraint; + constraint.objects.push_back({g, index}); + constraint.resolve = [=, this]{ + auto & ss = impl().groups[g].static_states[index]; + + geom::vector t; + t[0] = c.target[0] - ss.position[0] - c.point[0] * std::cos(ss.rotation) + c.point[1] * std::sin(ss.rotation); + t[1] = c.target[1] - ss.position[1] - c.point[0] * std::sin(ss.rotation) - c.point[1] * std::cos(ss.rotation); + + geom::matrix J; + J[0][0] = -1.f; + J[0][1] = 0.f; + J[0][2] = c.point[0] * std::sin(ss.rotation) + c.point[1] * std::cos(ss.rotation); + J[1][0] = 0.f; + J[1][1] = -1.f; + J[1][2] = - c.point[0] * std::cos(ss.rotation) + c.point[1] * std::sin(ss.rotation); + + auto delta = geom::least_squares(J, -t); + + if (delta) + { + ss.position[0] += (*delta)[0]; + ss.position[1] += (*delta)[1]; + ss.rotation += (*delta)[2]; + } + }; + + auto handle = impl().constraints.insert(std::move(constraint)); + impl().constraint_handles.push_back(handle); + impl().groups[g].constraints[index].push_back(handle); + return handle; + } + + engine::constraint_handle engine::add_constraint(group_handle g1, std::size_t index1, group_handle g2, std::size_t index2, joint_constraint const & c) + { + impl::constraint constraint; + constraint.objects.push_back({g1, index1}); + constraint.objects.push_back({g2, index2}); + constraint.resolve = [=, this]{ + auto & ss1 = impl().groups[g1].static_states[index1]; + auto & ss2 = impl().groups[g2].static_states[index2]; + + geom::vector t; + t[0] = ss2.position[0] + c.point2[0] * std::cos(ss2.rotation) - c.point2[1] * std::sin(ss2.rotation) - ss1.position[0] - c.point1[0] * std::cos(ss1.rotation) + c.point1[1] * std::sin(ss1.rotation); + t[1] = ss2.position[1] + c.point2[0] * std::sin(ss2.rotation) + c.point2[1] * std::cos(ss2.rotation) - ss1.position[1] - c.point1[0] * std::sin(ss1.rotation) - c.point1[1] * std::cos(ss1.rotation); + + geom::matrix J; + J[0][0] = -1.f; + J[0][1] = 0.f; + J[0][2] = c.point1[0] * std::sin(ss1.rotation) + c.point1[1] * std::cos(ss1.rotation); + J[0][3] = 1.f; + J[0][4] = 0.f; + J[0][5] = - c.point2[0] * std::sin(ss2.rotation) - c.point2[1] * std::cos(ss2.rotation); + J[1][0] = 0.f; + J[1][1] = -1.f; + J[1][2] = - c.point1[0] * std::cos(ss1.rotation) + c.point1[1] * std::sin(ss1.rotation); + J[1][3] = 0.f; + J[1][4] = 1.f; + J[1][5] = c.point2[0] * std::cos(ss2.rotation) - c.point2[1] * std::sin(ss2.rotation); + + auto delta = geom::least_squares(J, -t); + + if (delta) + { + ss1.position[0] += (*delta)[0]; + ss1.position[1] += (*delta)[1]; + ss1.rotation += (*delta)[2]; + ss2.position[0] += (*delta)[3]; + ss2.position[1] += (*delta)[4]; + ss2.rotation += (*delta)[5]; + } + }; + + auto handle = impl().constraints.insert(std::move(constraint)); + impl().constraint_handles.push_back(handle); + impl().groups[g1].constraints[index1].push_back(handle); + impl().groups[g2].constraints[index2].push_back(handle); + return handle; + } + + void engine::remove_constraint(constraint_handle handle) + { + auto const & constraint = impl().constraints[handle]; + for (auto [g, i] : constraint.objects) + { + auto & cs = impl().groups[g].constraints[i]; + if (auto it = std::find(cs.begin(), cs.end(), handle); it != cs.end()) + cs.erase(it); + } + impl().constraints.erase(handle); + impl().constraint_handles.erase(std::find(impl().constraint_handles.begin(), impl().constraint_handles.end(), handle)); + } + void engine::set_gravity(geom::vector const & g) { impl().gravity = g; } + void engine::set_air_friction(float friction) + { + impl().air_friction = friction; + } + void engine::explode(geom::point const & center, float strength, float attenuation) { impl().explode(center, strength, attenuation);