From f0628f97001a4b5e3dc955ab21a348dc1b93bda8 Mon Sep 17 00:00:00 2001 From: fallenatlas Date: Wed, 3 Jul 2024 13:09:46 +0100 Subject: [PATCH] feat(penetration_constraint): add restititution --- .../constraints/penetration_constraint.cpp | 2 + .../constraints/penetration_constraint.hpp | 6 + .../solver/penetration_constraint/plugin.cpp | 195 ++++++++++++------ .../solver/penetration_constraint/plugin.hpp | 1 + 4 files changed, 137 insertions(+), 67 deletions(-) diff --git a/engine/src/physics/constraints/penetration_constraint.cpp b/engine/src/physics/constraints/penetration_constraint.cpp index ebc5c8f96..14e55eab2 100644 --- a/engine/src/physics/constraints/penetration_constraint.cpp +++ b/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) diff --git a/engine/src/physics/constraints/penetration_constraint.hpp b/engine/src/physics/constraints/penetration_constraint.hpp index 9d7f8d329..1feb8afc1 100644 --- a/engine/src/physics/constraints/penetration_constraint.hpp +++ b/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; diff --git a/engine/src/physics/solver/penetration_constraint/plugin.cpp b/engine/src/physics/solver/penetration_constraint/plugin.cpp index a2fee0622..ded64ae5b 100644 --- a/engine/src/physics/solver/penetration_constraint/plugin.cpp +++ b/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 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 + 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 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; @@ -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() * 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}); 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.