closed_list.cpp
Go to the documentation of this file.
00001 #include "closed_list.h"
00002 
00003 // #include "state.h"
00004 
00005 #include <algorithm>
00006 #include <cassert>
00007 using namespace std;
00008 
00025 ClosedList::ClosedList()
00026 {
00027 }
00028 
00029 ClosedList::~ClosedList()
00030 {
00031 }
00032 
00033 std::size_t TssHash::operator()(const TimeStampedState &tss) const
00034 {
00035     std::size_t ret = 0;
00036     for(int i = 0; i < tss.state.size(); ++i) {
00037         ret += tss.state[i] * (i + 1);
00038     }
00039     ret += tss.scheduled_effects.size() * (tss.state.size() + 1);
00040     ret += tss.conds_over_all.size() * (tss.state.size() + 2);
00041     ret += tss.conds_at_end.size() * (tss.state.size() + 3);
00042 
00043     return ret;
00044 }
00045 
00046 bool prevailEquals(const Prevail &prev1, const Prevail &prev2)
00047 {
00048     if(prev1.var != prev2.var)
00049         return false;
00050     if(!double_equals(prev1.prev, prev2.prev))
00051         return false;
00052     return true;
00053 }
00054 
00055 bool scheduledConditionEquals(const ScheduledCondition &cond1, const ScheduledCondition &cond2)
00056 {
00057     if(!double_equals(cond1.time_increment, cond2.time_increment))
00058         return false;
00059     if(cond1.var != cond2.var)
00060         return false;
00061     if(!double_equals(cond1.prev, cond2.prev))
00062         return false;
00063     return true;
00064 }
00065 
00066 bool scheduledEffectEquals(const ScheduledEffect &eff1, const ScheduledEffect &eff2)
00067 {
00068     if(!double_equals(eff1.time_increment, eff2.time_increment))
00069         return false;
00070     if(eff1.var != eff2.var)
00071         return false;
00072     if(!double_equals(eff1.pre, eff2.pre))
00073         return false;
00074     if(eff1.var_post != eff2.var_post)
00075         return false;
00076     if(!double_equals(eff1.post, eff2.post))
00077         return false;
00078     if(eff1.fop != eff2.fop)
00079         return false;
00080     if(eff1.cond_start.size() != eff2.cond_start.size())
00081         return false;
00082     if(eff1.cond_overall.size() != eff2.cond_overall.size())
00083         return false;
00084     if(eff1.cond_end.size() != eff2.cond_end.size())
00085         return false;
00086     if(!equal(eff1.cond_start.begin(), eff1.cond_start.end(),
00087                 eff2.cond_start.begin(), prevailEquals))
00088         return false;
00089     if(!equal(eff1.cond_overall.begin(), eff1.cond_overall.end(),
00090                 eff2.cond_overall.begin(), prevailEquals))
00091         return false;
00092     if(!equal(eff1.cond_end.begin(), eff1.cond_end.end(),
00093                 eff2.cond_end.begin(), prevailEquals))
00094         return false;
00095     return true;
00096 }
00097 
00098 bool TssEquals::operator()(const TimeStampedState &tss1, const TimeStampedState &tss2) const
00099 {
00100     assert(tss1.state.size() == tss2.state.size());
00101     if(tss1.scheduled_effects.size() != tss2.scheduled_effects.size())
00102         return false;
00103     if(tss1.conds_over_all.size() != tss2.conds_over_all.size())
00104         return false;
00105     if(tss1.conds_at_end.size() != tss2.conds_at_end.size())
00106         return false;
00107     if(!equal(tss1.scheduled_effects.begin(), tss1.scheduled_effects.end(),
00108                 tss2.scheduled_effects.begin(), scheduledEffectEquals))
00109         return false;
00110     if(!equal(tss1.conds_over_all.begin(), tss1.conds_over_all.end(),
00111                 tss2.conds_over_all.begin(), scheduledConditionEquals))
00112         return false;
00113     if(!equal(tss1.conds_at_end.begin(), tss1.conds_at_end.end(),
00114                 tss2.conds_at_end.begin(), scheduledConditionEquals))
00115         return false;
00116 
00117     for(int i = 0; i < tss1.state.size(); ++i) {
00118         if (!(g_variable_types[i] == primitive_functional
00119                     || g_variable_types[i] == logical)) {
00120             continue;
00121         }
00122 
00123         if (!double_equals(tss1.state[i], tss2.state[i]))
00124             return false;
00125     }
00126     return true;
00127 }
00128 
00129 //bool ClosedList::TssCompareIgnoreTimestamps::operator()(const TimeStampedState &tss1, const TimeStampedState &tss2) const {
00130 //    if(tss1.timestamp < tss2.timestamp)
00131 //      return true;
00132 //    if(tss1.timestamp > tss2.timestamp)
00133 //      return false;
00134 //    if(lexicographical_compare(tss1.state.begin(), tss1.state.end(),
00135 //          tss2.state.begin(), tss2.state.end()))
00136 //      return true;
00137 //    if(lexicographical_compare(tss2.state.begin(), tss2.state.end(),
00138 //          tss1.state.begin(), tss1.state.end()))
00139 //      return false;
00140 //    if(tss1.scheduled_effects.size() < tss2.scheduled_effects.size())
00141 //      return true;
00142 //    if(tss1.scheduled_effects.size() > tss2.scheduled_effects.size())
00143 //      return false;
00144 //    if(tss1.conds_over_all.size() < tss2.conds_over_all.size())
00145 //      return true;
00146 //    if(tss1.conds_over_all.size() > tss2.conds_over_all.size())
00147 //      return false;
00148 //    if(tss1.conds_at_end.size() < tss2.conds_at_end.size())
00149 //      return true;
00150 //    if(tss1.conds_at_end.size() > tss2.conds_at_end.size())
00151 //      return false;
00152 //    if(lexicographical_compare(tss1.scheduled_effects.begin(),
00153 //          tss1.scheduled_effects.end(), tss2.scheduled_effects.begin(),
00154 //          tss2.scheduled_effects.end()))
00155 //      return true;
00156 //    if(lexicographical_compare(tss2.scheduled_effects.begin(),
00157 //          tss2.scheduled_effects.end(), tss1.scheduled_effects.begin(),
00158 //          tss1.scheduled_effects.end()))
00159 //      return false;
00160 //    if(lexicographical_compare(tss1.conds_over_all.begin(),
00161 //          tss1.conds_over_all.end(), tss2.conds_over_all.begin(),
00162 //          tss2.conds_over_all.end()))
00163 //      return true;
00164 //    if(lexicographical_compare(tss2.conds_over_all.begin(),
00165 //          tss2.conds_over_all.end(), tss1.conds_over_all.begin(),
00166 //          tss1.conds_over_all.end()))
00167 //      return false;
00168 //    if(lexicographical_compare(tss1.conds_at_end.begin(),
00169 //          tss1.conds_at_end.end(), tss2.conds_at_end.begin(),
00170 //          tss2.conds_at_end.end()))
00171 //      return true;
00172 //    if(lexicographical_compare(tss2.conds_at_end.begin(),
00173 //          tss2.conds_at_end.end(), tss1.conds_at_end.begin(),
00174 //          tss1.conds_at_end.end()))
00175 //      return false;
00176 //    return false;
00177 //}
00178 
00179 
00180 const TimeStampedState *ClosedList::insert(
00181         TimeStampedState &entry, const TimeStampedState *predecessor,
00182         const Operator *annotation)
00183 {
00184     ClosedListMap::iterator ret =
00185         closed.insert(ValuePair(entry, PredecessorInfo(predecessor, annotation)));
00186     //    assert(ret.second);
00187     return &ret->first;
00188 }
00189 
00190 void ClosedList::clear()
00191 {
00192     closed.clear();
00193 }
00194 
00195 bool ClosedList::contains(const TimeStampedState &entry) const
00196 {
00197     double min_so_far = get_min_ts_of_key(entry);
00198     double diff = entry.timestamp - min_so_far;
00199     return (!(diff + EPSILON < 0));
00200 }
00201 
00202 const TimeStampedState& ClosedList::get(const TimeStampedState &state) const
00203 {
00204     std::pair<ClosedListMap::const_iterator, ClosedListMap::const_iterator>
00205         entries = closed.equal_range(state);
00206     const TimeStampedState *ret = &(closed.find(state)->first);
00207     ClosedListMap::const_iterator it = entries.first;
00208     for (; it != entries.second; ++it) {
00209         if (it->first.timestamp + EPSILON < ret->timestamp) {
00210             ret = &(it->first);
00211         }
00212     }
00213     return *ret;
00214 }
00215 
00216 double ClosedList::get_min_ts_of_key(const TimeStampedState &state) const
00217 {
00218     double ret = REALLYBIG;
00219     std::pair<ClosedListMap::const_iterator, ClosedListMap::const_iterator>
00220         entries = closed.equal_range(state);
00221     ClosedListMap::const_iterator it = entries.first;
00222     for(; it != entries.second; ++it) {
00223         ret = min(ret, it->first.timestamp);
00224     }
00225     return ret;
00226 }
00227 
00228 int ClosedList::size() const
00229 {
00230     return closed.size();
00231 }
00232 
00233 double ClosedList::getCostOfPath(const TimeStampedState &entry) const
00234 {
00235     double ret = 0.0;
00236     TimeStampedState current_entry = entry;
00237     for (;;) {
00238         double min_timestamp = current_entry.timestamp;
00239         std::pair<ClosedListMap::const_iterator, ClosedListMap::const_iterator>
00240             entries = closed.equal_range(current_entry);
00241         ClosedListMap::const_iterator it = entries.first;
00242         const PredecessorInfo* info_helper = NULL;
00243         for (; it != entries.second; ++it) {
00244             if (it->first.timestamp + EPSILON < min_timestamp || !info_helper) {
00245                 info_helper = &(it->second);
00246                 min_timestamp = it->first.timestamp;
00247             }
00248         }
00249         if (!info_helper || info_helper->predecessor == 0)
00250             break;
00251         const PredecessorInfo &info = *info_helper;
00252         const TimeStampedState* pred = info.predecessor;
00253         if (info.annotation != g_let_time_pass
00254                 && info.annotation->get_name().compare("wait")) {
00255             const Operator* op = info.annotation;
00256 
00257             double duration = op->get_duration(pred);
00258             ret += duration;
00259         }
00260         current_entry = *(info.predecessor);
00261     }
00262     return ret;
00263 }
00264 
00265 double ClosedList::trace_path(const TimeStampedState &entry,
00266         vector<PlanStep> &path, PlanTrace &states) const
00267 {
00268     double ret = 0.0;
00269     assert(path.empty());
00270     TimeStampedState current_entry = entry;
00271     states.push_back(new TimeStampedState(entry));
00272     for(;;) {
00273         double min_timestamp = current_entry.timestamp;
00274         double timestamp = min_timestamp;
00275         std::pair<ClosedListMap::const_iterator, ClosedListMap::const_iterator>
00276             entries = closed.equal_range(current_entry);
00277         ClosedListMap::const_iterator it = entries.first;
00278         const PredecessorInfo* info_helper = NULL;
00279         for(; it != entries.second; ++it) {
00280             if(it->first.timestamp + EPSILON < min_timestamp || !info_helper) {
00281                 info_helper = &(it->second);
00282                 min_timestamp = it->first.timestamp;
00283             }
00284         }
00285         double diff = timestamp - min_timestamp;
00286         if(!info_helper || info_helper->predecessor == 0)
00287             break;
00288         const PredecessorInfo &info = *info_helper;
00289         if(diff > EPSILON && states.size() > 1) {
00290             for(int i = 0; i < path.size(); i++) {
00291                 path[i].start_time -= diff;
00292             }
00293             for(int i = 0; i < states.size(); i++) {
00294                 states[i]->timestamp -= diff;
00295             }
00296         }
00297         const TimeStampedState* pred = info.predecessor;
00298         if(info.annotation != g_let_time_pass
00299                 && info.annotation->get_name().compare("wait")) {
00300             const Operator* op = info.annotation;
00301             double duration = op->get_duration(pred);
00302             path.push_back(PlanStep(pred->get_timestamp(), duration, op, pred));
00303             ret += duration;
00304         }
00305         states.push_back(new TimeStampedState(*pred));
00306         current_entry = *pred;
00307     }
00308     reverse(path.begin(), path.end());
00309     reverse(states.begin(), states.end());
00310     return ret;
00311 }
00312 
00313 double getSumOfSubgoals(const vector<PlanStep> &plan)
00314 {
00315     double ret = 0.0;
00316     for (int i = 0; i < plan.size(); ++i) {
00317         ret += plan[i].duration;
00318     }
00319     return ret;
00320 }
00321 
00322 double getSumOfSubgoals(const PlanTrace &path)
00323 {
00324     double ret = 0.0;
00325     for (int i = 0; i < g_goal.size(); ++i) {
00326         assert(g_variable_types[g_goal[i].first] == logical || g_variable_types[g_goal[i].first] == comparison);
00327         //        cout << "Goal " << i << ": " << g_variable_name[g_goal[i].first] << " has to be " << g_goal[i].second << endl;
00328         double actualIncrement = 0.0;
00329         for (int j = path.size() - 1; j >= 0; --j) {
00330             //            cout << "At timstamp " << path[j]->timestamp << " it is " << path[j]->state[g_goal[i].first] << endl;
00331             if (double_equals(path[j]->state[g_goal[i].first], g_goal[i].second)) {
00332                 actualIncrement = path[j]->timestamp;
00333             } else {
00334                 //                cout << "Goal " << i << " is satiesfied at timestamp " << actualIncrement << endl;
00335                 break;
00336             }
00337         }
00338         ret += actualIncrement;
00339     }
00340     return ret;
00341 }
00342 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines


tfd_modules
Author(s): Maintained by Christian Dornhege (see AUTHORS file).
autogenerated on Tue Jan 22 2013 12:25:02