feat(collisions): implement contact caching for box-box collisions

core/include/cubos/core/geom/intersections.hpp

@@ -11,6 +11,7 @@
 #include <cubos/core/ecs/entity/entity.hpp>
 #include <cubos/core/geom/box.hpp>
 #include <cubos/core/geom/plane.hpp>
+#include <cubos/core/geom/polygonal_feature.hpp>
 #include <cubos/core/reflection/reflect.hpp>
 
 namespace cubos::core::geom
@@ -34,15 +35,13 @@ namespace cubos::core::geom
                                    const glm::mat4& localToWorld2, Intersection& intersect);
 
     /// @brief Performs the Sutherland-Hodgman Clipping algorithm.
-    /// @param inputPolygon The polygon to perform the clipping on.
+    /// @param polygon The polygon to perform the clipping on.
     /// @param numClipPlanes Number of cliping planes.
     /// @param clipPlanes Clipping planes
     /// @param removeNotClipToPlane Whether to remove the points if they're outside the plane.
-    /// @return The polygon resulting from the clipping.
-    CUBOS_CORE_API std::vector<glm::vec3> sutherlandHodgmanClipping(const std::vector<glm::vec3>& inputPolygon,
-                                                                    int numClipPlanes,
-                                                                    const cubos::core::geom::Plane* clipPlanes,
-                                                                    bool removeNotClipToPlane);
+    CUBOS_CORE_API void sutherlandHodgmanClipping(PolygonalFeature& polygon, int numClipPlanes,
+                                                  const cubos::core::geom::Plane* clipPlanes,
+                                                  bool removeNotClipToPlane);
 
     /// @brief Compute the intersection between a plane and an edge.
     /// @param plane The plane.

core/include/cubos/core/geom/polygonal_feature.hpp

@@ -0,0 +1,22 @@
+/// @file
+/// @brief Class @ref cubos::core::geom::PolygonalFeature.
+/// @ingroup core-geom
+
+#pragma once
+
+#include <vector>
+
+#include <glm/vec3.hpp>
+
+namespace cubos::core::geom
+{
+    /// @brief Represents a polygonal feature. Used internally for manifold computation.
+    /// @ingroup core-geom
+    struct PolygonalFeature
+    {
+        uint32_t faceId;
+        std::vector<uint32_t> vertexIds;
+        std::vector<glm::vec3> vertices;
+        glm::vec3 normal;
+    };
+} // namespace cubos::core::geom

core/include/cubos/core/geom/utils.hpp

@@ -11,6 +11,7 @@
 #include <cubos/core/ecs/entity/entity.hpp>
 #include <cubos/core/geom/box.hpp>
 #include <cubos/core/geom/plane.hpp>
+#include <cubos/core/geom/polygonal_feature.hpp>
 #include <cubos/core/reflection/reflect.hpp>
 
 namespace cubos::core::geom
@@ -31,14 +32,15 @@ namespace cubos::core::geom
     /// @brief Computes the candidate face of the polygon to be reference, as well as it's normal and adjacent planes.
     /// @param shape Collider shape.
     /// @param normal The reference normal in world coordinates.
-    /// @param outPoints The points of the resulting face.
-    /// @param outNormal The resulting normal.
+    /// @param outPolygon The polygon of the resulting face.
     /// @param outAdjacentPlanes The resulting adjacent planes (4).
+    /// @param outAdjacentPlanesIds The resulting adjacent planes ids (4).
     /// @param localToWorld The localToWorld matrix of the body.
     /// @param scale The scale of the body.
     CUBOS_CORE_API void getIncidentReferencePolygon(const cubos::core::geom::Box& shape, const glm::vec3& normal,
-                                                    std::vector<glm::vec3>& outPoints, glm::vec3& outNormal,
+                                                    PolygonalFeature& outPolygon,
                                                     std::vector<cubos::core::geom::Plane>& outAdjacentPlanes,
+                                                    std::vector<uint32_t> outAdjacentPlanesIds,
                                                     const glm::mat4& localToWorld, float scale);
 
     /// @brief Computes the closest point on the line (edge) to point.

core/src/geom/intersections.cpp

@@ -150,23 +150,26 @@ bool cubos::core::geom::intersects(const Box& box1, const glm::mat4& localToWorl
     return true;
 }
 
-std::vector<glm::vec3> cubos::core::geom::sutherlandHodgmanClipping(const std::vector<glm::vec3>& inputPolygon,
-                                                                    int numClipPlanes,
-                                                                    const cubos::core::geom::Plane* clipPlanes,
-                                                                    bool removeNotClipToPlane)
+void cubos::core::geom::sutherlandHodgmanClipping(PolygonalFeature& polygon, int numClipPlanes,
+                                                  const cubos::core::geom::Plane* clipPlanes, bool removeNotClipToPlane)
 {
     if (numClipPlanes == 0)
     {
-        return inputPolygon;
+        return;
     }
 
     // Create temporary list of vertices
     std::vector<glm::vec3> tempPolygon1;
+    std::vector<uint32_t> tempPolygonIds1;
     std::vector<glm::vec3> tempPolygon2;
+    std::vector<uint32_t> tempPolygonIds2;
     std::vector<glm::vec3>* input = &tempPolygon1;
+    std::vector<uint32_t>* inputIds = &tempPolygonIds1;
     std::vector<glm::vec3>* output = &tempPolygon2;
+    std::vector<uint32_t>* outputIds = &tempPolygonIds2;
 
-    *input = inputPolygon;
+    *input = polygon.vertices;
+    *inputIds = polygon.vertexIds;
 
     // Iterate over each clip plane
     for (int i = 0; i < numClipPlanes; i++)
@@ -181,8 +184,9 @@ std::vector<glm::vec3> cubos::core::geom::sutherlandHodgmanClipping(const std::v
 
         glm::vec3 tempPoint;
         glm::vec3 startPoint = input->back();
-        for (const glm::vec3& endPoint : *input)
+        for (int p = 0; p < input->size(); p++)
         {
+            const glm::vec3& endPoint = (*input)[p];
             bool startInPlane = pointInPlane(startPoint, plane);
             bool endInPlane = pointInPlane(endPoint, plane);
 
@@ -191,29 +195,34 @@ std::vector<glm::vec3> cubos::core::geom::sutherlandHodgmanClipping(const std::v
                 if (endInPlane)
                 {
                     output->push_back(endPoint);
+                    outputIds->push_back((*inputIds)[p]);
                 }
             }
             else
             {
                 if (startInPlane && endInPlane)
                 {
                     output->push_back(endPoint);
+                    outputIds->push_back((*inputIds)[p]);
                 }
                 else if (startInPlane && !endInPlane)
                 {
                     if (planeEdgeIntersection(plane, startPoint, endPoint, tempPoint))
                     {
                         output->push_back(tempPoint);
+                        outputIds->push_back((*inputIds)[p]);
                     }
                 }
                 else if (!startInPlane && endInPlane)
                 {
                     if (planeEdgeIntersection(plane, startPoint, endPoint, tempPoint))
                     {
                         output->push_back(tempPoint);
+                        outputIds->push_back((*inputIds)[p]);
                     }
 
                     output->push_back(endPoint);
+                    outputIds->push_back((*inputIds)[p]);
                 }
             }
 
@@ -223,9 +232,13 @@ std::vector<glm::vec3> cubos::core::geom::sutherlandHodgmanClipping(const std::v
         // Swap input/output polygons, and clear output list to generate next
         std::swap(input, output);
         output->clear();
+
+        std::swap(inputIds, outputIds);
+        outputIds->clear();
     }
 
-    return *input;
+    polygon.vertices = *input;
+    polygon.vertexIds = *inputIds;
 }
 
 bool cubos::core::geom::planeEdgeIntersection(const cubos::core::geom::Plane& plane, const glm::vec3& start,

core/src/geom/utils.cpp

@@ -4,6 +4,7 @@
 #include <glm/glm.hpp>
 #include <glm/mat3x3.hpp>
 
+#include <cubos/core/geom/intersections.hpp>
 #include <cubos/core/geom/utils.hpp>
 
 bool cubos::core::geom::pointInPlane(const glm::vec3& point, const cubos::core::geom::Plane& plane)
@@ -32,8 +33,9 @@ int cubos::core::geom::getMaxVertexInAxis(const int numVertices, const glm::vec3
 }
 
 void cubos::core::geom::getIncidentReferencePolygon(const cubos::core::geom::Box& shape, const glm::vec3& normal,
-                                                    std::vector<glm::vec3>& outPoints, glm::vec3& outNormal,
+                                                    PolygonalFeature& outPolygon,
                                                     std::vector<cubos::core::geom::Plane>& outAdjacentPlanes,
+                                                    std::vector<uint32_t> outAdjacentPlanesIds,
                                                     const glm::mat4& localToWorld, float scale)
 {
     glm::mat3 m = glm::mat3(localToWorld) / scale;
@@ -71,7 +73,9 @@ void cubos::core::geom::getIncidentReferencePolygon(const cubos::core::geom::Box
     }
 
     // Output face normal
-    outNormal = glm::normalize(normalMatrix * faceNormals[bestFaceIndex]);
+    outPolygon.normal = glm::normalize(normalMatrix * faceNormals[bestFaceIndex]);
+    // Output face id
+    outPolygon.faceId = (uint32_t)bestFaceIndex;
 
     glm::ivec4 faces[6];
     cubos::core::geom::Box::faces(faces);
@@ -80,7 +84,8 @@ void cubos::core::geom::getIncidentReferencePolygon(const cubos::core::geom::Box
     for (int i = 0; i < 4; i++)
     {
         int vertexIndex = faces[bestFaceIndex][i];
-        outPoints.emplace_back(localToWorld * glm::vec4(vertices[vertexIndex], 1.0F));
+        outPolygon.vertices.emplace_back(localToWorld * glm::vec4(vertices[vertexIndex], 1.0F));
+        outPolygon.vertexIds.emplace_back(vertexIndex);
     }
 
     // Loop over all adjacent faces and output a clip plane for each.
@@ -109,6 +114,7 @@ void cubos::core::geom::getIncidentReferencePolygon(const cubos::core::geom::Box
                 plane.normal = planeNormal;
                 plane.d = planeDistance;
                 outAdjacentPlanes.push_back(plane);
+                outAdjacentPlanesIds.emplace_back(faceIndex);
             }
         }
     }

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

@@ -15,6 +15,91 @@
 
 namespace cubos::engine
 {
+    /// @brief Indicates the type of a geometric feature (vertex, edge, or face) and the feature index.
+    struct FeatureId
+    {
+        CUBOS_REFLECT;
+
+        /// Feature id identifying an undefined feature.
+        static const uint32_t UNDEFINED = 0x0;
+
+        /// Container of the current feature Id.
+        uint32_t id = UNDEFINED;
+
+    public:
+        /// @brief Assign a vertex feature id.
+        /// @param vertexId index of the vertex on the shape.
+        void setAsVertex(uint32_t vertexId)
+        {
+            if ((vertexId & TYPE_MASK) != 0)
+            {
+                return;
+            }
+            id = TYPE_VERTEX | vertexId;
+        }
+
+        /// @brief Assign an edge feature id.
+        /// @param edgeId index of the edge on the shape.
+        void setAsEdge(uint32_t edgeId)
+        {
+            if ((edgeId & TYPE_MASK) != 0)
+            {
+                return;
+            }
+            id = TYPE_EDGE | edgeId;
+        }
+
+        /// @brief Assign a face feature id.
+        /// @param faceId index of the face on the shape.
+        void setAsFace(uint32_t faceId)
+        {
+            if ((faceId & TYPE_MASK) != 0)
+            {
+                return;
+            }
+            id = TYPE_FACE | faceId;
+        }
+
+        /// @brief Check if the feature type is a vertex.
+        /// @return true if it identifies a vertex.
+        inline bool isVertex()
+        {
+            return (id & TYPE_MASK) == TYPE_VERTEX;
+        }
+
+        /// @brief Check if the feature type is an edge.
+        /// @return true if it identifies an edge.
+        inline bool isEdge()
+        {
+            return (id & TYPE_MASK) == TYPE_EDGE;
+        }
+
+        /// @brief Check if the feature type is a face.
+        /// @return true if it identifies a face.
+        inline bool isFace()
+        {
+            return (id & TYPE_MASK) == TYPE_FACE;
+        }
+
+        inline constexpr bool operator==(const FeatureId& other) const
+        {
+            return id != UNDEFINED && other.id != UNDEFINED && id == other.id;
+        }
+
+    private:
+        /// TODO: reduce size of the id to less bits?
+        /// For now our id has 32 bits (2 bits for the feature type, 30 for the feature index). If we need to change it
+        /// in the future change the CODE_MASK and the each of the types' header to the adequate size.
+        /// Identifies the part of the id that corresponds to the feature index. 0011'1111'1111'1111'1111'1111'1111'1111
+        static const std::uint32_t ID_MASK = 0x3fffffff;
+        /// Identifies the part of the id that corresponds to the feature type. 1100'0000'0000'0000'0000'0000'0000'0000
+        static const std::uint32_t TYPE_MASK = !ID_MASK;
+        /// The index corresponding to each feature type.
+        static const std::uint32_t TYPE_VERTEX = 0x40000000; // 0b01 << 30
+        static const std::uint32_t TYPE_EDGE = 0x80000000;   // 0b10 << 30
+        static const std::uint32_t TYPE_FACE = 0xC0000000;   // 0b11 << 30
+    };
+
     /// @brief Contains info regarding a contact point of a @ContactManifold.
     struct ContactPointData
     {
@@ -26,9 +111,14 @@ namespace cubos::engine
         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.
+
+        float normalImpulse;
+        float frictionImpulse1;
+        float frictionImpulse2;
         /// 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
+        FeatureId fid1;
+        FeatureId fid2;
     };
 
     /// @brief Represents a contact interface between two bodies.

engine/samples/collisions/main.cpp

@@ -249,11 +249,13 @@ int main(int argc, char** argv)
                 {
                     for (auto point : manifold.points)
                     {
-                        gizmos.color({0.0F, 0.0F, 1.0F});
+                        // Have a set color for each of the entities for an easier distinction
+                        bool isEnt1 = ent1 == manifold.entity ? true : false;
+                        gizmos.color((isEnt1 ? glm::vec3(0.0F, 0.0F, 1.0F) : glm::vec3(1.0F, 0.0F, 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.color((isEnt1 ? glm::vec3(1.0F, 0.0F, 1.0F) : glm::vec3(0.0F, 0.0F, 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/src/collisions/interface/contact_manifold.cpp

@@ -5,6 +5,11 @@
 
 #include <cubos/engine/collisions/contact_manifold.hpp>
 
+CUBOS_REFLECT_IMPL(cubos::engine::FeatureId)
+{
+    return core::ecs::TypeBuilder<FeatureId>("cubos::engine::ContactPointData").withField("id", &FeatureId::id).build();
+}
+
 CUBOS_REFLECT_IMPL(cubos::engine::ContactPointData)
 {
     return core::ecs::TypeBuilder<ContactPointData>("cubos::engine::ContactPointData")
@@ -14,7 +19,11 @@ CUBOS_REFLECT_IMPL(cubos::engine::ContactPointData)
         .withField("localOn1", &ContactPointData::localOn1)
         .withField("localOn2", &ContactPointData::localOn2)
         .withField("penetration", &ContactPointData::penetration)
-        .withField("id", &ContactPointData::id)
+        .withField("normalImpulse", &ContactPointData::normalImpulse)
+        .withField("frictionImpulse1", &ContactPointData::frictionImpulse1)
+        .withField("frictionImpulse2", &ContactPointData::frictionImpulse2)
+        .withField("fid1", &ContactPointData::fid1)
+        .withField("fid2", &ContactPointData::fid2)
         .build();
 }