26 #include <Eigen/Dense>
40 double Px,
double Py,
double x1,
double y1,
double x2,
double y2,
41 double& out_x,
double& out_y)
43 if (x1 == x2 && y1 == y2)
52 double Ratio = ((Px - x1) * Dx + (Py - y1) * Dy) / (Dx * Dx + Dy * Dy);
67 out_x = x1 + (Ratio * Dx);
68 out_y = y1 + (Ratio * Dy);
78 double Px,
double Py,
double x1,
double y1,
double x2,
double y2,
79 double& out_x,
double& out_y)
81 if (x1 == x2 && y1 == y2)
90 double Ratio = ((Px - x1) * Dx + (Py - y1) * Dy) / (Dx * Dx + Dy * Dy);
92 out_x = x1 + (Ratio * Dx);
93 out_y = y1 + (Ratio * Dy);
101 double Px,
double Py,
double x1,
double y1,
double x2,
double y2)
103 if (x1 == x2 && y1 == y2)
111 double Ratio = ((Px - x1) * Dx + (Py - y1) * Dy) / (Dx * Dx + Dy * Dy);
113 return square(x1 + (Ratio * Dx) - Px) +
square(y1 + (Ratio * Dy) - Py);
121 const double x1,
const double y1,
const double x2,
const double y2,
122 const double x3,
const double y3,
const double x4,
const double y4,
123 double& ix,
double& iy)
125 double UpperX, UpperY, LowerX, LowerY, Ax, Bx, Cx, Ay, By, Cy, d, f, e,
144 if (UpperX < x4 || x3 < LowerX)
return false;
146 else if (UpperX < x3 || x4 < LowerX)
165 if (UpperY < y4 || y3 < LowerY)
return false;
167 else if (UpperY < y3 || y4 < LowerY)
172 d = (By * Cx) - (Bx * Cy);
173 f = (Ay * Bx) - (Ax * By);
177 if (d < 0 || d > f)
return false;
179 else if (d > 0 || d < f)
182 e = (Ax * Cy) - (Ay * Cx);
186 if (e < 0 || e > f)
return false;
188 else if (e > 0 || e < f)
191 Ratio = (Ax * -By) - (Ay * -Bx);
195 Ratio = ((Cy * -Bx) - (Cx * -By)) / Ratio;
196 ix = x1 + (Ratio * Ax);
197 iy = y1 + (Ratio * Ay);
201 if ((Ax * -Cy) == (-Cx * Ay))
219 const double x1,
const double y1,
const double x2,
const double y2,
220 const double x3,
const double y3,
const double x4,
const double y4,
221 float& ix,
float& iy)
234 double px,
double py,
unsigned int polyEdges,
const double* poly_xs,
235 const double* poly_ys)
240 if (polyEdges < 3)
return res;
244 for (i = 0; i < polyEdges; i++)
246 if ((poly_ys[i] <= py && py < poly_ys[j]) ||
247 (poly_ys[j] <= py && py < poly_ys[i]))
250 if (px - poly_xs[i] <
251 ((poly_xs[j] - poly_xs[i]) * (py - poly_ys[i]) /
252 (poly_ys[j] - poly_ys[i])))
265 double px,
double py,
unsigned int polyEdges,
const double* poly_xs,
266 const double* poly_ys)
269 double minDist = 1e20f;
277 for (i = 0; i < polyEdges; i++)
281 double closestX, closestY;
283 px, py, poly_xs[j], poly_ys[j], poly_xs[i], poly_ys[i], closestX,
286 minDist = min(d, minDist);
299 double p1_x,
double p1_y,
double p1_z,
double p2_x,
double p2_y,
300 double p2_z,
double p3_x,
double p3_y,
double p3_z,
double p4_x,
301 double p4_y,
double p4_z,
double& x,
double& y,
double& z,
double& dist)
303 const double EPS = 1e-30f;
305 double p13_x, p13_y, p13_z;
306 double p43_x, p43_y, p43_z;
307 double p21_x, p21_y, p21_z;
309 double d1343, d4321, d1321, d4343, d2121;
320 if (fabs(p43_x) < EPS && fabs(p43_y) < EPS && fabs(p43_z) < EPS)
326 if (fabs(p21_x) < EPS && fabs(p21_y) < EPS && fabs(p21_z) < EPS)
329 d1343 = p13_x * p43_x + p13_y * p43_y + p13_z * p43_z;
330 d4321 = p43_x * p21_x + p43_y * p21_y + p43_z * p21_z;
331 d1321 = p13_x * p21_x + p13_y * p21_y + p13_z * p21_z;
332 d4343 = p43_x * p43_x + p43_y * p43_y + p43_z * p43_z;
333 d2121 = p21_x * p21_x + p21_y * p21_y + p21_z * p21_z;
335 denom = d2121 * d4343 - d4321 * d4321;
336 if (fabs(denom) < EPS)
return false;
338 numer = d1343 * d4321 - d1321 * d4343;
340 double mua = numer / denom;
341 double mub = (d1343 + d4321 * mua) / d4343;
342 double pa_x, pa_y, pa_z;
343 double pb_x, pb_y, pb_z;
345 pa_x = p1_x + mua * p21_x;
346 pa_y = p1_y + mua * p21_y;
347 pa_z = p1_z + mua * p21_z;
349 pb_x = p3_x + mub * p43_x;
350 pb_y = p3_y + mub * p43_y;
351 pb_z = p3_z + mub * p43_z;
357 x = 0.5 * (pa_x + pb_x);
358 y = 0.5 * (pa_y + pb_y);
359 z = 0.5 * (pa_z + pb_z);
368 double R1_x_min,
double R1_x_max,
double R1_y_min,
double R1_y_max,
369 double R2_x_min,
double R2_x_max,
double R2_y_min,
double R2_y_max,
370 double R2_pose_x,
double R2_pose_y,
double R2_pose_phi)
375 double ccos = cos(R2_pose_phi);
376 double ssin = sin(R2_pose_phi);
378 xs[0] = R2_pose_x + ccos * R2_x_min - ssin * R2_y_min;
379 ys[0] = R2_pose_y + ssin * R2_x_min + ccos * R2_y_min;
381 xs[1] = R2_pose_x + ccos * R2_x_max - ssin * R2_y_min;
382 ys[1] = R2_pose_y + ssin * R2_x_max + ccos * R2_y_min;
384 xs[2] = R2_pose_x + ccos * R2_x_max - ssin * R2_y_max;
385 ys[2] = R2_pose_y + ssin * R2_x_max + ccos * R2_y_max;
387 xs[3] = R2_pose_x + ccos * R2_x_min - ssin * R2_y_max;
388 ys[3] = R2_pose_y + ssin * R2_x_min + ccos * R2_y_max;
392 if (R1_x_min <= xs[0] && xs[0] <= R1_x_max && R1_y_min <= ys[0] &&
395 if (R1_x_min <= xs[1] && xs[1] <= R1_x_max && R1_y_min <= ys[1] &&
398 if (R1_x_min <= xs[2] && xs[2] <= R1_x_max && R1_y_min <= ys[2] &&
401 if (R1_x_min <= xs[3] && xs[3] <= R1_x_max && R1_y_min <= ys[3] &&
420 for (
int idx = 0; idx < 4; idx++)
423 R1_x_min, R1_y_min, R1_x_max, R1_y_min, xs[idx], ys[idx],
424 xs[(idx + 1) % 4], ys[(idx + 1) % 4], ix, iy))
427 R1_x_max, R1_y_min, R1_x_max, R1_y_max, xs[idx], ys[idx],
428 xs[(idx + 1) % 4], ys[(idx + 1) % 4], ix, iy))
431 R1_x_max, R1_y_max, R1_x_min, R1_y_max, xs[idx], ys[idx],
432 xs[(idx + 1) % 4], ys[(idx + 1) % 4], ix, iy))
435 R1_x_min, R1_y_max, R1_x_min, R1_y_min, xs[idx], ys[idx],
436 xs[(idx + 1) % 4], ys[(idx + 1) % 4], ix, iy))
446 template <
class T2D,
class U2D,
class T3D,
class U3D>
461 proj1.generate2DObject(proj1_2D);
462 proj2.generate2DObject(proj2_2D);
465 if (
intersect(proj1_2D, proj2_2D, obj2D))
485 TPoint3D pMin = ((s11[0][i1] < s21[0][i1]) ? s21 : s11)[0];
486 TPoint3D pMax = ((s11[1][i1] < s21[1][i1]) ? s11 : s21)[1];
492 else if (pMax[i1] < pMin[i1])
508 TPoint2D pMin = ((s11[0][i1] < s21[0][i1]) ? s21 : s11)[0];
509 TPoint2D pMax = ((s11[1][i1] < s21[1][i1]) ? s11 : s21)[1];
515 else if (pMax[i1] < pMin[i1])
528 newPoint.
x = dummy.
x;
529 newPoint.
y = dummy.
y;
534 size_t N = poly.size();
536 for (
size_t i = 0; i < N; i++)
548 for (
size_t i = 0; i < 3; i++)
554 if (d < 0 || d > bestKnown)
return false;
582 TObject3D obj3D1, obj3D2, obj3Dp1, obj3Dp2;
686 for (
size_t i = 0; i < 3; i++)
707 size_t c1 = (i1 + 1) % 3, c2 = (i1 + 2) % 3;
757 const static size_t c1[] = {1, 2, 0};
758 const static size_t c2[] = {2, 0, 1};
760 for (
size_t i = 0; i < 3; i++)
770 for (
size_t k = 0; k < 3; k++) p[k] = r2.
pBase[k] + u * r2.
director[k];
848 double c = 0, n1 = 0, n2 = 0;
849 for (
size_t i = 0; i < 3; i++)
855 double s = sqrt(n1 * n2);
857 if (std::abs(s) < std::abs(c))
858 return (c / s < 0) ?
M_PI : 0;
865 double c = 0, n1 = 0, n2 = 0;
866 for (
size_t i = 0; i < 3; i++)
872 double s = sqrt(n1 * n2);
873 if (s <
geometryEpsilon)
throw std::logic_error(
"Invalid plane or line");
874 if (std::abs(s) < std::abs(c))
882 double c = 0, n1 = 0, n2 = 0;
883 for (
size_t i = 0; i < 3; i++)
889 double s = sqrt(n1 * n2);
891 if (std::abs(s) < std::abs(c))
892 return (c / s < 0) ?
M_PI : 0;
899 const double ang1 = std::atan2(-r1.
coefs[0], r1.
coefs[1]);
900 const double ang2 = std::atan2(-r2.
coefs[0], r2.
coefs[1]);
909 for (
size_t i = 0; i < 3; i++)
911 r.
pBase[i] = m(i, 3);
937 for (
size_t i = 0; i < 3; i++)
939 r.
pBase[i] = m(i, 3);
941 for (
size_t j = 0; j < 3; j++) r.
director[i] += m(i, j) * vector[j];
962 double c = cos(p.
phi);
963 double s = sin(p.
phi);
964 r.
coefs[0] = vector[0] * c + vector[1] * s;
965 r.
coefs[1] = -vector[0] * s + vector[1] * c;
971 size_t N = points.size();
972 if (N < 3)
return false;
974 const TPoint3D& orig = points[N - 1];
975 for (
size_t i = 0; i < N - 1; i++)
978 mat(i, 0) = p.
x - orig.
x;
979 mat(i, 1) = p.
y - orig.
y;
980 mat(i, 2) = p.
z - orig.
z;
992 size_t N = points.size();
993 if (N < 2)
return false;
995 const TPoint2D& orig = points[N - 1];
996 for (
size_t i = 0; i < N - 1; i++)
999 mat(i, 0) = p.
x - orig.
x;
1000 mat(i, 1) = p.
y - orig.
y;
1009 for (
size_t i = 1;; i++)
1016 catch (logic_error&)
1024 size_t N = points.size();
1025 if (N < 2)
return false;
1027 const TPoint3D& orig = points[N - 1];
1028 for (
size_t i = 0; i < N - 1; i++)
1031 mat(i, 0) = p.
x - orig.
x;
1032 mat(i, 1) = p.
y - orig.
y;
1033 mat(i, 2) = p.
z - orig.
z;
1042 for (
size_t i = 1;; i++)
try
1047 catch (logic_error&)
1058 for (
size_t i = 0; i < 3; i++)
1061 for (
size_t j = 0; j < 3; j++)
1072 for (
size_t i = 0; i < 3; i++)
1074 newPlane.
coefs[i] = 0;
1075 for (
size_t j = 0; j < 3; j++)
1076 newPlane.
coefs[i] += mat(i, j) * plane.
coefs[j];
1088 squareNorm<3, double>(newPlane.
coefs) /
1089 squareNorm<3, double>(plane.
coefs));
1096 size_t N = polygon.size();
1097 newPolygon.resize(N);
1098 for (
size_t i = 0; i < N; i++)
1099 project3D(polygon[i], newXYpose, newPolygon[i]);
1105 switch (
object.getType())
1118 object.getSegment(p);
1142 object.getPolygon(p);
1161 double c = cos(newXpose.
phi);
1162 double s = sin(newXpose.
phi);
1166 newLine.
coefs[1] * newXpose.
y);
1173 size_t N = line.size();
1175 for (
size_t i = 0; i < N; i++) newLine[i] = newXpose + line[i];
1224 if (!
intersect(p1, l2, obj))
return false;
1239 if (p1.size() < 3)
return false;
1242 poseNeg =
TPose2D(0, 0, 0) - pose;
1245 size_t N = projPoly.size();
1246 projPoly.push_back(projPoly[0]);
1247 double pre = projPoly[0].y;
1248 vector<TPoint2D> pnts;
1250 for (
size_t i = 1; i <= N; i++)
1252 double cur = projPoly[i].y;
1258 pnts[0] = projPoly[i - 1];
1259 pnts[1] = projPoly[i];
1263 pnts.push_back(projPoly[i]);
1267 double a = projPoly[i - 1].x;
1268 double c = projPoly[i].x;
1269 double x = a - pre * (c - a) / (cur - pre);
1270 pnts.emplace_back(x, 0);
1275 switch (pnts.size())
1294 throw std::logic_error(
"Polygon is not convex");
1313 segment = std::make_shared<TSegment2D>(s);
1319 std::shared_ptr<T2ListsOfSegments>
segms;
1324 segms = std::make_shared<T2ListsOfSegments>(s);
1328 common = std::make_shared<TCommonRegion>(c);
1357 size_t N = poly.size();
1359 for (
size_t i = 0; i < N - 1; i++)
1389 std::vector<TSegmentWithLine> segs1,segs2;
1392 unsigned int hmInters=0;
1393 for (
size_t i=0;i<segs1.size();i++) {
1395 for (
size_t j=0;j<segs2.size();j++)
if (
intersect(s1,segs2[j],obj)) {
1396 intersections(i,j)=obj;
1400 for (
size_t i=0;i<intersections.
rows();i++) {
1401 for (
size_t j=0;j<intersections.
cols();j++) cout<<
fromObject(intersections(i,j));
1405 if (p1.contains(p2[0])) {
1408 }
else if (p2.contains(p1[0])) {
1411 }
else return false;
1433 if (!
intersect(p, s2, obj))
return false;
1451 return intersectInCommonPlane<TPolygon2D, TSegment2D>(p1, s2, p, obj);
1459 if (!
intersect(p, r2, obj))
return false;
1476 return intersectInCommonPlane<TPolygon2D, TLine2D>(p1, r2, p, obj);
1484 if (!
intersect(p, p2, obj))
return false;
1493 return intersectInCommonPlane<TPolygon2D, TLine2D>(p1, ln, p, obj);
1502 if (!
intersect(pl1, pl2, obj))
return false;
1505 return intersectInCommonPlane<TPolygon2D, TPolygon2D>(
1510 if (!intersectInCommonPlane<TPolygon2D, TLine2D>(p1, ln, pl1, obj1))
1512 if (!intersectInCommonPlane<TPolygon2D, TLine2D>(p2, ln, pl2, obj2))
1533 for (
size_t i = 0; i < 3; i++)
1534 if ((min1[i] > max2[i]) || (min2[i] > max1[i]))
return false;
1546 if (!p1.
getPlane(pl1))
return false;
1547 if (!p2.
getPlane(pl2))
return false;
1553 const std::vector<TPolygon3D>& polys, std::vector<TPlane>& planes)
1555 size_t N = polys.size();
1562 const std::vector<TPolygon3D>& v1, std::vector<TPoint3D>& minP,
1563 std::vector<TPoint3D>& maxP)
1567 size_t N = v1.size();
1571 for (
const auto& it : v1)
1580 const std::vector<TPolygon3D>& v1,
const std::vector<TPolygon3D>& v2,
1583 std::vector<TPlane> w1, w2;
1586 std::vector<TPoint3D> minBounds1, maxBounds1, minBounds2, maxBounds2;
1589 size_t M = v1.size(), N = v2.size();
1593 for (
size_t i = 0; i < M; i++)
1594 for (
size_t j = 0; j < N; j++)
1596 minBounds1[i], maxBounds1[i], minBounds2[j], maxBounds2[j]))
1598 else if (
intersectAux(v1[i], w1[i], v2[j], w2[j], obj))
1604 const std::vector<TPolygon3D>& v1,
const std::vector<TPolygon3D>& v2,
1605 std::vector<TObject3D>& objs)
1608 std::vector<TPlane> w1, w2;
1611 std::vector<TPoint3D> minBounds1, maxBounds1, minBounds2, maxBounds2;
1615 auto itP1 = w1.begin();
1616 auto itMin1 = minBounds1.begin();
1617 auto itMax1 = maxBounds1.begin();
1618 for (
auto it1 = v1.begin(); it1 != v1.end();
1619 ++it1, ++itP1, ++itMin1, ++itMax1)
1622 const TPlane& plane1 = *itP1;
1623 auto itP2 = w2.begin();
1624 const TPoint3D &min1 = *itMin1, max1 = *itMax1;
1625 auto itMin2 = minBounds2.begin();
1626 auto itMax2 = maxBounds2.begin();
1627 for (
auto it2 = v2.begin(); it2 != v2.end();
1628 ++it2, ++itP2, ++itMin2, ++itMax2)
1631 else if (
intersectAux(poly1, plane1, *it2, *itP2, obj))
1632 objs.push_back(obj);
1809 double dx = p2.
x - p1.
x;
1810 double dy = p2.
y - p1.
y;
1811 return sqrt(dx * dx + dy * dy);
1816 double dx = p2.
x - p1.
x;
1817 double dy = p2.
y - p1.
y;
1818 double dz = p2.
z - p1.
z;
1819 return sqrt(dx * dx + dy * dy + dz * dz);
1825 size_t N = poly.size();
1826 if (N < 1)
throw logic_error(
"Empty polygon");
1829 for (
size_t i = 1; i < N; i++)
1831 pMin.
x = min(pMin.
x, poly[i].x);
1832 pMin.
y = min(pMin.
y, poly[i].y);
1833 pMax.
x = max(pMax.
x, poly[i].x);
1834 pMax.
y = max(pMax.
y, poly[i].y);
1876 for (
size_t i = 0; i < 3; i++)
1937 size_t N = poly.size();
1938 if (N < 1)
throw logic_error(
"Empty polygon");
1941 for (
size_t i = 1; i < N; i++)
1943 pMin.
x = min(pMin.
x, poly[i].x);
1944 pMin.
y = min(pMin.
y, poly[i].y);
1945 pMin.
z = min(pMin.
z, poly[i].z);
1946 pMax.
x = max(pMax.
x, poly[i].x);
1947 pMax.
y = max(pMax.
y, poly[i].y);
1948 pMax.
z = max(pMax.
z, poly[i].z);
1957 for (
size_t i = 0; i < 3; i++)
1959 plane.
coefs[i] = m(i, axis);
1980 const TPose3D& pose,
const double (&normal)[3],
TPlane& plane)
1985 for (
size_t i = 0; i < 3; i++)
1988 for (
size_t j = 0; j < 3; j++) plane.
coefs[i] += normal[j] * m(i, j);
1999 const uint8_t coord1 = (coord + 1) % 3;
2000 const uint8_t coord2 = (coord + 2) % 3;
2003 for (
size_t i = 0; i < 3; i++) m(i, coord) = vec[i];
2005 double h = hypot(vec[1], vec[2]);
2013 m(1, coord1) = -vec[2] / h;
2014 m(2, coord1) = vec[1] / h;
2016 m(0, coord2) = m(1, coord) * m(2, coord1) - m(2, coord) * m(1, coord1);
2017 m(1, coord2) = m(2, coord) * m(0, coord1) - m(0, coord) * m(2, coord1);
2018 m(2, coord2) = m(0, coord) * m(1, coord1) - m(1, coord) * m(0, coord1);
2028 std::vector<double> eigenVals;
2033 line.
coefs[0] = -eigenVecs(1, 1);
2034 line.
coefs[1] = eigenVecs(0, 1);
2036 return std::sqrt(eigenVals[0] / eigenVals[1]);
2042 return (e1 < e2) ? ((e1 < e3) ? 0 : 2) : ((e2 < e3) ? 1 : 2);
2051 std::vector<double> eigenVal;
2055 const size_t selected = 2;
2056 for (
size_t i = 0; i < 3; i++)
2058 line.
pBase[i] = means[i];
2059 line.
director[i] = eigenVec(i, selected);
2061 size_t i1 = (selected + 1) % 3, i2 = (selected + 2) % 3;
2062 return std::sqrt((eigenVal[i1] + eigenVal[i2]) / eigenVal[selected]);
2067 vector<double> means;
2071 std::vector<double> eigenVal;
2075 for (
size_t i = 0; i < 3; ++i)
2079 const size_t selected = 0;
2081 for (
size_t i = 0; i < 3; i++)
2083 plane.
coefs[i] = eigenVec(i, selected);
2086 size_t i1 = (selected + 1) % 3, i2 = (selected + 2) % 3;
2087 return std::sqrt(eigenVal[selected] / (eigenVal[i1] + eigenVal[i2]));
2091 const std::vector<TSegment3D>& segms, std::vector<TPolygon3D>& polys)
2093 std::vector<TSegment3D> tmp;
2105 : seg1(s1), seg2(s2), seg1Point(s1p), seg2Point(s2p)
2112 const std::vector<TSegment3D>&
segs;
2117 size_t N = vertices.size();
2119 for (
const auto& vertice : vertices)
2120 res.push_back(segs[vertice.seg2][vertice.seg2Point ? 1 : 0]);
2126 if (v.size() > 0 && v[0].seg1 == i)
return true;
2127 for (
const auto& it : v)
2128 if (it.seg1 == i || it.seg2 == i)
return false;
2133 std::vector<std::vector<MatchingVertex>>& res, std::vector<bool>& used,
2134 size_t searching,
unsigned char mask, std::vector<MatchingVertex>& current)
2136 for (
size_t i = 0; i < mat.
cols(); i++)
2137 if (!used[i] && mat.
isNotNull(searching, i))
2139 unsigned char match = mat(searching, i) & mask;
2145 if (
true == (s1p = (!(match & 3)))) match >>= 2;
2147 if (current.size() >= 2 && current[0].seg1 == i)
2149 if (s2p != current[0].seg1Point)
2151 current.emplace_back(searching, i, s1p, s2p);
2152 for (
auto it = current.begin(); it != current.end();
2154 used[it->seg2] =
true;
2155 res.push_back(current);
2164 current.emplace_back(searching, i, s1p, s2p);
2166 mat, res, used, i, s2p ? 0x3 : 0xC, current))
2177 std::vector<std::vector<MatchingVertex>>& res, std::vector<bool>& used)
2179 vector<MatchingVertex> cur;
2180 for (
size_t i = 0; i < used.size(); i++)
2185 const std::vector<TSegment3D>& segms, std::vector<TPolygon3D>& polys,
2186 std::vector<TSegment3D>& remainder)
2188 std::vector<TSegment3D> tmp;
2189 tmp.reserve(segms.size());
2190 for (
const auto& segm : segms)
2192 tmp.push_back(segm);
2194 remainder.push_back(segm);
2195 size_t N = tmp.size();
2197 for (
size_t i = 0; i < N - 1; i++)
2198 for (
size_t j = i + 1; j < N; j++)
2221 std::vector<std::vector<MatchingVertex>>
results;
2222 std::vector<bool> usedSegments(N,
false);
2227 for (
size_t i = 0; i < N; i++)
2228 if (!usedSegments[i]) remainder.push_back(tmp[i]);
2232 const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys)
2234 std::vector<TObject3D> tmp;
2235 std::vector<TSegment3D> sgms;
2236 TObject3D::getPolygons(objs, polys, tmp);
2237 TObject3D::getSegments(tmp, sgms);
2242 const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys,
2243 std::vector<TObject3D>& remainder)
2245 std::vector<TObject3D> tmp;
2246 std::vector<TSegment3D> sgms, remainderSgms;
2247 TObject3D::getPolygons(objs, polys, tmp);
2248 TObject3D::getSegments(tmp, sgms, remainder);
2251 remainder.end(), remainderSgms.begin(), remainderSgms.end());
2255 const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys,
2256 std::vector<TSegment3D>& remainder1, std::vector<TObject3D>& remainder2)
2258 std::vector<TObject3D> tmp;
2259 std::vector<TSegment3D> sgms;
2260 TObject3D::getPolygons(objs, polys, tmp);
2261 TObject3D::getSegments(tmp, sgms, remainder2);
2292 const TPolygon2D& poly, vector<TPolygon2D>& components)
2295 size_t N = poly.size();
2296 if (N <= 3)
return false;
2297 vector<TSegmentWithLine> segms(N);
2298 for (
size_t i = 0; i < N - 1; i++)
2303 for (
size_t i = 0; i < N; i++)
2305 size_t ii = (i + 2) % N, i_ = (i + N - 1) % N;
2306 for (
size_t j = ii; j != i_; j = (j + 1) % N)
2310 pnt, segms[i].segment
2311 [(
distance(pnt, segms[i].segment.point1) <
2312 distance(pnt, segms[i].segment.point2))
2317 for (
size_t k = 0; (k < N) && !cross; k++)
2327 if (cross)
continue;
2332 if (
sign(segms[i].line.evaluatePoint(poly[(i + N - 1) % N])) ==
2333 sign(segms[i].line.evaluatePoint(poly[(i + 2) % N])))
2338 p1.insert(p1.end(), poly.begin() + i + 1, poly.end());
2339 p1.insert(p1.end(), poly.begin(), poly.begin() + j + 1);
2341 p2.end(), poly.begin() + j + 1, poly.begin() + i + 1);
2346 p1.end(), poly.begin() + i + 1, poly.begin() + j + 1);
2347 p2.insert(p2.end(), poly.begin() + j + 1, poly.end());
2348 p2.insert(p2.end(), poly.begin(), poly.begin() + i + 1);
2356 vector<TPolygon2D> tempComps;
2359 components.end(), tempComps.begin(), tempComps.end());
2361 components.push_back(p1);
2364 components.end(), tempComps.begin(), tempComps.end());
2366 components.push_back(p2);
2387 const TPolygon3D& poly, vector<TPolygon3D>& components)
2390 if (!poly.
getPlane(p))
throw std::logic_error(
"Polygon is skew");
2397 vector<TPolygon2D> components2D;
2400 components.resize(components2D.size());
2402 components2D.begin(), components2D.end(), components.begin(),
2456 double ang1 = atan2(-l1.
coefs[0], l1.
coefs[1]);
2457 double ang2 = atan2(-l2.
coefs[0], l2.
coefs[1]);
2458 double ang = (ang1 + ang2) / 2;
2459 bis.
coefs[0] = -sin(ang);
2460 bis.
coefs[1] = cos(ang);
2485 const vector<TPolygonWithPlane>& vec,
const TPose3D& pose,
double& dist)
2493 for (
const auto& it : vec)
2503 double dx,
double dy,
double dz)
2507 if (std::abs(dx) < 1e-10 && std::abs(dy) < 1e-10 && std::abs(dz) < 1e-10)
2514 double n = sqrt(n_xy +
square(dz));
2521 if (fabs(dx) > 1e-4 || fabs(dy) > 1e-4)
2523 P(0, 1) = -dy / n_xy;
2524 P(1, 1) = dx / n_xy;