MRPT  2.0.4
TPolygon3D.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "math-precomp.h" // Precompiled headers
11 
12 #include <mrpt/math/TLine2D.h>
13 #include <mrpt/math/TPlane.h>
14 #include <mrpt/math/TPolygon2D.h>
15 #include <mrpt/math/TPolygon3D.h>
16 #include <mrpt/math/TPose3D.h>
17 #include <mrpt/math/TSegment2D.h>
18 #include <mrpt/math/epsilon.h>
19 #include <mrpt/math/geometry.h> // project3D()
20 #include "polygons_utils.h"
21 
22 using namespace mrpt::math;
23 
25 {
26  p = TPolygon2D(*this);
27 }
28 
29 double TPolygon3D::distance(const TPoint3D& point) const
30 {
31  TPlane pl;
32  if (!getPlane(pl))
33  throw std::logic_error("Polygon does not conform a plane");
34  TPoint3D newPoint;
35  TPolygon3D newPoly;
36  TPose3D pose;
37  pl.getAsPose3DForcingOrigin(operator[](0), pose);
38  project3D(point, pose, newPoint);
39  project3D(*this, pose, newPoly);
40  double distance2D = TPolygon2D(newPoly).distance(TPoint2D(newPoint));
41  return sqrt(newPoint.z * newPoint.z + distance2D * distance2D);
42 }
43 bool TPolygon3D::contains(const TPoint3D& point) const
44 {
45  TPoint3D pMin, pMax;
46  getPrismBounds(*this, pMin, pMax);
47  if (point.x + getEpsilon() < pMin.x || point.y + getEpsilon() < pMin.y ||
48  point.z + getEpsilon() < pMin.z || point.x > pMax.x + getEpsilon() ||
49  point.y > pMax.y + getEpsilon() || point.z > pMax.z + getEpsilon())
50  return false;
51  TPlane plane;
52  if (!getPlane(plane))
53  throw std::logic_error("Polygon does not conform a plane");
54  TPolygon3D projectedPoly;
55  TPoint3D projectedPoint;
56  TPose3D pose;
57  // plane.getAsPose3DForcingOrigin(operator[](0),pose);
58  plane.getAsPose3D(pose);
59  CMatrixDouble44 P_inv;
60  pose.getInverseHomogeneousMatrix(P_inv);
61  pose.fromHomogeneousMatrix(P_inv);
62  project3D(point, pose, projectedPoint);
63  if (std::abs(projectedPoint.z) >= getEpsilon())
64  return false; // Point is not inside the polygon's plane.
65  project3D(*this, pose, projectedPoly);
66  return TPolygon2D(projectedPoly).contains(TPoint2D(projectedPoint));
67 }
68 void TPolygon3D::getAsSegmentList(vector<TSegment3D>& v) const
69 {
70  size_t N = size();
71  v.resize(N);
72  for (size_t i = 0; i < N - 1; i++)
73  v[i] = TSegment3D(operator[](i), operator[](i + 1));
74  v[N - 1] = TSegment3D(operator[](N - 1), operator[](0));
75 }
76 bool TPolygon3D::getPlane(TPlane& p) const { return conformAPlane(*this, p); }
78 {
79  getRegressionPlane(*this, p);
80 }
82 {
83  std::for_each(begin(), end(), FAddPoint<TPoint3D, 3>(p));
84  size_t N = size();
85  p.x /= N;
86  p.y /= N;
87  p.z /= N;
88 }
89 bool TPolygon3D::isSkew() const { return !mrpt::math::conformAPlane(*this); }
92 {
94  removeUnusedVertices(*this);
95 }
97 {
98  size_t N = p.size();
99  resize(N);
100  for (size_t i = 0; i < N; i++) operator[](i) = p[i];
101 }
103  size_t numEdges, double radius, TPolygon3D& poly)
104 {
105  if (numEdges < 3 || std::abs(radius) < getEpsilon())
106  throw std::logic_error(
107  "Invalid arguments for regular polygon creations");
108  poly.resize(numEdges);
109  for (size_t i = 0; i < numEdges; i++)
110  {
111  double angle = i * 2 * M_PI / numEdges;
112  poly[i] = TPoint3D(radius * cos(angle), radius * sin(angle), 0);
113  }
114 }
116  size_t numEdges, double radius, TPolygon3D& poly, const TPose3D& pose)
117 {
118  createRegularPolygon(numEdges, radius, poly);
119  for (size_t i = 0; i < numEdges; i++) pose.composePoint(poly[i], poly[i]);
120 }
void project3D(const TPoint3D &point, const mrpt::math::TPose3D &newXYpose, TPoint3D &newPoint)
Uses the given pose 3D to project a point into a new base.
Definition: geometry.h:300
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
Auxiliary functor class to compute polygon&#39;s center.
TPoint2D_< double > TPoint2D
Lightweight 2D point.
Definition: TPoint2D.h:213
void getCenter(TPoint3D &p) const
Get polygon&#39;s central point.
Definition: TPolygon3D.cpp:81
size_t size(const MATRIXLIKE &m, const int dim)
TPolygon3D()
Default constructor.
Definition: TPolygon3D.h:48
void fromHomogeneousMatrix(const mrpt::math::CMatrixDouble44 &HG)
Definition: TPose3D.cpp:201
STL namespace.
void removeRepVertices(T &poly)
bool contains(const TPoint2D &point) const
Check whether a point is inside (or within geometryEpsilon of a polygon edge).
Definition: TPolygon2D.cpp:69
bool conformAPlane(const std::vector< TPoint3D > &points)
Checks whether this polygon or set of points acceptably fits a plane.
Definition: geometry.cpp:969
static void createRegularPolygon(size_t numEdges, double radius, TPolygon3D &poly)
Static method to create a regular polygon, given its size and radius.
Definition: TPolygon3D.cpp:102
void getAsPose3D(mrpt::math::TPose3D &outPose) const
Definition: TPlane.cpp:73
This base provides a set of functions for maths stuff.
3D segment, consisting of two points.
Definition: TSegment3D.h:20
void removeRedundantVertices()
Erase every redundant vertex, thus saving space.
Definition: TPolygon3D.cpp:91
void getAsSegmentList(std::vector< TSegment3D > &v) const
Gets as set of segments, instead of set of points.
Definition: TPolygon3D.cpp:68
void getAsPose3DForcingOrigin(const TPoint3D &center, TPose3D &pose) const
Definition: TPlane.cpp:85
double distance(const TPoint2D &point) const
Distance to a point (always >=0)
Definition: TPolygon2D.cpp:22
3D Plane, represented by its equation
Definition: TPlane.h:22
void composePoint(const TPoint3D &l, TPoint3D &g) const
Definition: TPose3D.cpp:80
double getRegressionPlane(const std::vector< TPoint3D > &points, TPlane &plane)
Using eigenvalues, gets the best fitting plane for a set of 3D points.
Definition: geometry.cpp:2065
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:268
void getBestFittingPlane(TPlane &p) const
Gets the best fitting plane, disregarding whether the polygon actually fits inside or not...
Definition: TPolygon3D.cpp:77
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
void removeUnusedVertices(T &poly)
const_iterator end() const
Definition: ts_hash_map.h:246
const_iterator begin() const
Definition: ts_hash_map.h:240
double getEpsilon()
Gets the value of the geometric epsilon (default = 1e-5)
Definition: geometry.cpp:34
void getPrismBounds(const std::vector< TPoint3D > &poly, TPoint3D &pMin, TPoint3D &pMax)
Gets the prism bounds of a 3D polygon or set of 3D points.
Definition: geometry.cpp:1934
void getInverseHomogeneousMatrix(mrpt::math::CMatrixDouble44 &HG) const
Definition: TPose3D.cpp:228
void removeRepeatedVertices()
Remove polygon&#39;s repeated vertices.
Definition: TPolygon3D.cpp:90
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
void generate2DObject(TPolygon2D &p) const
Projects into a 2D space, discarding the z.
Definition: TPolygon3D.cpp:24
images resize(NUM_IMGS)
bool isSkew() const
Check whether the polygon is skew.
Definition: TPolygon3D.cpp:89
bool getPlane(TPlane &p) const
Gets a plane which contains the polygon.
Definition: TPolygon3D.cpp:76
bool contains(const TPoint3D &point) const
Check whether a point is inside (or within geometryEpsilon of a polygon edge).
Definition: TPolygon3D.cpp:43
2D polygon, inheriting from std::vector<TPoint2D>.
Definition: TPolygon2D.h:21
3D polygon, inheriting from std::vector<TPoint3D>
Definition: TPolygon3D.h:20
double distance(const TPoint3D &point) const
Distance to point (always >=0)
Definition: TPolygon3D.cpp:29



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 02a75dd25 Thu Jun 4 01:49:55 2020 +0200 at jue jun 4 02:00:12 CEST 2020