Surface.cpp
Go to the documentation of this file.
00001 
00011 // worldlib
00012 #include "worldlib/world/Surface.h"
00013 
00014 using namespace std;
00015 using namespace rail::spatial_temporal_learning::worldlib::geometry;
00016 using namespace rail::spatial_temporal_learning::worldlib::world;
00017 
00018 Surface::Surface(const string &name, const string &frame_id, const geometry::Pose &pose, const double width,
00019     const double depth, const double height) : Object(name, frame_id, pose, width, depth, height)
00020 {
00021 }
00022 
00023 const vector<PlacementSurface> &Surface::getPlacementSurfaces() const
00024 {
00025   return placement_surfaces_;
00026 }
00027 
00028 vector<PlacementSurface> &Surface::getPlacementSurfaces()
00029 {
00030   return placement_surfaces_;
00031 }
00032 
00033 size_t Surface::getNumPlacementSurfaces() const
00034 {
00035   return placement_surfaces_.size();
00036 }
00037 
00038 const PlacementSurface &Surface::getPlacementSurface(const size_t index) const
00039 {
00040   // check the index value first
00041   if (index < placement_surfaces_.size())
00042   {
00043     return placement_surfaces_[index];
00044   } else
00045   {
00046     throw out_of_range("Surface::getPlacementSurface : Placement surface index does not exist.");
00047   }
00048 }
00049 
00050 PlacementSurface &Surface::getPlacementSurface(const size_t index)
00051 {
00052   // check the index value first
00053   if (index < placement_surfaces_.size())
00054   {
00055     return placement_surfaces_[index];
00056   } else
00057   {
00058     throw out_of_range("Surface::getPlacementSurface : Placement surface index does not exist.");
00059   }
00060 }
00061 
00062 void Surface::addPlacementSurface(const PlacementSurface &placement_surface)
00063 {
00064   placement_surfaces_.push_back(placement_surface);
00065 }
00066 
00067 void Surface::removePlacementSurface(const size_t index)
00068 {
00069   // check the index value first
00070   if (index < placement_surfaces_.size())
00071   {
00072     placement_surfaces_.erase(placement_surfaces_.begin() + index);
00073   } else
00074   {
00075     throw out_of_range("Surface::removePlacementSurface : Placement surface index does not exist.");
00076   }
00077 }
00078 
00079 bool Surface::placementSurfaceExists(const string &name) const
00080 {
00081   // check if we can find it and catch any exceptions
00082   try
00083   {
00084     this->findPlacementSurface(name);
00085     return true;
00086   } catch (out_of_range &e)
00087   {
00088     return false;
00089   }
00090 }
00091 
00092 const PlacementSurface &Surface::findPlacementSurface(const string &name) const
00093 {
00094   // check each surface
00095   for (size_t i = 0; i < placement_surfaces_.size(); i++)
00096   {
00097     // perform a check
00098     if (placement_surfaces_[i].checkName(name))
00099     {
00100       return placement_surfaces_[i];
00101     }
00102   }
00103   // no match found
00104   throw out_of_range("Surface::findPlacementSurface : Placement surface name does not exist.");
00105 }
00106 
00107 PlacementSurface &Surface::findPlacementSurface(const string &name)
00108 {
00109   // check each surface
00110   for (size_t i = 0; i < placement_surfaces_.size(); i++)
00111   {
00112     // perform a check
00113     if (placement_surfaces_[i].checkName(name))
00114     {
00115       return placement_surfaces_[i];
00116     }
00117   }
00118   // no match found
00119   throw out_of_range("Surface::findPlacementSurface : Placement surface name does not exist.");
00120 }
00121 
00122 const PlacementSurface &Surface::findClosestPlacementSurface(const Position &position) const
00123 {
00124   return placement_surfaces_[this->findClosestPlacementSurfaceIndex(position)];
00125 }
00126 
00127 PlacementSurface &Surface::findClosestPlacementSurface(const Position &position)
00128 {
00129   return placement_surfaces_[this->findClosestPlacementSurfaceIndex(position)];
00130 }
00131 
00132 size_t Surface::findClosestPlacementSurfaceIndex(const Position &position) const
00133 {
00134   if (placement_surfaces_.empty())
00135   {
00136     throw out_of_range("Surface::findClosestPlacementSurface : No placement surfaces exist.");
00137   } else
00138   {
00139     // go through each and check the distance
00140     double best = numeric_limits<double>::infinity();
00141     size_t best_index = 0;
00142     for (size_t i = 0; i < placement_surfaces_.size(); i++)
00143     {
00144       // calculate distance
00145       double distance = position.distance(placement_surfaces_[i].getPose().getPosition());
00146       if (distance < best)
00147       {
00148         best = distance;
00149         best_index = i;
00150       }
00151     }
00152 
00153     return best_index;
00154   }
00155 }
00156 
00157 const vector<PointOfInterest> &Surface::getPointsOfInterest() const
00158 {
00159   return pois_;
00160 }
00161 
00162 vector<PointOfInterest> &Surface::getPointsOfInterest()
00163 {
00164   return pois_;
00165 }
00166 
00167 size_t Surface::getNumPointsOfInterest() const
00168 {
00169   return pois_.size();
00170 }
00171 
00172 const PointOfInterest &Surface::getPointOfInterest(const size_t index) const
00173 {
00174   // check the index value first
00175   if (index < pois_.size())
00176   {
00177     return pois_[index];
00178   } else
00179   {
00180     throw out_of_range("Surface::getPointOfInterest : Point of interest index does not exist.");
00181   }
00182 }
00183 
00184 PointOfInterest &Surface::getPointOfInterest(const size_t index)
00185 {
00186   // check the index value first
00187   if (index < pois_.size())
00188   {
00189     return pois_[index];
00190   } else
00191   {
00192     throw out_of_range("Surface::getPointOfInterest : Point of interest index does not exist.");
00193   }
00194 }
00195 
00196 void Surface::addPointOfInterest(const PointOfInterest &poi)
00197 {
00198   pois_.push_back(poi);
00199 }
00200 
00201 void Surface::removePointOfInterest(const size_t index)
00202 {
00203   // check the index value first
00204   if (index < pois_.size())
00205   {
00206     pois_.erase(pois_.begin() + index);
00207   } else
00208   {
00209     throw out_of_range("Surface::removePointOfInterest : Point of interest index does not exist.");
00210   }
00211 }
00212 
00213 bool Surface::pointOfInterestExists(const string &name) const
00214 {
00215   // check if we can find it and catch any exceptions
00216   try
00217   {
00218     this->findPointOfInterest(name);
00219     return true;
00220   } catch (out_of_range &e)
00221   {
00222     return false;
00223   }
00224 }
00225 
00226 const PointOfInterest &Surface::findPointOfInterest(const string &name) const
00227 {
00228   // check each surface
00229   for (size_t i = 0; i < pois_.size(); i++)
00230   {
00231     // perform a check
00232     if (pois_[i].checkName(name))
00233     {
00234       return pois_[i];
00235     }
00236   }
00237   // no match found
00238   throw out_of_range("Surface::findPointOfInterest : Point of interest name does not exist.");
00239 }
00240 
00241 
00242 PointOfInterest &Surface::findPointOfInterest(const string &name)
00243 {
00244   // check each surface
00245   for (size_t i = 0; i < pois_.size(); i++)
00246   {
00247     // perform a check
00248     if (pois_[i].checkName(name))
00249     {
00250       return pois_[i];
00251     }
00252   }
00253   // no match found
00254   throw out_of_range("Surface::findPointOfInterest : Point of interest name does not exist.");
00255 }


worldlib
Author(s): Russell Toris
autogenerated on Thu Jun 6 2019 20:55:36