feat(penetration_constraint): add rotation on solving

engine/src/physics/constraints/penetration_constraint.cpp

@@ -3,22 +3,36 @@
 #include <cubos/core/ecs/reflection.hpp>
 #include <cubos/core/reflection/external/glm.hpp>
 #include <cubos/core/reflection/external/primitives.hpp>
+#include <cubos/core/reflection/external/vector.hpp>
+
+CUBOS_REFLECT_IMPL(cubos::engine::PenetrationConstraintPointData)
+{
+    return cubos::core::ecs::TypeBuilder<PenetrationConstraintPointData>(
+               "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<PenetrationConstraint>("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)

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<PenetrationConstraintPointData> points; ///< Contact points in the contact manifold.
 
         // soft constraint
         float biasCoefficient;