22529124718

Committed 28 Feb 2026 09:12PM UTC coverage: 70.977% (+0.007%) from 70.97%

Build # 22529124718

Build Type

push

github

Committed by

web-flow

Commit Message

Merge pull request #778 from eyal0/reentrant_path_finder

feat: Make path_finding re-entrant.

Coverage Stats

4693 of 7905 branches covered (59.37%)

Branch coverage included in aggregate %.

35 of 38 new or added lines in 2 files covered. (92.11%)

3952 of 4275 relevant lines covered (92.44%)

1692490.86 hits per line

Source File
Press 'n' to go to next uncovered line, 'b' for previous

68.37

/path_finding.cpp

#include <vector>
using std::vector;

#include <unordered_map>
using std::unordered_map;

#include <unordered_set>
using std::unordered_set;

#include <queue>
using std::priority_queue;

#include <utility>
using std::pair;
using std::make_pair;

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/astar_search.hpp>
#include <boost/optional.hpp>

#include "path_finding.hpp"
#include "options.hpp"
#include "geometry.hpp"
#include "bg_operators.hpp"
#include "bg_helpers.hpp"
#include "segment_tree.hpp"

namespace path_finding {

using boost::optional;
using boost::make_optional;

Neighbors::iterator Neighbors::iterator::operator++() {
  const auto& all_vertices_size = neighbors->vertices.size();
  do {
    // Move to a new valid point, even if it isn't a neighbor.
    point_index++;
  } while (point_index < all_vertices_size + 2 &&
           !neighbors->is_neighbor(**this));
  return *this;
}

bool Neighbors::iterator::operator!=(const Neighbors::iterator& other) const {
  return (point_index != other.point_index);
}

bool Neighbors::iterator::operator==(const Neighbors::iterator& other) const {
  return !(*this != other);
}

const point_type_fp& Neighbors::iterator::operator*() const {
  if (point_index == 0) {
    return neighbors->start;
  } else if (point_index == 1) {
    return neighbors->goal;
  } else {
    return neighbors->vertices[point_index-2];
  }
}

Neighbors::Neighbors(const point_type_fp& start, const point_type_fp& goal,
                     const point_type_fp& current,
                     const coordinate_type_fp& max_path_length,
                     const std::vector<point_type_fp>& vertices,
                     PathFindingSurfaceWithTries* pfs) :
    start(start),
    goal(goal),
    current(current),
    max_path_length_squared(max_path_length),
    vertices(vertices),
    pfs(pfs) {}

// Returns a valid neighbor index that is either the one provided or
// the next higher valid one.
inline bool Neighbors::is_neighbor(const point_type_fp p) {
  if (p == current) {
    return false;
  }
  pfs->decrement_tries();
  if (bg::distance(current, p) + bg::distance(p, goal) > max_path_length_squared) {
    return false;
  }
  if (!pfs->in_surface(current, p)) {
    return false;
  }
  return true;
}

Neighbors::iterator Neighbors::begin() {
  auto ret = iterator(this, 0);
  if (ret == end()) {
    // Can't dereferfence the end.
    return ret;
  }
  if (is_neighbor(*ret)) {
    // This is a valid begin.
    return ret;
  } else {
    // This position is invalid but we can increment to get to a valid one.
    ++ret;
    return ret;
  }
}

Neighbors::iterator Neighbors::end() {
  return iterator(this, vertices.size()+2);
}

vector<pair<point_type_fp, point_type_fp>> get_all_segments(
    const vector<vector<std::reference_wrapper<const ring_type_fp>>>& all_rings) {
  vector<pair<point_type_fp, point_type_fp>> ret;
  for (const auto& x0 : all_rings) {
    for (auto x1 : x0) {
      for (size_t i = 0; i+1 < x1.get().size(); i++) {
        ret.emplace_back(x1.get()[i], x1.get()[i+1]);
      }
    }
  }
  return ret;
}

vector<std::reference_wrapper<const ring_type_fp>> get_all_rings(const multi_polygon_type_fp& mpolys) {
  vector<std::reference_wrapper<const ring_type_fp>> ret;
  for (const auto& poly : mpolys) {
    ret.push_back(poly.outer());
    for (const auto& inner : poly.inners()) {
      ret.push_back(inner);
    }
  }
  return ret;
}

vector<vector<std::reference_wrapper<const ring_type_fp>>> get_all_rings(
    const nested_multipolygon_type_fp& mpolys) {
  vector<vector<std::reference_wrapper<const ring_type_fp>>> ret;
  for (const auto& poly : mpolys) {
    ret.push_back(get_all_rings(poly.outer()));
    for (const auto& inner: poly.inners()) {
      ret.push_back(get_all_rings(inner));
    }
  }
  return ret;
}

PathFindingSurface::PathFindingSurface(const optional<multi_polygon_type_fp>& keep_in,
                                       const multi_polygon_type_fp& keep_out,
                                       const coordinate_type_fp tolerance) {
  if (keep_in) {
    multi_polygon_type_fp total_keep_in = *keep_in - keep_out;

    total_keep_in_grown.emplace();
    for (const auto& poly : total_keep_in) {
      all_vertices.emplace_back();
      all_vertices.back().emplace_back(poly.outer().cbegin(), poly.outer().cend());
      total_keep_in_grown->emplace_back(bg_helpers::buffer_miter(poly.outer(), tolerance));
      for (const auto& inner : poly.inners()) {
        all_vertices.back().emplace_back(inner.cbegin(), inner.cend());
        // Because the inner is reversed, we need to reverse it so
        // that the buffer algorithm won't get confused.
        auto temp_inner = inner;
        bg::reverse(temp_inner);
        // tolerance needs to be inverted because growing a shape
        // shrinks the holes in it.
        total_keep_in_grown->back().inners().push_back(bg_helpers::buffer_miter(temp_inner, -tolerance));
      }
    }
  } else {
    for (const auto& poly : keep_out){
      all_vertices.emplace_back();
      all_vertices.back().emplace_back(poly.outer().cbegin(), poly.outer().cend());
      keep_out_shrunk.emplace_back(bg_helpers::buffer_miter(poly.outer(), -tolerance));
      for (const auto& inner : poly.inners()) {
        all_vertices.back().emplace_back(inner.cbegin(), inner.cend());
        // Because the inner is reversed, we need to reverse it so
        // that the buffer algorithm won't get confused.
        auto temp_inner = inner;
        bg::reverse(temp_inner);
        // tolerance needs to be inverted because shrinking a shape
        // grows the holes in it.
        keep_out_shrunk.back().inners().push_back(bg_helpers::buffer_miter(temp_inner, tolerance));
      }
    }
  }
  const nested_multipolygon_type_fp& poly_to_search = total_keep_in_grown ? *total_keep_in_grown : keep_out_shrunk;
  const vector<vector<std::reference_wrapper<const ring_type_fp>>>& all_rings = get_all_rings(poly_to_search);
  const auto& all_segments = get_all_segments(all_rings);
  segment_tree::SegmentTree x(all_segments);
  tree = std::move(x);

  for (auto av : all_vertices) {
    sort(av.begin(), av.end());
    av.erase(std::unique(av.begin(), av.end()), av.end());
  }
}

boost::optional<MPRingIndices> inside_multipolygon(const point_type_fp& p,
                                                   const multi_polygon_type_fp& mp) {
  for (size_t poly_index = 0; poly_index < mp.size(); poly_index++) {
    const auto& poly = mp[poly_index];
    if (point_in_ring(p, poly.outer())) {
      // Might be part of this shape but only if the point isn't in
      // the inners.
      MPRingIndices ring_indices{{poly_index, {0}}};
      for (size_t inner_index = 0; inner_index < poly.inners().size(); inner_index++) {
        const auto& inner = poly.inners()[inner_index];
        if (!point_in_ring(p, inner)) {
          // We'll have to make sure not to cross this inner.
          ring_indices.back().second.emplace_back(inner_index+1);
        } else {
          break; // We're inside one of the inners so give up.
        }
      }
      if (ring_indices.back().second.size() == poly.inners().size() + 1) {
        // We never hit the break so we're inside this shape so we're
        // done.
        return {ring_indices};
      }
      // We're inside the outer but also inside an inner!  There might
      // be another shape inside this hold so we'll ignore this one
      // and keep searching.
    }
  }
  return boost::none;
}

boost::optional<MPRingIndices> outside_multipolygon(const point_type_fp& p,
                                                    const multi_polygon_type_fp& mp) {
  MPRingIndices ring_indices;
  for (size_t poly_index = 0; poly_index < mp.size(); poly_index++) {
    const auto& poly = mp[poly_index];
    if (point_in_ring(p, poly.outer())) {
      // We're inside the outer, maybe we're in an inner?  If not, we
      // aren't outside at all and we'll just give up.
      bool in_any_inner = false;
      for (size_t i = 0; i < poly.inners().size(); i++) {
        const auto& inner = poly.inners()[i];
        if (point_in_ring(p, inner)) {
          in_any_inner = true;
          ring_indices.emplace_back(poly_index, vector<size_t>{i+1});
          break;
        }
      }
      if (!in_any_inner) {
        // We're inside the outer but not in any of the inners, so
        // we're in the shape, but we want to be outside the shape, so
        // we've failed.
        return boost::none;
      }
    } else {
      // We need to keep out of this outer.
      ring_indices.emplace_back(poly_index, vector<size_t>{0});
      // No need to examine the inners which we can't possibly be inside.
    }
  }
  return {ring_indices};
}

boost::optional<RingIndices> inside_multipolygons(
    const point_type_fp& p,
    const nested_multipolygon_type_fp& mp) {
  for (size_t poly_index = 0; poly_index < mp.size(); poly_index++) {
    const auto& poly = mp[poly_index];
    boost::optional<MPRingIndices> inside_mp = inside_multipolygon(p, poly.outer());
    if (inside_mp) {
      // Might be part of this shape but only if the point isn't in
      // the inners.
      RingIndices ring_indices{{poly_index, {{0, *inside_mp}}}};
      for (size_t inner_index = 0; inner_index < poly.inners().size(); inner_index++) {
        const auto& inner = poly.inners()[inner_index];
        auto outside_mp = outside_multipolygon(p, inner);
        if (outside_mp) {
          // We'll have to make sure not to cross this inner.
          ring_indices.back().second.emplace_back(inner_index+1, *outside_mp);
        } else {
          break; // We're inside one of the inners so give up.
        }
      }
      if (ring_indices.back().second.size() == poly.inners().size() + 1) {
        // We never hit the break so we're inside this shape so we're
        // done.
        return {ring_indices};
      }
      // We're inside the outer but also inside an inner!  It might
      // be an outer in an inner so we'll ignore this one and keep
      // searching.
    }
  }
  return boost::none;
}

boost::optional<RingIndices> outside_multipolygons(
    const point_type_fp& p,
    const nested_multipolygon_type_fp& mp) {
  RingIndices ring_indices;
  for (size_t poly_index = 0; poly_index < mp.size(); poly_index++) {
    const auto& poly = mp[poly_index];
    auto outside_mp = outside_multipolygon(p, poly.outer());
    if (!outside_mp) {
      // We're inside the outer, maybe we're in an inner?  If not, we
      // aren't outside at all and we'll just give up.
      bool in_any_inner = false;
      for (size_t inner_index = 0; inner_index < poly.inners().size(); inner_index++) {
        const auto& inner = poly.inners()[inner_index];
        auto inside_mp = inside_multipolygon(p, inner);
        if (inside_mp) {
          in_any_inner = true;
          ring_indices.emplace_back(poly_index, vector<pair<size_t, MPRingIndices>>{{inner_index + 1, *inside_mp}});
          break;
        }
      }
      if (!in_any_inner) {
        // We're inside the outer but not in any of the inners, so
        // we're in the shape, but we want to be outside the shape, so
        // we've failed.
        return boost::none;
      }
    } else {
      // We need to keep out of this outer.
      ring_indices.emplace_back(poly_index, vector<pair<size_t, MPRingIndices>>{{0, *outside_mp}});
    }
  }
  return {ring_indices};
}

/* Given a point, determine if the point is in the search surface.  If
   so, return a non-default value, otherwise default value.  If two
   points return the same value, there is a path between them in the
   surface.  If not then there cannot be a path between them.  The
   value will actually be a vector of size_t that indicates which
   rings in the stored polygon should be used for the generated points
   in the path and also for the collision detection. */
const boost::optional<SearchKey>& PathFindingSurface::in_surface(point_type_fp p) const {
  auto memoized_result = point_in_surface_memo.find(p);
  if (memoized_result != point_in_surface_memo.cend()) {
    return memoized_result->second;
  }
  boost::optional<RingIndices> maybe_ring_indices;
  if (total_keep_in_grown) {
    maybe_ring_indices = inside_multipolygons(p, *total_keep_in_grown);
  } else {
    maybe_ring_indices = outside_multipolygons(p, keep_out_shrunk);
  }
  if (!maybe_ring_indices) {
    return point_in_surface_memo.emplace(p, boost::none).first->second;
  }
  const auto& ring_indices = *maybe_ring_indices;
  // Check if this one is already in the cache.
  const auto& find_result = ring_indices_lookup.find(std::cref(ring_indices));
  if (find_result != ring_indices_lookup.cend()) {
    // Found in the cache so we can use that.
    return point_in_surface_memo.emplace(p, find_result->second).first->second;
  }
  // Not found so we need to add it to the cache.
  ring_indices_cache.push_back(ring_indices);
  ring_indices_lookup.emplace(ring_indices_cache.back(), ring_indices_cache.size()-1);
  return point_in_surface_memo.emplace(p, ring_indices_cache.size()-1).first->second;
}

// Return true if this edge from a to b is part of the path finding surface.
bool PathFindingSurface::in_surface(
    const point_type_fp& a, const point_type_fp& b) const {
  if (b < a) {
    return in_surface(b, a);
  }
  const auto key = make_pair(a, b);
  auto memoized_result = edge_in_surface_memo.find(key);
  if (memoized_result != edge_in_surface_memo.cend()) {
    return memoized_result->second;
  }
  auto found_intersection = tree.intersects(a, b);
  edge_in_surface_memo.emplace(key, !found_intersection);
  return !found_intersection;
}

// Return a path from the start to the current.  Always return at
// least two points.
linestring_type_fp build_path(
    point_type_fp current,
    const unordered_map<point_type_fp, point_type_fp>& came_from) {
  linestring_type_fp result;
  while (came_from.count(current)) {
    result.push_back(current);
    current = came_from.at(current);
  }
  result.push_back(current);
  bg::reverse(result);
  return result;
}

optional<linestring_type_fp> PathFindingSurface::find_path(
    const point_type_fp& start, const point_type_fp& goal,
    const coordinate_type_fp& max_path_length,
    const boost::optional<size_t>& max_tries,
    SearchKey search_key) const {
  if (max_tries && *max_tries == 0) {
    return boost::none;
  }
  PathFindingSurfaceWithTries pfs_with_tries(*this, max_tries);
  return pfs_with_tries.find_path(start, goal, max_path_length, search_key);
}

optional<linestring_type_fp> PathFindingSurface::find_path(
    const point_type_fp& start, const point_type_fp& goal,
    const coordinate_type_fp& max_path_length,
    const boost::optional<size_t>& max_tries) const {
  if (max_tries && *max_tries == 0) {
    return boost::none;
  }
  PathFindingSurfaceWithTries pfs_with_tries(*this, max_tries);

  auto ring_indices = in_surface(start);
  if (!ring_indices) {
    // Start is not in the surface.
    return boost::none;
  }
  if (ring_indices != in_surface(goal)) {
    // Either goal is not in the surface or it's in a region unreachable by start.
    return boost::none;
  }
  return pfs_with_tries.find_path(start, goal, max_path_length, *ring_indices);
}

const std::vector<point_type_fp>&
PathFindingSurface::vertices(SearchKey search_key) const {
  auto memoized_result = vertices_memo.find(search_key);
  if (memoized_result != vertices_memo.cend()) {
    return memoized_result->second;
  }
  std::vector<point_type_fp> ret;
  const auto& vertices = all_vertices;
  const auto& ring_indices = ring_indices_cache.at(search_key);
  for (size_t poly_index = 0; poly_index < ring_indices.size() ; poly_index++) {
    // This is the poly to look at.
    const auto& poly_ring_index = ring_indices[poly_index];
    // These are the vertices for that poly.
    const auto& poly_vertices = vertices[poly_ring_index.first];
    for (size_t ring_index = 0; ring_index < poly_ring_index.second.size(); ring_index++) {
      const auto& ring_ring_index = poly_ring_index.second[ring_index];
      const auto& ring_vertices = poly_vertices[ring_ring_index.first];
      ret.insert(ret.cend(), ring_vertices.cbegin(), ring_vertices.cend());
    }
  }
  return vertices_memo.emplace(search_key, ret).first->second;
}

optional<linestring_type_fp> PathFindingSurfaceWithTries::find_path(
    const point_type_fp& start, const point_type_fp& goal,
    const coordinate_type_fp& max_path_length,
    SearchKey search_key) {
  // Connect if a direct connection is possible.  This also takes care
  // of the case where start == goal.
  try {
    if (in_surface(start, goal)) {
      decrement_tries();
      if (bg::comparable_distance(start, goal) < max_path_length * max_path_length) {
        // in_surface builds up some structures that are only efficient if
        // we're doing many tries.
        return {{start, goal}};
      } else {
        // If the straight line was too long then there is no way to connect.
        return boost::none;
      }
    }
  } catch (GiveUp g) {
    return boost::none;
  }
  // Do astar.
  priority_queue<pair<coordinate_type_fp, point_type_fp>,
                 vector<pair<coordinate_type_fp, point_type_fp>>,
                 std::greater<pair<coordinate_type_fp, point_type_fp>>> open_set;
  open_set.emplace(bg::distance(start, goal), start);
  unordered_set<point_type_fp> closed_set;
  unordered_map<point_type_fp, point_type_fp> came_from;
  unordered_map<point_type_fp, coordinate_type_fp> g_score; // Empty should be considered infinity.
  g_score[start] = 0;
  while (!open_set.empty()) {
    const auto current = open_set.top().second;
    open_set.pop();
    if (current == goal) {
      // We're done.
      return boost::make_optional(build_path(current, came_from));
    }
    if (closed_set.count(current) > 0) {
      // Skip this because we already "removed it", sort of.
      continue;
    }
    try {
      auto current_neighbors = neighbors(
          start, goal,
          max_path_length - g_score.at(current),
          search_key,
          current);
      for (const auto& neighbor : current_neighbors) {
        const auto tentative_g_score = g_score.at(current) + bg::distance(current, neighbor);
        if (g_score.count(neighbor) == 0 || tentative_g_score < g_score.at(neighbor)) {
          // This path to neighbor is better than any previous one.
          came_from[neighbor] = current;
          g_score[neighbor] = tentative_g_score;
          open_set.emplace(tentative_g_score + bg::distance(neighbor, goal), neighbor);
        }
      }
    } catch (GiveUp g) {
      return boost::none;
    }
    // Because we can't delete from the open_set, we'll just marked
    // items as closed and ignore them later.
    closed_set.insert(current);
  }
  return boost::none;
}

} //namespace path_finding

1	#include <vector>
2	using std::vector;
3
4	#include <unordered_map>
5	using std::unordered_map;
6
7	#include <unordered_set>
8	using std::unordered_set;
9
10	#include <queue>
11	using std::priority_queue;
12
13	#include <utility>
14	using std::pair;
15	using std::make_pair;
16
17	#include <boost/graph/adjacency_list.hpp>
18	#include <boost/graph/astar_search.hpp>
19	#include <boost/optional.hpp>
20
21	#include "path_finding.hpp"
22	#include "options.hpp"
23	#include "geometry.hpp"
24	#include "bg_operators.hpp"
25	#include "bg_helpers.hpp"
26	#include "segment_tree.hpp"
27
28	namespace path_finding {
29
30	using boost::optional;
31	using boost::make_optional;
32
33	Neighbors::iterator Neighbors::iterator::operator++() {	47,802✔
34	const auto& all_vertices_size = neighbors->vertices.size();	47,802✔
35	do {
36	// Move to a new valid point, even if it isn't a neighbor.
37	point_index++;	95,604✔
38	} while (point_index < all_vertices_size + 2 &&	95,604!
39	!neighbors->is_neighbor(**this));	95,604✔
40	return *this;	×
41	}
42
43	bool Neighbors::iterator::operator!=(const Neighbors::iterator& other) const {	×
44	return (point_index != other.point_index);	×
45	}
46
47	bool Neighbors::iterator::operator==(const Neighbors::iterator& other) const {	×
48	return !(*this != other);	×
49	}
50
51	const point_type_fp& Neighbors::iterator::operator*() const {	143,406✔
52	if (point_index == 0) {	143,406✔
53	return neighbors->start;	47,802✔
54	} else if (point_index == 1) {	95,604✔
55	return neighbors->goal;	47,802✔
56	} else {
57	return neighbors->vertices[point_index-2];	47,802✔
58	}
59	}
60
61	Neighbors::Neighbors(const point_type_fp& start, const point_type_fp& goal,	×
62	const point_type_fp& current,
63	const coordinate_type_fp& max_path_length,
64	const std::vector<point_type_fp>& vertices,
65	PathFindingSurfaceWithTries* pfs) :	47,802✔
66	start(start),	23,901✔
67	goal(goal),	23,901✔
68	current(current),	23,901✔
69	max_path_length_squared(max_path_length),	23,901✔
70	vertices(vertices),	23,901✔
71	pfs(pfs) {}	47,802✔
72
73	// Returns a valid neighbor index that is either the one provided or
74	// the next higher valid one.
75	inline bool Neighbors::is_neighbor(const point_type_fp p) {	143,406✔
76	if (p == current) {	143,406✔
77	return false;
78	}
79	pfs->decrement_tries();	95,604✔
80	if (bg::distance(current, p) + bg::distance(p, goal) > max_path_length_squared) {	47,802✔
81	return false;
82	}
83	if (!pfs->in_surface(current, p)) {	34,390!
84	return false;	17,195✔
85	}
86	return true;
87	}
88
89	Neighbors::iterator Neighbors::begin() {	47,802!
90	auto ret = iterator(this, 0);
91	if (ret == end()) {	47,802!
92	// Can't dereferfence the end.
93	return ret;	×
94	}
95	if (is_neighbor(*ret)) {	47,802!
96	// This is a valid begin.
97	return ret;	×
98	} else {
99	// This position is invalid but we can increment to get to a valid one.
100	++ret;	47,802✔
101	return ret;	×
102	}
103	}
104
NEW 105	Neighbors::iterator Neighbors::end() {	×
106	return iterator(this, vertices.size()+2);	47,802!
107	}
108
109	vector<pair<point_type_fp, point_type_fp>> get_all_segments(	290✔
110	const vector<vector<std::reference_wrapper<const ring_type_fp>>>& all_rings) {
111	vector<pair<point_type_fp, point_type_fp>> ret;
112	for (const auto& x0 : all_rings) {	2,428✔
113	for (auto x1 : x0) {	4,264✔
114	for (size_t i = 0; i+1 < x1.get().size(); i++) {	577,492✔
115	ret.emplace_back(x1.get()[i], x1.get()[i+1]);	575,366!
116	}
117	}
118	}
119	return ret;	290✔
120	}
121
122	vector<std::reference_wrapper<const ring_type_fp>> get_all_rings(const multi_polygon_type_fp& mpolys) {	2,138✔
123	vector<std::reference_wrapper<const ring_type_fp>> ret;
124	for (const auto& poly : mpolys) {	4,264✔
125	ret.push_back(poly.outer());	2,126✔
126	for (const auto& inner : poly.inners()) {	2,126!
127	ret.push_back(inner);	×
128	}
129	}
130	return ret;	2,138✔
131	}
132
133	vector<vector<std::reference_wrapper<const ring_type_fp>>> get_all_rings(	290✔
134	const nested_multipolygon_type_fp& mpolys) {
135	vector<vector<std::reference_wrapper<const ring_type_fp>>> ret;
136	for (const auto& poly : mpolys) {	994✔
137	ret.push_back(get_all_rings(poly.outer()));	1,408!
138	for (const auto& inner: poly.inners()) {	2,138✔
139	ret.push_back(get_all_rings(inner));	2,868!
140	}
141	}
142	return ret;	290✔
143	}
144
145	PathFindingSurface::PathFindingSurface(const optional<multi_polygon_type_fp>& keep_in,	290✔
146	const multi_polygon_type_fp& keep_out,
147	const coordinate_type_fp tolerance) {	435!
148	if (keep_in) {	290✔
149	multi_polygon_type_fp total_keep_in = *keep_in - keep_out;	234!
150
151	total_keep_in_grown.emplace();
152	for (const auto& poly : total_keep_in) {	574✔
153	all_vertices.emplace_back();	340!
154	all_vertices.back().emplace_back(poly.outer().cbegin(), poly.outer().cend());	340!
155	total_keep_in_grown->emplace_back(bg_helpers::buffer_miter(poly.outer(), tolerance));	680!
156	for (const auto& inner : poly.inners()) {	1,486✔
157	all_vertices.back().emplace_back(inner.cbegin(), inner.cend());	1,146!
158	// Because the inner is reversed, we need to reverse it so
159	// that the buffer algorithm won't get confused.
160	auto temp_inner = inner;
161	bg::reverse(temp_inner);
162	// tolerance needs to be inverted because growing a shape
163	// shrinks the holes in it.
164	total_keep_in_grown->back().inners().push_back(bg_helpers::buffer_miter(temp_inner, -tolerance));	2,292!
165	}
166	}
167	} else {
168	for (const auto& poly : keep_out){	420✔
169	all_vertices.emplace_back();	364!
170	all_vertices.back().emplace_back(poly.outer().cbegin(), poly.outer().cend());	364!
171	keep_out_shrunk.emplace_back(bg_helpers::buffer_miter(poly.outer(), -tolerance));	728!
172	for (const auto& inner : poly.inners()) {	652✔
173	all_vertices.back().emplace_back(inner.cbegin(), inner.cend());	288!
174	// Because the inner is reversed, we need to reverse it so
175	// that the buffer algorithm won't get confused.
176	auto temp_inner = inner;
177	bg::reverse(temp_inner);
178	// tolerance needs to be inverted because shrinking a shape
179	// grows the holes in it.
180	keep_out_shrunk.back().inners().push_back(bg_helpers::buffer_miter(temp_inner, tolerance));	576!
181	}
182	}
183	}
184	const nested_multipolygon_type_fp& poly_to_search = total_keep_in_grown ? *total_keep_in_grown : keep_out_shrunk;	290✔
185	const vector<vector<std::reference_wrapper<const ring_type_fp>>>& all_rings = get_all_rings(poly_to_search);	435!
186	const auto& all_segments = get_all_segments(all_rings);	290!
187	segment_tree::SegmentTree x(all_segments);	290!
188	tree = std::move(x);
189
190	for (auto av : all_vertices) {	1,346✔
191	sort(av.begin(), av.end());	704✔
192	av.erase(std::unique(av.begin(), av.end()), av.end());	704✔
193	}	352✔
194	}	290✔
195
196	boost::optional<MPRingIndices> inside_multipolygon(const point_type_fp& p,	40,596✔
197	const multi_polygon_type_fp& mp) {
198	for (size_t poly_index = 0; poly_index < mp.size(); poly_index++) {	75,238✔
199	const auto& poly = mp[poly_index];
200	if (point_in_ring(p, poly.outer())) {	40,596✔
201	// Might be part of this shape but only if the point isn't in
202	// the inners.
203	MPRingIndices ring_indices{{poly_index, {0}}};	17,862!
204	for (size_t inner_index = 0; inner_index < poly.inners().size(); inner_index++) {	5,954!
205	const auto& inner = poly.inners()[inner_index];
206	if (!point_in_ring(p, inner)) {	×
207	// We'll have to make sure not to cross this inner.
208	ring_indices.back().second.emplace_back(inner_index+1);	×
209	} else {
210	break; // We're inside one of the inners so give up.
211	}
212	}
213	if (ring_indices.back().second.size() == poly.inners().size() + 1) {	5,954!
214	// We never hit the break so we're inside this shape so we're
215	// done.
216	return {ring_indices};	2,977✔
217	}
218	// We're inside the outer but also inside an inner! There might
219	// be another shape inside this hold so we'll ignore this one
220	// and keep searching.
221	}	2,977✔
222	}
223	return boost::none;	34,642✔
224	}	5,954!
225
226	boost::optional<MPRingIndices> outside_multipolygon(const point_type_fp& p,	59,202✔
227	const multi_polygon_type_fp& mp) {
228	MPRingIndices ring_indices;	29,601✔
229	for (size_t poly_index = 0; poly_index < mp.size(); poly_index++) {	109,816✔
230	const auto& poly = mp[poly_index];
231	if (point_in_ring(p, poly.outer())) {	54,162✔
232	// We're inside the outer, maybe we're in an inner? If not, we
233	// aren't outside at all and we'll just give up.
234	bool in_any_inner = false;
235	for (size_t i = 0; i < poly.inners().size(); i++) {	3,548!
236	const auto& inner = poly.inners()[i];
237	if (point_in_ring(p, inner)) {	×
238	in_any_inner = true;
239	ring_indices.emplace_back(poly_index, vector<size_t>{i+1});	×
240	break;
241	}
242	}
243	if (!in_any_inner) {
244	// We're inside the outer but not in any of the inners, so
245	// we're in the shape, but we want to be outside the shape, so
246	// we've failed.
247	return boost::none;	3,548✔
248	}
249	} else {
250	// We need to keep out of this outer.
251	ring_indices.emplace_back(poly_index, vector<size_t>{0});	75,921!
252	// No need to examine the inners which we can't possibly be inside.
253	}
254	}
255	return {ring_indices};
256	}	29,601✔
257
258	boost::optional<RingIndices> inside_multipolygons(	2,698✔
259	const point_type_fp& p,
260	const nested_multipolygon_type_fp& mp) {
261	for (size_t poly_index = 0; poly_index < mp.size(); poly_index++) {	7,274✔
262	const auto& poly = mp[poly_index];
263	boost::optional<MPRingIndices> inside_mp = inside_multipolygon(p, poly.outer());	7,154✔
264	if (inside_mp) {	7,154✔
265	// Might be part of this shape but only if the point isn't in
266	// the inners.
267	RingIndices ring_indices{{poly_index, {{0, *inside_mp}}}};	12,301!
268	for (size_t inner_index = 0; inner_index < poly.inners().size(); inner_index++) {	23,516✔
269	const auto& inner = poly.inners()[inner_index];
270	auto outside_mp = outside_multipolygon(p, inner);	20,938!
271	if (outside_mp) {	20,938✔
272	// We'll have to make sure not to cross this inner.
273	ring_indices.back().second.emplace_back(inner_index+1, *outside_mp);	20,798!
274	} else {
275	break; // We're inside one of the inners so give up.
276	}
277	}
278	if (ring_indices.back().second.size() == poly.inners().size() + 1) {	2,718✔
279	// We never hit the break so we're inside this shape so we're
280	// done.
281	return {ring_indices};	1,289✔
282	}
283	// We're inside the outer but also inside an inner! It might
284	// be an outer in an inner so we'll ignore this one and keep
285	// searching.
286	}	1,359✔
287	}
288	return boost::none;	120✔
289	}	4,077!
290
291	boost::optional<RingIndices> outside_multipolygons(	3,152✔
292	const point_type_fp& p,
293	const nested_multipolygon_type_fp& mp) {
294	RingIndices ring_indices;	1,576✔
295	for (size_t poly_index = 0; poly_index < mp.size(); poly_index++) {	41,244✔
296	const auto& poly = mp[poly_index];
297	auto outside_mp = outside_multipolygon(p, poly.outer());	38,264!
298	if (!outside_mp) {	38,264✔
299	// We're inside the outer, maybe we're in an inner? If not, we
300	// aren't outside at all and we'll just give up.
301	bool in_any_inner = false;
302	for (size_t inner_index = 0; inner_index < poly.inners().size(); inner_index++) {	33,614✔
303	const auto& inner = poly.inners()[inner_index];
304	auto inside_mp = inside_multipolygon(p, inner);	33,442!
305	if (inside_mp) {	33,442✔
306	in_any_inner = true;
307	ring_indices.emplace_back(poly_index, vector<pair<size_t, MPRingIndices>>{{inner_index + 1, *inside_mp}});	8,090!
308	break;
309	}
310	}
311	if (!in_any_inner) {
312	// We're inside the outer but not in any of the inners, so
313	// we're in the shape, but we want to be outside the shape, so
314	// we've failed.
315	return boost::none;
316	}
317	} else {
318	// We need to keep out of this outer.
319	ring_indices.emplace_back(poly_index, vector<pair<size_t, MPRingIndices>>{{0, *outside_mp}});	69,712!
320	}
321	}
322	return {ring_indices};
323	}	1,576✔
324
325	/* Given a point, determine if the point is in the search surface. If
326	so, return a non-default value, otherwise default value. If two
327	points return the same value, there is a path between them in the
328	surface. If not then there cannot be a path between them. The
329	value will actually be a vector of size_t that indicates which
330	rings in the stored polygon should be used for the generated points
331	in the path and also for the collision detection. */
332	const boost::optional<SearchKey>& PathFindingSurface::in_surface(point_type_fp p) const {	22,802✔
333	auto memoized_result = point_in_surface_memo.find(p);
334	if (memoized_result != point_in_surface_memo.cend()) {	22,802✔
335	return memoized_result->second;	16,952✔
336	}
337	boost::optional<RingIndices> maybe_ring_indices;
338	if (total_keep_in_grown) {	5,850✔
339	maybe_ring_indices = inside_multipolygons(p, *total_keep_in_grown);	5,396!
340	} else {
341	maybe_ring_indices = outside_multipolygons(p, keep_out_shrunk);	6,304!
342	}
343	if (!maybe_ring_indices) {	5,850✔
344	return point_in_surface_memo.emplace(p, boost::none).first->second;	292✔
345	}
346	const auto& ring_indices = *maybe_ring_indices;
347	// Check if this one is already in the cache.
348	const auto& find_result = ring_indices_lookup.find(std::cref(ring_indices));
349	if (find_result != ring_indices_lookup.cend()) {	5,558✔
350	// Found in the cache so we can use that.
351	return point_in_surface_memo.emplace(p, find_result->second).first->second;	7,449!
352	}
353	// Not found so we need to add it to the cache.
354	ring_indices_cache.push_back(ring_indices);	592!
355	ring_indices_lookup.emplace(ring_indices_cache.back(), ring_indices_cache.size()-1);	592!
356	return point_in_surface_memo.emplace(p, ring_indices_cache.size()-1).first->second;	888!
357	}
358
359	// Return true if this edge from a to b is part of the path finding surface.
360	bool PathFindingSurface::in_surface(	90,452✔
361	const point_type_fp& a, const point_type_fp& b) const {
362	if (b < a) {	90,452✔
363	return in_surface(b, a);	5,498✔
364	}
365	const auto key = make_pair(a, b);
366	auto memoized_result = edge_in_surface_memo.find(key);
367	if (memoized_result != edge_in_surface_memo.cend()) {	84,954✔
368	return memoized_result->second;	60,322✔
369	}
370	auto found_intersection = tree.intersects(a, b);	24,632✔
371	edge_in_surface_memo.emplace(key, !found_intersection);	24,632✔
372	return !found_intersection;	24,632✔
373	}
374
375	// Return a path from the start to the current. Always return at
376	// least two points.
377	linestring_type_fp build_path(	×
378	point_type_fp current,
379	const unordered_map<point_type_fp, point_type_fp>& came_from) {
380	linestring_type_fp result;
381	while (came_from.count(current)) {	×
382	result.push_back(current);	×
383	current = came_from.at(current);	×
384	}
385	result.push_back(current);	×
386	bg::reverse(result);
387	return result;	×
388	}
389
390	optional<linestring_type_fp> PathFindingSurface::find_path(	47,434!
391	const point_type_fp& start, const point_type_fp& goal,
392	const coordinate_type_fp& max_path_length,
393	const boost::optional<size_t>& max_tries,
394	SearchKey search_key) const {
395	if (max_tries && *max_tries == 0) {	47,434!
NEW 396	return boost::none;	×
397	}
398	PathFindingSurfaceWithTries pfs_with_tries(*this, max_tries);
399	return pfs_with_tries.find_path(start, goal, max_path_length, search_key);	47,434✔
400	}
401
402	optional<linestring_type_fp> PathFindingSurface::find_path(	10,532!
403	const point_type_fp& start, const point_type_fp& goal,
404	const coordinate_type_fp& max_path_length,
405	const boost::optional<size_t>& max_tries) const {
406	if (max_tries && *max_tries == 0) {	10,532!
NEW 407	return boost::none;	×
408	}
409	PathFindingSurfaceWithTries pfs_with_tries(*this, max_tries);
410
411	auto ring_indices = in_surface(start);	10,532✔
412	if (!ring_indices) {	10,532✔
413	// Start is not in the surface.
414	return boost::none;	1,102✔
415	}
416	if (ring_indices != in_surface(goal)) {	18,860✔
417	// Either goal is not in the surface or it's in a region unreachable by start.
418	return boost::none;	6,300✔
419	}
420	return pfs_with_tries.find_path(start, goal, max_path_length, *ring_indices);	3,130✔
421	}
422
423	const std::vector<point_type_fp>&
424	PathFindingSurface::vertices(SearchKey search_key) const {	47,802✔
425	auto memoized_result = vertices_memo.find(search_key);
426	if (memoized_result != vertices_memo.cend()) {	47,802✔
427	return memoized_result->second;	47,644✔
428	}
429	std::vector<point_type_fp> ret;
430	const auto& vertices = all_vertices;
431	const auto& ring_indices = ring_indices_cache.at(search_key);	158!
432	for (size_t poly_index = 0; poly_index < ring_indices.size() ; poly_index++) {	1,114✔
433	// This is the poly to look at.
434	const auto& poly_ring_index = ring_indices[poly_index];
435	// These are the vertices for that poly.
436	const auto& poly_vertices = vertices[poly_ring_index.first];	956✔
437	for (size_t ring_index = 0; ring_index < poly_ring_index.second.size(); ring_index++) {	2,812✔
438	const auto& ring_ring_index = poly_ring_index.second[ring_index];
439	const auto& ring_vertices = poly_vertices[ring_ring_index.first];	1,856!
440	ret.insert(ret.cend(), ring_vertices.cbegin(), ring_vertices.cend());	1,856!
441	}
442	}
443	return vertices_memo.emplace(search_key, ret).first->second;	158✔
444	}	79✔
445
446	optional<linestring_type_fp> PathFindingSurfaceWithTries::find_path(	50,564!
447	const point_type_fp& start, const point_type_fp& goal,
448	const coordinate_type_fp& max_path_length,
449	SearchKey search_key) {
450	// Connect if a direct connection is possible. This also takes care
451	// of the case where start == goal.
452	try {
453	if (in_surface(start, goal)) {	50,564✔
454	decrement_tries();	2,762!
455	if (bg::comparable_distance(start, goal) < max_path_length * max_path_length) {	2,762✔
456	// in_surface builds up some structures that are only efficient if
457	// we're doing many tries.
458	return {{start, goal}};	3,384!
459	} else {
460	// If the straight line was too long then there is no way to connect.
461	return boost::none;	1,070✔
462	}
463	}
464	} catch (GiveUp g) {	×
465	return boost::none;
466	}
467	// Do astar.
468	priority_queue<pair<coordinate_type_fp, point_type_fp>,
469	vector<pair<coordinate_type_fp, point_type_fp>>,
470	std::greater<pair<coordinate_type_fp, point_type_fp>>> open_set;
471	open_set.emplace(bg::distance(start, goal), start);	47,802!
472	unordered_set<point_type_fp> closed_set;
473	unordered_map<point_type_fp, point_type_fp> came_from;
474	unordered_map<point_type_fp, coordinate_type_fp> g_score; // Empty should be considered infinity.
475	g_score[start] = 0;	47,802✔
476	while (!open_set.empty()) {	47,802!
477	const auto current = open_set.top().second;	47,802✔
478	open_set.pop();	47,802✔
479	if (current == goal) {
480	// We're done.
481	return boost::make_optional(build_path(current, came_from));	×
482	}
483	if (closed_set.count(current) > 0) {	23,901!
484	// Skip this because we already "removed it", sort of.
485	continue;	×
486	}
487	try {
488	auto current_neighbors = neighbors(
489	start, goal,
490	max_path_length - g_score.at(current),	47,802!
491	search_key,
492	current);
493	for (const auto& neighbor : current_neighbors) {	47,802!
494	const auto tentative_g_score = g_score.at(current) + bg::distance(current, neighbor);	×
495	if (g_score.count(neighbor) == 0 \|\| tentative_g_score < g_score.at(neighbor)) {	×
496	// This path to neighbor is better than any previous one.
497	came_from[neighbor] = current;	×
498	g_score[neighbor] = tentative_g_score;	×
499	open_set.emplace(tentative_g_score + bg::distance(neighbor, goal), neighbor);	×
500	}
501	}
502	} catch (GiveUp g) {	47,802!
503	return boost::none;
504	}	23,901✔
505	// Because we can't delete from the open_set, we'll just marked
506	// items as closed and ignore them later.
507	closed_set.insert(current);
508	}
509	return boost::none;	×
510	}
511
512	} //namespace path_finding

pcb2gcode / pcb2gcode / 22529124718

Source File Press 'n' to go to next uncovered line, 'b' for previous

Source File
Press 'n' to go to next uncovered line, 'b' for previous