diff --git a/CHANGELOG.md b/CHANGELOG.md index 30ac297d0..efa08b912 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/engine/include/cubos/engine/physics/components/accumulated_correction.hpp b/engine/include/cubos/engine/physics/components/accumulated_correction.hpp index d30c990b4..ef507d4e4 100644 --- a/engine/include/cubos/engine/physics/components/accumulated_correction.hpp +++ b/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 diff --git a/engine/samples/complex_physics/assets/scenes/red_cube.cubos b/engine/samples/complex_physics/assets/scenes/red_cube.cubos index 71aee7b69..55e8b8541 100644 --- a/engine/samples/complex_physics/assets/scenes/red_cube.cubos +++ b/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, diff --git a/engine/samples/complex_physics/assets/scenes/white_cube.cubos b/engine/samples/complex_physics/assets/scenes/white_cube.cubos index 19c3d6d1c..27680df7e 100644 --- a/engine/samples/complex_physics/assets/scenes/white_cube.cubos +++ b/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, diff --git a/engine/src/physics/constraints/penetration_constraint.cpp b/engine/src/physics/constraints/penetration_constraint.cpp index d93ac39b1..14e55eab2 100644 --- a/engine/src/physics/constraints/penetration_constraint.cpp +++ b/engine/src/physics/constraints/penetration_constraint.cpp @@ -14,6 +14,13 @@ 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("restitution", &PenetrationConstraint::restitution) + .withField("relativeVelocity", &PenetrationConstraint::relativeVelocity) .withField("biasCoefficient", &PenetrationConstraint::biasCoefficient) .withField("impulseCoefficient", &PenetrationConstraint::impulseCoefficient) .withField("massCoefficient", &PenetrationConstraint::massCoefficient) diff --git a/engine/src/physics/constraints/penetration_constraint.hpp b/engine/src/physics/constraints/penetration_constraint.hpp index d68847df7..1feb8afc1 100644 --- a/engine/src/physics/constraints/penetration_constraint.hpp +++ b/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; diff --git a/engine/src/physics/plugin.cpp b/engine/src/physics/plugin.cpp index 3d3b16723..f3604a225 100644 --- a/engine/src/physics/plugin.cpp +++ b/engine/src/physics/plugin.cpp @@ -13,7 +13,6 @@ CUBOS_REFLECT_IMPL(AccumulatedCorrection) { return cubos::core::ecs::TypeBuilder("cubos::engine::AccumulatedCorrection") .withField("position", &AccumulatedCorrection::position) - .withField("impulse", &AccumulatedCorrection::impulse) .build(); } diff --git a/engine/src/physics/solver/integration/plugin.cpp b/engine/src/physics/solver/integration/plugin.cpp index 6b67046b1..a2c311fda 100644 --- a/engine/src/physics/solver/integration/plugin.cpp +++ b/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; } }); diff --git a/engine/src/physics/solver/penetration_constraint/plugin.cpp b/engine/src/physics/solver/penetration_constraint/plugin.cpp index 4f0645ac4..6894035f4 100644 --- a/engine/src/physics/solver/penetration_constraint/plugin.cpp +++ b/engine/src/physics/solver/penetration_constraint/plugin.cpp @@ -16,8 +16,23 @@ 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 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 query, @@ -28,37 +43,31 @@ void solvePenetrationConstraint(Query 0.0F) { - bias = penetration * 1.0F / subDeltaTime; + bias = 0.2F * penetration * (1.0F / subDeltaTime); } else if (useBias) { @@ -68,20 +77,14 @@ void solvePenetrationConstraint(Query 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 @@ 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") @@ -149,19 +202,82 @@ 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 + 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 query, + .call([](Commands cmds, + Query + 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() * contactHertz; @@ -178,6 +294,13 @@ 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, + .restitution = restitution, + .relativeVelocity = relativeVelocity, .biasCoefficient = biasCoefficient, .impulseCoefficient = impulseCoefficient, .massCoefficient = massCoefficient}); diff --git a/engine/src/physics/solver/penetration_constraint/plugin.hpp b/engine/src/physics/solver/penetration_constraint/plugin.hpp index 3ce2738cb..a0b23aaf9 100644 --- a/engine/src/physics/solver/penetration_constraint/plugin.hpp +++ b/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.