Go to the documentation of this file.00001
00011
00012 #include "worldlib/world/Room.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 Room::Room(const string &name, const string &frame_id, const Pose &pose, const double width, const double depth,
00019 const double height) : Object(name, frame_id, pose, width, depth, height)
00020 {
00021 }
00022
00023 const vector<Surface> &Room::getSurfaces() const
00024 {
00025 return surfaces_;
00026 }
00027
00028 vector<Surface> &Room::getSurfaces()
00029 {
00030 return surfaces_;
00031 }
00032
00033 size_t Room::getNumSurfaces() const
00034 {
00035 return surfaces_.size();
00036 }
00037
00038 const Surface &Room::getSurface(const size_t index) const
00039 {
00040
00041 if (index < surfaces_.size())
00042 {
00043 return surfaces_[index];
00044 } else
00045 {
00046 throw out_of_range("Room::getSurface : Surface index does not exist.");
00047 }
00048 }
00049
00050 Surface &Room::getSurface(const size_t index)
00051 {
00052
00053 if (index < surfaces_.size())
00054 {
00055 return surfaces_[index];
00056 } else
00057 {
00058 throw out_of_range("Room::getSurface : Surface index does not exist.");
00059 }
00060 }
00061
00062 void Room::addSurface(const Surface &room)
00063 {
00064 surfaces_.push_back(room);
00065 }
00066
00067 void Room::removeSurface(const size_t index)
00068 {
00069
00070 if (index < surfaces_.size())
00071 {
00072 surfaces_.erase(surfaces_.begin() + index);
00073 } else
00074 {
00075 throw out_of_range("Room::removeSurface : Surface index does not exist.");
00076 }
00077 }
00078
00079 bool Room::surfaceExists(const string &name) const
00080 {
00081
00082 try
00083 {
00084 this->findSurface(name);
00085 return true;
00086 } catch (out_of_range &e)
00087 {
00088 return false;
00089 }
00090 }
00091
00092 const Surface &Room::findSurface(const string &name) const
00093 {
00094
00095 for (size_t i = 0; i < surfaces_.size(); i++)
00096 {
00097
00098 if (surfaces_[i].checkName(name))
00099 {
00100 return surfaces_[i];
00101 }
00102 }
00103
00104 throw out_of_range("Room::findSurface : Surface name does not exist.");
00105 }
00106
00107 Surface &Room::findSurface(const string &name)
00108 {
00109
00110 for (size_t i = 0; i < surfaces_.size(); i++)
00111 {
00112
00113 if (surfaces_[i].checkName(name))
00114 {
00115 return surfaces_[i];
00116 }
00117 }
00118
00119 throw out_of_range("Room::findSurface : Surface name does not exist.");
00120 }
00121
00122 const Surface &Room::findClosestSurface(const Position &position) const
00123 {
00124 return surfaces_[this->findClosestSurfaceIndex(position)];
00125 }
00126
00127 Surface &Room::findClosestSurface(const Position &position)
00128 {
00129 return surfaces_[this->findClosestSurfaceIndex(position)];
00130 }
00131
00132 size_t Room::findClosestSurfaceIndex(const Position &position) const
00133 {
00134 if (surfaces_.empty())
00135 {
00136 throw out_of_range("Room::findClosestSurface : No surfaces exist.");
00137 } else
00138 {
00139
00140 double best = numeric_limits<double>::infinity();
00141 size_t best_index = 0;
00142 for (size_t i = 0; i < surfaces_.size(); i++)
00143 {
00144
00145 double distance = position.distance(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