inf cost states always go last

Also update sorted interface when state becomes inf or get's new cost.
This commit is contained in:
v4hn 2018-04-10 16:16:13 +08:00 committed by Robert Haschke
parent 728c1b40e2
commit bbb64f2f8c
2 changed files with 12 additions and 2 deletions

View File

@ -94,6 +94,14 @@ public:
this->cost() + other.cost()); this->cost() + other.cost());
} }
inline bool operator<(const Priority& other) const { inline bool operator<(const Priority& other) const {
/* infinite cost should always be last */
if (std::isinf(this->cost()) && std::isinf(other.cost()))
return this->depth() > other.depth();
else if (std::isinf(this->cost()))
return false;
else if (std::isinf(other.cost()))
return true;
if (this->depth() == other.depth()) if (this->depth() == other.depth())
return this->cost() < other.cost(); return this->cost() < other.cost();
else else
@ -155,7 +163,7 @@ public:
/// remove a state from the interface and return it as a one-element list /// remove a state from the interface and return it as a one-element list
container_type remove(iterator it); container_type remove(iterator it);
/// update state's priority if new priority is smaller and call notify_ /// update state's priority if new priority is smaller or became infeasible and call notify_
void updatePriority(InterfaceState *state, const InterfaceState::Priority &priority); void updatePriority(InterfaceState *state, const InterfaceState::Priority &priority);
private: private:

View File

@ -106,11 +106,13 @@ Interface::container_type Interface::remove(iterator it)
void Interface::updatePriority(InterfaceState *state, const InterfaceState::Priority& priority) void Interface::updatePriority(InterfaceState *state, const InterfaceState::Priority& priority)
{ {
if (priority < state->priority()) { double old_cost = state->priority_.cost();
if (priority < state->priority() || std::isinf(priority.cost())) {
auto it = std::find_if(begin(), end(), [state](const InterfaceState& other) { return state == &other; }); auto it = std::find_if(begin(), end(), [state](const InterfaceState& other) { return state == &other; });
// state should be part of the interface // state should be part of the interface
assert(it != end()); assert(it != end());
state->priority_ = priority; state->priority_ = priority;
update(it);
if (notify_) notify_(it, true); if (notify_) notify_(it, true);
} }
} }