Add rotation on constraint solving

CHANGELOG.md

@@ -21,6 +21,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 - Print stacktrace with *cpptrace* on calls to CUBOS_FAIL (#1172, **@RiscadoA**).
 - Orthographic Camera component (#1182, **@mkuritsu**).
 - Importer plugin (#1299, **@Scarface1809**).
+- Handle body rotation on penetration solving (#1272, **&fallenatlas**).
 
 ### Changed
 

engine/include/cubos/engine/collisions/contact_manifold.hpp

@@ -21,10 +21,11 @@ namespace cubos::engine
         CUBOS_REFLECT;
 
         cubos::core::ecs::Entity entity; ///< Entity to which the normal is relative to.
-        glm::vec3 position1;             ///< Position on the entity of the contact in global coordinates.
-        glm::vec3 position2;             ///< Position on the other entity of the contact in global coordinates.
+        glm::vec3 globalOn1;             ///< Position on the entity of the contact in global coordinates.
+        glm::vec3 globalOn2;             ///< Position on the other entity of the contact in global coordinates.
+        glm::vec3 localOn1;              ///< Position on the entity of the contact in local coordinates.
+        glm::vec3 localOn2;              ///< Position on the other entity of the contact in local coordinates.
         float penetration;               ///< Penetration of the contact point. Always positive.
-        /// TODO: normal and tangent impulse?
         /// The contact feature ID on the first shape. This indicates the ID of
         /// the vertex, edge, or face of the contact, if one can be determined.
         int id; /// TODO: use this

engine/include/cubos/engine/physics/physics_bundle.hpp

@@ -28,6 +28,7 @@ namespace cubos::engine
         glm::vec3 impulse = {0.0F, 0.0F, 0.0F};
         glm::vec3 angularImpulse = {0.0F, 0.0F, 0.0F};
         PhysicsMaterial material = PhysicsMaterial{};
+        glm::vec3 centerOfMass = {0.0F, 0.0F, 0.0F};
         glm::mat3 inertiaTensor = glm::mat3(0.0F); // Temporary value. We use this value to check if the inertia tensor
                                                    // is to be calculated automatically.
     };

engine/samples/collisions/main.cpp

@@ -237,7 +237,7 @@ int main(int argc, char** argv)
                     for (const cubos::engine::ContactPointData& end : manifold.points)
                     {
                         gizmos.color({1.0F, 1.0F, 0.0F});
-                        gizmos.drawArrow("line", start.position1, end.position1 - start.position1, 0.01F, 0.05F, 1.0F,
+                        gizmos.drawArrow("line", start.globalOn1, end.globalOn1 - start.globalOn1, 0.01F, 0.05F, 1.0F,
                                          0.05F, Gizmos::Space::World);
                         start = end;
                     }
@@ -248,11 +248,11 @@ int main(int argc, char** argv)
                     for (auto point : manifold.points)
                     {
                         gizmos.color({0.0F, 0.0F, 1.0F});
-                        gizmos.drawArrow("point", point.position1, glm::vec3(0.02F, 0.02F, 0.02F), 0.03F, 0.05F, 1.0F,
+                        gizmos.drawArrow("point", point.globalOn1, glm::vec3(0.02F, 0.02F, 0.02F), 0.03F, 0.05F, 1.0F,
                                          0.05F, Gizmos::Space::World);
 
                         gizmos.color({1.0F, 0.0F, 1.0F});
-                        gizmos.drawArrow("point", point.position2, glm::vec3(0.02F, 0.02F, 0.02F), 0.03F, 0.05F, 1.0F,
+                        gizmos.drawArrow("point", point.globalOn2, glm::vec3(0.02F, 0.02F, 0.02F), 0.03F, 0.05F, 1.0F,
                                          0.05F, Gizmos::Space::World);
                     }
                 }

engine/samples/complex_physics/assets/scenes/red_cube.cubos

@@ -62,8 +62,8 @@
         "z": 0.0
       },
       "cubos::engine::PhysicsMaterial": {
-        "friction": 0.02,
-        "bounciness": 1.0,
+        "friction": 0.5,
+        "bounciness": 0.5,
         "frictionMix": "Average",
         "bouncinessMix": "Average"
       },

engine/samples/complex_physics/assets/scenes/white_cube.cubos

@@ -62,8 +62,8 @@
         "z": 0.0
       },
       "cubos::engine::PhysicsMaterial": {
-        "friction": 0.02,
-        "bounciness": 1.0,
+        "friction": 0.5,
+        "bounciness": 0.5,
         "frictionMix": "Average",
         "bouncinessMix": "Average"
       },

engine/samples/complex_physics/main.cpp

@@ -114,12 +114,18 @@ int main(int argc, char** argv)
             .add(BoxCollisionShape{cubos::core::geom::Box{.halfSize = glm::vec3{20.0F, 0.5F, 20.0F}}})
             .add(LocalToWorld{})
             .add(Position{{0.0F, 0.0F, 0.0F}})
+            .add(Rotation{})
             .add(Velocity{.vec = {0.0F, 0.0F, 0.0F}})
+            .add(AngularVelocity{})
             .add(Force{})
+            .add(Torque{})
             .add(Impulse{})
+            .add(AngularImpulse{})
             .add(Mass{.mass = 1.0F, .inverseMass = 0.0F})
+            .add(CenterOfMass{})
             .add(AccumulatedCorrection{{0.0F, 0.0F, 0.0F}})
-            .add(PhysicsMaterial{});
+            .add(Inertia{.inertia = glm::mat3(0.0F), .inverseInertia = glm::mat3(0.0F), .autoUpdate = true})
+            .add(PhysicsMaterial{.friction = 0.1F});
 
         auto redCube = assets.read(RedCubeSceneAsset);
         auto whiteCube = assets.read(WhiteCubeSceneAsset);

engine/src/collisions/interface/contact_manifold.cpp

@@ -9,8 +9,10 @@ CUBOS_REFLECT_IMPL(cubos::engine::ContactPointData)
 {
     return core::ecs::TypeBuilder<ContactPointData>("cubos::engine::ContactPointData")
         .withField("entity", &ContactPointData::entity)
-        .withField("position1", &ContactPointData::position1)
-        .withField("position2", &ContactPointData::position2)
+        .withField("globalOn1", &ContactPointData::globalOn1)
+        .withField("globalOn2", &ContactPointData::globalOn2)
+        .withField("localOn1", &ContactPointData::localOn1)
+        .withField("localOn2", &ContactPointData::localOn2)
         .withField("penetration", &ContactPointData::penetration)
         .withField("id", &ContactPointData::id)
         .build();

engine/src/collisions/narrow_phase/plugin.cpp

@@ -91,30 +91,39 @@
     cubos.system("collision manifolds")
         .tagged(collisionsManifoldTag)
         .after(collisionsNarrowTag)
-        .call([](Commands cmds,
-                 Query<Entity, const Position&, const LocalToWorld&, const BoxCollisionShape&, CollidingWith&, Entity,
-                       const Position&, const LocalToWorld&, const BoxCollisionShape&>
-                     query) {
-            for (auto [ent1, position1, localToWorld1, boxShape1, collidingWith, ent2, position2, localToWorld2,
-                       boxShape2] : query)
+        .call([](Commands cmds, Query<Entity, const LocalToWorld&, const BoxCollisionShape&, CollidingWith&, Entity,
+                                      const LocalToWorld&, const BoxCollisionShape&>
+                                    query) {
+            for (auto [ent1, localToWorld1, boxShape1, collidingWith, ent2, localToWorld2, boxShape2] : query)
             {
                 // If penetration not bigger than 0 continue
                 if (collidingWith.penetration < 0)
                 {
                     continue;
                 }
 
+                // Make sure that shape1 corresponds to the entity refered to in collidingWith
+                const cubos::core::geom::Box* matchedShape1 = &boxShape1.box;
+                const cubos::core::geom::Box* matchedShape2 = &boxShape2.box;
+                const LocalToWorld* matchedLocalToWorld1 = &localToWorld1;
+                const LocalToWorld* matchedLocalToWorld2 = &localToWorld2;
+                if (ent1 != collidingWith.entity)
+                {
+                    std::swap(matchedShape1, matchedShape2);
+                    std::swap(matchedLocalToWorld1, matchedLocalToWorld2);
+                }
+
                 // Calculate incident and reference face
                 std::vector<glm::vec3> polygon1;
                 std::vector<glm::vec3> polygon2;
                 glm::vec3 normal1;
                 glm::vec3 normal2;
                 std::vector<cubos::core::geom::Plane> adjPlanes1;
                 std::vector<cubos::core::geom::Plane> adjPlanes2;
-                getIncidentReferencePolygon(boxShape1.box, collidingWith.normal, polygon1, normal1, adjPlanes1,
-                                            localToWorld1.mat, localToWorld1.worldScale());
-                getIncidentReferencePolygon(boxShape2.box, -collidingWith.normal, polygon2, normal2, adjPlanes2,
-                                            localToWorld2.mat, localToWorld2.worldScale());
+                getIncidentReferencePolygon(*matchedShape1, collidingWith.normal, polygon1, normal1, adjPlanes1,
+                                            matchedLocalToWorld1->mat, matchedLocalToWorld1->worldScale());
+                getIncidentReferencePolygon(*matchedShape2, -collidingWith.normal, polygon2, normal2, adjPlanes2,
+                                            matchedLocalToWorld2->mat, matchedLocalToWorld2->worldScale());
 
                 // Each face will always have more than 1 point so we proceed to clipping
                 // See which one of the normals is the reference one by checking which has the highest dot product
@@ -147,36 +156,41 @@
                     float contactPenetration = -glm::dot(pointDiff, collidingWith.normal); // is this positive
 
                     // Set Contact data
-                    /// TODO: we need to pass these to local space for solving (probably)
-                    glm::vec3 globalOnA = point;                                             // world coordinates
-                    glm::vec3 globalOnB = point + collidingWith.normal * contactPenetration; // world coordinates
+                    glm::vec3 globalOn1 = point;                                             // world coordinates
+                    glm::vec3 globalOn2 = point + collidingWith.normal * contactPenetration; // world coordinates
 
                     // If we flipped incident and reference planes, we will
                     // need to flip it back before sending it to the manifold.
                     if (flipped)
                     {
                         contactPenetration = -contactPenetration;
-                        globalOnA = point - collidingWith.normal * contactPenetration;
-                        globalOnB = point;
+                        globalOn1 = point - collidingWith.normal * contactPenetration;
+                        globalOn2 = point;
                     }
 
+                    glm::vec3 localOn1 = glm::vec3(matchedLocalToWorld1->inverse() * glm::vec4(globalOn1, 1.0F));
+                    glm::vec3 localOn2 = glm::vec3(matchedLocalToWorld2->inverse() * glm::vec4(globalOn2, 1.0F));
+
                     // Just make a final sanity check that the contact point
                     // is actual a point of contact not just a clipping bug
                     // and consider only points with positive penetration
                     if (contactPenetration >= 0.0F)
                     {
-                        auto contact = ContactPointData{.entity = ent1,
-                                                        .position1 = globalOnA,
-                                                        .position2 = globalOnB,
+                        auto contact = ContactPointData{.entity = collidingWith.entity,
+                                                        .globalOn1 = globalOn1,
+                                                        .globalOn2 = globalOn2,
+                                                        .localOn1 = localOn1,
+                                                        .localOn2 = localOn2,
                                                         .penetration = collidingWith.penetration,
                                                         .id = 0};
 
                         points.push_back(contact);
                     }
                 }
 
-                cmds.relate(ent1, ent2,
-                            ContactManifold{.entity = ent1, .normal = collidingWith.normal, .points = points});
+                cmds.relate(
+                    ent1, ent2,
+                    ContactManifold{.entity = collidingWith.entity, .normal = collidingWith.normal, .points = points});
             }
         });
 }

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;

engine/src/physics/plugin.cpp

@@ -112,6 +112,7 @@
         .withField("impulse", &PhysicsBundle::impulse)
         .withField("angularImpulse", &PhysicsBundle::angularImpulse)
         .withField("material", &PhysicsBundle::material)
+        .withField("centerOfMass", &PhysicsBundle::centerOfMass)
         .withField("inertiaTensor", &PhysicsBundle::inertiaTensor)
         .build();
 }
@@ -179,6 +180,7 @@
                 angularImpulse.add(bundle.angularImpulse);
 
                 cmds.add(ent, Mass{.mass = bundle.mass, .inverseMass = 1.0F / bundle.mass});
+                cmds.add(ent, CenterOfMass{.vec = bundle.centerOfMass});
                 if (bundle.inertiaTensor != glm::mat3(0.0F))
                 {
                     cmds.add(ent, Inertia{.inertia = bundle.inertiaTensor,