| 185 | } |
| 186 | |
| 187 | double Astar::pathLength(const vector<Eigen::Vector3d>& path) |
| 188 | { |
| 189 | double length = 0.0; |
| 190 | if (path.size() < 2) |
| 191 | return length; |
| 192 | for (int i = 0; i < path.size() - 1; ++i) length += (path[i + 1] - path[i]).norm(); |
| 193 | return length; |
| 194 | } |
| 195 | |
| 196 | void Astar::backtrack(const NodePtr& end_node, const Eigen::Vector3d& end) |
| 197 | { |