Try to implement constraints in 2d physics engine

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<float, 2> point, target;
+	};
+
+	struct joint_constraint
+	{
+		geom::point<float, 2> point1, point2;
+	};
+
 }

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<float, 2> const & g);
+		void set_air_friction(float friction);
 
 		// Effects
 

libs/phys/source/engine_2d.cpp

@@ -460,10 +460,21 @@ namespace psemek::phys2d
 
 		util::flat_list<material, engine::material_handle> materials;
 
+		struct constraint
+		{
+			std::function<void()> resolve;
+			std::vector<std::pair<group_handle, std::size_t>> objects;
+		};
+
+		util::flat_list<constraint> constraints;
+		std::vector<constraint_handle> constraint_handles;
+
 		struct group
 		{
 			std::vector<object_info> infos;
+			std::vector<std::vector<constraint_handle>> constraints;
 			std::vector<static_state> static_states;
+			std::vector<static_state> static_states_temp;
 			std::vector<dynamic_state> dynamic_states;
 
 			std::vector<std::vector<std::set<contact, velocity_comparator>::iterator>> contacts_by_velocity;
@@ -472,7 +483,8 @@ namespace psemek::phys2d
 
 		std::vector<group> groups;
 
-		std::optional<geom::vector<float, 2>> gravity;
+		geom::vector<float, 2> gravity{0.f, 0.f};
+		float air_friction = 0.f;
 
 		std::set<contact, velocity_comparator> contacts_by_velocity;
 		std::set<contact, penetration_comparator> 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<float, 2>::zero()) / g.infos[i].inv_mass;
-					Eg -= geom::dot(*gravity, g.static_states[i].position - geom::point<float, 2>::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<float, 2>::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<float, 2> 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<float, 2, 3> 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<float, 2> 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<float, 2, 6> 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<float, 2> const & g)
 	{
 		impl().gravity = g;
 	}
 
+	void engine::set_air_friction(float friction)
+	{
+		impl().air_friction = friction;
+	}
+
 	void engine::explode(geom::point<float, 2> const & center, float strength, float attenuation)
 	{
 		impl().explode(center, strength, attenuation);