Fields.cpp
Go to the documentation of this file.
1 //
2 // Fields.cpp
3 //
4 // Created on: 30.08.2011
5 // Author: wahnfla
6 //
7 
8 #include "Fields.hpp"
9 #include "../tools/errorhandler.hpp" // for print...
10 
11 namespace datatypes
12 {
13 
14 
15 //
16 // //////////////////////////// FieldSegmentated /////////////////////////////////////////
17 //
18 
21 {
22  m_fieldPolygon.clear();
23 
24  Polygon2D startPoints;
25 
26  for (FieldSegmentedPoints::const_iterator p = m_points.begin(); p != m_points.end(); ++p)
27  {
28  double endDist = p->getEndDist();
29 
30  if (!::isNaN(endDist))
31  {
32  m_fieldPolygon.push_back(Point2D::fromPolar(endDist, p->getAngle()));
33  }
34 
35  double startDist = p->getStartDist();
36  double angle = p->getAngle();
37 
38  if (::isNaN(startDist) == false)
39  {
40  startPoints.push_back(Point2D::fromPolar(startDist, angle));
41  }
42  }
43 
44  if (startPoints.empty())
45  {
46  // add origin if empty
47  m_fieldPolygon.push_back(Point2D(0., 0.));
48  }
49  else
50  {
51  for (Polygon2D::const_reverse_iterator r = startPoints.rbegin(); r != startPoints.rend(); ++r)
52  {
53  m_fieldPolygon.push_back(*r);
54  }
55  }
56 
57 // printInfoMessage("Field: " + toString(m_fieldPolygon) + ".", true);
58 }
59 
60 //
61 // Returns the number of points.
62 //
64 {
65  return m_points.size();
66 }
67 
68 //
69 // Returns a copy of the points.
70 //
72 {
73  return m_points;
74 }
75 
76 //
77 // //////////////////////////// FieldRectangle /////////////////////////////////////////
78 //
79 
80 
83 {
84  m_fieldPolygon.clear();
85 
86  Point2D l = Point2D::fromPolar(m_length, m_rotAngle);
87  Point2D w = Point2D::fromPolar(m_width, m_rotAngle - 90 * deg2rad);
88 
89  m_fieldPolygon.push_back(Point2D::fromPolar(m_refPointDist, m_refPointAngle));
90  m_fieldPolygon.push_back(l + m_fieldPolygon[0]);
91  m_fieldPolygon.push_back(w + l + m_fieldPolygon[0]);
92  m_fieldPolygon.push_back(w + m_fieldPolygon[0]);
93 
94 // printInfoMessage("Field: " + ::toString(m_fieldPolygon) + ".", true);
95 }
96 
97 
99 {
100  return m_length;
101 }
102 
104 {
105  return m_refPointAngle;
106 }
107 
109 {
110  return m_refPointDist;
111 }
112 
114 {
115  return m_rotAngle;
116 }
117 
119 {
120  return m_width;
121 }
122 
123 //
124 // Sets the length (x-direction) of the rectangle.
125 //
126 void FieldRectangle::setLength(double length)
127 {
128  if (length < 0.00001)
129  {
130  // Is this value mistakenly 0 or negative?
131  printError("FieldRectangle::setLength: Length must not be 0 or negative - aborting!");
132  return;
133  }
134 
135  this->m_length = length;
136 }
137 
138 void FieldRectangle::setRefPointAngle(double refPointAngle)
139 {
140  this->m_refPointAngle = refPointAngle;
141 }
142 
143 void FieldRectangle::setRefPointDist(double refPointDist)
144 {
145  this->m_refPointDist = refPointDist;
146 }
147 
148 //
149 // Set the rotation angle around the reference point.
150 // Positive angles turn counter-clockwise.
151 //
152 void FieldRectangle::setRotAngle(double rotAngle)
153 {
154  if ((rotAngle < -PI) || (rotAngle > PI))
155  {
156  // Is this value mistakenly written in [deg] instead of [rad]?
157  printWarning("FieldRectangle::setRotAngle: rotAngle is outside of the limit [-PI, PI], did you forget deg-2-rad conversion?");
158  }
159  this->m_rotAngle = rotAngle;
160 }
161 
162 
163 void FieldRectangle::setWidth(double width)
164 {
165  if (width < 0.00001)
166  {
167  // Is this value mistakenly 0 or negative?
168  printError("FieldRectangle::setWidth: Width must not be 0 or negative - aborting!");
169  return;
170  }
171 
172  this->m_width = width;
173 }
174 
175 //
176 // ///////////////////////// FieldRadial ////////////////////////////////////////////
177 //
178 
180 {
181  printError("FieldRadial::computePolygon: The MRS does not support radial fields.");
182 }
183 
184 
186 {
187  return m_firstAngle;
188 }
189 
191 {
192  return m_lastAngle;
193 }
194 
196 {
197  return m_maxDist;
198 }
199 
201 {
202  return m_minDist;
203 }
204 
206 {
207  this->m_firstAngle = firstAngle;
208 }
209 
211 {
212  this->m_lastAngle = lastAngle;
213 }
214 
216 {
217  this->m_maxDist = maxDist;
218 }
219 
221 {
222  this->m_minDist = minDist;
223 }
224 
225 //
226 // ///////////////////////////// FieldDynamic ////////////////////////////////////////
227 //
228 
229 
231 {
232  return m_maxLength;
233 }
234 
236 {
237  return m_speedMax;
238 }
239 
240 void FieldDynamic::setMaxLength(double maxLength)
241 {
242  this->m_maxLength = maxLength;
243 }
244 
245 void FieldDynamic::setSpeedMax(double speedMax)
246 {
247  this->m_speedMax = speedMax;
248 }
249 
250 //
251 // ///////////////////////////// Fields ////////////////////////////////////////
252 //
253 
254 const FieldParameter& Fields::getField(UINT16 fieldNumber) const
255 {
256  FieldVector::const_iterator f;
257  for (f = m_fields.begin(); f != m_fields.end(); ++f)
258  {
259  const FieldParameter* fp = *f;
260  if (fp->getFieldNumber() == fieldNumber)
261  {
262  break;
263  }
264  }
265 
266  if (f == m_fields.end())
267  {
268  dieWithError("Fields::getField(): No field available with the given number.");
269  }
270 
271  // return the field as reference
272  return *(*f);
273 }
274 
275 //
276 // Add a new field to the field vector.
277 //
279 {
280  m_fields.push_back(field);
281 }
282 
284 {
285  UINT32 sum = sizeof(*this);
286  FieldVector::const_iterator iter;
287  for (iter = m_fields.begin(); iter != m_fields.end(); iter++)
288  {
289 // sum += iter->getUsedMemory();
290  sum += 1000; // Panic! Failed to solve the const correctness issue.
291  }
292  return sum;
293 }
294 
295 //
296 //
297 //
299 {
300  UINT16 numValid = 0;
301  FieldVector::iterator iter;
302  for (iter = m_fields.begin(); iter != m_fields.end(); iter++)
303  {
304  if ((*iter)->getFieldNumber() != 0)
305  {
306  numValid++;
307  }
308  }
309  return numValid;
310 }
311 
312 
313 } // namespace datatypes
void printError(std::string message)
UINT16 getLastAngle() const
Definition: Fields.cpp:190
FieldSegmentedPoints getPoints()
Definition: Fields.cpp:71
double getMaxLength() const
Definition: Fields.cpp:230
void setMaxLength(double maxLength)
Definition: Fields.cpp:240
const FieldParameter & getField(UINT16 fieldNumber) const
Definition: Fields.cpp:254
const UINT32 getUsedMemory() const
Definition: Fields.cpp:283
UINT16 getNumberOfValidFields()
Definition: Fields.cpp:298
uint16_t UINT16
double getRefPointDist() const
Definition: Fields.cpp:108
UINT32 getMaxDist() const
Definition: Fields.cpp:195
double getLength() const
Definition: Fields.cpp:98
uint32_t UINT32
const UINT16 getFieldNumber() const
double getRotAngle() const
Definition: Fields.cpp:113
A polygon of 2D-points.
Definition: Polygon2D.hpp:43
double getRefPointAngle() const
Definition: Fields.cpp:103
void setLastAngle(UINT16 m_lastAngle)
Definition: Fields.cpp:210
#define deg2rad
static Point2D fromPolar(value_type dist, value_type angle)
Definition: Point2D.cpp:25
void dieWithError(std::string errorMessage)
void setRefPointDist(double refPointDist)
Definition: Fields.cpp:143
FieldSegmentedPoints m_points
Definition: Fields.hpp:122
double getSpeedMax() const
Definition: Fields.cpp:235
void setMaxDist(UINT32 m_maxDist)
Definition: Fields.cpp:215
std::vector< FieldSegmentedPoint > FieldSegmentedPoints
Definition: Fields.hpp:90
void setRotAngle(double rotAngle)
Definition: Fields.cpp:152
void setWidth(double width)
Definition: Fields.cpp:163
UINT32 getMinDist() const
Definition: Fields.cpp:200
double getWidth() const
Definition: Fields.cpp:118
void setSpeedMax(double speedMax)
Definition: Fields.cpp:245
UINT32 getNumberOfPoints()
Definition: Fields.cpp:63
void setFirstAngle(UINT16 m_firstAngle)
Definition: Fields.cpp:205
void setMinDist(UINT32 m_minDist)
Definition: Fields.cpp:220
void add(FieldParameter *field)
Definition: Fields.cpp:278
bool isNaN(floatT x)
Checks if a floating point value is Not-a-Number (NaN)
Definition: MathToolbox.hpp:63
UINT16 getFirstAngle() const
Definition: Fields.cpp:185
void setRefPointAngle(double refPointAngle)
Definition: Fields.cpp:138
void computePolygon()
fills the polygon clockwise
Definition: Fields.cpp:82
void printWarning(std::string message)
#define PI
void setLength(double length)
Definition: Fields.cpp:126
void computePolygon()
fills the polygon clockwise
Definition: Fields.cpp:20


libsick_ldmrs
Author(s): SICK AG , Martin Günther , Jochen Sprickerhof
autogenerated on Mon Oct 26 2020 03:27:30