| 30 | } |
| 31 | |
| 32 | double ViewNode::searchPath(const Vector3d& p1, const Vector3d& p2, vector<Vector3d>& path) { |
| 33 | // Try connect two points with straight line |
| 34 | bool safe = true; |
| 35 | Vector3i idx; |
| 36 | caster_->input(p1, p2); |
| 37 | while (caster_->nextId(idx)) { |
| 38 | if (map_->getInflateOccupancy(idx) == 1 || !map_->isInBox(idx)) { |
| 39 | // map_->getOccupancy(idx) == SDFMap::UNKNOWN |
| 40 | safe = false; |
| 41 | break; |
| 42 | } |
| 43 | } |
| 44 | if (safe) { |
| 45 | path = { p1, p2 }; |
| 46 | return (p1 - p2).norm(); |
| 47 | } |
| 48 | // Search a path using decreasing resolution |
| 49 | vector<double> res = { 0.4 }; |
| 50 | for (int k = 0; k < res.size(); ++k) { |
| 51 | astar_->reset(); |
| 52 | astar_->setResolution(res[k]); |
| 53 | if (astar_->search(p1, p2) == Astar::REACH_END) { |
| 54 | path = astar_->getPath(); |
| 55 | return astar_->pathLength(path); |
| 56 | } |
| 57 | } |
| 58 | // Use Astar early termination cost as an estimate |
| 59 | path = { p1, p2 }; |
| 60 | return 100; |
| 61 | } |
| 62 | |
| 63 | double ViewNode::computeCost(const Vector3d& p1, const Vector3d& p2, const double& y1, |
| 64 | const double& y2, const Vector3d& v1, const double& yd1, vector<Vector3d>& path) { |
nothing calls this directly
no test coverage detected