731519691

Build # 731519691

Build Type

github

Committed by GitHub

Commit Message

Merge 1a7969bff into 56a6103e2

Pull Request Pull Request #573: fix: Use backtracking limit in path_finding

Run Details

5126 of 8720 branches covered (58.78%)

Branch coverage included in aggregate %.

5 of 5 new or added lines in 2 files covered. (100.0%)

1 existing line in 1 file now uncovered.

3528 of 3781 relevant lines covered (93.31%)

115711.38 hits per line

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

76.98

/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,
                     const PathFindingSurface* 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) const {
  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() const {
  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() const {
  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;
}

void PathFindingSurface::decrement_tries() const {
  if (tries) {
    if (*tries == 0) {
      throw GiveUp();
    }
    (*tries)--;
  }
}

// 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 all possible neighbors of current.  A neighbor can be
// start, end, or any of the points in all_vertices.  But only the
// ones that are in_surface are returned.
Neighbors PathFindingSurface::neighbors(const point_type_fp& start, const point_type_fp& goal,
                                        const coordinate_type_fp& max_path_length,
                                        SearchKey search_key,
                                        const point_type_fp& current) const {
  return Neighbors(start, goal, current, max_path_length, vertices(search_key), this);
}

// 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,
    SearchKey search_key) const {
  // 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 make_optional(build_path(current, came_from));
    }
    if (closed_set.count(current) > 0) {
      // Skip this because we already "removed it", sort of.
      continue;
    }
    try {
      const 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;
}

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) {
    if (*max_tries == 0) {
      return boost::none;
    }
    tries.emplace(*max_tries);
  } else {
    tries = boost::none;
  }
  return 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) {
    if (*max_tries == 0) {
      return boost::none;
    }
    tries.emplace(*max_tries);
  } else {
    tries = boost::none;
  }

  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 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;
}

} //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++() {	13,823✔
34	const auto& all_vertices_size = neighbors->vertices.size();	13,823✔
35	do {
36	// Move to a new valid point, even if it isn't a neighbor.
37	point_index++;	27,590✔
38	} while (point_index < all_vertices_size + 2 &&	41,521✔
39	!neighbors->is_neighbor(**this));	27,561✔
40	return *this;	193✔
41	}
42
43	bool Neighbors::iterator::operator!=(const Neighbors::iterator& other) const {	×
44	return (point_index != other.point_index);	211✔
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 {	×
52	if (point_index == 0) {	27,743!
53	return neighbors->start;	18✔
54	} else if (point_index == 1) {	27,725!
55	return neighbors->goal;	13,666✔
56	} else {
57	return neighbors->vertices[point_index-2];	14,059✔
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	const PathFindingSurface* pfs) :	13,659✔
66	start(start),
67	goal(goal),
68	current(current),
69	max_path_length_squared(max_path_length),	×
70	vertices(vertices),
71	pfs(pfs) {}	13,659✔
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) const {	41,220✔
76	if (p == current) {	41,220✔
77	return false;
78	}
79	pfs->decrement_tries();	27,555✔
80	if (bg::distance(current, p) + bg::distance(p, goal) > max_path_length_squared) {	41,775✔
81	return false;
82	}
83	if (!pfs->in_surface(current, p)) {	8,324✔
84	return false;	8,142✔
85	}
86	return true;
87	}
88
89	Neighbors::iterator Neighbors::begin() const {	13,659!
90	auto ret = iterator(this, 0);
91	if (ret == end()) {	13,659✔
92	// Can't dereferfence the end.
93	return ret;	×
94	}
95	if (is_neighbor(*ret)) {	13,659✔
96	// This is a valid begin.
97	return ret;	18✔
98	} else {
99	// This position is invalid but we can increment to get to a valid one.
100	++ret;	13,641✔
101	return ret;	12✔
102	}
103	}
104
105	Neighbors::iterator Neighbors::end() const {	×
106	return iterator(this, vertices.size()+2);	27,378!
107	}
108
109	vector<pair<point_type_fp, point_type_fp>> get_all_segments(	147✔
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) {	1,288✔
113	for (auto x1 : x0) {	2,281✔
114	for (size_t i = 0; i+1 < x1.get().size(); i++) {	170,190✔
115	ret.emplace_back(x1.get()[i], x1.get()[i+1]);	83,955!
116	}
117	}
118	}
119	return ret;	147✔
120	}
121
122	vector<std::reference_wrapper<const ring_type_fp>> get_all_rings(const multi_polygon_type_fp& mpolys) {	1,141✔
123	vector<std::reference_wrapper<const ring_type_fp>> ret;
124	for (const auto& poly : mpolys) {	2,279✔
125	ret.push_back(poly.outer());	1,138✔
126	for (const auto& inner : poly.inners()) {	1,140✔
127	ret.push_back(inner);	2!
128	}
129	}
130	return ret;	1,141✔
131	}
132
133	vector<vector<std::reference_wrapper<const ring_type_fp>>> get_all_rings(	147✔
134	const nested_multipolygon_type_fp& mpolys) {
135	vector<vector<std::reference_wrapper<const ring_type_fp>>> ret;
136	for (const auto& poly : mpolys) {	634✔
137	ret.push_back(get_all_rings(poly.outer()));	487!
138	for (const auto& inner: poly.inners()) {	1,141✔
139	ret.push_back(get_all_rings(inner));	1,308!
140	}
141	}
142	return ret;	147✔
143	}
144
145	PathFindingSurface::PathFindingSurface(const optional<multi_polygon_type_fp>& keep_in,	147✔
146	const multi_polygon_type_fp& keep_out,
147	const coordinate_type_fp tolerance) {	1,029!
148	if (keep_in) {	147✔
149	multi_polygon_type_fp total_keep_in = *keep_in - keep_out;	113!
150
151	total_keep_in_grown.emplace();
152	for (const auto& poly : total_keep_in) {	249✔
153	all_vertices.emplace_back();	136!
154	all_vertices.back().emplace_back(poly.outer().cbegin(), poly.outer().cend());	136!
155	total_keep_in_grown->emplace_back(bg_helpers::buffer_miter(poly.outer(), tolerance));	136!
156	for (const auto& inner : poly.inners()) {	457✔
157	all_vertices.back().emplace_back(inner.cbegin(), inner.cend());	321!
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));	642!
165	}
166	}
167	} else {
168	for (const auto& poly : keep_out){	385✔
169	all_vertices.emplace_back();	351!
170	all_vertices.back().emplace_back(poly.outer().cbegin(), poly.outer().cend());	351!
171	keep_out_shrunk.emplace_back(bg_helpers::buffer_miter(poly.outer(), -tolerance));	702!
172	for (const auto& inner : poly.inners()) {	684✔
173	all_vertices.back().emplace_back(inner.cbegin(), inner.cend());	333!
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));	666!
181	}
182	}
183	}
184	const nested_multipolygon_type_fp& poly_to_search = total_keep_in_grown ? *total_keep_in_grown : keep_out_shrunk;	147✔
185	const vector<vector<std::reference_wrapper<const ring_type_fp>>>& all_rings = get_all_rings(poly_to_search);	294!
186	const auto& all_segments = get_all_segments(all_rings);	147!
187	segment_tree::SegmentTree x(all_segments);	147!
188	tree = std::move(x);
189
190	for (auto av : all_vertices) {	1,121!
191	sort(av.begin(), av.end());	487✔
192	av.erase(std::unique(av.begin(), av.end()), av.end());	487✔
193	}
194	}	147✔
195
196	boost::optional<MPRingIndices> inside_multipolygon(const point_type_fp& p,	29,996✔
197	const multi_polygon_type_fp& mp) {
198	for (size_t poly_index = 0; poly_index < mp.size(); poly_index++) {	86,898✔
199	const auto& poly = mp[poly_index];
200	if (point_in_ring(p, poly.outer())) {	30,002✔
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}}};	12,419!
204	for (size_t inner_index = 0; inner_index < poly.inners().size(); inner_index++) {	6,228✔
205	const auto& inner = poly.inners()[inner_index];
206	if (!point_in_ring(p, inner)) {	18✔
207	// We'll have to make sure not to cross this inner.
208	ring_indices.back().second.emplace_back(inner_index+1);	11!
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) {	6,206✔
214	// We never hit the break so we're inside this shape so we're
215	// done.
216	return {ring_indices};	3,096✔
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	}
222	}
223	return boost::none;	26,900✔
224	}
225
226	boost::optional<MPRingIndices> outside_multipolygon(const point_type_fp& p,	57,146✔
227	const multi_polygon_type_fp& mp) {
228	MPRingIndices ring_indices;	57,146✔
229	for (size_t poly_index = 0; poly_index < mp.size(); poly_index++) {	109,158✔
230	const auto& poly = mp[poly_index];
231	if (point_in_ring(p, poly.outer())) {	54,218✔
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++) {	4,429✔
236	const auto& inner = poly.inners()[i];
237	if (point_in_ring(p, inner)) {	11✔
238	in_any_inner = true;
239	ring_indices.emplace_back(poly_index, vector<size_t>{i+1});	12!
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;	2,206✔
248	}
249	} else {
250	// We need to keep out of this outer.
251	ring_indices.emplace_back(poly_index, vector<size_t>{0});	104,012!
252	// No need to examine the inners which we can't possibly be inside.
253	}
254	}
255	return {ring_indices};
256	}
257
258	boost::optional<RingIndices> inside_multipolygons(	1,048✔
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++) {	3,129✔
262	const auto& poly = mp[poly_index];
263	boost::optional<MPRingIndices> inside_mp = inside_multipolygon(p, poly.outer());	2,011!
264	if (inside_mp) {	2,011✔
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}}}};	4,227!
268	for (size_t inner_index = 0; inner_index < poly.inners().size(); inner_index++) {	7,054✔
269	const auto& inner = poly.inners()[inner_index];
270	auto outside_mp = outside_multipolygon(p, inner);	5,035!
271	if (outside_mp) {	5,035✔
272	// We'll have to make sure not to cross this inner.
273	ring_indices.back().second.emplace_back(inner_index+1, *outside_mp);	4,972!
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) {	3,123✔
279	// We never hit the break so we're inside this shape so we're
280	// done.
281	return {ring_indices};	978✔
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	}
287	}
288	return boost::none;	70✔
289	}
290
291	boost::optional<RingIndices> outside_multipolygons(	1,979✔
292	const point_type_fp& p,
293	const nested_multipolygon_type_fp& mp) {
294	RingIndices ring_indices;	1,979✔
295	for (size_t poly_index = 0; poly_index < mp.size(); poly_index++) {	53,990✔
296	const auto& poly = mp[poly_index];
297	auto outside_mp = outside_multipolygon(p, poly.outer());	52,099!
298	if (!outside_mp) {	52,099✔
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++) {	30,199✔
303	const auto& inner = poly.inners()[inner_index];
304	auto inside_mp = inside_multipolygon(p, inner);	27,973!
305	if (inside_mp) {	27,973✔
306	in_any_inner = true;
307	ring_indices.emplace_back(poly_index, vector<pair<size_t, MPRingIndices>>{{inner_index + 1, *inside_mp}});	4,100!
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}});	99,922!
320	}
321	}
322	return {ring_indices};
323	}
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 {	13,375✔
333	auto memoized_result = point_in_surface_memo.find(p);
334	if (memoized_result != point_in_surface_memo.cend()) {	13,375✔
335	return memoized_result->second;	10,348✔
336	}
337	boost::optional<RingIndices> maybe_ring_indices;
338	if (total_keep_in_grown) {	3,027✔
339	maybe_ring_indices = inside_multipolygons(p, *total_keep_in_grown);	2,096!
340	} else {
341	maybe_ring_indices = outside_multipolygons(p, keep_out_shrunk);	3,958!
342	}
343	if (!maybe_ring_indices) {	3,027✔
344	return point_in_surface_memo.emplace(p, boost::none).first->second;	158✔
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()) {	2,869✔
350	// Found in the cache so we can use that.
351	return point_in_surface_memo.emplace(p, find_result->second).first->second;	2,404!
352	}
353	// Not found so we need to add it to the cache.
354	ring_indices_cache.push_back(ring_indices);	465!
355	ring_indices_lookup.emplace(ring_indices_cache.back(), ring_indices_cache.size()-1);	930!
356	return point_in_surface_memo.emplace(p, ring_indices_cache.size()-1).first->second;	930!
357	}
358
359	void PathFindingSurface::decrement_tries() const {	28,894✔
360	if (tries) {	28,894✔
361	if (*tries == 0) {	28,595✔
362	throw GiveUp();	13,630✔
363	}
364	(*tries)--;	14,965✔
365	}
366	}	15,264✔
367
368	// Return true if this edge from a to b is part of the path finding surface.
369	bool PathFindingSurface::in_surface(	25,963✔
370	const point_type_fp& a, const point_type_fp& b) const {
371	if (b < a) {
372	return in_surface(b, a);	2,663✔
373	}
374	const auto key = make_pair(a, b);
375	auto memoized_result = edge_in_surface_memo.find(key);
376	if (memoized_result != edge_in_surface_memo.cend()) {	23,300✔
377	return memoized_result->second;	13,366✔
378	}
379	auto found_intersection = tree.intersects(a, b);	9,934✔
380	edge_in_surface_memo.emplace(key, !found_intersection);	9,934✔
381	return !found_intersection;	9,934✔
382	}
383
384	// Return all possible neighbors of current. A neighbor can be
385	// start, end, or any of the points in all_vertices. But only the
386	// ones that are in_surface are returned.
387	Neighbors PathFindingSurface::neighbors(const point_type_fp& start, const point_type_fp& goal,	×
388	const coordinate_type_fp& max_path_length,
389	SearchKey search_key,
390	const point_type_fp& current) const {
391	return Neighbors(start, goal, current, max_path_length, vertices(search_key), this);	×
392	}
393
394	// Return a path from the start to the current. Always return at
395	// least two points.
396	linestring_type_fp build_path(	7✔
397	point_type_fp current,
398	const unordered_map<point_type_fp, point_type_fp>& came_from) {
399	linestring_type_fp result;
400	while (came_from.count(current)) {	25✔
401	result.push_back(current);	18!
402	current = came_from.at(current);	18✔
403	}
404	result.push_back(current);	7!
405	bg::reverse(result);
406	return result;	7✔
407	}
408	optional<linestring_type_fp> PathFindingSurface::find_path(	14,976✔
409	const point_type_fp& start, const point_type_fp& goal,
410	const coordinate_type_fp& max_path_length,
411	SearchKey search_key) const {
412	// Connect if a direct connection is possible. This also takes care
413	// of the case where start == goal.
414	try {
415	if (in_surface(start, goal)) {	14,976!
416	decrement_tries();	1,339!
417	if (bg::comparable_distance(start, goal) < max_path_length * max_path_length) {	1,339✔
418	// in_surface builds up some structures that are only efficient if
419	// we're doing many tries.
420	return {{start, goal}};	795!
421	} else {
422	// If the straight line was too long then there is no way to connect.
423	return boost::none;	544✔
424	}
425	}
426	} catch (GiveUp g) {	×
427	return boost::none;
428	}
429	// Do astar.
430	priority_queue<pair<coordinate_type_fp, point_type_fp>,
431	vector<pair<coordinate_type_fp, point_type_fp>>,
432	std::greater<pair<coordinate_type_fp, point_type_fp>>> open_set;
433	open_set.emplace(bg::distance(start, goal), start);	13,637!
434	unordered_set<point_type_fp> closed_set;
435	unordered_map<point_type_fp, point_type_fp> came_from;
436	unordered_map<point_type_fp, coordinate_type_fp> g_score; // Empty should be considered infinity.
437	g_score[start] = 0;	13,637✔
438	while (!open_set.empty()) {	13,666!
439	const auto current = open_set.top().second;	13,666✔
440	open_set.pop();	13,666✔
441	if (current == goal) {
442	// We're done.
443	return make_optional(build_path(current, came_from));	7!
444	}
445	if (closed_set.count(current) > 0) {	13,659!
446	// Skip this because we already "removed it", sort of.
447	continue;	×
448	}
449	try {
450	const auto current_neighbors = neighbors(
451	start, goal,
452	max_path_length - g_score.at(current),	13,659!
453	search_key,
454	current);
455	for (const auto& neighbor : current_neighbors) {	13,871✔
456	const auto tentative_g_score = g_score.at(current) + bg::distance(current, neighbor);	364✔
457	if (g_score.count(neighbor) == 0 \|\| tentative_g_score < g_score.at(neighbor)) {	301✔
458	// This path to neighbor is better than any previous one.
459	came_from[neighbor] = current;	64!
460	g_score[neighbor] = tentative_g_score;	64!
461	open_set.emplace(tentative_g_score + bg::distance(neighbor, goal), neighbor);	13,694!
462	}
463	}
464	} catch (GiveUp g) {	13,630✔
465	return boost::none;
466	}
467	// Because we can't delete from the open_set, we'll just marked
468	// items as closed and ignore them later.
469	closed_set.insert(current);
470	}
UNCOV 471	return boost::none;	×
472	}
473
474	optional<linestring_type_fp> PathFindingSurface::find_path(	13,277!
475	const point_type_fp& start, const point_type_fp& goal,
476	const coordinate_type_fp& max_path_length,
477	const boost::optional<size_t>& max_tries,
478	SearchKey search_key) const {
479	if (max_tries) {	13,277!
480	if (*max_tries == 0) {	13,277!
481	return boost::none;	×
482	}
483	tries.emplace(*max_tries);
484	} else {
485	tries = boost::none;
486	}
487	return find_path(start, goal, max_path_length, search_key);	13,277✔
488	}
489
490	optional<linestring_type_fp> PathFindingSurface::find_path(	6,364✔
491	const point_type_fp& start, const point_type_fp& goal,
492	const coordinate_type_fp& max_path_length,
493	const boost::optional<size_t>& max_tries) const {
494	if (max_tries) {	6,364✔
495	if (*max_tries == 0) {	6,349✔
496	return boost::none;	1✔
497	}
498	tries.emplace(*max_tries);
499	} else {
500	tries = boost::none;
501	}
502
503	auto ring_indices = in_surface(start);	6,363✔
504	if (!ring_indices) {	6,363✔
505	// Start is not in the surface.
506	return boost::none;	702✔
507	}
508	if (ring_indices != in_surface(goal)) {	11,322✔
509	// Either goal is not in the surface or it's in a region unreachable by start.
510	return boost::none;	3,962✔
511	}
512	return find_path(start, goal, max_path_length, *ring_indices);	1,699✔
513	}
514
515	const std::vector<point_type_fp>&
516	PathFindingSurface::vertices(SearchKey search_key) const {	13,659✔
517	auto memoized_result = vertices_memo.find(search_key);
518	if (memoized_result != vertices_memo.cend()) {	13,659✔
519	return memoized_result->second;	13,578✔
520	}
521	std::vector<point_type_fp> ret;
522	const auto& vertices = all_vertices;
523	const auto& ring_indices = ring_indices_cache.at(search_key);
524	for (size_t poly_index = 0; poly_index < ring_indices.size() ; poly_index++) {	809✔
525	// This is the poly to look at.
526	const auto& poly_ring_index = ring_indices[poly_index];
527	// These are the vertices for that poly.
528	const auto& poly_vertices = vertices[poly_ring_index.first];	647✔
529	for (size_t ring_index = 0; ring_index < poly_ring_index.second.size(); ring_index++) {	2,168✔
530	const auto& ring_ring_index = poly_ring_index.second[ring_index];
531	const auto& ring_vertices = poly_vertices[ring_ring_index.first];	874!
532	ret.insert(ret.cend(), ring_vertices.cbegin(), ring_vertices.cend());	874!
533	}
534	}
535	return vertices_memo.emplace(search_key, ret).first->second;	81!
536	}
537
538	} //namespace path_finding

pcb2gcode / pcb2gcode / 731519691

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