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