feat(penetration_constraint): add friction impulse

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,11 @@ CUBOS_REFLECT_IMPL(cubos::engine::PenetrationConstraint)
         .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("biasCoefficient", &PenetrationConstraint::biasCoefficient)
         .withField("impulseCoefficient", &PenetrationConstraint::impulseCoefficient)
         .withField("massCoefficient", &PenetrationConstraint::massCoefficient)

engine/src/physics/constraints/penetration_constraint.hpp

@@ -26,7 +26,13 @@ namespace cubos::engine
         float penetration;                 ///< Penetration depth of the collision.
         glm::vec3 normal;                  ///< Normal of contact on the surface of the entity.
 
-        float normalMass;
+        float normalMass;    ///< Mass to use for normal impulse calculation.
+        float normalImpulse; ///< Accumulated impulse for separation.
+
+        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.
 
         // 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

@@ -28,16 +28,16 @@ void solvePenetrationConstraint(Query<Entity, const Mass&, AccumulatedCorrection
 
         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++)
         {
-            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)
             {
@@ -50,15 +50,12 @@ void solvePenetrationConstraint(Query<Entity, const Mass&, AccumulatedCorrection
                 penetration += glm::dot(deltaPenetration, constraint.normal);
             }
 
-            // penetration is positive but we need negative value for separation
-            penetration = -penetration;
-
             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)
             {
@@ -91,11 +88,11 @@ void solvePenetrationConstraint(Query<Entity, const Mass&, AccumulatedCorrection
             impulse = newImpulse - totalImpulse;
             if (i == 0)
             {
-                correction1.impulse = newImpulse;
+                constraint.normalImpulse = newImpulse;
             }
             else
             {
-                correction2.impulse = newImpulse;
+                constraint.normalImpulse = newImpulse;
             }
 
             glm::vec3 p = constraint.normal * impulse;
@@ -111,6 +108,73 @@ 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
+        {
+            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 vn1 = glm::dot(vr, tangent1);
+            float vn2 = glm::dot(vr, tangent2);
+            if (vn1 < 1e-3)
+            {
+                vn1 = 0.0F;
+            }
+            if (vn2 < 1e-3)
+            {
+                vn2 = 0.0F;
+            }
+
+            // 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;
+            }
+        }
+
         velocity1.vec = v1;
         velocity2.vec = v2;
     }
@@ -162,6 +226,13 @@ void cubos::engine::penetrationConstraintPlugin(Cubos& cubos)
                 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;
+
                 // Soft contact
                 const float zeta = 10.0F;
                 float omega = 2.0F * glm::pi<float>() * contactHertz;
@@ -178,6 +249,11 @@ void cubos::engine::penetrationConstraintPlugin(Cubos& cubos)
                                                   .penetration = collidingWith.penetration,
                                                   .normal = collidingWith.normal,
                                                   .normalMass = normalMass,
+                                                  .normalImpulse = 0.0F,
+                                                  .friction = friction,
+                                                  .frictionMass = frictionMass,
+                                                  .frictionImpulse1 = 0.0F,
+                                                  .frictionImpulse2 = 0.0F,
                                                   .biasCoefficient = biasCoefficient,
                                                   .impulseCoefficient = impulseCoefficient,
                                                   .massCoefficient = massCoefficient});