17#include <Eigen/Geometry>
34 const_cast<double*
>(b.
data()),
35 const_cast<double*
>(c.data()));
43 const_cast<double*
>(b.
data()),
44 const_cast<double*
>(c.data()));
70 double const orientation =
83bool parallel(Eigen::Vector3d v, Eigen::Vector3d w)
85 const double eps(std::numeric_limits<double>::epsilon());
86 double const eps_squared = eps * eps;
89 if (v.squaredNorm() < eps_squared)
94 if (w.squaredNorm() < eps_squared)
103 if (std::abs(v[0] - w[0]) > eps)
107 if (std::abs(v[1] - w[1]) > eps)
111 if (std::abs(v[2] - w[2]) > eps)
122 if (std::abs(v[0] - w[0]) > eps)
126 if (std::abs(v[1] - w[1]) > eps)
130 if (std::abs(v[2] - w[2]) > eps)
83bool parallel(Eigen::Vector3d v, Eigen::Vector3d w) {
…}
148 if (!isCoplanar(pa, pb, pc, pd))
158 Eigen::Vector3d
const v = b - a;
159 Eigen::Vector3d
const w = d - c;
160 Eigen::Vector3d
const qp = c - a;
161 Eigen::Vector3d
const pq = a - c;
163 double const eps = std::numeric_limits<double>::epsilon();
164 double const squared_eps = eps * eps;
166 if (qp.squaredNorm() < squared_eps || (d - a).squaredNorm() < squared_eps)
171 if ((c - b).squaredNorm() < squared_eps ||
172 (d - b).squaredNorm() < squared_eps)
178 auto isLineSegmentIntersectingAB =
179 [&v](Eigen::Vector3d
const& ap, std::size_t i)
182 return 0.0 <= ap[i] / v[i] && ap[i] / v[i] <= 1.0;
201 std::size_t i_max(std::abs(v[0]) <= std::abs(v[1]) ? 1 : 0);
202 i_max = std::abs(v[i_max]) <= std::abs(v[2]) ? 2 : i_max;
203 if (isLineSegmentIntersectingAB(qp, i_max))
208 Eigen::Vector3d
const ad = d - a;
209 if (isLineSegmentIntersectingAB(ad, i_max))
220 const double sqr_len_v(v.squaredNorm());
221 const double sqr_len_w(w.squaredNorm());
224 mat(0, 0) = sqr_len_v;
225 mat(0, 1) = -v.dot(w);
226 mat(1, 1) = sqr_len_w;
227 mat(1, 0) = mat(0, 1);
229 Eigen::Vector2d rhs{v.dot(qp), w.dot(pq)};
231 rhs = mat.partialPivLu().solve(rhs);
235 const double l(-1.0 * std::numeric_limits<float>::epsilon());
237 const double u(1.0 + std::numeric_limits<float>::epsilon());
238 if (rhs[0] < l || u < rhs[0] || rhs[1] < l || u < rhs[1])
244 GeoLib::Point const p0(a[0] + rhs[0] * v[0], a[1] + rhs[0] * v[1],
245 a[2] + rhs[0] * v[2]);
246 GeoLib::Point const p1(c[0] + rhs[1] * w[0], c[1] + rhs[1] * w[1],
247 c[2] + rhs[1] * w[2]);
250 double const min_seg_len(
251 std::min(std::sqrt(sqr_len_v), std::sqrt(sqr_len_w)));
252 if (min_dist < min_seg_len * 1e-6)
254 s[0] = 0.5 * (p0[0] + p1[0]);
255 s[1] = 0.5 * (p0[1] + p1[1]);
256 s[2] = 0.5 * (p0[2] + p1[2]);
271 for (seg_it0 = ply->
begin(); seg_it0 != ply->
end() - 2; ++seg_it0)
273 seg_it1 = seg_it0 + 2;
275 for (; seg_it1 != ply->
end(); ++seg_it1)
292 std::vector<GeoLib::Point*>& pnts)
299 Eigen::Matrix3d rot_mat = Eigen::Matrix3d::Zero();
301 if (n[0] == 0 && n[1] == 0)
314 rot_mat(0, 0) = -1.0;
315 rot_mat(2, 2) = -1.0;
322 double const h0(std::sqrt(n[0] * n[0] + n[2] * n[2]));
328 if (h0 < std::numeric_limits<double>::epsilon())
333 rot_mat(1, 2) = -1.0;
339 rot_mat(2, 1) = -1.0;
344 double const h1(1 / n.norm());
347 rot_mat(0, 0) = n[2] / h0;
349 rot_mat(0, 2) = -n[0] / h0;
350 rot_mat(1, 0) = -n[1] * n[0] / h0 * h1;
351 rot_mat(1, 1) = h0 * h1;
352 rot_mat(1, 2) = -n[1] * n[2] / h0 * h1;
353 rot_mat(2, 0) = n[0] * h1;
354 rot_mat(2, 1) = n[1] * h1;
355 rot_mat(2, 2) = n[2] * h1;
362 return rotatePointsToXY(pnts.begin(), pnts.end(), pnts.begin(), pnts.end());
373 Eigen::Vector3d
const pc = c.asEigenVector3d() - p.asEigenVector3d();
375 double u = pq.cross(pc).dot(pb);
380 double v = pq.cross(pa).dot(pc);
385 double w = pq.cross(pb).dot(pa);
391 const double denom(1.0 / (u + v + w));
395 return std::make_unique<GeoLib::Point>(u * a[0] + v * b[0] + w * c[0],
396 u * a[1] + v * b[1] + w * c[1],
397 u * a[2] + v * b[2] + w * c[2]);
401 std::vector<GeoLib::Polyline*>& plys)
403 auto computeSegmentIntersections =
406 for (
auto seg0_it(poly0.
begin()); seg0_it != poly0.
end(); ++seg0_it)
408 for (
auto seg1_it(poly1.begin()); seg1_it != poly1.end(); ++seg1_it)
413 std::size_t
const id(
415 poly0.
insertPoint(seg0_it.getSegmentNumber() + 1,
id);
416 poly1.insertPoint(seg1_it.getSegmentNumber() + 1,
id);
422 for (
auto it0(plys.begin()); it0 != plys.end(); ++it0)
426 for (; it1 != plys.end(); ++it1)
428 computeSegmentIntersections(*(*it0), *(*it1));
433std::tuple<std::vector<GeoLib::Point*>, Eigen::Vector3d>
437 std::vector<GeoLib::Point*> polygon_points;
446 Eigen::Matrix3d
const rot_mat =
451 std::for_each(polygon_points.begin(), polygon_points.end(),
454 return {polygon_points, plane_normal};
469 if ((orient_abc > 0 && orient_abd > 0) ||
470 (orient_abc < 0 && orient_abd < 0))
472 return std::vector<MathLib::Point3d>();
476 if (orient_abc == 0.0 && orient_abd == 0.0)
478 double const eps(std::numeric_limits<double>::epsilon());
490 auto isPointOnSegment = [](
double q,
double p0,
double p1)
492 double const t((q - p0) / (p1 - p0));
493 return 0 <= t && t <= 1;
497 if (isPointOnSegment(c[0], a[0], b[0]))
500 if (isPointOnSegment(a[0], c[0], d[0]))
510 if (isPointOnSegment(b[0], c[0], d[0]))
515 if (isPointOnSegment(d[0], a[0], b[0]))
519 std::stringstream err;
520 err.precision(std::numeric_limits<double>::max_digits10);
521 err << ab <<
" x " << cd;
523 "The case of parallel line segments ({:s}) is not handled yet. "
529 if (isPointOnSegment(d[0], a[0], b[0]))
532 if (isPointOnSegment(a[0], c[0], d[0]))
542 if (isPointOnSegment(b[0], c[0], d[0]))
547 if (isPointOnSegment(c[0], a[0], b[0]))
552 std::stringstream err;
553 err.precision(std::numeric_limits<double>::max_digits10);
554 err << ab <<
" x " << cd;
556 "The case of parallel line segments ({:s}) is not handled yet. "
560 return std::vector<MathLib::Point3d>();
569 if (b[0] - a[0] != 0)
571 double const t = (c[0] - a[0]) / (b[0] - a[0]);
572 return 0.0 <= t && t <= 1.0;
574 if (b[1] - a[1] != 0)
576 double const t = (c[1] - a[1]) / (b[1] - a[1]);
577 return 0.0 <= t && t <= 1.0;
579 if (b[2] - a[2] != 0)
581 double const t = (c[2] - a[2]) / (b[2] - a[2]);
582 return 0.0 <= t && t <= 1.0;
587 if (orient_abc == 0.0)
589 if (isCollinearPointOntoLineSegment(a, b, c))
593 return std::vector<MathLib::Point3d>();
596 if (orient_abd == 0.0)
598 if (isCollinearPointOntoLineSegment(a, b, d))
602 return std::vector<MathLib::Point3d>();
608 if ((orient_cda > 0 && orient_cdb > 0) ||
609 (orient_cda < 0 && orient_cdb < 0))
611 return std::vector<MathLib::Point3d>();
618 mat(0, 0) = b[0] - a[0];
619 mat(0, 1) = c[0] - d[0];
620 mat(1, 0) = b[1] - a[1];
621 mat(1, 1) = c[1] - d[1];
622 Eigen::Vector2d rhs{c[0] - a[0], c[1] - a[1]};
624 rhs = mat.partialPivLu().solve(rhs);
625 if (0 <= rhs[1] && rhs[1] <= 1.0)
628 {c[0] + rhs[1] * (d[0] - c[0]), c[1] + rhs[1] * (d[1] - c[1]),
629 c[2] + rhs[1] * (d[2] - c[2])}}}};
631 return std::vector<MathLib::Point3d>();
636 std::vector<GeoLib::LineSegment>& sub_segments)
638 double const eps(std::numeric_limits<double>::epsilon());
640 auto findNextSegment =
642 std::vector<GeoLib::LineSegment>& sub_segments,
643 std::vector<GeoLib::LineSegment>::iterator& sub_seg_it)
645 if (sub_seg_it == sub_segments.end())
650 auto act_beg_seg_it = std::find_if(
651 sub_seg_it, sub_segments.end(),
654 return MathLib::sqrDist(seg_beg_pnt, seg.getBeginPoint()) <
656 MathLib::sqrDist(seg_beg_pnt, seg.getEndPoint()) < eps;
658 if (act_beg_seg_it == sub_segments.end())
667 std::swap(act_beg_seg_it->getBeginPoint(),
668 act_beg_seg_it->getEndPoint());
670 assert(sub_seg_it != sub_segments.end());
672 if (sub_seg_it != act_beg_seg_it)
674 std::swap(*sub_seg_it, *act_beg_seg_it);
679 auto seg_it = sub_segments.begin();
680 findNextSegment(seg_beg_pnt, sub_segments, seg_it);
682 while (seg_it != sub_segments.end())
686 if (seg_it != sub_segments.end())
688 findNextSegment(new_seg_beg_pnt, sub_segments, seg_it);
695 Eigen::Matrix3d rot_mat = Eigen::Matrix3d::Zero();
696 const double cos_theta = v[0];
697 const double sin_theta = v[1];
698 rot_mat(0, 0) = rot_mat(1, 1) = cos_theta;
699 rot_mat(0, 1) = sin_theta;
700 rot_mat(1, 0) = -sin_theta;
708 Eigen::Vector3d yy = Eigen::Vector3d::Zero();
709 auto const eps = std::numeric_limits<double>::epsilon();
710 if (std::abs(v[0]) > 0.0 && std::abs(v[1]) + std::abs(v[2]) < eps)
714 else if (std::abs(v[1]) > 0.0 && std::abs(v[0]) + std::abs(v[2]) < eps)
718 else if (std::abs(v[2]) > 0.0 && std::abs(v[0]) + std::abs(v[1]) < eps)
724 for (
unsigned i = 0; i < 3; i++)
726 if (std::abs(v[i]) > 0.0)
734 Eigen::Vector3d
const zz = v.cross(yy).normalized();
736 yy = zz.cross(v).normalized();
738 Eigen::Matrix3d rot_mat;
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 deletio...
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
SegmentIterator end() const
Eigen::Vector3d const & asEigenVector3d() const
const double * data() 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)
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)
std::tuple< std::vector< GeoLib::Point * >, Eigen::Vector3d > rotatePolygonPointsToXY(GeoLib::Polygon const &polygon_in)
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)
double sqrDist2d(MathLib::Point3d const &p0, MathLib::Point3d const &p1)
double sqrDist(MathLib::Point3d const &p0, MathLib::Point3d const &p1)
Definitions of the predicates functions.
double orient2dfast(double *, double *, double *)
double orient2d(double *, double *, double *)