/** * @file ControlConfig.cpp * @brief Implementation of ControlConfig * @author Alex Cunningham */ #include #include #include #include #include "ControlConfig.h" using namespace std; using namespace gtsam; // trick from some reading group #define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) // convert to strings template string toStr(const T& t) { ostringstream oss; oss << t; return oss.str(); } /* *************************************************************** */ void ControlConfig::print(const std::string& name) const { cout << "Config: " << name << endl; string agent; path p; FOREACH_PAIR(agent, p, paths_) { cout << "Agent: " << agent << "\n"; int i = 0; BOOST_FOREACH(ControlPoint pt, p) { ostringstream oss; oss << "Point: " << i++; pt.print(oss.str()); } cout << endl; } } /* *************************************************************** */ bool ControlConfig::equals(const ControlConfig& expected, double tol) const { if (paths_.size() != expected.paths_.size()) return false; string j; path pa; FOREACH_PAIR(j, pa, paths_) { if (!expected.involvesAgent(j)) return false; path pb = expected.getPath(j); if (pa.size() != pb.size()) return false; for (int i=0; isecond; } else { throw invalid_argument("Attempting to access path that does not exist"); } } /* *************************************************************** */ bool ControlConfig::involvesAgent(const std::string& agentID) const { return paths_.find(agentID) != paths_.end(); } /* *************************************************************** */ void ControlConfig::clearAgent(const std::string& agentID) { const_iterator it = paths_.find(agentID); if (it != paths_.end()) { path &p = paths_[agentID]; p.clear(); } else { throw invalid_argument("Attempting to clear agent that is not present"); } } /* *************************************************************** */ ControlConfig ControlConfig::exmap(const VectorConfig & delta) const { ControlConfig newConfig; string agent; path p; FOREACH_PAIR(agent, p, paths_) { newConfig.addAgent(agent); for (size_t i=0; i