33 const double& Px,
const double& Py,
const double& x1,
const double& y1,
34 const double& x2,
const double& y2,
double& out_x,
double& out_y)
36 if (x1 == x2 && y1 == y2)
45 double Ratio = ((Px - x1) * Dx + (Py - y1) * Dy) / (Dx * Dx + Dy * Dy);
60 out_x = x1 + (Ratio * Dx);
61 out_y = y1 + (Ratio * Dy);
71 const double& Px,
const double& Py,
const double& x1,
const double& y1,
72 const double& x2,
const double& y2,
double& out_x,
double& out_y)
74 if (x1 == x2 && y1 == y2)
83 double Ratio = ((Px - x1) * Dx + (Py - y1) * Dy) / (Dx * Dx + Dy * Dy);
85 out_x = x1 + (Ratio * Dx);
86 out_y = y1 + (Ratio * Dy);
94 const double& Px,
const double& Py,
const double& x1,
const double& y1,
95 const double& x2,
const double& y2)
97 if (x1 == x2 && y1 == y2)
105 double Ratio = ((Px - x1) * Dx + (Py - y1) * Dy) / (Dx * Dx + Dy * Dy);
107 return square(x1 + (Ratio * Dx) - Px) +
square(y1 + (Ratio * Dy) - Py);
115 const double x1,
const double y1,
const double x2,
const double y2,
116 const double x3,
const double y3,
const double x4,
const double y4,
117 double& ix,
double& iy)
119 double UpperX, UpperY, LowerX, LowerY, Ax, Bx, Cx, Ay, By, Cy, d, f, e,
138 if (UpperX < x4 || x3 < LowerX)
return false;
140 else if (UpperX < x3 || x4 < LowerX)
159 if (UpperY < y4 || y3 < LowerY)
return false;
161 else if (UpperY < y3 || y4 < LowerY)
166 d = (By * Cx) - (Bx * Cy);
167 f = (Ay * Bx) - (Ax * By);
171 if (d < 0 || d > f)
return false;
173 else if (d > 0 || d < f)
176 e = (Ax * Cy) - (Ay * Cx);
180 if (e < 0 || e > f)
return false;
182 else if (e > 0 || e < f)
185 Ratio = (Ax * -By) - (Ay * -Bx);
189 Ratio = ((Cy * -Bx) - (Cx * -By)) / Ratio;
190 ix = x1 + (Ratio * Ax);
191 iy = y1 + (Ratio * Ay);
195 if ((Ax * -Cy) == (-Cx * Ay))
213 const double x1,
const double y1,
const double x2,
const double y2,
214 const double x3,
const double y3,
const double x4,
const double y4,
215 float& ix,
float& iy)
219 ix =
static_cast<float>(
x);
220 iy =
static_cast<float>(
y);
228 const double& px,
const double& py,
unsigned int polyEdges,
229 const double* poly_xs,
const double* poly_ys)
234 if (polyEdges < 3)
return res;
238 for (i = 0; i < polyEdges; i++)
240 if ((poly_ys[i] <= py && py < poly_ys[j]) ||
241 (poly_ys[j] <= py && py < poly_ys[i]))
244 if (px - poly_xs[i] <
245 ((poly_xs[j] - poly_xs[i]) * (py - poly_ys[i]) /
246 (poly_ys[j] - poly_ys[i])))
259 const double& px,
const double& py,
unsigned int polyEdges,
260 const double* poly_xs,
const double* poly_ys)
263 double minDist = 1e20f;
271 for (i = 0; i < polyEdges; i++)
275 double closestX, closestY;
277 px, py, poly_xs[j], poly_ys[j], poly_xs[i], poly_ys[i], closestX,
280 minDist =
min(d, minDist);
293 const double& p1_x,
const double& p1_y,
const double& p1_z,
294 const double& p2_x,
const double& p2_y,
const double& p2_z,
295 const double& p3_x,
const double& p3_y,
const double& p3_z,
296 const double& p4_x,
const double& p4_y,
const double& p4_z,
double&
x,
297 double&
y,
double&
z,
double& dist)
299 const double EPS = 1e-30f;
301 double p13_x, p13_y, p13_z;
302 double p43_x, p43_y, p43_z;
303 double p21_x, p21_y, p21_z;
305 double d1343, d4321, d1321, d4343, d2121;
316 if (fabs(p43_x) < EPS && fabs(p43_y) < EPS && fabs(p43_z) < EPS)
322 if (fabs(p21_x) < EPS && fabs(p21_y) < EPS && fabs(p21_z) < EPS)
325 d1343 = p13_x * p43_x + p13_y * p43_y + p13_z * p43_z;
326 d4321 = p43_x * p21_x + p43_y * p21_y + p43_z * p21_z;
327 d1321 = p13_x * p21_x + p13_y * p21_y + p13_z * p21_z;
328 d4343 = p43_x * p43_x + p43_y * p43_y + p43_z * p43_z;
329 d2121 = p21_x * p21_x + p21_y * p21_y + p21_z * p21_z;
331 denom = d2121 * d4343 - d4321 * d4321;
332 if (fabs(denom) < EPS)
return false;
334 numer = d1343 * d4321 - d1321 * d4343;
336 double mua = numer / denom;
337 double mub = (d1343 + d4321 * mua) / d4343;
338 double pa_x, pa_y, pa_z;
339 double pb_x, pb_y, pb_z;
341 pa_x = p1_x + mua * p21_x;
342 pa_y = p1_y + mua * p21_y;
343 pa_z = p1_z + mua * p21_z;
345 pb_x = p3_x + mub * p43_x;
346 pb_y = p3_y + mub * p43_y;
347 pb_z = p3_z + mub * p43_z;
353 x = 0.5 * (pa_x + pb_x);
354 y = 0.5 * (pa_y + pb_y);
355 z = 0.5 * (pa_z + pb_z);
364 const double& R1_x_min,
const double& R1_x_max,
const double& R1_y_min,
365 const double& R1_y_max,
const double& R2_x_min,
const double& R2_x_max,
366 const double& R2_y_min,
const double& R2_y_max,
const double& R2_pose_x,
367 const double& R2_pose_y,
const double& R2_pose_phi)
372 double ccos = cos(R2_pose_phi);
373 double ssin = sin(R2_pose_phi);
375 xs[0] = R2_pose_x + ccos * R2_x_min - ssin * R2_y_min;
376 ys[0] = R2_pose_y + ssin * R2_x_min + ccos * R2_y_min;
378 xs[1] = R2_pose_x + ccos * R2_x_max - ssin * R2_y_min;
379 ys[1] = R2_pose_y + ssin * R2_x_max + ccos * R2_y_min;
381 xs[2] = R2_pose_x + ccos * R2_x_max - ssin * R2_y_max;
382 ys[2] = R2_pose_y + ssin * R2_x_max + ccos * R2_y_max;
384 xs[3] = R2_pose_x + ccos * R2_x_min - ssin * R2_y_max;
385 ys[3] = R2_pose_y + ssin * R2_x_min + ccos * R2_y_max;
389 if (R1_x_min <= xs[0] && xs[0] <= R1_x_max && R1_y_min <= ys[0] &&
392 if (R1_x_min <= xs[1] && xs[1] <= R1_x_max && R1_y_min <= ys[1] &&
395 if (R1_x_min <= xs[2] && xs[2] <= R1_x_max && R1_y_min <= ys[2] &&
398 if (R1_x_min <= xs[3] && xs[3] <= R1_x_max && R1_y_min <= ys[3] &&
417 for (
int idx = 0; idx < 4; idx++)
420 R1_x_min, R1_y_min, R1_x_max, R1_y_min, xs[idx], ys[idx],
421 xs[(idx + 1) % 4], ys[(idx + 1) % 4], ix, iy))
424 R1_x_max, R1_y_min, R1_x_max, R1_y_max, xs[idx], ys[idx],
425 xs[(idx + 1) % 4], ys[(idx + 1) % 4], ix, iy))
428 R1_x_max, R1_y_max, R1_x_min, R1_y_max, xs[idx], ys[idx],
429 xs[(idx + 1) % 4], ys[(idx + 1) % 4], ix, iy))
432 R1_x_min, R1_y_max, R1_x_min, R1_y_min, xs[idx], ys[idx],
433 xs[(idx + 1) % 4], ys[(idx + 1) % 4], ix, iy))
443 template <
class T2D,
class U2D,
class T3D,
class U3D>
458 proj1.generate2DObject(proj1_2D);
459 proj2.generate2DObject(proj2_2D);
462 if (
intersect(proj1_2D, proj2_2D, obj2D))
482 TPoint3D pMin = ((s11[0][i1] < s21[0][i1]) ? s21 : s11)[0];
483 TPoint3D pMax = ((s11[1][i1] < s21[1][i1]) ? s11 : s21)[1];
489 else if (pMax[i1] < pMin[i1])
505 TPoint2D pMin = ((s11[0][i1] < s21[0][i1]) ? s21 : s11)[0];
506 TPoint2D pMax = ((s11[1][i1] < s21[1][i1]) ? s11 : s21)[1];
512 else if (pMax[i1] < pMin[i1])
525 newPoint.
x = dummy.
x;
526 newPoint.
y = dummy.
y;
531 size_t N = poly.size();
533 for (
size_t i = 0; i < N; i++)
545 for (
size_t i = 0; i < 3; i++)
551 if (d < 0 || d > bestKnown)
return false;
566 if (
obj.getLine(lin3D))
578 TObject3D obj3D1, obj3D2, obj3Dp1, obj3Dp2;
624 const vector<TPolygon3D>& oldPolys, vector<TPolygonWithPlane>& newPolys)
626 size_t N = oldPolys.size();
628 for (
size_t i = 0; i < N; i++) newPolys[i] = oldPolys[i];
667 if (!
obj.getPoint(
p))
return false;
686 if (!
obj.getPoint(
p))
return false;
700 for (
size_t i = 0; i < 3; i++)
720 size_t c1 = (i1 + 1) % 3, c2 = (i1 + 2) % 3;
770 const static size_t c1[] = {1, 2, 0};
771 const static size_t c2[] = {2, 0, 1};
773 for (
size_t i = 0; i < 3; i++)
783 for (
size_t k = 0; k < 3; k++)
p[k] = r2.
pBase[k] + u * r2.
director[k];
840 else if (
obj.getPoint(
p))
853 else if (
obj.getPoint(
p))
861 double c = 0, n1 = 0, n2 = 0;
862 for (
size_t i = 0; i < 3; i++)
868 double s = sqrt(n1 * n2);
871 return (
c /
s < 0) ?
M_PI : 0;
878 double c = 0, n1 = 0, n2 = 0;
879 for (
size_t i = 0; i < 3; i++)
885 double s = sqrt(n1 * n2);
895 double c = 0, n1 = 0, n2 = 0;
896 for (
size_t i = 0; i < 3; i++)
902 double s = sqrt(n1 * n2);
905 return (
c /
s < 0) ?
M_PI : 0;
912 double c = 0, n1 = 0, n2 = 0;
913 for (
size_t i = 0; i < 2; i++)
919 double s = sqrt(n1 * n2);
922 return (
c /
s < 0) ?
M_PI : 0;
924 return acos(
c / sqrt(n1 * n2));
931 p.getHomogeneousMatrix(m);
932 for (
size_t i = 0; i < 3; i++)
934 r.pBase[i] = m.get_unsafe(i, 3);
935 r.director[i] = m.get_unsafe(i, axis);
959 p.getHomogeneousMatrix(m);
960 for (
size_t i = 0; i < 3; i++)
962 r.pBase[i] = m.get_unsafe(i, 3);
964 for (
size_t j = 0; j < 3; j++)
965 r.director[i] += m.get_unsafe(i, j) * vector[j];
971 r.coefs[0] = cos(
p.phi);
972 r.coefs[1] = -sin(
p.phi);
973 r.coefs[2] = -
r.coefs[0] *
p.x -
r.coefs[1] *
p.y;
978 r.coefs[0] = sin(
p.phi);
979 r.coefs[1] = cos(
p.phi);
980 r.coefs[2] = -
r.coefs[0] *
p.x -
r.coefs[1] *
p.y;
986 double c = cos(
p.phi);
987 double s = sin(
p.phi);
988 r.coefs[0] = vector[0] *
c + vector[1] *
s;
989 r.coefs[1] = -vector[0] *
s + vector[1] *
c;
990 r.coefs[2] = -
r.coefs[0] *
p.x -
r.coefs[1] *
p.y;
996 if (N < 3)
return false;
999 for (
size_t i = 0; i < N - 1; i++)
1002 mat(i, 0) =
p.x - orig.
x;
1003 mat(i, 1) =
p.y - orig.
y;
1004 mat(i, 2) =
p.z - orig.
z;
1016 size_t N =
points.size();
1017 if (N < 2)
return false;
1020 for (
size_t i = 0; i < N - 1; i++)
1023 mat(i, 0) =
p.x - orig.
x;
1024 mat(i, 1) =
p.y - orig.
y;
1033 for (
size_t i = 1;; i++)
try
1038 catch (logic_error&)
1045 size_t N =
points.size();
1046 if (N < 2)
return false;
1049 for (
size_t i = 0; i < N - 1; i++)
1052 mat(i, 0) =
p.x - orig.
x;
1053 mat(i, 1) =
p.y - orig.
y;
1054 mat(i, 2) =
p.z - orig.
z;
1063 for (
size_t i = 1;; i++)
try
1068 catch (logic_error&)
1079 for (
size_t i = 0; i < 3; i++)
1082 for (
size_t j = 0; j < 3; j++)
1093 for (
size_t i = 0; i < 3; i++)
1095 newPlane.
coefs[i] = 0;
1096 for (
size_t j = 0; j < 3; j++)
1097 newPlane.
coefs[i] += mat.get_unsafe(i, j) * plane.
coefs[j];
1109 squareNorm<3, double>(newPlane.
coefs) /
1110 squareNorm<3, double>(plane.
coefs));
1117 size_t N = polygon.size();
1118 newPolygon.resize(N);
1119 for (
size_t i = 0; i < N; i++)
1120 project3D(polygon[i], newXYpose, newPolygon[i]);
1126 switch (
object.getType())
1139 object.getSegment(
p);
1163 object.getPolygon(
p);
1182 double c = cos(newXpose.
phi);
1183 double s = sin(newXpose.
phi);
1187 newLine.
coefs[1] * newXpose.
y);
1194 size_t N = line.size();
1196 for (
size_t i = 0; i < N; i++) newLine[i] = newXpose + line[i];
1203 switch (
obj.getType())
1248 if (
obj.getPoint(
p))
1250 else if (
obj.getSegment(
s))
1260 if (p1.size() < 3)
return false;
1263 poseNeg =
TPose2D(0, 0, 0) - pose;
1266 size_t N = projPoly.size();
1267 projPoly.push_back(projPoly[0]);
1268 double pre = projPoly[0].y;
1269 vector<TPoint2D> pnts;
1271 for (
size_t i = 1; i <= N; i++)
1273 double cur = projPoly[i].y;
1279 pnts[0] = projPoly[i - 1];
1280 pnts[1] = projPoly[i];
1284 pnts.push_back(projPoly[i]);
1288 double a = projPoly[i - 1].x;
1289 double c = projPoly[i].x;
1290 double x =
a - pre * (
c -
a) / (cur - pre);
1296 switch (pnts.size())
1315 throw std::logic_error(
"Polygon is not convex");
1340 delete data.segment;
1353 if (&
r ==
this)
return *
this;
1355 switch (
type =
r.type)
1399 if (&
t ==
this)
return *
this;
1401 switch (
type =
t.type)
1440 size_t N = poly.size();
1442 for (
size_t i = 0; i < N - 1; i++)
1449 switch (
obj.getType())
1472 std::vector<TSegmentWithLine> segs1,segs2;
1475 unsigned int hmInters=0;
1476 for (
size_t i=0;i<segs1.size();i++) {
1478 for (
size_t j=0;j<segs2.size();j++)
if (
intersect(s1,segs2[j],
obj)) {
1479 intersections(i,j)=
obj;
1483 for (
size_t i=0;i<intersections.
rows();i++) {
1484 for (
size_t j=0;j<intersections.
cols();j++) cout<<
fromObject(intersections(i,j));
1488 if (p1.contains(p2[0])) {
1491 }
else if (p2.contains(p1[0])) {
1494 }
else return false;
1519 if (
obj.getPoint(pnt))
1522 p.getAsPose3DForcingOrigin(p1[0], pose);
1533 else if (
obj.getSegment(sgm))
1534 return intersectInCommonPlane<TPolygon2D, TSegment2D>(p1, s2,
p,
obj);
1544 if (
obj.getPoint(pnt))
1547 p.getAsPose3DForcingOrigin(p1[0], pose);
1558 else if (
obj.isLine())
1559 return intersectInCommonPlane<TPolygon2D, TLine2D>(p1, r2,
p,
obj);
1575 else if (
obj.getLine(ln))
1576 return intersectInCommonPlane<TPolygon2D, TLine2D>(p1, ln,
p,
obj);
1588 return intersectInCommonPlane<TPolygon2D, TPolygon2D>(
1590 else if (
obj.getLine(ln))
1593 if (!intersectInCommonPlane<TPolygon2D, TLine2D>(p1, ln, pl1, obj1))
1595 if (!intersectInCommonPlane<TPolygon2D, TLine2D>(p2, ln, pl2, obj2))
1616 for (
size_t i = 0; i < 3; i++)
1617 if ((min1[i] > max2[i]) || (min2[i] > max1[i]))
return false;
1629 if (!p1.
getPlane(pl1))
return false;
1630 if (!p2.
getPlane(pl2))
return false;
1636 const std::vector<TPolygon3D>& polys, std::vector<TPlane>& planes)
1638 size_t N = polys.size();
1645 const std::vector<TPolygon3D>&
v1, std::vector<TPoint3D>& minP,
1646 std::vector<TPoint3D>& maxP)
1650 size_t N =
v1.size();
1655 it !=
v1.end(); ++it)
1664 const std::vector<TPolygon3D>&
v1,
const std::vector<TPolygon3D>&
v2,
1667 std::vector<TPlane>
w1,
w2;
1670 std::vector<TPoint3D> minBounds1, maxBounds1, minBounds2, maxBounds2;
1673 size_t M =
v1.size(), N =
v2.size();
1677 for (
size_t i = 0; i < M; i++)
1678 for (
size_t j = 0; j < N; j++)
1680 minBounds1[i], maxBounds1[i], minBounds2[j], maxBounds2[j]))
1688 const std::vector<TPolygon3D>&
v1,
const std::vector<TPolygon3D>&
v2,
1689 std::vector<TObject3D>& objs)
1692 std::vector<TPlane>
w1,
w2;
1695 std::vector<TPoint3D> minBounds1, maxBounds1, minBounds2, maxBounds2;
1703 it1 !=
v1.end(); ++it1, ++itP1, ++itMin1, ++itMax1)
1706 const TPlane& plane1 = *itP1;
1708 const TPoint3D &min1 = *itMin1, max1 = *itMax1;
1712 it2 !=
v2.end(); ++it2, ++itP2, ++itMin2, ++itMax2)
1716 objs.push_back(
obj);
1893 double dx = p2.
x - p1.
x;
1894 double dy = p2.
y - p1.
y;
1895 return sqrt(dx * dx + dy * dy);
1900 double dx = p2.
x - p1.
x;
1901 double dy = p2.
y - p1.
y;
1902 double dz = p2.
z - p1.
z;
1903 return sqrt(dx * dx + dy * dy + dz * dz);
1909 size_t N = poly.size();
1910 if (N < 1)
throw logic_error(
"Empty polygon");
1913 for (
size_t i = 1; i < N; i++)
1915 pMin.
x =
min(pMin.
x, poly[i].x);
1916 pMin.
y =
min(pMin.
y, poly[i].y);
1917 pMax.
x = max(pMax.
x, poly[i].x);
1918 pMax.
y = max(pMax.
y, poly[i].y);
1948 -(
p.coefs[0] * r1.
pBase[0] +
p.coefs[1] * r1.
pBase[1] +
1949 p.coefs[2] * r1.
pBase[2]);
1950 return p.distance(r2.
pBase);
1960 for (
size_t i = 0; i < 3; i++)
2024 size_t N = poly.size();
2025 if (N < 1)
throw logic_error(
"Empty polygon");
2028 for (
size_t i = 1; i < N; i++)
2030 pMin.
x =
min(pMin.
x, poly[i].x);
2031 pMin.
y =
min(pMin.
y, poly[i].y);
2032 pMin.
z =
min(pMin.
z, poly[i].z);
2033 pMax.
x = max(pMax.
x, poly[i].x);
2034 pMax.
y = max(pMax.
y, poly[i].y);
2035 pMax.
z = max(pMax.
z, poly[i].z);
2044 for (
size_t i = 0; i < 3; i++)
2046 plane.
coefs[i] = m.get_unsafe(i, axis);
2047 plane.
coefs[3] -= plane.
coefs[i] * m.get_unsafe(i, 3);
2067 const TPose3D& pose,
const double (&normal)[3],
TPlane& plane)
2072 for (
size_t i = 0; i < 3; i++)
2075 for (
size_t j = 0; j < 3; j++)
2076 plane.
coefs[i] += normal[j] * m.get_unsafe(i, j);
2077 plane.
coefs[3] -= plane.
coefs[i] * m.get_unsafe(i, 3);
2086 char coord1 = (
coord + 1) % 3;
2087 char coord2 = (
coord + 2) % 3;
2090 for (
size_t i = 0; i < 3; i++)
matrix.set_unsafe(i,
coord, vec[i]);
2091 matrix.set_unsafe(0, coord1, 0);
2092 double h = hypot(vec[1], vec[2]);
2095 matrix.set_unsafe(1, coord1, 1);
2096 matrix.set_unsafe(2, coord1, 0);
2100 matrix.set_unsafe(1, coord1, -vec[2] / h);
2101 matrix.set_unsafe(2, coord1, vec[1] / h);
2122 covars.eigenVectors(eigenVec, eigenVal);
2124 (eigenVal.get_unsafe(0, 0) >= eigenVal.get_unsafe(1, 1)) ? 0 : 1;
2125 line.
coefs[0] = -eigenVec.get_unsafe(1, selected);
2126 line.
coefs[1] = eigenVec.get_unsafe(0, selected);
2129 eigenVal.get_unsafe(1 - selected, 1 - selected) /
2130 eigenVal.get_unsafe(selected, selected));
2136 return (e1 < e2) ? ((e1 < e3) ? 0 : 2) : ((e2 < e3) ? 1 : 2);
2142 return (e1 > e2) ? ((e1 > e3) ? 0 : 2) : ((e2 > e3) ? 1 : 2);
2150 covars.eigenVectors(eigenVec, eigenVal);
2152 eigenVal.get_unsafe(0, 0), eigenVal.get_unsafe(1, 1),
2153 eigenVal.get_unsafe(2, 2));
2154 for (
size_t i = 0; i < 3; i++)
2156 line.
pBase[i] = means[i];
2157 line.
director[i] = eigenVec.get_unsafe(i, selected);
2159 size_t i1 = (selected + 1) % 3, i2 = (selected + 2) % 3;
2161 (eigenVal.get_unsafe(i1, i1) + eigenVal.get_unsafe(i2, i2)) /
2162 eigenVal.get_unsafe(selected, selected));
2167 vector<double> means;
2171 covars.eigenVectors(eigenVec, eigenVal);
2172 for (
size_t i = 0; i < 3; ++i)
2173 if (eigenVal.get_unsafe(i, i) < 0 &&
2175 eigenVal.set_unsafe(i, i, 0);
2177 eigenVal.get_unsafe(0, 0), eigenVal.get_unsafe(1, 1),
2178 eigenVal.get_unsafe(2, 2));
2180 for (
size_t i = 0; i < 3; i++)
2182 plane.
coefs[i] = eigenVec.get_unsafe(i, selected);
2185 size_t i1 = (selected + 1) % 3, i2 = (selected + 2) % 3;
2187 eigenVal.get_unsafe(selected, selected) /
2188 (eigenVal.get_unsafe(i1, i1) + eigenVal.get_unsafe(i2, i2)));
2192 const std::vector<TSegment3D>& segms, std::vector<TPolygon3D>& polys)
2194 std::vector<TSegment3D> tmp;
2206 : seg1(s1), seg2(s2), seg1Point(s1p), seg2Point(s2p)
2213 const std::vector<TSegment3D>&
segs;
2218 size_t N = vertices.size();
2221 it != vertices.end(); ++it)
2222 res.push_back(segs[it->seg2][it->seg2Point ? 1 : 0]);
2228 if (
v.size() > 0 &&
v[0].seg1 == i)
return true;
2230 it !=
v.end(); ++it)
2231 if (it->seg1 == i || it->seg2 == i)
return false;
2236 std::vector<std::vector<MatchingVertex>>&
res, std::vector<bool>& used,
2237 size_t searching,
unsigned char mask, std::vector<MatchingVertex>& current)
2239 for (
size_t i = 0; i < mat.
cols(); i++)
2240 if (!used[i] && mat.
isNotNull(searching, i))
2242 unsigned char match = mat(searching, i) &
mask;
2248 if (
true == (s1p = (!(match & 3)))) match >>= 2;
2250 if (current.size() >= 2 && current[0].seg1 == i)
2252 if (s2p != current[0].seg1Point)
2258 it != current.end(); ++it)
2259 used[it->seg2] =
true;
2260 res.push_back(current);
2271 mat,
res, used, i, s2p ? 0x3 : 0xC, current))
2282 std::vector<std::vector<MatchingVertex>>&
res, std::vector<bool>& used)
2284 vector<MatchingVertex> cur;
2285 for (
size_t i = 0; i < used.size(); i++)
2290 const std::vector<TSegment3D>& segms, std::vector<TPolygon3D>& polys,
2291 std::vector<TSegment3D>& remainder)
2293 std::vector<TSegment3D> tmp;
2294 tmp.reserve(segms.size());
2296 it != segms.end(); ++it)
2300 remainder.push_back(*it);
2301 size_t N = tmp.size();
2303 for (
size_t i = 0; i < N - 1; i++)
2304 for (
size_t j = i + 1; j < N; j++)
2327 std::vector<std::vector<MatchingVertex>>
results;
2328 std::vector<bool> usedSegments(N,
false);
2333 for (
size_t i = 0; i < N; i++)
2334 if (!usedSegments[i]) remainder.push_back(tmp[i]);
2338 const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys)
2340 std::vector<TObject3D> tmp;
2341 std::vector<TSegment3D> sgms;
2348 const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys,
2349 std::vector<TObject3D>& remainder)
2351 std::vector<TObject3D> tmp;
2352 std::vector<TSegment3D> sgms, remainderSgms;
2357 remainder.end(), remainderSgms.begin(), remainderSgms.end());
2361 const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys,
2362 std::vector<TSegment3D>& remainder1, std::vector<TObject3D>& remainder2)
2364 std::vector<TObject3D> tmp;
2365 std::vector<TSegment3D> sgms;
2401 size_t N = poly.size();
2402 if (N <= 3)
return false;
2403 vector<TSegmentWithLine> segms(N);
2404 for (
size_t i = 0; i < N - 1; i++)
2409 for (
size_t i = 0; i < N; i++)
2411 size_t ii = (i + 2) % N, i_ = (i + N - 1) % N;
2412 for (
size_t j = ii; j != i_; j = (j + 1) % N)
2417 segms[i].segment[(
distance(pnt, segms[i].segment.point1) <
2418 distance(pnt, segms[i].segment.point2))
2423 for (
size_t k = 0; (k < N) && !
cross; k++)
2426 if (
obj.getPoint(pTmp) &&
2433 if (
cross)
continue;
2438 if (
sign(segms[i].line.evaluatePoint(poly[(i + N - 1) % N])) ==
2439 sign(segms[i].line.evaluatePoint(poly[(i + 2) % N])))
2444 p1.insert(p1.end(), poly.begin() + i + 1, poly.end());
2445 p1.insert(p1.end(), poly.begin(), poly.begin() + j + 1);
2447 p2.end(), poly.begin() + j + 1, poly.begin() + i + 1);
2452 p1.end(), poly.begin() + i + 1, poly.begin() + j + 1);
2453 p2.insert(p2.end(), poly.begin() + j + 1, poly.end());
2454 p2.insert(p2.end(), poly.begin(), poly.begin() + i + 1);
2462 vector<TPolygon2D> tempComps;
2465 components.end(), tempComps.begin(), tempComps.end());
2470 components.end(), tempComps.begin(), tempComps.end());
2496 if (!poly.
getPlane(
p))
throw std::logic_error(
"Polygon is skew");
2498 p.getAsPose3DForcingOrigin(poly[0], pose1);
2503 vector<TPolygon2D> components2D;
2508 components2D.begin(), components2D.end(),
components.begin(),
2559 else if (
obj.getPoint(
p))
2562 double ang1 = atan2(-l1.
coefs[0], l1.
coefs[1]);
2563 double ang2 = atan2(-l2.
coefs[0], l2.
coefs[1]);
2564 double ang = (ang1 + ang2) / 2;
2565 bis.
coefs[0] = -sin(ang);
2566 bis.
coefs[1] = cos(ang);
2582 p.getAsPose3D(pose);
2591 const vector<TPolygonWithPlane>& vec,
const TPose3D& pose,
double& dist)
2600 it != vec.end(); ++it)