Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

GridWorld.cc

Go to the documentation of this file.
00001 #include "Planners/GridWorld.h"
00002 #include <cstdlib>
00003 
00004 const float GridWorld::HCOST=1;
00005 const float GridWorld::VCOST=2;
00006 //const float GridWorld::DCOST=0; // no diagonals
00007 //const float GridWorld::DCOST=HCOST+VCOST; // equal cost
00008 //const float GridWorld::DCOST=std::min(HCOST,VCOST)/2; // low cost
00009 //const float GridWorld::DCOST=(HCOST+VCOST)/2; // avg cost
00010 const float GridWorld::DCOST=hypotf(HCOST,VCOST); // cartesian
00011 
00012 float GridWorld::heuristic(const State& st, const State& goal) const {
00013   float dx=std::abs((ssize_t)(goal.c-st.c));
00014   float dy=std::abs((ssize_t)(goal.r-st.r));
00015   
00016   if(DCOST<=0 || DCOST>=HCOST+VCOST)
00017     return dx*HCOST + dy*VCOST; // manhattan distance (no diagonals)
00018   
00019   if(DCOST<(HCOST+VCOST)/2)
00020     return std::max(dx,dy)*DCOST;
00021   
00022   if(DCOST<hypotf(HCOST,VCOST))
00023     return (dx>dy) ? dy*DCOST+(dx-dy)*HCOST : dx*DCOST+(dy-dx)*VCOST; // low cost diagonals
00024   
00025   return hypotf(dx*HCOST, dy*VCOST); // cartesian space (assume sqrt(2) diagonals)
00026 }
00027 
00028 const std::vector<std::pair<float,GridWorld::State> >& GridWorld::expand(const State* /*parent*/, const State& st, const State& /*goal*/) const {
00029   using std::make_pair;
00030   static std::vector<std::pair<float,GridWorld::State> > neighbors;
00031   neighbors.resize(0);
00032   if(st.r>0 && isspace(world[st.r-1][st.c])) {
00033     neighbors.push_back(make_pair(VCOST,State(st.r-1,st.c)));
00034     if(DCOST>0) {
00035       // diagonals:
00036       if(st.c>0 && isspace(world[st.r-1][st.c-1]))
00037         neighbors.push_back(make_pair(DCOST,State(st.r-1,st.c-1)));
00038       if(st.c<world[st.r-1].size()-1 && isspace(world[st.r-1][st.c+1]))
00039         neighbors.push_back(make_pair(DCOST,State(st.r-1,st.c+1)));
00040     }
00041   }
00042   if(st.r<world.size()-1 && isspace(world[st.r+1][st.c])) {
00043     neighbors.push_back(make_pair(VCOST,State(st.r+1,st.c)));
00044     if(DCOST>0) {
00045       // diagonals:
00046       if(st.c>0 && isspace(world[st.r+1][st.c-1]))
00047         neighbors.push_back(make_pair(DCOST,State(st.r+1,st.c-1)));
00048       if(st.c<world[st.r+1].size()-1 && isspace(world[st.r+1][st.c+1]))
00049         neighbors.push_back(make_pair(DCOST,State(st.r+1,st.c+1)));
00050     }
00051   }
00052   if(st.c>0 && isspace(world[st.r][st.c-1]))
00053     neighbors.push_back(make_pair(HCOST,State(st.r,st.c-1)));
00054   if(st.c<world[st.r].size()-1 && isspace(world[st.r][st.c+1]))
00055     neighbors.push_back(make_pair(HCOST,State(st.r,st.c+1)));
00056   
00057   return neighbors;
00058 }
00059 
00060 std::istream& operator>>(std::istream& is, GridWorld& gw) {
00061   gw.world.clear();
00062   std::string str;
00063   while(std::getline(is,str))
00064     gw.world.push_back(std::vector<char>(str.begin(),str.end()));
00065   return is;
00066 }
00067 
00068 std::ostream& operator<<(std::ostream& os, const GridWorld& gw) {
00069   using std::setw;
00070   const size_t WIDTH=4;
00071   os << setw(WIDTH) << ' ' << "  ";
00072   for(size_t c=0; c<gw.world[0].size(); ++c)
00073     os << (c%10);
00074   os << '\n' << setw(WIDTH) << ' ' << ' ';
00075   for(size_t c=0; c<gw.world[0].size()+2; ++c)
00076     os << "X";
00077   os << '\n';
00078   for(size_t r=0; r<gw.world.size(); ++r) {
00079     os << setw(WIDTH) << r << ' ';
00080     os << "X";
00081     for(size_t c=0; c<gw.world[r].size(); ++c) {
00082       os << gw.world[r][c];
00083     }
00084     os << "X\n";
00085   }
00086   os << setw(WIDTH) << ' ' << ' ';
00087   for(size_t c=0; c<gw.world[0].size()+2; ++c)
00088     os << "X";
00089   os << '\n';
00090   return os;
00091 }
00092 

Tekkotsu v5.1CVS
Generated Mon May 9 04:58:41 2016 by Doxygen 1.6.3