Fields.cpp
Go to the documentation of this file.
00001 //
00002 // Fields.cpp
00003 //
00004 //  Created on: 30.08.2011
00005 //      Author: wahnfla
00006 //
00007 
00008 #include "Fields.hpp"
00009 #include "../tools/errorhandler.hpp"    // for print...
00010 
00011 namespace datatypes
00012 {
00013 
00014 
00015 //
00016 // ////////////////////////////  FieldSegmentated /////////////////////////////////////////
00017 //
00018 
00020 void FieldSegmented::computePolygon()
00021 {
00022         m_fieldPolygon.clear();
00023 
00024         Polygon2D startPoints;
00025 
00026         for (FieldSegmentedPoints::const_iterator p = m_points.begin(); p != m_points.end(); ++p)
00027         {
00028                 double endDist = p->getEndDist();
00029 
00030                 if (!::isNaN(endDist))
00031                 {
00032                         m_fieldPolygon.push_back(Point2D::fromPolar(endDist, p->getAngle()));
00033                 }
00034 
00035                 double startDist = p->getStartDist();
00036                 double angle = p->getAngle();
00037 
00038                 if (::isNaN(startDist) == false)
00039                 {
00040                         startPoints.push_back(Point2D::fromPolar(startDist, angle));
00041                 }
00042         }
00043 
00044         if (startPoints.empty())
00045         {
00046                 // add origin if empty
00047                 m_fieldPolygon.push_back(Point2D(0., 0.));
00048         }
00049         else
00050         {
00051                 for (Polygon2D::const_reverse_iterator r = startPoints.rbegin(); r != startPoints.rend(); ++r)
00052                 {
00053                         m_fieldPolygon.push_back(*r);
00054                 }
00055         }
00056 
00057 //      printInfoMessage("Field: " + toString(m_fieldPolygon) + ".", true);
00058 }
00059 
00060 //
00061 // Returns the number of points.
00062 //
00063 UINT32 FieldSegmented::getNumberOfPoints()
00064 {
00065         return m_points.size();
00066 }
00067 
00068 //
00069 // Returns a copy of the points.
00070 //
00071 FieldSegmentedPoints FieldSegmented::getPoints()
00072 {
00073         return m_points;
00074 }
00075 
00076 //
00077 // //////////////////////////// FieldRectangle  /////////////////////////////////////////
00078 //
00079 
00080 
00082 void FieldRectangle::computePolygon()
00083 {
00084         m_fieldPolygon.clear();
00085 
00086         Point2D l = Point2D::fromPolar(m_length, m_rotAngle);
00087         Point2D w = Point2D::fromPolar(m_width, m_rotAngle - 90 * deg2rad);
00088 
00089         m_fieldPolygon.push_back(Point2D::fromPolar(m_refPointDist, m_refPointAngle));
00090         m_fieldPolygon.push_back(l + m_fieldPolygon[0]);
00091         m_fieldPolygon.push_back(w + l + m_fieldPolygon[0]);
00092         m_fieldPolygon.push_back(w + m_fieldPolygon[0]);
00093 
00094 //      printInfoMessage("Field: " + ::toString(m_fieldPolygon) + ".", true);
00095 }
00096 
00097 
00098 double FieldRectangle::getLength() const
00099 {
00100         return m_length;
00101 }
00102 
00103 double FieldRectangle::getRefPointAngle() const
00104 {
00105         return m_refPointAngle;
00106 }
00107 
00108 double FieldRectangle::getRefPointDist() const
00109 {
00110         return m_refPointDist;
00111 }
00112 
00113 double FieldRectangle::getRotAngle() const
00114 {
00115         return m_rotAngle;
00116 }
00117 
00118 double FieldRectangle::getWidth() const
00119 {
00120         return m_width;
00121 }
00122 
00123 //
00124 // Sets the length (x-direction) of the rectangle.
00125 //
00126 void FieldRectangle::setLength(double length)
00127 {
00128         if (length < 0.00001)
00129         {
00130                 // Is this value mistakenly 0 or negative?
00131                 printError("FieldRectangle::setLength: Length must not be 0 or negative - aborting!");
00132                 return;
00133         }
00134         
00135         this->m_length = length;
00136 }
00137 
00138 void FieldRectangle::setRefPointAngle(double refPointAngle)
00139 {
00140         this->m_refPointAngle = refPointAngle;
00141 }
00142 
00143 void FieldRectangle::setRefPointDist(double refPointDist)
00144 {
00145         this->m_refPointDist = refPointDist;
00146 }
00147 
00148 //
00149 // Set the rotation angle around the reference point.
00150 // Positive angles turn counter-clockwise.
00151 //
00152 void FieldRectangle::setRotAngle(double rotAngle)
00153 {
00154         if ((rotAngle < -PI) || (rotAngle > PI))
00155         {
00156                 // Is this value mistakenly written in [deg] instead of [rad]?
00157                 printWarning("FieldRectangle::setRotAngle: rotAngle is outside of the limit [-PI, PI], did you forget deg-2-rad conversion?");
00158         }
00159         this->m_rotAngle = rotAngle;
00160 }
00161 
00162 
00163 void FieldRectangle::setWidth(double width)
00164 {
00165         if (width < 0.00001)
00166         {
00167                 // Is this value mistakenly 0 or negative?
00168                 printError("FieldRectangle::setWidth: Width must not be 0 or negative - aborting!");
00169                 return;
00170         }
00171         
00172         this->m_width = width;
00173 }
00174 
00175 //
00176 // ///////////////////////// FieldRadial  ////////////////////////////////////////////
00177 //
00178 
00179 void FieldRadial::computePolygon()
00180 {
00181         printError("FieldRadial::computePolygon: The MRS does not support radial fields.");
00182 }
00183 
00184 
00185 UINT16 FieldRadial::getFirstAngle() const
00186 {
00187         return m_firstAngle;
00188 }
00189 
00190 UINT16 FieldRadial::getLastAngle() const
00191 {
00192         return m_lastAngle;
00193 }
00194 
00195 UINT32 FieldRadial::getMaxDist() const
00196 {
00197         return m_maxDist;
00198 }
00199 
00200 UINT32 FieldRadial::getMinDist() const
00201 {
00202         return m_minDist;
00203 }
00204 
00205 void FieldRadial::setFirstAngle(UINT16 firstAngle)
00206 {
00207         this->m_firstAngle = firstAngle;
00208 }
00209 
00210 void FieldRadial::setLastAngle(UINT16 lastAngle)
00211 {
00212         this->m_lastAngle = lastAngle;
00213 }
00214 
00215 void FieldRadial::setMaxDist(UINT32 maxDist)
00216 {
00217         this->m_maxDist = maxDist;
00218 }
00219 
00220 void FieldRadial::setMinDist(UINT32 minDist)
00221 {
00222         this->m_minDist = minDist;
00223 }
00224 
00225 //
00226 // ///////////////////////////// FieldDynamic ////////////////////////////////////////
00227 //
00228 
00229 
00230 double FieldDynamic::getMaxLength() const
00231 {
00232         return m_maxLength;
00233 }
00234 
00235 double FieldDynamic::getSpeedMax() const
00236 {
00237         return m_speedMax;
00238 }
00239 
00240 void FieldDynamic::setMaxLength(double maxLength)
00241 {
00242         this->m_maxLength = maxLength;
00243 }
00244 
00245 void FieldDynamic::setSpeedMax(double speedMax)
00246 {
00247         this->m_speedMax = speedMax;
00248 }
00249 
00250 //
00251 // /////////////////////////////  Fields ////////////////////////////////////////
00252 //
00253 
00254 const FieldParameter& Fields::getField(UINT16 fieldNumber) const
00255 {
00256         FieldVector::const_iterator f;
00257         for (f = m_fields.begin(); f != m_fields.end(); ++f)
00258         {
00259                 const FieldParameter* fp = *f;
00260                 if (fp->getFieldNumber() == fieldNumber)
00261                 {
00262                         break;
00263                 }
00264         }
00265 
00266         if (f == m_fields.end())
00267         {
00268                 dieWithError("Fields::getField(): No field available with the given number.");
00269         }
00270 
00271         // return the field as reference
00272         return *(*f);
00273 }
00274 
00275 //
00276 // Add a new field to the field vector.
00277 //
00278 void Fields::add(FieldParameter* field)
00279 {
00280         m_fields.push_back(field);
00281 }
00282 
00283 const UINT32 Fields::getUsedMemory() const
00284 {
00285         UINT32 sum = sizeof(*this);
00286         FieldVector::const_iterator iter;
00287         for (iter = m_fields.begin(); iter != m_fields.end(); iter++)
00288         {
00289 //              sum += iter->getUsedMemory();
00290                 sum += 1000;    // Panic! Failed to solve the const correctness issue.
00291         }
00292         return sum;
00293 }
00294 
00295 //
00296 //
00297 //
00298 UINT16 Fields::getNumberOfValidFields()
00299 {
00300         UINT16 numValid = 0;
00301         FieldVector::iterator iter;
00302         for (iter = m_fields.begin(); iter != m_fields.end(); iter++)
00303         {
00304                 if ((*iter)->getFieldNumber() != 0)
00305                 {
00306                         numValid++;
00307                 }
00308         }
00309         return numValid;
00310 }
00311 
00312 
00313 } // namespace datatypes


libsick_ldmrs
Author(s): SICK AG , Martin Günther , Jochen Sprickerhof
autogenerated on Thu Jun 6 2019 21:02:36