diff --git a/src/aliceVision/fuseCut/DelaunayGraphCut.cpp b/src/aliceVision/fuseCut/DelaunayGraphCut.cpp index 6e91793941..ff4d5aedae 100644 --- a/src/aliceVision/fuseCut/DelaunayGraphCut.cpp +++ b/src/aliceVision/fuseCut/DelaunayGraphCut.cpp @@ -1370,6 +1370,162 @@ DelaunayGraphCut::Facet DelaunayGraphCut::getFacetFromVertexOnTheRayToTheCam(Ver return Facet(); } + +DelaunayGraphCut::GeometryIntersection +DelaunayGraphCut::intersectNextGeom(const DelaunayGraphCut::GeometryIntersection& inGeometry, + const Point3d& originPt, + const Point3d& dirVect, Point3d& intersectPt, + const float epsilon, const Point3d& lastIntersectPt) const +{ + + switch (inGeometry.type) + { + case EGeometryType::Facet: + { + const CellIndex tetrahedronIndex = inGeometry.facet.cellIndex; + + // Test all facets of the tetrahedron using i as localVertexIndex to define next intersectionFacet + for (int i = 0; i < 4; ++i) + { + // Because we can't intersect with incoming facet (same localVertexIndex) + if (i == inGeometry.facet.localVertexIndex) + continue; + + const Facet intersectionFacet(tetrahedronIndex, i); + + const GeometryIntersection result = rayIntersectTriangle(originPt, dirVect, intersectionFacet, intersectPt, epsilon, &lastIntersectPt); + if (result.type != EGeometryType::None) + return result; + } + } + break; + + case EGeometryType::Vertex: + { + for (CellIndex adjCellIndex : getNeighboringCellsByVertexIndex(inGeometry.vertexIndex)) + { + if (isInfiniteCell(adjCellIndex)) + continue; + + // Get local vertex index + const VertexIndex localVertexIndex = _tetrahedralization->index(adjCellIndex, inGeometry.vertexIndex); + + // Define the facet to intersect + const Facet facet(adjCellIndex, localVertexIndex); + + const GeometryIntersection result = rayIntersectTriangle(originPt, dirVect, facet, intersectPt, epsilon, &lastIntersectPt); + if (result.type != EGeometryType::None) + return result; + } + } + break; + + case EGeometryType::Edge: + { + GeometryIntersection result; + + for (CellIndex adjCellIndex : getNeighboringCellsByEdge(inGeometry.edge)) + { + if (isInfiniteCell(adjCellIndex)) + continue; + // Local vertices indices + const VertexIndex lvi0 = _tetrahedralization->index(adjCellIndex, inGeometry.edge.v0); + const VertexIndex lvi1 = _tetrahedralization->index(adjCellIndex, inGeometry.edge.v1); + + // The two facets that do not touch this edge need to be tested + const std::array opositeFacets{ {{adjCellIndex, lvi0}, {adjCellIndex, lvi1}} }; + + for (const Facet& facet : opositeFacets) + { + const GeometryIntersection result = rayIntersectTriangle(originPt, dirVect, facet, intersectPt, epsilon, &lastIntersectPt); + if (result.type != EGeometryType::None) + return result; + } + } + } + break; + + case EGeometryType::None: + throw std::runtime_error("[intersectNextGeom] intersection with input none geometry should not happen."); + break; + } + return GeometryIntersection(); +} + +DelaunayGraphCut::GeometryIntersection DelaunayGraphCut::rayIntersectTriangle(const Point3d& originPt, + const Point3d& DirVec, + const DelaunayGraphCut::Facet& facet, + Point3d& intersectPt, + const float epsilon, const Point3d* lastIntersectPt) const +{ + const VertexIndex AvertexIndex = getVertexIndex(facet, 0); + const VertexIndex BvertexIndex = getVertexIndex(facet, 1); + const VertexIndex CvertexIndex = getVertexIndex(facet, 2); + + const Point3d* A = &_verticesCoords[AvertexIndex]; + const Point3d* B = &_verticesCoords[BvertexIndex]; + const Point3d* C = &_verticesCoords[CvertexIndex]; + + Point3d tempIntersectPt; + const Point2d triangleUv = getLineTriangleIntersectBarycCoords(&tempIntersectPt, A, B, C, &originPt, &DirVec); + + if (!isnormal(tempIntersectPt.x) || !isnormal(tempIntersectPt.y) || !isnormal(tempIntersectPt.z)) + { + ALICEVISION_LOG_DEBUG("Invalid/notNormal intersection point found during rayIntersectTriangle."); + } + + const float u = triangleUv.x; // A to C + const float v = triangleUv.y; // A to B + + // If we find invalid uv coordinate + if (!std::isfinite(u) && !std::isfinite(v)) + return GeometryIntersection(); + + // Ouside the triangle with epsilon margin + if (u < -epsilon || v < -epsilon || (u + v) >(1 + epsilon)) + return GeometryIntersection(); + + // In case intersectPt is provided, check if intersectPt is in front of lastIntersectionPt + // in the DirVec direction to ensure that we are moving forward in the right direction + if (lastIntersectPt != nullptr && dot(DirVec, (tempIntersectPt - *lastIntersectPt).normalize()) <= 0) + return GeometryIntersection(); + + // Change intersection point only if tempIntersectionPt is in the right direction (mean we intersect something) + intersectPt = tempIntersectPt; + + if (v < epsilon) // along A C edge + { + if (u < epsilon) + { + intersectPt = *A; + return GeometryIntersection(AvertexIndex); // vertex A + } + if (u > 1.0f - epsilon) + { + intersectPt = *C; + return GeometryIntersection(CvertexIndex); // vertex C + } + + return GeometryIntersection(Edge(AvertexIndex, CvertexIndex)); // edge AC + } + + if (u < epsilon) // along A B edge + { + if (v > 1.0f - epsilon) + { + intersectPt = *B; + return GeometryIntersection(BvertexIndex); // vertex B + } + + return GeometryIntersection(Edge(AvertexIndex, BvertexIndex)); // edge AB + } + + if (u + v > 1.0f - epsilon) + return GeometryIntersection(Edge(BvertexIndex, CvertexIndex)); // edge BC + + return GeometryIntersection(facet); +} + DelaunayGraphCut::Facet DelaunayGraphCut::getFirstFacetOnTheRayFromCamToThePoint(int cam, const Point3d& p, Point3d& intersectPt) const { int cam_vi = _camsVertexes[cam]; diff --git a/src/aliceVision/fuseCut/DelaunayGraphCut.hpp b/src/aliceVision/fuseCut/DelaunayGraphCut.hpp index b8862ae342..1c6f767acc 100644 --- a/src/aliceVision/fuseCut/DelaunayGraphCut.hpp +++ b/src/aliceVision/fuseCut/DelaunayGraphCut.hpp @@ -360,6 +360,36 @@ class DelaunayGraphCut Facet getFacetFromVertexOnTheRayToTheCam(VertexIndex globalVertexIndex, int cam, bool nearestFarest) const; Facet getFirstFacetOnTheRayFromCamToThePoint(int cam, const Point3d& p, Point3d& intersectPt) const; + /** + * @brief Function that returns the next geometry intersected by the ray. + * The function handles different cases, whether we come from an edge, a facet or a vertex. + * + * @param inGeometry the geometry we come from + * @param originPt ray origin point + * @param dirVect ray direction + * @param intersectPt a reference that will store the computed intersection point for the intersected geometry + * @param epsilon used to define the boundary when we have to consider either a collision with an edge/vertex or a facet + * @param lastIntersectPt constant reference to the last intersection point used to test the direction. + * @return + */ + GeometryIntersection intersectNextGeom(const GeometryIntersection& inGeometry, const Point3d& originPt, + const Point3d& dirVect, Point3d& intersectPt, const float epsilon, const Point3d& lastIntersectPt) const; + + /** + * @brief Function that returns the next geometry intersected by the ray on a given facet or None if there are no intersected geometry. + * The function distinguishes the intersections cases using epsilon. + * + * @param originPt ray origin point + * @param DirVec ray direction + * @param facet the given facet to intersect with + * @param intersectPt a reference that will store the computed intersection point for the next intersecting geometry + * @param epsilon used to define the boundary when we have to consider either a collision with an edge/vertex or a facet + * @param lastIntersectPt pointer to the last intersection point used to test the direction (if not nulllptr) + * @return + */ + GeometryIntersection rayIntersectTriangle(const Point3d& originPt, const Point3d& DirVec, const Facet& facet, + Point3d& intersectPt, const float epsilon, const Point3d* lastIntersectPt = nullptr) const; + float distFcn(float maxDist, float dist, float distFcnHeight) const; inline double conj(double val) const { return val; }