| 24 | } |
| 25 | |
| 26 | double ViewNode::costTo(const ViewNode::Ptr& node) |
| 27 | { |
| 28 | vector<Vector3d> path; |
| 29 | double c = ViewNode::computeCost(pos_, node->pos_, yaw_, node->yaw_, vel_, yaw_dot_, path); |
| 30 | // std::cout << "cost from " << id_ << " to " << node->id_ << " is: " << c << std::endl; |
| 31 | return c; |
| 32 | } |
| 33 | |
| 34 | double ViewNode::searchPath(const Vector3d& p1, const Vector3d& p2, vector<Vector3d>& path) |
| 35 | { |