Polygon3D.cpp
Go to the documentation of this file.
1 //
2 //
3 // Polygon3d.cpp
4 //
5 
6 #include <cmath>
7 
8 #include "Polygon3D.hpp"
9 #include "Polygon2D.hpp"
10 #include <algorithm> // for std::sort()
11 #include <iterator>
12 #include <sstream>
13 
14 
15 // ////////////////////////////////////////////////////////////
16 namespace datatypes
17 {
18 
20 {
21 }
22 
24 {
25  push_back(p1); // Insert the element at the end of the list
26 }
27 
28 Polygon3D::Polygon3D(const Point3D& p1, const Point3D& p2)
29 {
30  push_back(p1); // Insert the element at the end of the list
31  push_back(p2); // Insert the element at the end of the list
32 }
33 Polygon3D::Polygon3D(const Point3D& p1, const Point3D& p2, const Point3D& p3)
34 {
35  push_back(p1); // Insert the element at the end of the list
36  push_back(p2); // Insert the element at the end of the list
37  push_back(p3); // Insert the element at the end of the list
38 }
39 
40 //
41 // Initialize this polygon with an existing one
42 //
43 Polygon3D::Polygon3D(const base_class& other_vector)
44  : base_class(other_vector)
45 {
46 }
47 
48 /*
49 void Polygon3D::fromString(const std::string& polyString)
50 {
51  clear();
52 
53  typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
54  tokenizer listOfTokens(polyString, boost::char_separator<char>(Polygon2D::getSeparatorCharacters()));
55  for (tokenizer::const_iterator iter = listOfTokens.begin();
56  iter != listOfTokens.end(); ++iter)
57  {
58  float x = boost::lexical_cast<float>(*iter);
59  ++iter;
60  if (iter == listOfTokens.end())
61  throw std::runtime_error("When parsing a Polygon3D string, the last point only has a x component.");
62  float y = boost::lexical_cast<float>(*iter);
63  ++iter;
64  if (iter == listOfTokens.end())
65  throw std::runtime_error("When parsing a Polygon3D string, the last point only has a x and y component.");
66  float z = boost::lexical_cast<float>(*iter);
67  this->push_back(Point3D(x, y, z));
68  }
69 }
70 */
71 
72 
74 std::string Polygon3D::toString() const
75 {
76  std::ostringstream o;
77  o << *this;
78  return o.str();
79 }
80 
82 static bool comparePointAngleXYPlane(const Point3D& p1, const Point3D& p2)
83 {
84  return (p1.getAngleAroundZ() < p2.getAngleAroundZ());
85 }
86 
92 {
93  std::sort(begin(), end(), comparePointAngleXYPlane);
94 }
95 
96 
97 
99 std::ostream& operator<<(std::ostream& os, const Polygon3D& poly)
100 {
101  os << "[ ";
102  if (!poly.empty())
103  {
104  // Polygon contains at least one element
105  std::copy (poly.begin(), poly.end() - 1,
106  std::ostream_iterator<Point3D>(os, ", "));
107  os << poly.back(); // Last element of the polygon
108  }
109  os << " ]";
110  return os;
111 }
112 
113 // Important: this code exists in a very similar for in the Polygon2D class.
114 // Bugfixes should be applied there also.
116 {
117  Polygon3D moved;
118 
119  // We need 2 points minimum to have a valid path
120  if (size() < 2)
121  {
122  if (size() > 0)
123  {
124  // only 1 point, move only y-direction.
125  moved.push_back(at(0) + Point3D(0.0, dist, 0.0));
126  }
127  return moved; // finished.
128  }
129 
130  // Here we have at least 2 points.
131 
132  // We need 2 segments to calculate the shift direction.
133  // Start with 90 deg angle.
134  double lastAng, ang; // Angles of the path segments.
135  Point3D lastPos = at(0);
136 
137  // The first segment is also handles in the loop. The needed but not existing preceeding segment
138  // is assumed to have the same direction.
139  lastAng = (at(1) - lastPos).getAngleAroundZ();
140  double edgeDist; // path width in a corner, is >= dist (path width)
141  Point3D shiftVec; // Vector from source edge to new edge.
142  Point3D pt;
143  size_t i;
144  for (i = 1; i < size(); i++)
145  {
146  pt = at(i);
147  ang = (pt - lastPos).getAngleAroundZ();
148  double diffAng = normalizeRadians(lastAng - ang);
149  double halfAngBetween = diffAng / 2.0;
150 
151  // Calculation of the length from edge to new edge
152  // The direction to the new path point (new edge) has the direction of the half angle between both segments.
153  double cosHalfAngBetween = std::abs(cos(halfAngBetween));
154 
155  // Do not calculate moved point if path angle to small
156  // This usually happens, if we have a zick-zack situation.
157  if ((cosHalfAngBetween) > 0.195) // 0.195 = cos( 1/2 * PI +- 1/16 * PI)
158  {
159  edgeDist = dist / cosHalfAngBetween;
160  shiftVec = Point3D(0.0, edgeDist, 0.0);
161  shiftVec.rotateAroundZ(lastAng - halfAngBetween);
162  Point3D np = lastPos + shiftVec;
163 
164  moved.push_back(np);
165  }
166 
167  lastAng = ang;
168  lastPos = pt;
169  }
170  // Add last point
171  moved.push_back(lastPos + shiftVec);
172 
173  return moved;
174 }
175 
176 } // namespace datatypes
177 
std::string toString() const
Text output for debugging; opposite of fromString().
Definition: Polygon3D.cpp:74
This class defines a point in the three-dimensional plane.
Definition: Point3D.hpp:25
double getAngleAroundZ() const
Returns the rotation angle around z (in the x-y-plane)
Definition: Point3D.cpp:179
void rotateAroundZ(double dYawAngle)
Rotate the point around the Z-axis ("Yaw angle")
Definition: Point3D.cpp:96
Polygon3D getMovedPath(double dist) const
Definition: Polygon3D.cpp:115
void sortForAngleXYPlane()
Definition: Polygon3D.cpp:91
Polygon3D()
Constructor for an empty polygon.
Definition: Polygon3D.cpp:19
double normalizeRadians(double radians)
Definition: MathToolbox.cpp:31
std::vector< Point3D > base_class
The base type. (Naming according to boost convention.)
Definition: Polygon3D.hpp:28
static bool comparePointAngleXYPlane(const Point3D &p1, const Point3D &p2)
Compare-function for the sort algorithm sortForAngleXYPlane().
Definition: Polygon3D.cpp:82
std::ostream & operator<<(std::ostream &os, const EvalCaseResult &result)


libsick_ldmrs
Author(s): SICK AG , Martin Günther , Jochen Sprickerhof
autogenerated on Sat Jun 8 2019 17:57:33