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