diff --git a/GeoLib/AnalyticalGeometry.cpp b/GeoLib/AnalyticalGeometry.cpp index c92de3ad6c004fc417ad6d3a148cf4b35d2f51b3..ff65af5e6e956a779ea06a8cf59757a373fa008d 100644 --- a/GeoLib/AnalyticalGeometry.cpp +++ b/GeoLib/AnalyticalGeometry.cpp @@ -25,6 +25,19 @@ #include "MathLib/LinAlg/Solvers/GaussAlgorithm.h" +extern double orient2d(double *, double *, double *); + +namespace ExactPredicates +{ +double getOrientation2d(MathLib::Point3d const& a, + MathLib::Point3d const& b, MathLib::Point3d const& c) +{ + return orient2d(const_cast<double*>(a.getCoords()), + const_cast<double*>(b.getCoords()), + const_cast<double*>(c.getCoords())); +} +} + namespace GeoLib { Orientation getOrientation(const double& p0_x, const double& p0_y, const double& p1_x, @@ -515,4 +528,77 @@ GeoLib::Polygon rotatePolygonToXY(GeoLib::Polygon const& polygon_in, return GeoLib::Polygon(rot_polyline); } +std::vector<MathLib::Point3d> +lineSegmentIntersect2d(MathLib::Point3d const& a, MathLib::Point3d const& b, + MathLib::Point3d const& c, MathLib::Point3d const& d) +{ + double const orient_abc(ExactPredicates::getOrientation2d(a, b, c)); + double const orient_abd(ExactPredicates::getOrientation2d(a, b, d)); + + // check if the segment (cd) lies on the left or on the right of (ab) + if ((orient_abc > 0 && orient_abd > 0) || (orient_abc < 0 && orient_abd < 0)) { + return std::vector<MathLib::Point3d>(); + } + + // check: (cd) and (ab) are on the same line + if (orient_abc == 0.0 && orient_abd == 0.0) { + double const eps(std::numeric_limits<double>::epsilon()); + if (MathLib::sqrDist(a,c) < eps && MathLib::sqrDist(b,d) < eps) + return {{ a, b }}; + if (MathLib::sqrDist(a,d) < eps && MathLib::sqrDist(b,c) < eps) + return {{ a, b }}; + ERR("This case of collinear points is not handled yet. Aborting."); + std::abort(); + } + + auto isCollinearPointOntoLineSegment = [](MathLib::Point3d const& a, + MathLib::Point3d const& b, MathLib::Point3d const& c) + { + double const t((c[0]-a[0])/(b[0]-a[0])); + if (0.0 <= t && t <= 1.0) + return true; + return false; + }; + + if (orient_abc == 0.0) { + if (isCollinearPointOntoLineSegment(a,b,c)) + return {{c}}; + return std::vector<MathLib::Point3d>(); + } + + if (orient_abd == 0.0) { + if (isCollinearPointOntoLineSegment(a,b,d)) + return {{d}}; + return std::vector<MathLib::Point3d>(); + } + + // check if the segment (ab) lies on the left or on the right of (cd) + double const orient_cda(ExactPredicates::getOrientation2d(c, d, a)); + double const orient_cdb(ExactPredicates::getOrientation2d(c, d, b)); + if ((orient_cda > 0 && orient_cdb > 0) || (orient_cda < 0 && orient_cdb < 0)) { + return std::vector<MathLib::Point3d>(); + } + + // at this point it is sure that there is an intersection and the system of + // linear equations will be invertible + // solve the two linear equations (b-a, c-d) (t, s)^T = (c-a) simultaneously + MathLib::DenseMatrix<double, std::size_t> mat(2,2); + mat(0,0) = b[0]-a[0]; + mat(0,1) = c[0]-d[0]; + mat(1,0) = b[1]-a[1]; + mat(1,1) = c[1]-d[1]; + std::vector<double> rhs = {{c[0]-a[0], c[1]-a[1]}}; + + MathLib::GaussAlgorithm< + MathLib::DenseMatrix<double, std::size_t>, std::vector<double>> solver(mat); + solver.solve(rhs); + if (0 <= rhs[1] && rhs[1] <= 1.0) { + return { MathLib::Point3d{std::array<double,3>{{ + c[0]+rhs[1]*(d[0]-c[0]), c[1]+rhs[1]*(d[1]-c[1]), + c[2]+rhs[1]*(d[2]-c[2])}} } }; + } else { + return std::vector<MathLib::Point3d>(); // parameter s not in the valid range + } +} + } // end namespace GeoLib diff --git a/GeoLib/AnalyticalGeometry.h b/GeoLib/AnalyticalGeometry.h index 488f3dc5e3c519c7af88a543e92b5f5958b418c1..929b214d76d0958fe9d095fdd0d38beccb7b7f1b 100644 --- a/GeoLib/AnalyticalGeometry.h +++ b/GeoLib/AnalyticalGeometry.h @@ -299,6 +299,21 @@ bool parallel(MathLib::Vector3 v, MathLib::Vector3 w); bool lineSegmentIntersect (const GeoLib::Point& a, const GeoLib::Point& b, const GeoLib::Point& c, const GeoLib::Point& d, GeoLib::Point& s); +/// A line segment is given by its two end-points. The function checks, +/// if the two line segments (ab) and (cd) intersects. This method checks the +/// intersection only in 2d. +/// @param a first end-point of the first line segment +/// @param b second end-point of the first line segment +/// @param c first end-point of the second line segment +/// @param d second end-point of the second line segment +/// @return empty vector in case there isn't an intersection point, a vector +/// containing one point if the line segments intersect in a single point, a +/// vector containing two points describing the line segment the original line +/// segments are interfering. +std::vector<MathLib::Point3d> +lineSegmentIntersect2d(MathLib::Point3d const& a, MathLib::Point3d const& b, + MathLib::Point3d const& c, MathLib::Point3d const& d); + /** * Calculates the intersection points of a line PQ and a triangle ABC. * This method requires ABC to be counterclockwise and PQ to point downward.