feat(penetration_constraint): add restititution

engine/src/physics/constraints/penetration_constraint.cpp

@@ -19,6 +19,8 @@ CUBOS_REFLECT_IMPL(cubos::engine::PenetrationConstraint)
         .withField("frictionMass", &PenetrationConstraint::frictionMass)
         .withField("frictionImpulse1", &PenetrationConstraint::frictionImpulse1)
         .withField("frictionImpulse2", &PenetrationConstraint::frictionImpulse2)
+        .withField("restitution", &PenetrationConstraint::restitution)
+        .withField("relativeVelocity", &PenetrationConstraint::relativeVelocity)
         .withField("biasCoefficient", &PenetrationConstraint::biasCoefficient)
         .withField("impulseCoefficient", &PenetrationConstraint::impulseCoefficient)
         .withField("massCoefficient", &PenetrationConstraint::massCoefficient)

engine/src/physics/constraints/penetration_constraint.hpp

@@ -26,14 +26,20 @@ namespace cubos::engine
         float penetration;                 ///< Penetration depth of the collision.
         glm::vec3 normal;                  ///< Normal of contact on the surface of the entity.
 
+        // separation
         float normalMass;    ///< Mass to use for normal impulse calculation.
         float normalImpulse; ///< Accumulated impulse for separation.
 
+        // friction
         float friction;         ///< Friction of the constraint.
         float frictionMass;     ///< Mass to use for friction impulse calculation.
         float frictionImpulse1; ///< Accumulated impulse for friction along the first tangent.
         float frictionImpulse2; ///< Accumulated impulse for friction along the second tangent.
 
+        // restitution
+        float restitution;      ///< Restitution coefficient of the constraint.
+        float relativeVelocity; ///< Relative velocity for computing restitution.
+
         // soft constraint
         float biasCoefficient;
         float impulseCoefficient;

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

@@ -16,6 +16,7 @@ using namespace cubos::engine;
 CUBOS_DEFINE_TAG(cubos::engine::addPenetrationConstraintTag);
 CUBOS_DEFINE_TAG(cubos::engine::penetrationConstraintSolveTag);
 CUBOS_DEFINE_TAG(cubos::engine::penetrationConstraintSolveRelaxTag);
+CUBOS_DEFINE_TAG(cubos::engine::penetrationConstraintRestitutionTag);
 CUBOS_DEFINE_TAG(cubos::engine::penetrationConstraintCleanTag);
 
 void solvePenetrationConstraint(Query<Entity, const Mass&, AccumulatedCorrection&, Velocity&, PenetrationConstraint&,
@@ -31,8 +32,7 @@ void solvePenetrationConstraint(Query<Entity, const Mass&, AccumulatedCorrection
         glm::vec3 v1 = velocity1.vec;
         glm::vec3 v2 = velocity2.vec;
 
-        // for each contact point, for now its for each entity
-        for (int i = 0; i < 2; i++)
+        // for each contact point in the future, since we have particles now we only need to do it once
         {
             float totalImpulse = constraint.normalImpulse;
 
@@ -86,14 +86,7 @@ void solvePenetrationConstraint(Query<Entity, const Mass&, AccumulatedCorrection
             // Clamp the accumulated impulse
             float newImpulse = glm::max(totalImpulse + impulse, 0.0F);
             impulse = newImpulse - totalImpulse;
-            if (i == 0)
-            {
-                constraint.normalImpulse = newImpulse;
-            }
-            else
-            {
-                constraint.normalImpulse = newImpulse;
-            }
+            constraint.normalImpulse = newImpulse;
 
             glm::vec3 p = constraint.normal * impulse;
             if (ent1 == constraint.entity)
@@ -109,69 +102,64 @@ void solvePenetrationConstraint(Query<Entity, const Mass&, AccumulatedCorrection
         }
 
         // Friction
-        float normalImpulse = constraint.normalImpulse;
-        float frictionImpulse1 = constraint.frictionImpulse1;
-        float frictionImpulse2 = constraint.frictionImpulse2;
-
-        // Relative velocity at contact
-        glm::vec3 vr2;
-        glm::vec3 vr1;
-        if (ent1 == constraint.entity)
-        {
-            vr2 = velocity2.vec;
-            vr1 = velocity1.vec;
-        }
-        else
+        // for each contact point in the future, since we have particles now we only need to do it once
         {
-            vr2 = velocity1.vec;
-            vr1 = velocity2.vec;
-        }
-
-        glm::vec3 vr = vr2 - vr1;
-        glm::vec3 tangent1 = vr - glm::dot(vr, constraint.normal) * constraint.normal;
-        glm::vec3 tangent2;
-        float tangentLenSq = glm::length2(tangent1);
-        if (tangentLenSq > 1e-3)
-        {
-            tangent1 = glm::normalize(tangent1);
-            tangent2 = glm::cross(constraint.normal, tangent1);
+            float normalImpulse = constraint.normalImpulse;
+            float frictionImpulse1 = constraint.frictionImpulse1;
+            float frictionImpulse2 = constraint.frictionImpulse2;
 
-            float vn1 = glm::dot(vr, tangent1);
-            float vn2 = glm::dot(vr, tangent2);
-            if (vn1 < 1e-3)
+            // Relative velocity at contact
+            glm::vec3 vr2;
+            glm::vec3 vr1;
+            if (ent1 == constraint.entity)
             {
-                vn1 = 0.0F;
+                vr2 = velocity2.vec;
+                vr1 = velocity1.vec;
             }
-            if (vn2 < 1e-3)
+            else
             {
-                vn2 = 0.0F;
+                vr2 = velocity1.vec;
+                vr1 = velocity2.vec;
             }
 
-            // Compute friction force
-            float impulse1 = -constraint.frictionMass * vn1;
-            float impulse2 = -constraint.frictionMass * vn2;
-
-            // Clamp the accumulated force
-            float maxFriction = constraint.friction * normalImpulse;
-            float newImpulse1 = glm::clamp(frictionImpulse1 + impulse1, -maxFriction, maxFriction);
-            float newImpulse2 = glm::clamp(frictionImpulse2 + impulse2, -maxFriction, maxFriction);
-            impulse1 = newImpulse1 - frictionImpulse1;
-            impulse2 = newImpulse2 - frictionImpulse2;
-            constraint.frictionImpulse1 = newImpulse1;
-            constraint.frictionImpulse2 = newImpulse2;
-
-            // Apply contact impulse
-            glm::vec3 p1 = tangent1 * impulse1;
-            glm::vec3 p2 = tangent2 * impulse2;
-            if (ent1 == constraint.entity)
+            glm::vec3 vr = vr2 - vr1;
+            glm::vec3 tangent1 = vr - glm::dot(vr, constraint.normal) * constraint.normal;
+            glm::vec3 tangent2;
+            float tangentLenSq = glm::length2(tangent1);
+            if (tangentLenSq > 1e-6)
             {
-                v1 -= p1 * mass1.inverseMass + p2 * mass1.inverseMass;
-                v2 += p1 * mass2.inverseMass + p2 * mass2.inverseMass;
-            }
-            else
-            {
-                v1 += p1 * mass1.inverseMass + p2 * mass1.inverseMass;
-                v2 -= p1 * mass2.inverseMass + p2 * mass2.inverseMass;
+                tangent1 = glm::normalize(tangent1);
+                tangent2 = glm::cross(constraint.normal, tangent1);
+
+                float vn1 = glm::dot(vr, tangent1);
+                float vn2 = glm::dot(vr, tangent2);
+
+                // Compute friction force
+                float impulse1 = -constraint.frictionMass * vn1;
+                float impulse2 = -constraint.frictionMass * vn2;
+
+                // Clamp the accumulated force
+                float maxFriction = constraint.friction * normalImpulse;
+                float newImpulse1 = glm::clamp(frictionImpulse1 + impulse1, -maxFriction, maxFriction);
+                float newImpulse2 = glm::clamp(frictionImpulse2 + impulse2, -maxFriction, maxFriction);
+                impulse1 = newImpulse1 - frictionImpulse1;
+                impulse2 = newImpulse2 - frictionImpulse2;
+                constraint.frictionImpulse1 = newImpulse1;
+                constraint.frictionImpulse2 = newImpulse2;
+
+                // Apply contact impulse
+                glm::vec3 p1 = tangent1 * impulse1;
+                glm::vec3 p2 = tangent2 * impulse2;
+                if (ent1 == constraint.entity)
+                {
+                    v1 -= p1 * mass1.inverseMass + p2 * mass1.inverseMass;
+                    v2 += p1 * mass2.inverseMass + p2 * mass2.inverseMass;
+                }
+                else
+                {
+                    v1 += p1 * mass1.inverseMass + p2 * mass1.inverseMass;
+                    v2 -= p1 * mass2.inverseMass + p2 * mass2.inverseMass;
+                }
             }
         }
 
@@ -193,6 +181,7 @@ void cubos::engine::penetrationConstraintPlugin(Cubos& cubos)
     cubos.tag(addPenetrationConstraintTag);
     cubos.tag(penetrationConstraintSolveTag);
     cubos.tag(penetrationConstraintSolveRelaxTag);
+    cubos.tag(penetrationConstraintRestitutionTag);
     cubos.tag(penetrationConstraintCleanTag);
 
     cubos.system("solve contacts bias")
@@ -213,15 +202,68 @@ void cubos::engine::penetrationConstraintPlugin(Cubos& cubos)
                  const FixedDeltaTime& fixedDeltaTime,
                  const Substeps& substeps) { solvePenetrationConstraint(query, fixedDeltaTime, substeps, false); });
 
+    cubos.system("add restitution")
+        .tagged(penetrationConstraintRestitutionTag)
+        .after(penetrationConstraintSolveRelaxTag)
+        .before(physicsFinalizePositionTag)
+        .tagged(fixedStepTag)
+        .call([](Query<Entity, const Mass&, AccumulatedCorrection&, Velocity&, PenetrationConstraint&, Entity,
+                       const Mass&, AccumulatedCorrection&, Velocity&>
+                     query) {
+            for (auto [ent1, mass1, correction1, velocity1, constraint, ent2, mass2, correction2, velocity2] : query)
+            {
+                if (constraint.restitution == 0.0F)
+                {
+                    continue;
+                }
+
+                if (constraint.relativeVelocity > -0.1F || constraint.normalImpulse == 0.0F)
+                {
+                    continue;
+                }
+
+                // Relative normal velocity at contact
+                glm::vec3 vr2;
+                glm::vec3 vr1;
+                if (ent1 == constraint.entity)
+                {
+                    vr2 = velocity2.vec;
+                    vr1 = velocity1.vec;
+                }
+                else
+                {
+                    vr2 = velocity1.vec;
+                    vr1 = velocity2.vec;
+                }
+
+                float vn = glm::dot(vr2 - vr1, constraint.normal);
+
+                // compute normal impulse
+                float impulse = -constraint.normalMass * (vn + constraint.restitution * constraint.relativeVelocity);
+
+                // Clamp the accumulated impulse
+                float newImpulse = glm::max(constraint.normalImpulse + impulse, 0.0F);
+                impulse = newImpulse - constraint.normalImpulse;
+                constraint.normalImpulse = newImpulse;
+
+                // Apply impulse
+                glm::vec3 p = constraint.normal * impulse;
+                velocity1.vec = velocity1.vec - p * mass1.inverseMass;
+                velocity2.vec = velocity2.vec + p * mass2.inverseMass;
+            }
+        });
+
     cubos.system("add penetration constraint pair")
         .tagged(addPenetrationConstraintTag)
         .tagged(physicsPrepareSolveTag)
-        .call([](Commands cmds, Query<Entity, const Mass&, const CollidingWith&, Entity, const Mass&> query,
+        .call([](Commands cmds,
+                 Query<Entity, const Mass&, const Velocity&, const CollidingWith&, Entity, const Mass&, const Velocity&>
+                     query,
                  const FixedDeltaTime& fixedDeltaTime, const Substeps& substeps) {
             float subDeltaTime = fixedDeltaTime.value / (float)substeps.value;
             float contactHertz = glm::min(30.0F, 0.25F * (1.0F / subDeltaTime));
 
-            for (auto [ent1, mass1, collidingWith, ent2, mass2] : query)
+            for (auto [ent1, mass1, velocity1, collidingWith, ent2, mass2, velocity2] : query)
             {
                 float kNormal = mass1.inverseMass + mass2.inverseMass;
                 float normalMass = kNormal > 0.0F ? 1.0F / kNormal : 0.0F;
@@ -233,6 +275,23 @@ void cubos::engine::penetrationConstraintPlugin(Cubos& cubos)
                 // determine friction (set to predefined value for now)
                 float friction = 0.01F;
 
+                // determine restitution (set to predefined value for now)
+                float restitution = 1.0F;
+                glm::vec3 vr2;
+                glm::vec3 vr1;
+                if (ent1 == collidingWith.entity)
+                {
+                    vr2 = velocity2.vec;
+                    vr1 = velocity1.vec;
+                }
+                else
+                {
+                    vr2 = velocity1.vec;
+                    vr1 = velocity2.vec;
+                }
+
+                float relativeVelocity = glm::dot(vr2 - vr1, collidingWith.normal);
+
                 // Soft contact
                 const float zeta = 10.0F;
                 float omega = 2.0F * glm::pi<float>() * contactHertz;
@@ -254,6 +313,8 @@ void cubos::engine::penetrationConstraintPlugin(Cubos& cubos)
                                                   .frictionMass = frictionMass,
                                                   .frictionImpulse1 = 0.0F,
                                                   .frictionImpulse2 = 0.0F,
+                                                  .restitution = restitution,
+                                                  .relativeVelocity = relativeVelocity,
                                                   .biasCoefficient = biasCoefficient,
                                                   .impulseCoefficient = impulseCoefficient,
                                                   .massCoefficient = massCoefficient});

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

@@ -16,6 +16,7 @@ namespace cubos::engine
     extern Tag addPenetrationConstraintTag;
     extern Tag penetrationConstraintCleanTag;
     extern Tag penetrationConstraintSolveTag;
+    extern Tag penetrationConstraintRestitutionTag;
     extern Tag penetrationConstraintSolveRelaxTag;
 
     /// @brief Plugin entry function.