Add box to box collision solver

CHANGELOG.md

@@ -34,6 +34,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 - Ephemeral trait, used to indicate that relations and components shouldn't be persisted.
 - Collider gizmos plugin, used to view collision shapes.
 - Delta Time multiplier, can be used to adjust simulation speed (#866).
+- Collision Solver plugin (#532)
 
 ### Changed
 

engine/CMakeLists.txt

@@ -63,6 +63,7 @@ set(CUBOS_ENGINE_SOURCE
 
 	"src/physics/plugin.cpp"
 	"src/physics/gravity.cpp"
+	"src/physics/solver.cpp"
 
 	"src/input/plugin.cpp"
 	"src/input/input.cpp"

engine/include/cubos/engine/physics/physics_bundle.hpp

@@ -0,0 +1,24 @@
+/// @file
+/// @brief Component @ref cubos::engine::PhysicsBundle.
+/// @ingroup physics-plugin
+
+#pragma once
+
+#include <glm/vec3.hpp>
+
+#include <cubos/core/reflection/reflect.hpp>
+
+namespace cubos::engine
+{
+    /// @brief Component which encapsulates the creation all components required for physics.
+    /// @ingroup physics-plugin
+    struct PhysicsBundle
+    {
+        CUBOS_REFLECT;
+
+        float mass = 1.0F;
+        glm::vec3 velocity = {0.0F, 0.0F, 0.0F};
+        glm::vec3 force = {0.0F, 0.0F, 0.0F};
+        glm::vec3 impulse = {0.0F, 0.0F, 0.0F};
+    };
+} // namespace cubos::engine

engine/include/cubos/engine/physics/plugin.hpp

@@ -18,6 +18,7 @@
 #include <cubos/engine/physics/components/mass.hpp>
 #include <cubos/engine/physics/components/previous_position.hpp>
 #include <cubos/engine/physics/components/velocity.hpp>
+#include <cubos/engine/physics/physics_bundle.hpp>
 #include <cubos/engine/physics/plugins/gravity.hpp>
 #include <cubos/engine/physics/resources/damping.hpp>
 #include <cubos/engine/physics/resources/fixed_delta_time.hpp>
@@ -39,6 +40,7 @@ namespace cubos::engine
     /// - @ref Substeps - holds the amount of substeps for the physics update.
     ///
     /// ## Components
+    /// - @ref PhysicsBundle - bundle that holds the physics information to give to a new entity.
     /// - @ref Velocity - holds the information for moving an object straight.
     /// - @ref Force - holds forces applied on a particle.
     /// - @ref Impulse - holds impulses applied on a particle.

engine/include/cubos/engine/physics/solver/solver.hpp

@@ -0,0 +1,23 @@
+/// @file
+/// @brief Solver plugin, resource @ref cubos::engine::Solver.
+/// @ingroup physics-solver-plugin
+
+#pragma once
+
+#include <cubos/engine/physics/components/force.hpp>
+#include <cubos/engine/physics/components/impulse.hpp>
+#include <cubos/engine/physics/components/mass.hpp>
+#include <cubos/engine/physics/components/velocity.hpp>
+#include <cubos/engine/prelude.hpp>
+
+namespace cubos::engine
+{
+    /// @defgroup physics-solver-plugin Solver
+    /// @ingroup physics-plugin
+    /// @brief Adds solver for constraints.
+
+    /// @brief Plugin entry function.
+    /// @param cubos @b CUBOS. main class
+    /// @ingroup physics-gravity-plugin
+    void solverPlugin(Cubos& cubos);
+} // namespace cubos::engine

engine/samples/collisions/main.cpp

@@ -5,11 +5,12 @@
 #include <cubos/core/log.hpp>
 
 #include <cubos/engine/collisions/collider.hpp>
-#include <cubos/engine/collisions/narrow_phase/colliding_with.hpp>
+#include <cubos/engine/collisions/colliding_with.hpp>
 #include <cubos/engine/collisions/plugin.hpp>
 #include <cubos/engine/collisions/shapes/box.hpp>
 #include <cubos/engine/collisions/shapes/capsule.hpp>
 #include <cubos/engine/input/plugin.hpp>
+#include <cubos/engine/physics/plugin.hpp>
 #include <cubos/engine/renderer/plugin.hpp>
 #include <cubos/engine/settings/settings.hpp>
 #include <cubos/engine/transform/plugin.hpp>
@@ -36,6 +37,7 @@ int main()
     auto cubos = Cubos();
 
     cubos.addPlugin(collisionsPlugin);
+    cubos.addPlugin(physicsPlugin);
     cubos.addPlugin(rendererPlugin);
     cubos.addPlugin(inputPlugin);
 
@@ -63,13 +65,14 @@ int main()
                 .entity();
     });
 
-    cubos.startupSystem("create colliders").call([](State& state, Commands commands) {
+    cubos.startupSystem("create colliders").call([](State& state, Commands commands, Gravity& gravity) {
         state.a = commands.create()
                       .add(Collider{})
                       .add(BoxCollisionShape{})
                       .add(LocalToWorld{})
                       .add(Position{glm::vec3{0.0F, 0.0F, -2.0F}})
                       .add(Rotation{})
+                      .add(PhysicsBundle{.mass = 500.0F, .velocity = {0.0F, 0.0F, 1.0F}})
                       .entity();
         state.aRotationAxis = glm::sphericalRand(1.0F);
 
@@ -79,15 +82,19 @@ int main()
                       .add(LocalToWorld{})
                       .add(Position{glm::vec3{0.0F, 0.0F, 2.0F}})
                       .add(Rotation{})
+                      .add(PhysicsBundle{.mass = 500.0F, .velocity = {0.0F, 0.0F, -1.0F}})
                       .entity();
         state.bRotationAxis = glm::sphericalRand(1.0F);
+
+        gravity.value = glm::vec3{0.0F, 0.0F, 0.0F};
     });
 
     cubos.system("move colliders")
         .before("cubos.transform.update")
-        .call([](State& state, const Input& input, Query<Position&, Rotation&> query) {
-            auto [aPos, aRot] = *query.at(state.a);
-            auto [bPos, bRot] = *query.at(state.b);
+        .after("cubos.physics.unpack_bundle")
+        .call([](State& state, const Input& input, Query<Position&, Rotation&, Velocity&> query) {
+            auto [aPos, aRot, aVel] = *query.at(state.a);
+            auto [bPos, bRot, bVel] = *query.at(state.b);
 
             if (state.collided)
             {
@@ -97,20 +104,21 @@ int main()
 
                     aPos.vec = glm::vec3{0.0F, 0.0F, -2.0F};
                     aRot.quat = glm::quat{1.0F, 0.0F, 0.0F, 0.0F};
+                    aVel.vec = glm::vec3{0.0F, 0.0F, 1.0F};
                     state.aRotationAxis = glm::sphericalRand(1.0F);
 
                     bPos.vec = glm::vec3{0.0F, 0.0F, 2.0F};
                     bRot.quat = glm::quat{1.0F, 0.0F, 0.0F, 0.0F};
+                    bVel.vec = glm::vec3{0.0F, 0.0F, -1.0F};
                     state.bRotationAxis = glm::sphericalRand(1.0F);
                 }
-                return;
             }
 
             aRot.quat = glm::rotate(aRot.quat, 0.001F, state.aRotationAxis);
-            aPos.vec += glm::vec3{0.0F, 0.0F, 0.001F};
+            aVel.vec += glm::vec3{0.0F, 0.0F, 0.01F};
 
             bRot.quat = glm::rotate(bRot.quat, 0.001F, state.bRotationAxis);
-            bPos.vec += glm::vec3{0.0F, 0.0F, -0.001F};
+            bVel.vec -= glm::vec3{0.0F, 0.0F, 0.01F};
         });
 
     cubos.system("check collisions")

engine/src/collisions/narrow_phase/colliding_with.cpp

@@ -2,7 +2,7 @@
 #include <cubos/core/reflection/external/glm.hpp>
 #include <cubos/core/reflection/external/primitives.hpp>
 
-#include <cubos/engine/collisions/narrow_phase/colliding_with.hpp>
+#include <cubos/engine/collisions/colliding_with.hpp>
 
 CUBOS_REFLECT_IMPL(cubos::engine::CollidingWith)
 {

engine/src/collisions/narrow_phase/plugin.hpp

@@ -7,7 +7,7 @@
 
 #pragma once
 
-#include <cubos/engine/collisions/narrow_phase/colliding_with.hpp>
+#include <cubos/engine/collisions/colliding_with.hpp>
 #include <cubos/engine/prelude.hpp>
 
 namespace cubos::engine

engine/src/physics/plugin.cpp

@@ -1,6 +1,7 @@
 #include <glm/glm.hpp>
 
 #include <cubos/engine/physics/plugin.hpp>
+#include <cubos/engine/physics/solver/solver.hpp>
 #include <cubos/engine/settings/plugin.hpp>
 #include <cubos/engine/transform/plugin.hpp>
 
@@ -49,6 +50,16 @@
         .build();
 }
 
+CUBOS_REFLECT_IMPL(PhysicsBundle)
+{
+    return cubos::core::ecs::TypeBuilder<PhysicsBundle>("cubos::engine::PhysicsBundle")
+        .withField("mass", &PhysicsBundle::mass)
+        .withField("velocity", &PhysicsBundle::velocity)
+        .withField("force", &PhysicsBundle::force)
+        .withField("impulse", &PhysicsBundle::impulse)
+        .build();
+}
+
 static bool simulatePhysicsStep(PhysicsAccumulator& accumulator, const FixedDeltaTime& fixedDeltaTime)
 {
     return accumulator.value >= fixedDeltaTime.value;
@@ -67,8 +78,32 @@
     cubos.addComponent<Mass>();
     cubos.addComponent<AccumulatedCorrection>();
     cubos.addComponent<PreviousPosition>();
+    cubos.addComponent<PhysicsBundle>();
 
     cubos.addPlugin(gravityPlugin);
+    cubos.addPlugin(solverPlugin);
+
+    // add components to entities created with PhysicsBundle
+    cubos.system("unpack PhysicsBundle's")
+        .tagged("cubos.physics.unpack_bundle")
+        .call([](Commands cmds, Query<Entity, const PhysicsBundle&> query) {
+            for (auto [ent, bundle] : query)
+            {
+                cmds.add(ent, PreviousPosition{});
+                cmds.add(ent, Mass{.mass = bundle.mass, .inverseMass = 1.0F / bundle.mass});
+                cmds.add(ent, Velocity{.vec = bundle.velocity});
+
+                auto force = Force{};
+                force.add(bundle.force);
+                cmds.add(ent, force);
+
+                auto impulse = Impulse{};
+                impulse.add(bundle.impulse);
+                cmds.add(ent, impulse);
+
+                cmds.add(ent, AccumulatedCorrection{});
+            }
+        });
 
     // executed every frame
     cubos.system("increase fixed-step accumulator")
@@ -107,7 +142,7 @@
 
                 if (mass.inverseMass <= 0.0F)
                 {
-                    return;
+                    continue;
                 }
 
                 // Apply damping
@@ -125,10 +160,14 @@
         .tagged("cubos.physics.simulation.substeps.correct_position")
         .after("cubos.physics.simulation.substeps.integrate")
         .onlyIf(simulatePhysicsStep)
-        .call([](Query<Position&, AccumulatedCorrection&> query) {
-            for (auto [position, correction] : query)
+        .call([](Query<Position&, AccumulatedCorrection&, Mass&> query) {
+            for (auto [position, correction, mass] : query)
             {
-                position.vec += correction.vec;
+                if (mass.inverseMass <= 0.0F)
+                {
+                    continue;
+                }
+                position.vec += correction.vec; // lagrange * correction * inverseMass
                 correction.vec = glm::vec3(0, 0, 0);
             }
         });

engine/src/physics/solver.cpp

@@ -0,0 +1,32 @@
+#include <glm/glm.hpp>
+
+#include <cubos/engine/collisions/colliding_with.hpp>
+#include <cubos/engine/physics/components/accumulated_correction.hpp>
+#include <cubos/engine/physics/solver/solver.hpp>
+
+using namespace cubos::engine;
+
+void cubos::engine::solverPlugin(Cubos& cubos)
+{
+    cubos.system("solve collision")
+        .tagged("cubos.physics.solve")
+        .after("cubos.physics.simulation.substeps.integrate")
+        .after("cubos.collisions.narrow")
+        .before("cubos.physics.simulation.substeps.correct_position")
+        .call([](Query<Entity, AccumulatedCorrection&, const CollidingWith&, Entity, AccumulatedCorrection&> query) {
+            for (auto [ent1, correction1, colliding, ent2, correction2] : query)
+            {
+                auto penetration = colliding.penetration;
+                if (ent1 == colliding.entity)
+                {
+                    correction1.vec -= colliding.normal * (penetration / 1.0F);
+                    correction2.vec += colliding.normal * (penetration / 1.0F);
+                }
+                else
+                {
+                    correction1.vec += colliding.normal * (penetration / 1.0F);
+                    correction2.vec -= colliding.normal * (penetration / 1.0F);
+                }
+            }
+        });
+}