void BranchBase::update(vertexName name) {
if(!visited_nodes[name].first) {
unexplored_nodes--;
}
updateVisited(name);
map.updateEdgeCount(getLatestNode()->name_,name);
distance += updateDistance(name);
updateOdom(name);
// double odomUnc = updateOdom(name);
// odom_unc_cost += odomUnc;
std::shared_ptr<Node> node_ptr = std::make_shared<Node>(name,0,distance,visited_nodes[name].first);
node_ptr->stamp = *(timestamps_.rbegin());
node_ptr->position_ = node_list.size();
node_ptr->previous_offset_ = (double)getPreviousDistance(name);
node_list.push_back(node_ptr);
visited_nodes[name].second = node_ptr;
if(odom_unc_cost > max_odom_unc) {
please_kill_me = true;
}
if(max_odom_cost < odom_unc_cost) {
max_odom_cost = odom_unc_cost;
}
}
double BranchBase::updateDistance(vertexName name) {
return (map.at(getLatestNode()->name_,name).weight);
}
const double BranchBase::getHeuristics() const {
if(!node_list.size()) return(0);
return /*map.unvisited_distance;*/-1e+07*(double)(getLatestNode()->previous_offset_);
}
void MergeNode::run(ExecutionNode* previous)
{
if(nullptr == previous)
{
m_errors.insert(ExecutionNode::NO_PREVIOUS_ERROR, QObject::tr("No previous node before Merge operator"));
return;
}
m_previousNode= previous;
m_result->setPrevious(previous->getResult());
ExecutionNode* previousLast= nullptr;
std::vector<Result*> pastResult;
for(auto start : *m_startList)
{
ExecutionNode* last= getLatestNode(start);
if(nullptr != last)
{
if(nullptr != previousLast)
{
auto startResult= start->getResult();
startResult->setPrevious(previousLast->getResult());
previousLast->setNextNode(start);
}
previousLast= last;
Result* tmpResult= last->getResult();
while(nullptr != tmpResult)
{
DiceResult* dice= dynamic_cast<DiceResult*>(tmpResult);
if(nullptr != dice)
{
///@todo TODO improve here to set homogeneous while is really
m_diceResult->setHomogeneous(false);
for(auto& die : dice->getResultList())
{
if(!m_diceResult->getResultList().contains(die) && (!die->hasBeenDisplayed()))
{
Die* tmpdie= new Die();
*tmpdie= *die;
die->displayed();
m_diceResult->getResultList().append(tmpdie);
}
}
}
auto it= std::find_if(pastResult.begin(), pastResult.end(),
[tmpResult](const Result* a) { return (a == tmpResult->getPrevious()); });
if(it == pastResult.end())
{
pastResult.push_back(previousLast->getResult());
tmpResult= tmpResult->getPrevious();
}
else
{
tmpResult->setPrevious(nullptr);
tmpResult= nullptr;
}
}
}
}
auto first= m_startList->front();
m_startList->clear();
m_startList->push_back(first);
if(nullptr != m_nextNode)
{
m_nextNode->run(this);
}
}
