Add friction and restitution on penetration constraint solving

CHANGELOG.md

@@ -23,6 +23,8 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 - Raycasting, collisions between rays and box/capsule colliders (#225, **@diogomsmiranda**).
 - Change speed of the debug-camera using Tab and LShift, positive and negative respectively (#1159, **@diogomsmiranda**)
 - Console plugin (#875, **@Scarface1809**).
+- Friction calculation for penetration constraint (#1244, **@fallenatlas**).
+- Bounciness calculation for penetration constraint (#1275, **@fallenatlas**).
 
 ### Changed
 

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

@@ -19,6 +19,5 @@ namespace cubos::engine
         CUBOS_REFLECT;
 
         glm::vec3 position = {0.0F, 0.0F, 0.0F}; ///< Accumulated position correction.
-        float impulse = 0.0F;                    ///< Accumulated impulse.
     };
 } // namespace cubos::engine

engine/samples/complex_physics/assets/scenes/red_cube.cubos

@@ -2,12 +2,9 @@
   "entities": {
     "cube": {
       "cubos::engine::AccumulatedCorrection": {
-        "position": {
           "x": 0.0,
           "y": 0.0,
           "z": 0.0
-        },
-        "impulse": 0.0
       },
       "cubos::engine::BoxCollisionShape": {
         "x": 0.5,

engine/samples/complex_physics/assets/scenes/white_cube.cubos

@@ -2,12 +2,9 @@
   "entities": {
     "cube": {
       "cubos::engine::AccumulatedCorrection": {
-        "position": {
           "x": 0.0,
           "y": 0.0,
           "z": 0.0
-        },
-        "impulse": 0.0
       },
       "cubos::engine::BoxCollisionShape": {
         "x": 0.5,

engine/src/physics/constraints/penetration_constraint.cpp

@@ -14,6 +14,13 @@
         .withField("penetration", &PenetrationConstraint::penetration)
         .withField("normal", &PenetrationConstraint::normal)
         .withField("normalMass", &PenetrationConstraint::normalMass)
+        .withField("normalImpulse", &PenetrationConstraint::normalImpulse)
+        .withField("friction", &PenetrationConstraint::friction)
+        .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,7 +26,19 @@ namespace cubos::engine
         float penetration;                 ///< Penetration depth of the collision.
         glm::vec3 normal;                  ///< Normal of contact on the surface of the entity.
 
-        float normalMass;
+        // 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;

engine/src/physics/plugin.cpp

@@ -13,7 +13,6 @@ CUBOS_REFLECT_IMPL(AccumulatedCorrection)
 {
     return cubos::core::ecs::TypeBuilder<AccumulatedCorrection>("cubos::engine::AccumulatedCorrection")
         .withField("position", &AccumulatedCorrection::position)
-        .withField("impulse", &AccumulatedCorrection::impulse)
         .build();
 }
 

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

@@ -86,9 +86,9 @@ void cubos::engine::physicsIntegrationPlugin(Cubos& cubos)
                 {
                     continue;
                 }
+
                 position.vec += correction.position;
                 correction.position = glm::vec3(0.0F);
-                correction.impulse = 0.0F;
             }
         });
 

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

@@ -16,8 +16,23 @@
 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 getPlaneSpace(const glm::vec3& n, glm::vec3& tangent1, glm::vec3& tangent2)
+{
+    if (glm::abs(n.z) > (1.0F / glm::sqrt(2.0F)))
+    {
+        tangent1 = glm::normalize(glm::vec3(0.0F, -n.z, n.y));
+        tangent2 = glm::cross(n, tangent1);
+    }
+    else
+    {
+        tangent1 = glm::normalize(glm::vec3(-n.y, n.x, 0.0F));
+        tangent2 = glm::cross(n, tangent1);
+    }
+}
+
 void solvePenetrationConstraint(Query<Entity, const Mass&, AccumulatedCorrection&, Velocity&, PenetrationConstraint&,
                                       Entity, const Mass&, AccumulatedCorrection&, Velocity&>
                                     query,
@@ -28,37 +43,31 @@
 
         float subDeltaTime = fixedDeltaTime.value / (float)substeps.value;
 
-        glm::vec3 v1;
-        glm::vec3 v2;
+        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 = i == 0 ? correction1.impulse : correction2.impulse;
+            float totalImpulse = constraint.normalImpulse;
 
-            // compute current penatration
-            float penetration = constraint.penetration;
+            // compute current penetration
+            float penetration = -constraint.penetration;
 
-            if (ent1 == constraint.entity)
-            {
-                glm::vec3 deltaPenetration = correction2.position - correction1.position;
-                penetration += glm::dot(deltaPenetration, constraint.normal);
-            }
-            else
+            glm::vec3 deltaPenetration = correction2.position - correction1.position;
+
+            if (ent1 != constraint.entity)
             {
-                glm::vec3 deltaPenetration = correction1.position - correction2.position;
-                penetration += glm::dot(deltaPenetration, constraint.normal);
+                deltaPenetration *= -1.0F;
             }
 
-            // penetration is positive but we need negative value for separation
-            penetration = -penetration;
+            penetration += glm::dot(deltaPenetration, constraint.normal);
 
             float bias = 0.0F;
             float massScale = 1.0F;
             float impulseScale = 0.0F;
             if (penetration > 0.0F)
             {
-                bias = penetration * 1.0F / subDeltaTime;
+                bias = 0.2F * penetration * (1.0F / subDeltaTime);
             }
             else if (useBias)
             {
@@ -68,47 +77,90 @@
             }
 
             // Relative velocity at contact
-            glm::vec3 vr2;
-            glm::vec3 vr1;
-            if (ent1 == constraint.entity)
-            {
-                vr2 = velocity2.vec;
-                vr1 = velocity1.vec;
-            }
-            else
+            glm::vec3 vr = velocity2.vec - velocity1.vec;
+
+            if (ent1 != constraint.entity)
             {
-                vr2 = velocity1.vec;
-                vr1 = velocity2.vec;
+                vr *= -1.0F;
             }
 
-            float vn = glm::dot(vr2 - vr1, constraint.normal);
+            float vn = glm::dot(vr, constraint.normal);
 
             // Compute normal impulse
             float impulse = -constraint.normalMass * massScale * (vn + bias) - impulseScale * totalImpulse;
 
             // Clamp the accumulated impulse
             float newImpulse = glm::max(totalImpulse + impulse, 0.0F);
             impulse = newImpulse - totalImpulse;
-            if (i == 0)
+            constraint.normalImpulse = newImpulse;
+
+            glm::vec3 p = impulse * constraint.normal;
+
+            if (ent1 != constraint.entity)
             {
-                correction1.impulse = newImpulse;
+                p *= -1.0F;
             }
-            else
+
+            v1 = velocity1.vec - p * mass1.inverseMass;
+            v2 = velocity2.vec + p * mass2.inverseMass;
+        }
+
+        // Friction
+        // for each contact point in the future, since we have particles now we only need to do it once
+        {
+            float normalImpulse = constraint.normalImpulse;
+            float frictionImpulse1 = constraint.frictionImpulse1;
+            float frictionImpulse2 = constraint.frictionImpulse2;
+
+            // Relative velocity at contact
+            glm::vec3 vr = velocity2.vec - velocity1.vec;
+
+            if (ent1 != constraint.entity)
             {
-                correction2.impulse = newImpulse;
+                vr *= -1.0F;
             }
 
-            glm::vec3 p = constraint.normal * impulse;
-            if (ent1 == constraint.entity)
+            glm::vec3 tangent1 = vr - glm::dot(vr, constraint.normal) * constraint.normal;
+            glm::vec3 tangent2;
+            float tangentLenSq = glm::length2(tangent1);
+            if (tangentLenSq > 1e-6 * 1e-6)
             {
-                v1 = velocity1.vec - p * mass1.inverseMass;
-                v2 = velocity2.vec + p * mass2.inverseMass;
+                tangent1 = glm::normalize(tangent1);
+                tangent2 = glm::cross(constraint.normal, tangent1);
             }
             else
             {
-                v1 = velocity1.vec + p * mass1.inverseMass;
-                v2 = velocity2.vec - p * mass2.inverseMass;
+                // if there is no tangent in relation that can be obtained from vr calculate a basis for the tangent
+                // plane.
+                getPlaneSpace(constraint.normal, tangent1, tangent2);
             }
+
+            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 p = impulse1 * tangent1 + impulse2 * tangent2;
+
+            if (ent1 != constraint.entity)
+            {
+                p *= -1.0F;
+            }
+
+            v1 -= p * mass1.inverseMass;
+            v2 += p * mass2.inverseMass;
         }
 
         velocity1.vec = v1;
@@ -129,6 +181,7 @@
     cubos.tag(addPenetrationConstraintTag);
     cubos.tag(penetrationConstraintSolveTag);
     cubos.tag(penetrationConstraintSolveRelaxTag);
+    cubos.tag(penetrationConstraintRestitutionTag);
     cubos.tag(penetrationConstraintCleanTag);
 
     cubos.system("solve contacts bias")
@@ -149,19 +202,82 @@
                  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 vr = velocity2.vec - velocity1.vec;
+
+                if (ent1 != constraint.entity)
+                {
+                    vr *= -1.0F;
+                }
+                float vn = glm::dot(vr, 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;
 
+                // friction mass
+                float kFriction = mass1.inverseMass + mass2.inverseMass;
+                float frictionMass = kFriction > 0.0F ? 1.0F / kFriction : 0.0F;
+
+                // 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 vr = velocity2.vec - velocity1.vec;
+
+                if (ent1 != collidingWith.entity)
+                {
+                    vr *= -1.0F;
+                }
+                float relativeVelocity = glm::dot(vr, collidingWith.normal);
+
                 // Soft contact
                 const float zeta = 10.0F;
                 float omega = 2.0F * glm::pi<float>() * contactHertz;
@@ -178,6 +294,13 @@
                                                   .penetration = collidingWith.penetration,
                                                   .normal = collidingWith.normal,
                                                   .normalMass = normalMass,
+                                                  .normalImpulse = 0.0F,
+                                                  .friction = friction,
+                                                  .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.