From 1e0fe6b3e63688a7cd26ffb7815684576e52fbea Mon Sep 17 00:00:00 2001 From: fallenatlas Date: Thu, 19 Sep 2024 23:54:00 +0100 Subject: [PATCH] feat(penetration_constraint): add rotation on solving --- .../constraints/penetration_constraint.cpp | 28 +- .../constraints/penetration_constraint.hpp | 45 ++- .../solver/penetration_constraint/plugin.cpp | 336 ++++++++++++------ 3 files changed, 277 insertions(+), 132 deletions(-) diff --git a/engine/src/physics/constraints/penetration_constraint.cpp b/engine/src/physics/constraints/penetration_constraint.cpp index 19a45eb14..794809243 100644 --- a/engine/src/physics/constraints/penetration_constraint.cpp +++ b/engine/src/physics/constraints/penetration_constraint.cpp @@ -3,22 +3,36 @@ #include #include #include +#include + +CUBOS_REFLECT_IMPL(cubos::engine::PenetrationConstraintPointData) +{ + return cubos::core::ecs::TypeBuilder( + "cubos::engine::PenetrationConstraintPointData") + .withField("initialSeparation", &PenetrationConstraintPointData::initialSeparation) + .withField("normalSpeed", &PenetrationConstraintPointData::normalSpeed) + .withField("localAnchor1", &PenetrationConstraintPointData::localAnchor1) + .withField("localAnchor2", &PenetrationConstraintPointData::localAnchor2) + .withField("fixedAnchor1", &PenetrationConstraintPointData::fixedAnchor1) + .withField("fixedAnchor2", &PenetrationConstraintPointData::fixedAnchor2) + .withField("normalMass", &PenetrationConstraintPointData::normalMass) + .withField("normalImpulse", &PenetrationConstraintPointData::normalImpulse) + .withField("frictionMass1", &PenetrationConstraintPointData::frictionMass1) + .withField("frictionMass2", &PenetrationConstraintPointData::frictionMass2) + .withField("frictionImpulse1", &PenetrationConstraintPointData::frictionImpulse1) + .withField("frictionImpulse2", &PenetrationConstraintPointData::frictionImpulse2) + .build(); +} CUBOS_REFLECT_IMPL(cubos::engine::PenetrationConstraint) { return cubos::core::ecs::TypeBuilder("cubos::engine::PenetrationConstraint") .symmetric() .withField("entity", &PenetrationConstraint::entity) - .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("points", &PenetrationConstraint::points) .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 0ceb9da1a..736a2fb8c 100644 --- a/engine/src/physics/constraints/penetration_constraint.hpp +++ b/engine/src/physics/constraints/penetration_constraint.hpp @@ -13,32 +13,49 @@ namespace cubos::engine { - /// @brief Relation which holds the information for resolving a penetration between particles. + /// @brief Holds the data for a contact point of the @ref PenetrationContraint. /// @ingroup physics-solver-plugin - struct PenetrationConstraint + struct PenetrationConstraintPointData { CUBOS_REFLECT; - // colision info - cubos::core::ecs::Entity entity; ///< Entity to which the normal is relative to. - float penetration; ///< Penetration depth of the collision. This is to be removed - glm::vec3 normal; ///< Normal of contact on the surface of the entity. (world space) + /// TODO: change to initialSeparation + float initialSeparation; ///< The separation of the contact point. Negative separation indicates + ///< penetration. + float normalSpeed; ///< The relative velocity of the bodies along the normal at the contact point the begging of + ///< the collision. + + glm::vec3 localAnchor1; ///< The local contact point relative to the center of mass of the first body. + glm::vec3 localAnchor2; ///< The local contact point relative to the center of mass of the second body. + + /// Store fixed world-space anchors. + /// This improves rolling behavior for shapes like balls and capsules. Used for restitution and friction. + glm::vec3 fixedAnchor1; ///< The world-space contact point relative to the center of mass of the first body. + glm::vec3 fixedAnchor2; ///< The world-space contact point relative to the center of mass of the second body. // separation - float normalMass; ///< Mass to use for normal impulse calculation. (can these be here or should be by point (in - ///< which case it always depends probably)) + float normalMass; ///< Mass to use for normal impulse calculation. float normalImpulse; ///< Accumulated impulse for separation. // friction - float friction; ///< Friction of the constraint. yes. - float frictionMass; ///< Mass to use for friction impulse calculation. - /// TODO: store tangents? + float frictionMass1; ///< Mass to use for friction impulse calculation along the first tangent.. + float frictionMass2; ///< Mass to use for friction impulse calculation along the second tangent.. float frictionImpulse1; ///< Accumulated impulse for friction along the first tangent. float frictionImpulse2; ///< Accumulated impulse for friction along the second tangent. + }; + + /// @brief Relation which holds the information for resolving a penetration between particles. + /// @ingroup physics-solver-plugin + struct PenetrationConstraint + { + CUBOS_REFLECT; + + cubos::core::ecs::Entity entity; ///< Entity to which the normal is relative to. + glm::vec3 normal; ///< Normal of contact on the surface of the entity. + float friction; ///< Friction of the constraint. + float restitution; ///< Restitution coefficient of the constraint. - // restitution - float restitution; ///< Restitution coefficient of the constraint. yes. - float relativeVelocity; ///< Relative velocity for computing restitution. This is to be removed (probably) + std::vector points; ///< Contact points in the contact manifold. // soft constraint float biasCoefficient; diff --git a/engine/src/physics/solver/penetration_constraint/plugin.cpp b/engine/src/physics/solver/penetration_constraint/plugin.cpp index b65032c3e..4dfb6237e 100644 --- a/engine/src/physics/solver/penetration_constraint/plugin.cpp +++ b/engine/src/physics/solver/penetration_constraint/plugin.cpp @@ -2,7 +2,7 @@ #include -#include +#include #include #include #include @@ -33,6 +33,28 @@ void getPlaneSpace(const glm::vec3& n, glm::vec3& tangent1, glm::vec3& tangent2) } } +void getTangents(const glm::vec3& velocity1, const glm::vec3& velocity2, const glm::vec3& n, glm::vec3& tangent1, + glm::vec3& tangent2) +{ + // Use linear relative velocity for determining tangents. + /// TODO: test this with the normal way + glm::vec3 linearVr = velocity2 - velocity1; + + tangent1 = linearVr - glm::dot(linearVr, n) * n; + float tangentLenSq = glm::length2(tangent1); + if (tangentLenSq > 1e-6 * 1e-6) /// TODO: check this + { + tangent1 = glm::normalize(tangent1); + tangent2 = glm::cross(n, tangent1); + } + else + { + // if there is no tangent in relation that can be obtained from vr calculate a basis for the tangent + // plane. + getPlaneSpace(n, tangent1, tangent2); + } +} + static PhysicsMaterial::MixProperty getMixProperty(PhysicsMaterial::MixProperty mixProperty1, PhysicsMaterial::MixProperty mixProperty2) { @@ -56,12 +78,15 @@ static float mixValues(float value1, float value2, PhysicsMaterial::MixProperty } } -void solvePenetrationConstraint(Query - query, - const FixedDeltaTime& fixedDeltaTime, const Substeps& substeps, const bool useBias) +void solvePenetrationConstraint( + Query + query, + const FixedDeltaTime& fixedDeltaTime, const Substeps& substeps, const bool useBias) { - for (auto [ent1, mass1, correction1, velocity1, constraint, ent2, mass2, correction2, velocity2] : query) + for (auto [ent1, mass1, inertia1, rotation1, correction1, velocity1, angVelocity1, constraint, ent2, mass2, + inertia2, rotation2, correction2, velocity2, angVelocity2] : query) { float subDeltaTime = fixedDeltaTime.value / (float)substeps.value; @@ -69,39 +94,42 @@ void solvePenetrationConstraint(Query 0.0F) + if (separation > 0.0F) { - bias = 0.2F * penetration * (1.0F / subDeltaTime); + bias = separation * (1.0F / subDeltaTime); } else if (useBias) { - bias = glm::max(constraint.biasCoefficient * penetration, -4.0F); + bias = glm::max(constraint.biasCoefficient * separation, -4.0F); massScale = constraint.massCoefficient; impulseScale = constraint.impulseCoefficient; } // Relative velocity at contact - glm::vec3 vr = velocity2.vec - velocity1.vec; - + glm::vec3 vr1 = v1 + glm::cross(w1, r1); + glm::vec3 vr2 = v2 + glm::cross(w2, r2); + glm::vec3 vr = vr2 - vr1; if (ent1 != constraint.entity) { vr *= -1.0F; @@ -110,84 +138,85 @@ void solvePenetrationConstraint(Query 1e-6 * 1e-6) - { - tangent1 = glm::normalize(tangent1); - tangent2 = glm::cross(constraint.normal, tangent1); - } - else - { - // 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; + float impulse1 = -contactPoint.frictionMass1 * vn1; + float impulse2 = -contactPoint.frictionMass2 * 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; + float maxFriction = constraint.friction * contactPoint.normalImpulse; + float newImpulse1 = glm::clamp(contactPoint.frictionImpulse1 + impulse1, -maxFriction, maxFriction); + float newImpulse2 = glm::clamp(contactPoint.frictionImpulse2 + impulse2, -maxFriction, maxFriction); + impulse1 = newImpulse1 - contactPoint.frictionImpulse1; + impulse2 = newImpulse2 - contactPoint.frictionImpulse2; + contactPoint.frictionImpulse1 = newImpulse1; + contactPoint.frictionImpulse2 = newImpulse2; // Apply contact impulse glm::vec3 p = impulse1 * tangent1 + impulse2 * tangent2; - if (ent1 != constraint.entity) { p *= -1.0F; } v1 -= p * mass1.inverseMass; + w1 -= inertia1.inverseInertia * glm::cross(r1, p); v2 += p * mass2.inverseMass; + w2 += inertia2.inverseInertia * glm::cross(r2, p); } velocity1.vec = v1; + angVelocity1.vec = w1; velocity2.vec = v2; + angVelocity2.vec = w2; } } @@ -210,8 +239,9 @@ void cubos::engine::penetrationConstraintPlugin(Cubos& cubos) cubos.system("solve contacts bias") .tagged(penetrationConstraintSolveTag) .tagged(physicsSolveContactTag) - .call([](Query + .call([](Query query, const FixedDeltaTime& fixedDeltaTime, const Substeps& substeps) { solvePenetrationConstraint(query, fixedDeltaTime, substeps, true); }); @@ -219,8 +249,9 @@ void cubos::engine::penetrationConstraintPlugin(Cubos& cubos) cubos.system("solve contacts no bias") .tagged(penetrationConstraintSolveRelaxTag) .tagged(physicsSolveRelaxContactTag) - .call([](Query + .call([](Query query, const FixedDeltaTime& fixedDeltaTime, const Substeps& substeps) { solvePenetrationConstraint(query, fixedDeltaTime, substeps, false); }); @@ -230,42 +261,64 @@ void cubos::engine::penetrationConstraintPlugin(Cubos& cubos) .after(penetrationConstraintSolveRelaxTag) .before(physicsFinalizePositionTag) .tagged(fixedStepTag) - .call([](Query + .call([](Query query) { - for (auto [ent1, mass1, correction1, velocity1, constraint, ent2, mass2, correction2, velocity2] : query) + for (auto [ent1, mass1, inertia1, correction1, velocity1, angVelocity1, constraint, ent2, mass2, inertia2, + correction2, velocity2, angVelocity2] : query) { if (constraint.restitution == 0.0F) { continue; } - if (constraint.relativeVelocity > -0.01F || constraint.normalImpulse == 0.0F) - { - continue; - } + glm::vec3 v1 = velocity1.vec; + glm::vec3 v2 = velocity2.vec; + glm::vec3 w1 = angVelocity1.vec; + glm::vec3 w2 = angVelocity2.vec; - // Relative normal velocity at contact - glm::vec3 vr = velocity2.vec - velocity1.vec; - - if (ent1 != constraint.entity) + for (auto point : constraint.points) { - vr *= -1.0F; + if (point.normalSpeed > -0.01F || point.normalImpulse == 0.0F) + { + continue; + } + + glm::vec3 r1 = point.fixedAnchor1; + glm::vec3 r2 = point.fixedAnchor2; + + // Relative velocity at contact + glm::vec3 vr1 = v1 + glm::cross(w1, r1); + glm::vec3 vr2 = v2 + glm::cross(w2, r2); + glm::vec3 vr = vr2 - vr1; + if (ent1 != constraint.entity) + { + vr *= -1.0F; + } + + float vn = glm::dot(vr, constraint.normal); + + // compute normal impulse + float impulse = -point.normalMass * (vn + constraint.restitution * point.normalSpeed); + + // Clamp the accumulated impulse + float newImpulse = glm::max(point.normalImpulse + impulse, 0.0F); + impulse = newImpulse - point.normalImpulse; + point.normalImpulse = newImpulse; + + // Apply impulse + glm::vec3 p = constraint.normal * impulse; + v1 -= p * mass1.inverseMass; + w1 -= inertia1.inverseInertia * glm::cross(r1, p); + v2 += p * mass2.inverseMass; + w2 += inertia2.inverseInertia * glm::cross(r2, p); } - 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; + velocity1.vec = v1; + angVelocity1.vec = w1; + velocity2.vec = v2; + angVelocity2.vec = w2; } }); @@ -273,21 +326,92 @@ void cubos::engine::penetrationConstraintPlugin(Cubos& cubos) .tagged(addPenetrationConstraintTag) .tagged(physicsPrepareSolveTag) .call([](Commands cmds, - Query + 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, velocity1, material1, collidingWith, ent2, mass2, velocity2, material2] : query) + for (auto [ent1, mass1, inertia1, centerOfMass1, rotation1, velocity1, angVelocity1, material1, manifold, + ent2, mass2, inertia2, centerOfMass2, rotation2, velocity2, angVelocity2, material2] : query) { - float kNormal = mass1.inverseMass + mass2.inverseMass; - float normalMass = kNormal > 0.0F ? 1.0F / kNormal : 0.0F; + glm::vec3 tangent1; + glm::vec3 tangent2; + if (ent1 != manifold.entity) + { + getTangents(velocity2.vec, velocity1.vec, manifold.normal, tangent1, tangent2); + } + else + { + getTangents(velocity1.vec, velocity2.vec, manifold.normal, tangent1, tangent2); + } - // friction mass - float kFriction = mass1.inverseMass + mass2.inverseMass; - float frictionMass = kFriction > 0.0F ? 1.0F / kFriction : 0.0F; + std::vector points; + for (auto point : manifold.points) + { + auto pointData = PenetrationConstraintPointData{}; + + /// TODO: when we have warm-start change this + pointData.normalImpulse = 0.0F; + pointData.frictionImpulse1 = 0.0F; + pointData.frictionImpulse2 = 0.0F; + + pointData.localAnchor1 = point.localOn1 - centerOfMass1.vec; + pointData.localAnchor2 = point.localOn2 - centerOfMass2.vec; + + glm::vec3 r1 = rotation1.quat * pointData.localAnchor1; + glm::vec3 r2 = rotation2.quat * pointData.localAnchor2; + + pointData.fixedAnchor1 = r1; + pointData.fixedAnchor2 = r2; + + pointData.initialSeparation = -point.penetration - glm::dot(r2 - r1, manifold.normal); + + // Relative velocity at contact + glm::vec3 vr1 = velocity1.vec + glm::cross(angVelocity1.vec, r1); + glm::vec3 vr2 = velocity2.vec + glm::cross(angVelocity2.vec, r2); + glm::vec3 vr = vr2 - vr1; + if (ent1 != manifold.entity) + { + vr *= -1.0F; + } + + pointData.normalSpeed = glm::dot(vr, manifold.normal); + + // normal mass + glm::vec3 rn1 = glm::cross(r1, manifold.normal); + glm::vec3 rn2 = glm::cross(r2, manifold.normal); + float kNormal = mass1.inverseMass + mass2.inverseMass + + glm::dot(inertia1.inverseInertia * rn1, rn1) + + glm::dot(inertia2.inverseInertia * rn2, rn2); + pointData.normalMass = kNormal > 0.0F ? 1.0F / kNormal : 0.0F; + + // friction mass + glm::vec3 rt11 = glm::cross(r1, tangent1); + glm::vec3 rt12 = glm::cross(r2, tangent1); + glm::vec3 rt21 = glm::cross(r1, tangent2); + glm::vec3 rt22 = glm::cross(r2, tangent2); + + // Multiply by the inverse inertia early to reuse the values + glm::vec3 i1Rt11 = inertia1.inverseInertia * rt11; + glm::vec3 i2Rt12 = inertia2.inverseInertia * rt12; + glm::vec3 i1Rt21 = inertia1.inverseInertia * rt21; + glm::vec3 i2Rt22 = inertia2.inverseInertia * rt22; + + float kFriction1 = + mass1.inverseMass + mass2.inverseMass + glm::dot(i1Rt11, rt11) + glm::dot(i2Rt12, rt12); + float kFriction2 = + mass1.inverseMass + mass2.inverseMass + glm::dot(i1Rt21, rt21) + glm::dot(i2Rt22, rt22); + + /// TODO: these could be an array in the point + pointData.frictionMass1 = kFriction1 > 0.0F ? 1.0F / kFriction1 : 0.0F; + pointData.frictionMass2 = kFriction2 > 0.0F ? 1.0F / kFriction2 : 0.0F; + + points.push_back(pointData); + } // determine friction float friction = mixValues(material1.friction, material2.friction, @@ -296,13 +420,6 @@ void cubos::engine::penetrationConstraintPlugin(Cubos& cubos) // determine restitution float restitution = mixValues(material1.bounciness, material2.bounciness, getMixProperty(material1.bouncinessMix, material2.bouncinessMix)); - 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; @@ -314,17 +431,11 @@ void cubos::engine::penetrationConstraintPlugin(Cubos& cubos) float massCoefficient = c * impulseCoefficient; cmds.relate(ent1, ent2, - PenetrationConstraint{.entity = ent1, - .penetration = collidingWith.penetration, - .normal = collidingWith.normal, - .normalMass = normalMass, - .normalImpulse = 0.0F, + PenetrationConstraint{.entity = manifold.entity, + .normal = manifold.normal, .friction = friction, - .frictionMass = frictionMass, - .frictionImpulse1 = 0.0F, - .frictionImpulse2 = 0.0F, .restitution = restitution, - .relativeVelocity = relativeVelocity, + .points = points, .biasCoefficient = biasCoefficient, .impulseCoefficient = impulseCoefficient, .massCoefficient = massCoefficient}); @@ -342,3 +453,6 @@ void cubos::engine::penetrationConstraintPlugin(Cubos& cubos) } }); } + +// cleanup code +// change name in creation of manifold \ No newline at end of file