Node.cpp

Go to the documentation of this file.

#include "Node.h"
 
#include <vector>
 
namespace armarx::navigation::algorithm::astar
{
 
    Node::Node(const Eigen::Vector2f& position, const float obstacleDistance) :
        position(position), obstacleDistance(obstacleDistance)
    {
    }
 
    std::vector<Eigen::Vector2f>
    Node::traversePredecessors() const
    {
        std::vector<Eigen::Vector2f> result;
        result.push_back(position);
 
        NodePtr predecessor = this->predecessor;
        while (predecessor)
        {
            result.push_back(predecessor->position);
            predecessor = predecessor->predecessor;
        }
        return result;
    }
 
} // namespace armarx::navigation::algorithm::astar