feat(physics): Interpolation

engine/CMakeLists.txt

@@ -239,6 +239,8 @@ set(CUBOS_ENGINE_SOURCE
 	"src/tools/ecs_statistics/plugin.cpp"
 	"src/tools/console/plugin.cpp"
 	"src/tools/plugin.cpp"
+
+	"src/interpolation/plugin.cpp"
 )
 
 # ---------------------- Configure engine library target ----------------------

engine/include/cubos/engine/interpolation/plugin.hpp

@@ -0,0 +1,52 @@
+/// @dir
+/// @brief @ref interpolation-plugin plugin directory.
+
+/// @file
+/// @brief Plugin entry point.
+/// @ingroup interpolation-plugin
+
+#pragma once
+
+#include <glm/glm.hpp>
+#include <glm/gtx/quaternion.hpp>
+
+#include <cubos/core/reflection/reflect.hpp>
+#include <cubos/core/reflection/type.hpp>
+
+#include <cubos/engine/api.hpp>
+#include <cubos/engine/prelude.hpp>
+
+namespace cubos::engine
+{
+
+    /// @brief Systems with this tag run once if this is fixed step update.
+    CUBOS_ENGINE_API extern Tag interpolationResetTag;
+
+    /// @brief Component which holds the the previous and next frames position, rotation and scale for interpolation.
+    /// @ingroup interpolation-plugin
+    struct CUBOS_ENGINE_API Interpolated
+    {
+        CUBOS_REFLECT;
+
+        glm::vec3 currentPosition; ///< Currently interpolated frame position.
+        // glm::quat currentRotation; ///< Currently interpolated frame rotation.
+        float currentScale; ///< Currently interpolated frame scale factor.
+
+        glm::vec3 previousPosition; ///< Previous frame position.
+        // glm::quat previousRotation; ///< Previous frame rotation.
+        float previousScale; ///< Previous frame scale factor.
+
+        glm::vec3 nextPosition; ///< Next frame position.
+        // glm::quat nextRotation; ///< Next frame rotation.
+        float nextScale; ///< Next frame scale factor.
+    };
+
+    /// @ingroup interpolation-plugin
+    /// @brief Interpolates position, rotation and scale of entities with the @ref Interpolated component in between
+    /// fixed updates.
+
+    /// @brief Plugin entry function.
+    /// @param cubos @b Cubos main class
+    /// @ingroup interpolation-plugin
+    void interpolationPlugin(Cubos& cubos);
+} // namespace cubos::engine

engine/samples/collisions/main.cpp

@@ -16,6 +16,7 @@
 #include <cubos/engine/gizmos/plugin.hpp>
 #include <cubos/engine/gizmos/target.hpp>
 #include <cubos/engine/input/plugin.hpp>
+#include <cubos/engine/interpolation/plugin.hpp>
 #include <cubos/engine/physics/plugin.hpp>
 #include <cubos/engine/physics/solver/plugin.hpp>
 #include <cubos/engine/render/camera/camera.hpp>
@@ -124,6 +125,7 @@ int main(int argc, char** argv)
                       .add(Position{glm::vec3{0.0F, 0.0F, -2.0F}})
                       .add(Rotation{})
                       .add(PhysicsBundle{.mass = 500.0F, .velocity = {0.0F, 0.0F, 1.0F}})
+                      .add(Interpolated{})
                       .entity();
         state.aRotationAxis = glm::sphericalRand(1.0F);
 
@@ -135,6 +137,7 @@ int main(int argc, char** argv)
                       .add(Position{glm::vec3{0.0F, 0.0F, 2.0F}})
                       .add(Rotation{})
                       .add(PhysicsBundle{.mass = 500.0F, .velocity = {0.0F, 0.0F, -1.0F}})
+                      .add(Interpolated{})
                       .entity();
         state.bRotationAxis = glm::sphericalRand(1.0F);
     });

engine/samples/complex_physics/main.cpp

@@ -8,6 +8,7 @@
 #include <cubos/engine/collisions/shapes/box.hpp>
 #include <cubos/engine/fixed_step/plugin.hpp>
 #include <cubos/engine/gizmos/plugin.hpp>
+#include <cubos/engine/interpolation/plugin.hpp>
 #include <cubos/engine/physics/plugin.hpp>
 #include <cubos/engine/physics/plugins/gravity.hpp>
 #include <cubos/engine/physics/solver/plugin.hpp>
@@ -150,6 +151,7 @@ int main(int argc, char** argv)
                 {
                     auto builder = cmds.spawn(red ? redCube->blueprint() : whiteCube->blueprint());
                     builder.add(Position{.vec = nextPosition});
+                    builder.add(Interpolated{});
                     nextPosition.y += 1.0F;
                     red = !red;
                 }
@@ -172,6 +174,7 @@ int main(int argc, char** argv)
                 auto builder = cmds.spawn(cube->blueprint());
                 glm::vec3 position = RandomDirection ? randomPosition() : glm::vec3{0.0F, 3.0F, -7.0F};
                 builder.add(Position{.vec = position});
+                builder.add(Interpolated{});
 
                 // push cube in direction to the center
                 glm::vec3 impulseDirection = calculateDirection(position);

engine/samples/distance_constraint/main.cpp

@@ -15,6 +15,7 @@
 #include <cubos/engine/gizmos/plugin.hpp>
 #include <cubos/engine/gizmos/target.hpp>
 #include <cubos/engine/input/plugin.hpp>
+#include <cubos/engine/interpolation/plugin.hpp>
 #include <cubos/engine/physics/constraints/distance_constraint.hpp>
 #include <cubos/engine/physics/plugin.hpp>
 #include <cubos/engine/physics/solver/plugin.hpp>
@@ -74,6 +75,7 @@ void createScenario(Commands& commands, State& state, Options& options)
                       .add(Position{glm::vec3{0.0F, 0.0F, -2.0F}})
                       .add(Rotation{})
                       .add(PhysicsBundle{.mass = 10000000000000000.0F, .velocity = {0.0F, 0.0F, 0.0F}})
+                      .add(Interpolated{})
                       .entity();
 
         state.b = commands.create()
@@ -84,6 +86,7 @@ void createScenario(Commands& commands, State& state, Options& options)
                       .add(Position{glm::vec3{0.0F, 0.0F, 2.0F}})
                       .add(Rotation{})
                       .add(PhysicsBundle{.mass = 500.0F, .velocity = {1.0F, 0.0F, 1.0F}})
+                      .add(Interpolated{})
                       .entity();
 
         commands.relate(state.a, state.b,
@@ -103,6 +106,7 @@ void createScenario(Commands& commands, State& state, Options& options)
                       .add(Position{glm::vec3{0.0F, 0.0F, 0.0F}})
                       .add(Rotation{})
                       .add(PhysicsBundle{.mass = 10000000000000000.0F, .velocity = {0.0F, 0.0F, 0.0F}})
+                      .add(Interpolated{})
                       .entity();
 
         state.b = commands.create()
@@ -113,6 +117,7 @@ void createScenario(Commands& commands, State& state, Options& options)
                       .add(Position{glm::vec3{0.0F, 0.0F, 5.0F}})
                       .add(Rotation{})
                       .add(PhysicsBundle{.mass = 500.0F, .velocity = {0.0F, 0.0F, 0.0F}})
+                      .add(Interpolated{})
                       .entity();
 
         commands.relate(state.a, state.b,
@@ -132,6 +137,7 @@ void createScenario(Commands& commands, State& state, Options& options)
                       .add(Position{glm::vec3{0.0F, 0.0F, 0.0F}})
                       .add(Rotation{})
                       .add(PhysicsBundle{.mass = 10000000000000000.0F, .velocity = {0.0F, 0.0F, 0.0F}})
+                      .add(Interpolated{})
                       .entity();
 
         state.b = commands.create()
@@ -142,6 +148,7 @@ void createScenario(Commands& commands, State& state, Options& options)
                       .add(Position{glm::vec3{0.0F, 0.0F, 5.0F}})
                       .add(Rotation{})
                       .add(PhysicsBundle{.mass = 500.0F, .velocity = {0.0F, 0.0F, 0.0F}})
+                      .add(Interpolated{})
                       .entity();
 
         state.c = commands.create()
@@ -152,6 +159,7 @@ void createScenario(Commands& commands, State& state, Options& options)
                       .add(Position{glm::vec3{0.0F, 0.0F, 10.0F}})
                       .add(Rotation{})
                       .add(PhysicsBundle{.mass = 500.0F, .velocity = {0.0F, 0.0F, 0.0F}})
+                      .add(Interpolated{})
                       .entity();
 
         commands.relate(state.a, state.b,
@@ -178,6 +186,7 @@ void createScenario(Commands& commands, State& state, Options& options)
                       .add(Position{glm::vec3{0.0F, 0.0F, 0.0F}})
                       .add(Rotation{})
                       .add(PhysicsBundle{.mass = 10000000000000000.0F, .velocity = {0.0F, 0.0F, 0.0F}})
+                      .add(Interpolated{})
                       .entity();
 
         state.b = commands.create()
@@ -188,6 +197,7 @@ void createScenario(Commands& commands, State& state, Options& options)
                       .add(Position{glm::vec3{0.0F, 0.0F, -5.0F}})
                       .add(Rotation{})
                       .add(PhysicsBundle{.mass = 500.0F, .velocity = {0.0F, 0.0F, 0.0F}})
+                      .add(Interpolated{})
                       .entity();
 
         state.c = commands.create()
@@ -198,6 +208,7 @@ void createScenario(Commands& commands, State& state, Options& options)
                       .add(Position{glm::vec3{0.0F, 0.0F, -15.0F}})
                       .add(Rotation{})
                       .add(PhysicsBundle{.mass = 500.0F, .velocity = {0.0F, 0.0F, 5.0F}})
+                      .add(Interpolated{})
                       .entity();
 
         commands.relate(state.a, state.b,

engine/samples/voxel-shape-collisions/main.cpp

@@ -17,6 +17,7 @@
 #include <cubos/engine/fixed_step/plugin.hpp>
 #include <cubos/engine/gizmos/plugin.hpp>
 #include <cubos/engine/gizmos/target.hpp>
+#include <cubos/engine/interpolation/plugin.hpp>
 #include <cubos/engine/physics/plugin.hpp>
 #include <cubos/engine/physics/solver/plugin.hpp>
 #include <cubos/engine/render/camera/camera.hpp>
@@ -107,6 +108,7 @@ int main()
                       .add(Position{glm::vec3{0.0F, -10.0F, -20.0F}})
                       .add(Rotation{})
                       .add(PhysicsBundle{.mass = 500.0F, .velocity = {0.0F, 0.0F, 1.0F}})
+                      .add(Interpolated{})
                       .entity();
         state.aRotationAxis = glm::sphericalRand(1.0F);
 
@@ -122,6 +124,7 @@ int main()
                                          .velocity = {0.0F, 0.0F, -1.0F},
                                          .material = PhysicsMaterial{.bounciness = 0.0},
                                          .inertiaTensor = glm::mat3(0.0F)})
+                      .add(Interpolated{})
                       .entity();
         state.bRotationAxis = glm::sphericalRand(1.0F);
     });

engine/src/interpolation/plugin.cpp

@@ -0,0 +1,120 @@
+#include <glm/glm.hpp>
+
+#include <cubos/core/ecs/reflection.hpp>
+#include <cubos/core/reflection/external/glm.hpp>
+#include <cubos/core/reflection/type.hpp>
+
+#include <cubos/engine/fixed_step/plugin.hpp>
+#include <cubos/engine/interpolation/plugin.hpp>
+#include <cubos/engine/physics/plugin.hpp>
+#include <cubos/engine/physics/solver/plugin.hpp>
+#include <cubos/engine/transform/plugin.hpp>
+
+using namespace cubos::engine;
+
+CUBOS_DEFINE_TAG(cubos::engine::interpolationResetTag);
+
+CUBOS_REFLECT_IMPL(cubos::engine::Interpolated)
+{
+    return cubos::core::ecs::TypeBuilder<Interpolated>("cubos::engine::Interpolated").build();
+}
+
+void cubos::engine::interpolationPlugin(Cubos& cubos)
+{
+    cubos.depends(fixedStepPlugin);
+    cubos.depends(transformPlugin);
+    cubos.depends(physicsPlugin);
+    cubos.depends(physicsSolverPlugin);
+
+    cubos.component<Interpolated>();
+
+    cubos.tag(interpolationResetTag).runIf([](FixedAccumulatedTime& timer, const FixedDeltaTime& step) {
+        if (timer.value >= step.value)
+        {
+            return true;
+        }
+        return false;
+    });
+
+    cubos.observer("set up Interpolated")
+        .onAdd<Interpolated>()
+        .call([](Query<Entity, Interpolated&, const Position&, const Rotation&, const Scale&> query) {
+            for (auto [ent, interp, position, rotation, scale] : query)
+            {
+                interp.previousPosition = position.vec;
+                // interp.previousRotation = rotation.quat;
+                interp.previousScale = scale.factor;
+                interp.nextPosition = position.vec;
+                // interp.nextRotation = rotation.quat;
+                interp.nextScale = scale.factor;
+            }
+        });
+
+    cubos.system("set position to real value before physics update")
+        .tagged(interpolationResetTag)
+        .before(transformUpdateTag)
+        .call([](Query<Position&, const Rotation&, const Scale&, const Interpolated&> query) {
+            for (auto [position, rotation, scale, interpolated] : query)
+            {
+                position.vec = interpolated.nextPosition;
+            }
+        });
+
+    cubos.system("prepare before update")
+        .tagged(fixedStepTag)
+        .before(physicsFinalizePositionTag)
+        .call([](Query<const Position&, const Rotation&, const Scale&, Interpolated&> query) {
+            for (auto [position, rotation, scale, interpolated] : query)
+            {
+                interpolated.previousPosition = interpolated.nextPosition;
+                // interpolated.previousRotation = interpolated.nextRotation;
+                interpolated.previousScale = interpolated.nextScale;
+            }
+        });
+
+    cubos.system("do interpolation on Interpolated entities")
+        .after(fixedStepTag)
+        .call([](const FixedAccumulatedTime& acc, const DeltaTime& dt, const FixedDeltaTime& fdt,
+                 Query<Position&, Rotation&, Scale&, Interpolated&> query) {
+            float alpha = (acc.value + dt.value()) / fdt.value;
+
+            for (auto [position, rotation, scale, interpolated] : query)
+            {
+
+                if (position.vec == interpolated.currentPosition)
+                {
+                    position.vec = glm::mix(interpolated.previousPosition, interpolated.nextPosition, alpha);
+                }
+                else
+                {
+                    interpolated.previousPosition = position.vec;
+                    interpolated.nextPosition = position.vec;
+                }
+                interpolated.currentPosition = position.vec;
+
+                /*
+                if (rotation.quat == interpolated.currentRotation)
+                {
+                    rotation.quat = glm::slerp(interpolated.previousRotation, interpolated.nextRotation, alpha);
+                }
+                else
+                {
+                    interpolated.previousRotation = rotation.quat;
+                    interpolated.nextRotation = rotation.quat;
+                }
+                interpolated.currentRotation = rotation.quat;
+                */
+
+                if (scale.factor == interpolated.nextScale)
+                {
+                    scale.factor = glm::mix(interpolated.previousScale, interpolated.nextScale, alpha);
+                }
+                else
+                {
+                    interpolated.previousScale = scale.factor;
+                    interpolated.nextScale = scale.factor;
+                }
+                interpolated.currentScale = scale.factor;
+            }
+        });
+}

engine/src/physics/solver/distance_constraint/plugin.cpp

@@ -245,4 +245,4 @@ void cubos::engine::distanceConstraintPlugin(Cubos& cubos)
                 angVelocity2.vec += inertia2.inverseInertia * glm::cross(r2, p);
             }
         });
-}
+}

engine/src/physics/solver/integration/plugin.cpp

@@ -3,6 +3,7 @@
 #include <glm/glm.hpp>
 
 #include <cubos/engine/fixed_step/plugin.hpp>
+#include <cubos/engine/interpolation/plugin.hpp>
 #include <cubos/engine/physics/components/accumulated_correction.hpp>
 #include <cubos/engine/physics/plugin.hpp>
 #include <cubos/engine/physics/solver/plugin.hpp>
@@ -21,6 +22,7 @@ void cubos::engine::physicsIntegrationPlugin(Cubos& cubos)
     cubos.depends(transformPlugin);
     cubos.depends(physicsPlugin);
     cubos.depends(physicsSolverPlugin);
+    cubos.depends(interpolationPlugin);
 
     cubos.tag(physicsApplyImpulsesTag);
     cubos.tag(physicsClearForcesTag).after(physicsFinalizePositionTag).tagged(fixedStepTag);
@@ -126,15 +128,23 @@ void cubos::engine::physicsIntegrationPlugin(Cubos& cubos)
 
     cubos.system("finalize position")
         .tagged(physicsFinalizePositionTag)
-        .call([](Query<Position&, AccumulatedCorrection&, const Mass&> query, const SolverConstants& solverConstants) {
-            for (auto [position, correction, mass] : query)
+        .call([](Query<Position&, AccumulatedCorrection&, const Mass&, Opt<Interpolated&>> query,
+                 const SolverConstants& solverConstants) {
+            for (auto [position, correction, mass, interpolated] : query)
             {
                 if (mass.inverseMass <= solverConstants.minInvMass)
                 {
                     continue;
                 }
 
-                position.vec += correction.position;
+                if (interpolated.contains())
+                {
+                    interpolated.value().nextPosition += correction.position;
+                }
+                else
+                {
+                    position.vec += correction.position;
+                }
                 correction.position = glm::vec3(0.0F);
             }
         });