diff --git a/core/include/cubos/core/geom/intersections.hpp b/core/include/cubos/core/geom/intersections.hpp index 92305270d0..fb499b8aa4 100644 --- a/core/include/cubos/core/geom/intersections.hpp +++ b/core/include/cubos/core/geom/intersections.hpp @@ -11,6 +11,7 @@ #include #include #include +#include #include 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 sutherlandHodgmanClipping(const std::vector& 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. diff --git a/core/include/cubos/core/geom/polygonal_feature.hpp b/core/include/cubos/core/geom/polygonal_feature.hpp new file mode 100644 index 0000000000..e6f797491f --- /dev/null +++ b/core/include/cubos/core/geom/polygonal_feature.hpp @@ -0,0 +1,22 @@ +/// @file +/// @brief Struct @ref cubos::core::geom::PolygonalFeature. +/// @ingroup core-geom + +#pragma once + +#include + +#include + +namespace cubos::core::geom +{ + /// @brief Represents a polygonal feature. Used internally for manifold computation. + /// @ingroup core-geom + struct PolygonalFeature + { + uint32_t faceId; + std::vector vertexIds; + std::vector vertices; + glm::vec3 normal; + }; +} // namespace cubos::core::geom diff --git a/core/include/cubos/core/geom/utils.hpp b/core/include/cubos/core/geom/utils.hpp index a8b8da8049..ff3b892d36 100644 --- a/core/include/cubos/core/geom/utils.hpp +++ b/core/include/cubos/core/geom/utils.hpp @@ -11,6 +11,7 @@ #include #include #include +#include #include 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& outPoints, glm::vec3& outNormal, + PolygonalFeature& outPolygon, std::vector& outAdjacentPlanes, + std::vector outAdjacentPlanesIds, const glm::mat4& localToWorld, float scale); /// @brief Computes the closest point on the line (edge) to point. diff --git a/core/src/geom/intersections.cpp b/core/src/geom/intersections.cpp index 06a00c6d54..2c7baa75a6 100644 --- a/core/src/geom/intersections.cpp +++ b/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 cubos::core::geom::sutherlandHodgmanClipping(const std::vector& 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 tempPolygon1; + std::vector tempPolygonIds1; std::vector tempPolygon2; + std::vector tempPolygonIds2; std::vector* input = &tempPolygon1; + std::vector* inputIds = &tempPolygonIds1; std::vector* output = &tempPolygon2; + std::vector* 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 cubos::core::geom::sutherlandHodgmanClipping(const std::v glm::vec3 tempPoint; glm::vec3 startPoint = input->back(); - for (const glm::vec3& endPoint : *input) + for (size_t p = 0; p < input->size(); p++) { + const glm::vec3& endPoint = (*input)[p]; bool startInPlane = pointInPlane(startPoint, plane); bool endInPlane = pointInPlane(endPoint, plane); @@ -191,6 +195,7 @@ std::vector cubos::core::geom::sutherlandHodgmanClipping(const std::v if (endInPlane) { output->push_back(endPoint); + outputIds->push_back((*inputIds)[p]); } } else @@ -198,12 +203,14 @@ std::vector cubos::core::geom::sutherlandHodgmanClipping(const std::v 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) @@ -211,9 +218,11 @@ std::vector cubos::core::geom::sutherlandHodgmanClipping(const std::v 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 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, diff --git a/core/src/geom/utils.cpp b/core/src/geom/utils.cpp index 1ee12d3b80..f13b9d383c 100644 --- a/core/src/geom/utils.cpp +++ b/core/src/geom/utils.cpp @@ -5,6 +5,7 @@ #include #include +#include #include bool cubos::core::geom::pointInPlane(const glm::vec3& point, const cubos::core::geom::Plane& plane) @@ -33,8 +34,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& outPoints, glm::vec3& outNormal, + PolygonalFeature& outPolygon, std::vector& outAdjacentPlanes, + std::vector outAdjacentPlanesIds, const glm::mat4& localToWorld, float scale) { glm::mat3 m = glm::mat3(localToWorld) / scale; @@ -72,7 +74,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); @@ -81,7 +85,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. @@ -110,6 +115,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); } } } diff --git a/engine/include/cubos/engine/collisions/contact_manifold.hpp b/engine/include/cubos/engine/collisions/contact_manifold.hpp index e3b82ea9dd..027adf62a4 100644 --- a/engine/include/cubos/engine/collisions/contact_manifold.hpp +++ b/engine/include/cubos/engine/collisions/contact_manifold.hpp @@ -15,8 +15,94 @@ namespace cubos::engine { + /// @brief Indicates the type of a geometric feature (vertex, edge, or face) and the feature index. Currently each + /// feature index is simply the index in the array with that information in the shape. + class CUBOS_ENGINE_API ContactFeatureId + { + public: + CUBOS_REFLECT; + + /// @brief Feature id identifying an undefined feature. + static const uint32_t UNDEFINED = 0x0; + + /// @brief Container of the current feature Id. + uint32_t id = UNDEFINED; + + /// @brief Assign a vertex feature id. + /// @param vertexId index of the vertex on the shape. + void setAsVertex(uint32_t vertexId) + { + if ((vertexId & TypeMask) != 0) + { + return; + } + id = TypeVertex | vertexId; + } + + /// @brief Assign an edge feature id. + /// @param edgeId index of the edge on the shape. + void setAsEdge(uint32_t edgeId) + { + if ((edgeId & TypeMask) != 0) + { + return; + } + id = TypeEdge | edgeId; + } + + /// @brief Assign a face feature id. + /// @param faceId index of the face on the shape. + void setAsFace(uint32_t faceId) + { + if ((faceId & TypeMask) != 0) + { + return; + } + id = TypeFace | faceId; + } + + /// @brief Check if the feature type is a vertex. + /// @return true if it identifies a vertex. + inline bool isVertex() const + { + return (id & TypeMask) == TypeVertex; + } + + /// @brief Check if the feature type is an edge. + /// @return true if it identifies an edge. + inline bool isEdge() const + { + return (id & TypeMask) == TypeEdge; + } + + /// @brief Check if the feature type is a face. + /// @return true if it identifies a face. + inline bool isFace() const + { + return (id & TypeMask) == TypeFace; + } + + inline constexpr bool operator==(const ContactFeatureId& 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 IdMask = 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 TypeMask = ~IdMask; + /// The index corresponding to each feature type. + static const std::uint32_t TypeVertex = 0x40000000; // 0b01 << 30 + static const std::uint32_t TypeEdge = 0x80000000; // 0b10 << 30 + static const std::uint32_t TypeFace = 0xC0000000; // 0b11 << 30 + }; + /// @brief Contains info regarding a contact point of a @ContactManifold. - struct ContactPointData + struct CUBOS_ENGINE_API ContactPointData { CUBOS_REFLECT; @@ -26,13 +112,18 @@ 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 + ContactFeatureId fid1; + ContactFeatureId fid2; }; /// @brief Represents a contact interface between two bodies. - struct ContactManifold + struct CUBOS_ENGINE_API ContactManifold { CUBOS_REFLECT; diff --git a/engine/samples/collisions/main.cpp b/engine/samples/collisions/main.cpp index 7f6b0f3edc..1141c4d3ed 100644 --- a/engine/samples/collisions/main.cpp +++ b/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; + 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); } diff --git a/engine/src/collisions/interface/contact_manifold.cpp b/engine/src/collisions/interface/contact_manifold.cpp index a7f09f7228..48b785f8d7 100644 --- a/engine/src/collisions/interface/contact_manifold.cpp +++ b/engine/src/collisions/interface/contact_manifold.cpp @@ -5,6 +5,13 @@ #include +CUBOS_REFLECT_IMPL(cubos::engine::ContactFeatureId) +{ + return core::ecs::TypeBuilder("cubos::engine::ContactFeatureId") + .withField("id", &ContactFeatureId::id) + .build(); +} + CUBOS_REFLECT_IMPL(cubos::engine::ContactPointData) { return core::ecs::TypeBuilder("cubos::engine::ContactPointData") @@ -14,7 +21,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(); } diff --git a/engine/src/collisions/narrow_phase/plugin.cpp b/engine/src/collisions/narrow_phase/plugin.cpp index fd576d5a97..fce03749a6 100644 --- a/engine/src/collisions/narrow_phase/plugin.cpp +++ b/engine/src/collisions/narrow_phase/plugin.cpp @@ -22,7 +22,9 @@ CUBOS_DEFINE_TAG(cubos::engine::collisionsNarrowCleanTag); CUBOS_DEFINE_TAG(cubos::engine::collisionsNarrowTag); CUBOS_DEFINE_TAG(cubos::engine::collisionsManifoldTag); +using cubos::core::geom::PolygonalFeature; using cubos::engine::CollidingWith; +using cubos::engine::ContactFeatureId; using cubos::engine::ContactPointData; using cubos::engine::LocalToWorld; @@ -33,56 +35,63 @@ std::vector computeContactPoints(const cubos::core::geom::Box* CollidingWith& collidingWith) { // Calculate incident and reference face - std::vector polygon1; - std::vector polygon2; - glm::vec3 normal1; - glm::vec3 normal2; + PolygonalFeature face1; + PolygonalFeature face2; + std::vector adjPlanes1Ids; + std::vector adjPlanes2Ids; std::vector adjPlanes1; std::vector adjPlanes2; - getIncidentReferencePolygon(*matchedShape1, collidingWith.normal, polygon1, normal1, adjPlanes1, + getIncidentReferencePolygon(*matchedShape1, collidingWith.normal, face1, adjPlanes1, adjPlanes1Ids, matchedLocalToWorld1->mat, matchedLocalToWorld1->worldScale()); - getIncidentReferencePolygon(*matchedShape2, -collidingWith.normal, polygon2, normal2, adjPlanes2, + getIncidentReferencePolygon(*matchedShape2, -collidingWith.normal, face2, adjPlanes2, adjPlanes2Ids, 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 - bool flipped = fabs(glm::dot(collidingWith.normal, normal1)) < fabs(glm::dot(collidingWith.normal, normal2)); + bool flipped = + fabs(glm::dot(collidingWith.normal, face1.normal)) < fabs(glm::dot(collidingWith.normal, face2.normal)); if (flipped) { - std::swap(polygon1, polygon2); - std::swap(normal1, normal2); + std::swap(face1, face2); std::swap(adjPlanes1, adjPlanes2); + std::swap(adjPlanes1Ids, adjPlanes2Ids); } // Clip the incident face to the adjacent edges of the reference face - polygon2 = cubos::core::geom::sutherlandHodgmanClipping(polygon2, (int)adjPlanes1.size(), adjPlanes1.data(), false); + cubos::core::geom::sutherlandHodgmanClipping(face2, (int)adjPlanes1.size(), adjPlanes1.data(), false); // Finally clip (and remove) any contact points that are above the reference face cubos::core::geom::Plane refPlane = - cubos::core::geom::Plane{.normal = -normal1, .d = -glm::dot(-normal1, polygon1.front())}; - polygon2 = cubos::core::geom::sutherlandHodgmanClipping(polygon2, 1, &refPlane, true); + cubos::core::geom::Plane{.normal = -face1.normal, .d = -glm::dot(-face1.normal, face1.vertices.front())}; + cubos::core::geom::sutherlandHodgmanClipping(face2, 1, &refPlane, true); // Use the remaining contact point on the manifold std::vector points; - for (const glm::vec3& point : polygon2) + for (size_t i = 0; i < face2.vertices.size(); i++) { // Compute distance to reference plane - glm::vec3 pointDiff = point - cubos::core::geom::getClosestPointPolygon(point, polygon1); + glm::vec3 pointDiff = + face2.vertices[i] - cubos::core::geom::getClosestPointPolygon(face2.vertices[i], face1.vertices); float contactPenetration = -glm::dot(pointDiff, collidingWith.normal); // is this positive // Set Contact data - glm::vec3 globalOn1 = point; // world coordinates - glm::vec3 globalOn2 = point + collidingWith.normal * contactPenetration; // world coordinates + glm::vec3 globalOn1 = face2.vertices[i] + collidingWith.normal * contactPenetration; // world coordinates + glm::vec3 globalOn2 = face2.vertices[i]; // world coordinates + + ContactFeatureId featureId1; + featureId1.setAsFace(face1.faceId); + ContactFeatureId featureId2; + featureId2.setAsVertex(face2.vertexIds[i]); // If we flipped incident and reference planes, we will // need to flip it back before sending it to the manifold. if (flipped) { contactPenetration = -contactPenetration; - globalOn1 = point - collidingWith.normal * contactPenetration; - globalOn2 = point; + std::swap(globalOn1, globalOn2); + std::swap(featureId1, featureId2); } glm::vec3 localOn1 = glm::vec3(matchedLocalToWorld1->inverse() * glm::vec4(globalOn1, 1.0F)); @@ -99,7 +108,11 @@ std::vector computeContactPoints(const cubos::core::geom::Box* .localOn1 = localOn1, .localOn2 = localOn2, .penetration = collidingWith.penetration, - .id = 0}; + .normalImpulse = 0.0F, + .frictionImpulse1 = 0.0F, + .frictionImpulse2 = 0.0F, + .fid1 = featureId1, + .fid2 = featureId2}; points.push_back(contact); } @@ -107,6 +120,23 @@ std::vector computeContactPoints(const cubos::core::geom::Box* return points; } +static void matchContactPoints(std::vector& newPoints, std::vector& oldPoints) +{ + for (auto newPoint : newPoints) + { + for (auto oldPoint : oldPoints) + { + if ((newPoint.fid1 == oldPoint.fid1) && (newPoint.fid2 == oldPoint.fid2)) + { + newPoint.normalImpulse = oldPoint.normalImpulse; + newPoint.frictionImpulse1 = oldPoint.frictionImpulse1; + newPoint.frictionImpulse2 = oldPoint.frictionImpulse2; + break; + } + } + } +} + void cubos::engine::narrowPhaseCollisionsPlugin(Cubos& cubos) { cubos.depends(transformPlugin); @@ -118,42 +148,77 @@ void cubos::engine::narrowPhaseCollisionsPlugin(Cubos& cubos) cubos.tag(collisionsNarrowTag).tagged(fixedStepTag); cubos.tag(collisionsManifoldTag).tagged(fixedStepTag); + // Remove CollidingWith when it isn't related by PotentiallyCollidingWith cubos.system("clean colliding pairs") .tagged(collisionsNarrowCleanTag) - .before(collisionsNarrowTag) - .call([](Commands cmds, Query query) { - for (auto [entity, collidingWith, other] : query) - { - cmds.unrelate(entity, other); - } - }); - - cubos.system("clean manifolds") - .tagged(collisionsNarrowCleanTag) - .before(collisionsNarrowTag) - .call([](Commands cmds, Query query) { - for (auto [entity, collidingWith, other] : query) + .after(collisionsNarrowTag) + .before(collisionsManifoldTag) + .call([](Commands cmds, Query pQuery, + Query cQuery) { + for (auto [entity, collidingWith, other] : cQuery) { - cmds.unrelate(entity, other); + if (!pQuery.pin(0, entity).pin(1, other).first()) + { + cmds.unrelate(entity, other); + cmds.unrelate(entity, other); + } } }); cubos.system("find colliding pairs") .tagged(collisionsNarrowTag) .after(collisionsBroadTag) - .call( - [](Commands cmds, - Query - query) { - for (auto [ent1, position1, localToWorld1, boxShape1, potentiallyCollidingWith, ent2, position2, - localToWorld2, boxShape2] : query) + .call([](Commands cmds, + Query + nQuery, + Query + yQuery) { + for (auto [ent1, position1, localToWorld1, boxShape1, potentiallyCollidingWith, ent2, position2, + localToWorld2, boxShape2] : nQuery) + { + + cubos::core::geom::Intersection intersectionInfo{}; + + bool intersects = cubos::core::geom::intersects(boxShape1.box, localToWorld1.mat, boxShape2.box, + localToWorld2.mat, intersectionInfo); + + // If CollidingWith present in previous frame update it + if (auto match = yQuery.pin(0, ent1).pin(1, ent2).first()) { - cubos::core::geom::Intersection intersectionInfo{}; + if (!intersects) + { + // Remove CollidingWith when it is related by PotentiallyCollidingWith but not intersecting + cmds.unrelate(ent1, ent2); + cmds.unrelate(ent1, ent2); + continue; + } - bool intersects = cubos::core::geom::intersects(boxShape1.box, localToWorld1.mat, boxShape2.box, - localToWorld2.mat, intersectionInfo); + auto [ent1, position1, localToWorld1, boxShape1, collidingWith, ent2, position2, localToWorld2, + boxShape2] = *match; + /// TODO: this can be simplyfied when we do the refactor + if (collidingWith.entity == ent1) + { + collidingWith.entity1OriginalPosition = position1.vec; + collidingWith.entity2OriginalPosition = position2.vec; + collidingWith.penetration = intersectionInfo.penetration; + collidingWith.position = {0.0F, 0.0F, 0.0F}; + collidingWith.normal = intersectionInfo.normal; + } + else + { + collidingWith.entity1OriginalPosition = position2.vec; + collidingWith.entity2OriginalPosition = position1.vec; + collidingWith.penetration = intersectionInfo.penetration; + collidingWith.position = {0.0F, 0.0F, 0.0F}; + collidingWith.normal = -intersectionInfo.normal; + } + } + else + { if (!intersects) { continue; @@ -167,7 +232,8 @@ void cubos::engine::narrowPhaseCollisionsPlugin(Cubos& cubos) .position = {0.0F, 0.0F, 0.0F}, .normal = intersectionInfo.normal}); } - }); + } + }); /// Our method to calculate contact manifolds (and all supporting functions) is inspired by the tutorial: /// https://research.ncl.ac.uk/game/mastersdegree/gametechnologies/previousinformation/physics5collisionmanifolds/2017%20Tutorial%205%20-%20Collision%20Manifolds.pdf @@ -177,10 +243,14 @@ void cubos::engine::narrowPhaseCollisionsPlugin(Cubos& cubos) cubos.system("collision manifolds") .tagged(collisionsManifoldTag) .after(collisionsNarrowTag) - .call([](Commands cmds, Query - query) { - for (auto [ent1, localToWorld1, boxShape1, collidingWith, ent2, localToWorld2, boxShape2] : query) + .call([](Commands cmds, + Query + cQuery, + Query + ymQuery) { + for (auto [ent1, localToWorld1, boxShape1, collidingWith, ent2, localToWorld2, boxShape2] : cQuery) { // If penetration not bigger than 0 continue if (collidingWith.penetration < 0) @@ -202,9 +272,21 @@ void cubos::engine::narrowPhaseCollisionsPlugin(Cubos& cubos) auto points = computeContactPoints(matchedShape1, matchedLocalToWorld1, matchedShape2, matchedLocalToWorld2, collidingWith); - cmds.relate( - ent1, ent2, - ContactManifold{.entity = collidingWith.entity, .normal = collidingWith.normal, .points = points}); + // TODO: in the refactor change this to work with the internal vector of manifolds + // If Manifold present in previous frame update it + if (auto match = ymQuery.pin(0, ent1).pin(1, ent2).first()) + { + auto [ent1, localToWorld1, boxShape1, contactManifold, ent2, localToWorld2, boxShape2] = *match; + contactManifold.normal = collidingWith.normal; + matchContactPoints(points, contactManifold.points); + contactManifold.points = points; + } + else + { + cmds.relate(ent1, ent2, + ContactManifold{ + .entity = collidingWith.entity, .normal = collidingWith.normal, .points = points}); + } } });