Add warm starting

engine/include/cubos/engine/physics/components/angular_impulse.hpp

@@ -35,7 +35,7 @@ namespace cubos::engine
         }
 
     private:
-        glm::vec3 mAngularImpulse;
+        glm::vec3 mAngularImpulse = {0.0F, 0.0F, 0.0F};
     };
 
 } // namespace cubos::engine

engine/include/cubos/engine/physics/components/angular_velocity.hpp

@@ -19,6 +19,6 @@ namespace cubos::engine
     {
         CUBOS_REFLECT;
 
-        glm::vec3 vec;
+        glm::vec3 vec = {0.0F, 0.0F, 0.0F};
     };
 } // namespace cubos::engine

engine/include/cubos/engine/physics/components/impulse.hpp

@@ -49,8 +49,8 @@ namespace cubos::engine
         }
 
     private:
-        glm::vec3 mImpulse;
-        glm::vec3 mAngularImpulse;
+        glm::vec3 mImpulse = {0.0F, 0.0F, 0.0F};
+        glm::vec3 mAngularImpulse = {0.0F, 0.0F, 0.0F};
     };
 
 } // namespace cubos::engine

engine/include/cubos/engine/physics/components/velocity.hpp

@@ -19,6 +19,6 @@ namespace cubos::engine
     {
         CUBOS_REFLECT;
 
-        glm::vec3 vec;
+        glm::vec3 vec = {0.0F, 0.0F, 0.0F};
     };
 } // namespace cubos::engine

engine/include/cubos/engine/physics/resources/solver_constants.hpp

@@ -24,7 +24,7 @@ namespace cubos::engine
         /// @brief Minimum inverse inertia for a body to rotate.
         glm::mat3 minInvInertia = glm::mat3(0.0F);
 
-        /// @brief The maximum "boost" for the initial solving of the constraint. 
+        /// @brief The maximum "boost" for the initial solving of the constraint.
         float maxBias = -4.0F;
 
         /// @brief The minimum update frequency of the collision solving.
@@ -36,17 +36,23 @@ namespace cubos::engine
         /// @brief The minimum value for KFriction (to avoid divisions by zero).
         float minKFriction = 0.0F;
 
-        /// @brief The minimum length of the tangent vector in the direction of the relative velocity (if it's below this we recalculate a basis for the tangents).
+        /// @brief The minimum length of the tangent vector in the direction of the relative velocity (if it's below
+        /// this we recalculate a basis for the tangents).
         float minTangentLenSq = 1e-6F * 1e-6F;
 
         /// @brief The minimum restitution value to calculate restitution for a body (restitution is bounciness).
         float minRestitution = 0.0F;
 
-        /// @brief The minimum speed in the normal direction to calculate restitution for a body (lower is closer to elastic collisions).
+        /// @brief The minimum speed in the normal direction to calculate restitution for a body (lower is closer to
+        /// elastic collisions).
         float minNormalSpeed = -0.01F;
 
-        /// @brief The minimum impulse being applied in the normal direction to calculate restitution for a body (to avoid applying restitution when body position didn't change).
+        /// @brief The minimum impulse being applied in the normal direction to calculate restitution for a body (to
+        /// avoid applying restitution when body position didn't change).
         float minNormalImpulse = 0.0F;
+
+        /// @brief Coefficient for how much of the impulse of the previous frame will be used for warm-starting.
+        float warmStartCoefficient = 1.0F;
     };
 
 } // namespace cubos::engine

engine/samples/complex_physics/main.cpp

@@ -158,7 +158,7 @@ int main(int argc, char** argv)
     });
 
     cubos.system("shoot cube")
-        .tagged(physicsApplyForcesTag)
+        .before(transformUpdateTag)
         .call([](Commands cmds, const DeltaTime& dt, const Assets& assets, TimeBetweenShoots& time) {
             time.current += dt.value();
 

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

@@ -126,8 +126,7 @@ void cubos::engine::physicsIntegrationPlugin(Cubos& cubos)
 
     cubos.system("finalize position")
         .tagged(physicsFinalizePositionTag)
-        .call([](Query<Position&, AccumulatedCorrection&, const Mass&> query,
-                 const SolverConstants& solverConstants) {
+        .call([](Query<Position&, AccumulatedCorrection&, const Mass&> query, const SolverConstants& solverConstants) {
             for (auto [position, correction, mass] : query)
             {
                 if (mass.inverseMass <= solverConstants.minInvMass)

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

@@ -14,6 +14,7 @@
 using namespace cubos::engine;
 
 CUBOS_DEFINE_TAG(cubos::engine::addPenetrationConstraintTag);
+CUBOS_DEFINE_TAG(cubos::engine::penetrationConstraintWarmStartTag);
 CUBOS_DEFINE_TAG(cubos::engine::penetrationConstraintSolveTag);
 CUBOS_DEFINE_TAG(cubos::engine::penetrationConstraintSolveRelaxTag);
 CUBOS_DEFINE_TAG(cubos::engine::penetrationConstraintRestitutionTag);
@@ -231,11 +232,68 @@ void cubos::engine::penetrationConstraintPlugin(Cubos& cubos)
     cubos.relation<PenetrationConstraint>();
 
     cubos.tag(addPenetrationConstraintTag);
+    cubos.tag(penetrationConstraintWarmStartTag);
     cubos.tag(penetrationConstraintSolveTag);
     cubos.tag(penetrationConstraintSolveRelaxTag);
     cubos.tag(penetrationConstraintRestitutionTag);
     cubos.tag(penetrationConstraintCleanTag);
 
+    cubos.system("warm start")
+        .tagged(penetrationConstraintWarmStartTag)
+        .after(addPenetrationConstraintTag)
+        .before(penetrationConstraintSolveTag)
+        .tagged(fixedSubstepTag)
+        .call([](Query<Entity, const Mass&, const Inertia&, const Rotation&, AccumulatedCorrection&, Velocity&,
+                       AngularVelocity&, PenetrationConstraint&, Entity, const Mass&, const Inertia&, const Rotation&,
+                       AccumulatedCorrection&, Velocity&, AngularVelocity&>
+                     query,
+                 const SolverConstants& solverConstants) {
+            for (auto [ent1, mass1, inertia1, rotation1, correction1, velocity1, angVelocity1, constraint, ent2, mass2,
+                       inertia2, rotation2, correction2, velocity2, angVelocity2] : query)
+            {
+                glm::vec3 v1 = velocity1.vec;
+                glm::vec3 v2 = velocity2.vec;
+
+                glm::vec3 w1 = angVelocity1.vec;
+                glm::vec3 w2 = angVelocity2.vec;
+
+                glm::vec3 tangent1;
+                glm::vec3 tangent2;
+                if (ent1 != constraint.entity)
+                {
+                    getTangents(v2, v1, constraint.normal, tangent1, tangent2, solverConstants);
+                }
+                else
+                {
+                    getTangents(v1, v2, constraint.normal, tangent1, tangent2, solverConstants);
+                }
+
+                for (PenetrationConstraintPointData& contactPoint : constraint.points)
+                {
+                    glm::vec3 r1 = contactPoint.fixedAnchor1;
+                    glm::vec3 r2 = contactPoint.fixedAnchor2;
+
+                    glm::vec3 p =
+                        solverConstants.warmStartCoefficient * (contactPoint.normalImpulse * constraint.normal) +
+                        (contactPoint.frictionImpulse1 * tangent1) + (contactPoint.frictionImpulse2 * tangent2);
+                    if (ent1 != constraint.entity)
+                    {
+                        p *= -1.0F;
+                    }
+
+                    v1 -= p * mass1.inverseMass;
+                    w1 -= inertia1.inverseInertia * glm::cross(r1, p);
+                    v2 += p * mass2.inverseMass;
+                    w2 += inertia2.inverseInertia * glm::cross(r2, p);
+                }
+
+                velocity1.vec = v1;
+                angVelocity1.vec = w1;
+                velocity2.vec = v2;
+                angVelocity2.vec = w2;
+            }
+        });
+
     cubos.system("solve contacts bias")
         .tagged(penetrationConstraintSolveTag)
         .tagged(physicsSolveContactTag)
@@ -361,10 +419,9 @@ void cubos::engine::penetrationConstraintPlugin(Cubos& cubos)
                 {
                     auto pointData = PenetrationConstraintPointData{};
 
-                    /// TODO: when we have warm-start change this
-                    pointData.normalImpulse = 0.0F;
-                    pointData.frictionImpulse1 = 0.0F;
-                    pointData.frictionImpulse2 = 0.0F;
+                    pointData.normalImpulse = point.normalImpulse;
+                    pointData.frictionImpulse1 = point.frictionImpulse1;
+                    pointData.frictionImpulse2 = point.frictionImpulse2;
 
                     pointData.localAnchor1 = point.localOn1 - centerOfMass1.vec;
                     pointData.localAnchor2 = point.localOn2 - centerOfMass2.vec;
@@ -449,13 +506,22 @@ void cubos::engine::penetrationConstraintPlugin(Cubos& cubos)
             }
         });
 
-    cubos.system("clean penetration constraint pairs")
-        .tagged(penetrationConstraintCleanTag)
-        .before(addPenetrationConstraintTag)
-        .tagged(fixedStepTag)
-        .call([](Commands cmds, Query<Entity, PenetrationConstraint&, Entity> query) {
-            for (auto [entity, constraint, other] : query)
+    cubos.system("store impulses and clean penetration constraint pairs")
+        .tagged(physicsFinalizePositionTag)
+        .call([](Commands cmds, Query<Entity, ContactManifold&, Entity> cQuery,
+                 Query<Entity, PenetrationConstraint&, Entity> pQuery) {
+            for (auto [entity, constraint, other] : cQuery)
             {
+                if (auto match = pQuery.pin(0, entity).pin(1, other).first())
+                {
+                    auto [entity, manifold, other] = *match;
+                    for (size_t i = 0; i < manifold.points.size(); i++)
+                    {
+                        manifold.points[i].normalImpulse = constraint.points[i].normalImpulse;
+                        manifold.points[i].frictionImpulse1 = constraint.points[i].frictionImpulse1;
+                        manifold.points[i].frictionImpulse2 = constraint.points[i].frictionImpulse2;
+                    }
+                }
                 cmds.unrelate<PenetrationConstraint>(entity, other);
             }
         });

engine/src/physics/solver/penetration_constraint/plugin.hpp

@@ -14,6 +14,7 @@ namespace cubos::engine
     /// @brief Adds solver for penetration constraint.
 
     extern Tag addPenetrationConstraintTag;
+    extern Tag penetrationConstraintWarmStartTag;
     extern Tag penetrationConstraintCleanTag;
     extern Tag penetrationConstraintSolveTag;
     extern Tag penetrationConstraintRestitutionTag;