17 #include <Eigen/Dense>
27 extern double orient2d(
double*,
double*,
double*);
37 const_cast<double*
>(
c.getCoords()));
46 const_cast<double*
>(
c.getCoords()));
72 double const orientation =
85 bool parallel(Eigen::Vector3d v, Eigen::Vector3d w)
87 const double eps(std::numeric_limits<double>::epsilon());
88 double const eps_squared = eps * eps;
91 if (v.squaredNorm() < eps_squared)
96 if (w.squaredNorm() < eps_squared)
105 if (std::abs(v[0] - w[0]) > eps)
109 if (std::abs(v[1] - w[1]) > eps)
113 if (std::abs(v[2] - w[2]) > eps)
124 if (std::abs(v[0] - w[0]) > eps)
128 if (std::abs(v[1] - w[1]) > eps)
132 if (std::abs(v[2] - w[2]) > eps)
164 Eigen::Vector3d
const v = b - a;
165 Eigen::Vector3d
const w = d -
c;
166 Eigen::Vector3d
const qp =
c - a;
167 Eigen::Vector3d
const pq = a -
c;
169 double const eps = std::numeric_limits<double>::epsilon();
170 double const squared_eps = eps * eps;
172 if (qp.squaredNorm() < squared_eps || (d - a).squaredNorm() < squared_eps)
177 if ((
c - b).squaredNorm() < squared_eps ||
178 (d - b).squaredNorm() < squared_eps)
184 auto isLineSegmentIntersectingAB =
185 [&v](Eigen::Vector3d
const& ap, std::size_t i)
188 return 0.0 <= ap[i] / v[i] && ap[i] / v[i] <= 1.0;
207 std::size_t i_max(std::abs(v[0]) <= std::abs(v[1]) ? 1 : 0);
208 i_max = std::abs(v[i_max]) <= std::abs(v[2]) ? 2 : i_max;
209 if (isLineSegmentIntersectingAB(qp, i_max))
214 Eigen::Vector3d
const ad = d - a;
215 if (isLineSegmentIntersectingAB(ad, i_max))
226 const double sqr_len_v(v.squaredNorm());
227 const double sqr_len_w(w.squaredNorm());
230 mat(0, 0) = sqr_len_v;
231 mat(0, 1) = -v.dot(w);
232 mat(1, 1) = sqr_len_w;
233 mat(1, 0) = mat(0, 1);
235 Eigen::Vector2d rhs{v.dot(qp), w.dot(pq)};
237 rhs = mat.partialPivLu().solve(rhs);
241 const double l(-1.0 * std::numeric_limits<float>::epsilon());
243 const double u(1.0 + std::numeric_limits<float>::epsilon());
244 if (rhs[0] < l || u < rhs[0] || rhs[1] < l || u < rhs[1])
250 GeoLib::Point const p0(a[0] + rhs[0] * v[0], a[1] + rhs[0] * v[1],
251 a[2] + rhs[0] * v[2]);
253 c[2] + rhs[1] * w[2]);
256 double const min_seg_len(
257 std::min(std::sqrt(sqr_len_v), std::sqrt(sqr_len_w)));
258 if (min_dist < min_seg_len * 1e-6)
260 s[0] = 0.5 * (p0[0] + p1[0]);
261 s[1] = 0.5 * (p0[1] + p1[1]);
262 s[2] = 0.5 * (p0[2] + p1[2]);
277 for (seg_it0 = ply->
begin(); seg_it0 != ply->
end() - 2; ++seg_it0)
279 seg_it1 = seg_it0 + 2;
281 for (; seg_it1 != ply->
end(); ++seg_it1)
298 std::vector<GeoLib::Point*>& pnts)
305 Eigen::Matrix3d rot_mat = Eigen::Matrix3d::Zero();
307 if (n[0] == 0 && n[1] == 0)
320 rot_mat(0, 0) = -1.0;
321 rot_mat(2, 2) = -1.0;
328 double const h0(std::sqrt(n[0] * n[0] + n[2] * n[2]));
334 if (h0 < std::numeric_limits<double>::epsilon())
339 rot_mat(1, 2) = -1.0;
345 rot_mat(2, 1) = -1.0;
350 double const h1(1 / n.norm());
353 rot_mat(0, 0) = n[2] / h0;
355 rot_mat(0, 2) = -n[0] / h0;
356 rot_mat(1, 0) = -n[1] * n[0] / h0 * h1;
357 rot_mat(1, 1) = h0 * h1;
358 rot_mat(1, 2) = -n[1] * n[2] / h0 * h1;
359 rot_mat(2, 0) = n[0] * h1;
360 rot_mat(2, 1) = n[1] * h1;
361 rot_mat(2, 2) = n[2] * h1;
368 return rotatePointsToXY(pnts.begin(), pnts.end(), pnts.begin(), pnts.end());
376 auto const va = Eigen::Map<Eigen::Vector3d const>(a.
getCoords());
377 auto const vb = Eigen::Map<Eigen::Vector3d const>(b.
getCoords());
378 auto const vc = Eigen::Map<Eigen::Vector3d const>(
c.getCoords());
379 auto const vp = Eigen::Map<Eigen::Vector3d const>(p.getCoords());
380 auto const vq = Eigen::Map<Eigen::Vector3d const>(
q.getCoords());
382 Eigen::Vector3d
const pq = vq - vp;
383 Eigen::Vector3d
const pa = va - vp;
384 Eigen::Vector3d
const pb = vb - vp;
385 Eigen::Vector3d
const pc = vc - vp;
387 double u = pq.cross(pc).dot(pb);
392 double v = pq.cross(pa).dot(pc);
397 double w = pq.cross(pb).dot(pa);
403 const double denom(1.0 / (u + v + w));
407 return std::make_unique<GeoLib::Point>(u * a[0] + v * b[0] + w *
c[0],
408 u * a[1] + v * b[1] + w *
c[1],
409 u * a[2] + v * b[2] + w *
c[2]);
413 std::vector<GeoLib::Polyline*>& plys)
415 auto computeSegmentIntersections =
418 for (
auto seg0_it(poly0.
begin()); seg0_it != poly0.
end(); ++seg0_it)
420 for (
auto seg1_it(poly1.begin()); seg1_it != poly1.end(); ++seg1_it)
425 std::size_t
const id(
427 poly0.
insertPoint(seg0_it.getSegmentNumber() + 1,
id);
428 poly1.insertPoint(seg1_it.getSegmentNumber() + 1,
id);
434 for (
auto it0(plys.begin()); it0 != plys.end(); ++it0)
438 for (; it1 != plys.end(); ++it1)
440 computeSegmentIntersections(*(*it0), *(*it1));
446 Eigen::Vector3d& plane_normal)
449 auto* polygon_pnts(
new std::vector<GeoLib::Point*>);
458 Eigen::Matrix3d
const rot_mat =
463 std::for_each(polygon_pnts->begin(), polygon_pnts->end(),
488 if ((orient_abc > 0 && orient_abd > 0) ||
489 (orient_abc < 0 && orient_abd < 0))
491 return std::vector<MathLib::Point3d>();
495 if (orient_abc == 0.0 && orient_abd == 0.0)
497 double const eps(std::numeric_limits<double>::epsilon());
509 auto isPointOnSegment = [](
double q,
double p0,
double p1)
511 double const t((
q - p0) / (p1 - p0));
512 return 0 <= t && t <= 1;
516 if (isPointOnSegment(
c[0], a[0], b[0]))
519 if (isPointOnSegment(a[0],
c[0], d[0]))
529 if (isPointOnSegment(b[0],
c[0], d[0]))
534 if (isPointOnSegment(d[0], a[0], b[0]))
538 std::stringstream err;
539 err.precision(std::numeric_limits<double>::digits10);
540 err << ab <<
" x " << cd;
542 "The case of parallel line segments ({:s}) is not handled yet. "
548 if (isPointOnSegment(d[0], a[0], b[0]))
551 if (isPointOnSegment(a[0],
c[0], d[0]))
561 if (isPointOnSegment(b[0],
c[0], d[0]))
566 if (isPointOnSegment(
c[0], a[0], b[0]))
571 std::stringstream err;
572 err.precision(std::numeric_limits<double>::digits10);
573 err << ab <<
" x " << cd;
575 "The case of parallel line segments ({:s}) is not handled yet. "
579 return std::vector<MathLib::Point3d>();
588 if (b[0] - a[0] != 0)
590 double const t = (
c[0] - a[0]) / (b[0] - a[0]);
591 return 0.0 <= t && t <= 1.0;
593 if (b[1] - a[1] != 0)
595 double const t = (
c[1] - a[1]) / (b[1] - a[1]);
596 return 0.0 <= t && t <= 1.0;
598 if (b[2] - a[2] != 0)
600 double const t = (
c[2] - a[2]) / (b[2] - a[2]);
601 return 0.0 <= t && t <= 1.0;
606 if (orient_abc == 0.0)
608 if (isCollinearPointOntoLineSegment(a, b,
c))
612 return std::vector<MathLib::Point3d>();
615 if (orient_abd == 0.0)
617 if (isCollinearPointOntoLineSegment(a, b, d))
621 return std::vector<MathLib::Point3d>();
627 if ((orient_cda > 0 && orient_cdb > 0) ||
628 (orient_cda < 0 && orient_cdb < 0))
630 return std::vector<MathLib::Point3d>();
637 mat(0, 0) = b[0] - a[0];
638 mat(0, 1) =
c[0] - d[0];
639 mat(1, 0) = b[1] - a[1];
640 mat(1, 1) =
c[1] - d[1];
641 Eigen::Vector2d rhs{
c[0] - a[0],
c[1] - a[1]};
643 rhs = mat.partialPivLu().solve(rhs);
644 if (0 <= rhs[1] && rhs[1] <= 1.0)
647 {
c[0] + rhs[1] * (d[0] -
c[0]),
c[1] + rhs[1] * (d[1] -
c[1]),
648 c[2] + rhs[1] * (d[2] -
c[2])}}}};
650 return std::vector<MathLib::Point3d>();
655 std::vector<GeoLib::LineSegment>& sub_segments)
657 double const eps(std::numeric_limits<double>::epsilon());
659 auto findNextSegment =
661 std::vector<GeoLib::LineSegment>& sub_segments,
662 std::vector<GeoLib::LineSegment>::iterator& sub_seg_it)
664 if (sub_seg_it == sub_segments.end())
669 auto act_beg_seg_it = std::find_if(
670 sub_seg_it, sub_segments.end(),
673 return MathLib::sqrDist(seg_beg_pnt, seg.getBeginPoint()) <
675 MathLib::sqrDist(seg_beg_pnt, seg.getEndPoint()) < eps;
677 if (act_beg_seg_it == sub_segments.end())
686 std::swap(act_beg_seg_it->getBeginPoint(),
687 act_beg_seg_it->getEndPoint());
689 assert(sub_seg_it != sub_segments.end());
691 if (sub_seg_it != act_beg_seg_it)
693 std::swap(*sub_seg_it, *act_beg_seg_it);
698 auto seg_it = sub_segments.begin();
699 findNextSegment(seg_beg_pnt, sub_segments, seg_it);
701 while (seg_it != sub_segments.end())
705 if (seg_it != sub_segments.end())
707 findNextSegment(new_seg_beg_pnt, sub_segments, seg_it);
714 Eigen::Matrix3d rot_mat = Eigen::Matrix3d::Zero();
715 const double cos_theta = v[0];
716 const double sin_theta = v[1];
717 rot_mat(0, 0) = rot_mat(1, 1) = cos_theta;
718 rot_mat(0, 1) = sin_theta;
719 rot_mat(1, 0) = -sin_theta;
727 Eigen::Vector3d yy = Eigen::Vector3d::Zero();
728 auto const eps = std::numeric_limits<double>::epsilon();
729 if (std::abs(v[0]) > 0.0 && std::abs(v[1]) + std::abs(v[2]) < eps)
733 else if (std::abs(v[1]) > 0.0 && std::abs(v[0]) + std::abs(v[2]) < eps)
737 else if (std::abs(v[2]) > 0.0 && std::abs(v[0]) + std::abs(v[1]) < eps)
743 for (
unsigned i = 0; i < 3; i++)
745 if (std::abs(v[i]) > 0.0)
753 Eigen::Vector3d
const zz = v.cross(yy).normalized();
755 yy = zz.cross(v).normalized();
757 Eigen::Matrix3d rot_mat;
double orient2dfast(double *, double *, double *)
double orient2d(double *, double *, double *)
Definition of analytical geometry functions.
Definition of the PointVec class.
Definition of the PolyLine class.
GeoLib::Point const & getBeginPoint() const
GeoLib::Point const & getEndPoint() const
This class manages pointers to Points in a std::vector along with a name. It also handles the deletin...
std::size_t push_back(Point *pnt)
std::size_t getSegmentNumber() const
Class Polyline consists mainly of a reference to a point vector and a vector that stores the indices ...
std::size_t getNumberOfSegments() const
std::size_t getNumberOfPoints() const
const Point * getPoint(std::size_t i) const
returns the i-th point contained in the polyline
virtual bool insertPoint(std::size_t pos, std::size_t pnt_id)
SegmentIterator begin() const
virtual bool addPoint(std::size_t pnt_id)
SegmentIterator end() const
const T * getCoords() const
double getOrientation2d(MathLib::Point3d const &a, MathLib::Point3d const &b, MathLib::Point3d const &c)
double getOrientation2dFast(MathLib::Point3d const &a, MathLib::Point3d const &b, MathLib::Point3d const &c)
Orientation getOrientationFast(MathLib::Point3d const &p0, MathLib::Point3d const &p1, MathLib::Point3d const &p2)
GeoLib::Polygon rotatePolygonToXY(GeoLib::Polygon const &polygon_in, Eigen::Vector3d &plane_normal)
Eigen::Matrix3d compute3DRotationMatrixToX(Eigen::Vector3d const &v)
bool lineSegmentsIntersect(const GeoLib::Polyline *ply, GeoLib::Polyline::SegmentIterator &seg_it0, GeoLib::Polyline::SegmentIterator &seg_it1, GeoLib::Point &intersection_pnt)
void computeAndInsertAllIntersectionPoints(GeoLib::PointVec &pnt_vec, std::vector< GeoLib::Polyline * > &plys)
void rotatePoints(Eigen::Matrix3d const &rot_mat, InputIterator pnts_begin, InputIterator pnts_end)
bool parallel(Eigen::Vector3d v, Eigen::Vector3d w)
Eigen::Matrix3d computeRotationMatrixToXY(Eigen::Vector3d const &n)
Eigen::Matrix3d rotatePointsToXY(InputIterator1 p_pnts_begin, InputIterator1 p_pnts_end, InputIterator2 r_pnts_begin, InputIterator2 r_pnts_end)
void sortSegments(MathLib::Point3d const &seg_beg_pnt, std::vector< GeoLib::LineSegment > &sub_segments)
std::vector< MathLib::Point3d > lineSegmentIntersect2d(GeoLib::LineSegment const &ab, GeoLib::LineSegment const &cd)
std::unique_ptr< GeoLib::Point > triangleLineIntersection(MathLib::Point3d const &a, MathLib::Point3d const &b, MathLib::Point3d const &c, MathLib::Point3d const &p, MathLib::Point3d const &q)
std::pair< Eigen::Vector3d, double > getNewellPlane(InputIterator pnts_begin, InputIterator pnts_end)
Eigen::Matrix3d compute2DRotationMatrixToX(Eigen::Vector3d const &v)
bool lineSegmentIntersect(GeoLib::LineSegment const &s0, GeoLib::LineSegment const &s1, GeoLib::Point &s)
Orientation getOrientation(MathLib::Point3d const &p0, MathLib::Point3d const &p1, MathLib::Point3d const &p2)
bool isCoplanar(const MathLib::Point3d &a, const MathLib::Point3d &b, const MathLib::Point3d &c, const MathLib::Point3d &d)
Checks if the four given points are located on a plane.
double sqrDist2d(MathLib::Point3d const &p0, MathLib::Point3d const &p1)
double sqrDist(MathLib::Point3d const &p0, MathLib::Point3d const &p1)