Fixes by @supermerill from pull request "ShortestPath fix" #3306

This commit is contained in:
bubnikv 2019-12-13 15:40:42 +01:00
parent e649a28ffc
commit b061904ad8

View file

@ -43,6 +43,7 @@ std::vector<std::pair<size_t, bool>> chain_segments_closest_point(std::vector<En
assert(next_idx < end_points.size());
EndPointType &end_point = end_points[next_idx];
end_point.chain_id = 1;
out.emplace_back(next_idx / 2, (next_idx & 1) != 0);
this_idx = next_idx ^ 1;
}
#ifndef NDEBUG
@ -72,7 +73,7 @@ std::vector<std::pair<size_t, bool>> chain_segments_greedy_constrained_reversals
else if (num_segments == 1)
{
// Just sort the end points so that the first point visited is closest to start_near.
out.emplace_back(0, start_near != nullptr &&
out.emplace_back(0, could_reverse_func(0) && start_near != nullptr &&
(end_point_func(0, true) - *start_near).template cast<double>().squaredNorm() < (end_point_func(0, false) - *start_near).template cast<double>().squaredNorm());
}
else