528534749

Build Type

github

Committed by GitHub

Commit Message

Merge 55f5c5a95 into 591322cbc

Pull Request Pull Request #523: Better path finding algorithm

Run Details

4868 of 8264 branches covered (58.91%)

Branch coverage included in aggregate %.

290 of 302 new or added lines in 4 files covered. (96.03%)

1 existing line in 1 file now uncovered.

3306 of 3540 relevant lines covered (93.39%)

199109.72 hits per line

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

78.24

/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"

namespace path_finding {

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

Neighbors::iterator Neighbors::iterator::operator++() {
  do {
    if (ring_index < 2) {
      ring_index++;
    } else {
      point_index++;
      while (ring_index-2 < neighbors->pfs->vertices().size() &&
             point_index >= neighbors->pfs->vertices()[ring_index-2].size()) {
        ring_index++;
        point_index = 0;
      }
    }
  } while (ring_index < neighbors->pfs->vertices().size()+2 &&
           !neighbors->is_neighbor(**this));
  return *this;
}

bool Neighbors::iterator::operator!=(const Neighbors::iterator& other) const {
  return ring_index != other.ring_index || 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 (ring_index == 0) {
    return neighbors->start;
  } else if (ring_index == 1) {
    return neighbors->goal;
  } else {
    return neighbors->pfs->vertices()[ring_index-2][point_index];
  }
}

Neighbors::Neighbors(const point_type_fp& start, const point_type_fp& goal,
                     const point_type_fp& current,
                     const RingIndices& ring_indices,
                     coordinate_type_fp g_score_current,
                     const PathLimiter path_limiter,
                     const PathFindingSurface* pfs) :
    start(start),
    goal(goal),
    current(current),
    ring_indices(ring_indices),
    g_score_current(g_score_current),
    path_limiter(path_limiter),
    pfs(pfs) {}

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

Neighbors::iterator Neighbors::begin() const {
  auto ret = iterator(this, 0, 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, 2+pfs->vertices().size(), 0);
}

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 && keep_out.size() == 0) {
    return;
  }
  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(poly.outer().cbegin(), poly.outer().cend());
      total_keep_in_grown->emplace_back(
          bg_helpers::buffer_miter(poly.outer(), tolerance),
          vector<multi_polygon_type_fp>());
      for (const auto& inner : poly.inners()) {
        all_vertices.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().second.push_back(bg_helpers::buffer_miter(temp_inner, -tolerance));
      }
    }
  } else {
    for (const auto& poly : keep_out){
      all_vertices.emplace_back(poly.outer().cbegin(), poly.outer().cend());
      keep_out_shrunk.emplace_back(
          bg_helpers::buffer_miter(poly.outer(), -tolerance),
          vector<multi_polygon_type_fp>());
      for (const auto& inner : poly.inners()) {
        all_vertices.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().second.push_back(bg_helpers::buffer_miter(temp_inner, tolerance));
      }
    }
  }

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

vector<size_t> inside_multipolygon(const point_type_fp& p,
                                   const multi_polygon_type_fp& mp) {
  size_t current_ring_index = 0;
  for (const auto& poly : mp) {
    if (point_in_ring(p, poly.outer())) {
      // Might be part of this shape but only if the point isn't in
      // the inners.
      vector<size_t> ring_indices{current_ring_index};
      current_ring_index++;
      for (const auto& inner : poly.inners()) {
        if (!point_in_ring(p, inner)) {
          // We'll have to make sure not to cross this inner.
          ring_indices.push_back(current_ring_index);
          current_ring_index++;
        } else {
          break; // We're inside one of the inners so give up.
        }
      }
      if (ring_indices.size() == poly.inners().size() + 1) {
        // We're inside this shape so we're done.
        return ring_indices;
      } else {
        // 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.  Reset the current_ring_index to what it was.
        current_ring_index = ring_indices.front();
        ring_indices.clear();
      }
    }
    current_ring_index += 1 + poly.inners().size();
  }
  return {};
}

vector<size_t> outside_multipolygon(const point_type_fp& p,
                                    const multi_polygon_type_fp& mp) {
  vector<size_t> ring_indices;
  size_t current_ring_index = 0;
  for (const auto& poly : mp) {
    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_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_inner = true;
          ring_indices.push_back(current_ring_index + i + 1);
          break;
        }
      }
      if (!in_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 {};
      }
      current_ring_index += 1 + poly.inners().size();
    } else {
      // We need to keep out of this outer.
      ring_indices.push_back(current_ring_index);
      // No need to examine the inners which we can't possibly be inside.
      current_ring_index += 1 + poly.inners().size();
    }
  }
  return ring_indices;
}

RingIndices inside_multipolygons(
    const point_type_fp& p,
    const nested_multipolygon_type_fp& mp){
  size_t current_ring_index = 0;
  for (const auto& poly : mp) {
    // The loop always starts on an outer.
    auto inside_mp = inside_multipolygon(p, poly.first);
    if (inside_mp.size() > 0) {
      // Might be part of this shape but only if the point isn't in
      // the inners.
      RingIndices ring_indices{{current_ring_index, inside_mp}};
      current_ring_index++;
      for (const auto& inner : poly.second) {
        auto outside_mp = outside_multipolygon(p, inner);
        if (outside_mp.size() > 0) {
          // We'll have to make sure not to cross this inner.
          ring_indices.push_back({current_ring_index, outside_mp});
          current_ring_index++;
        } else {
          break; // We're inside one of the inners so give up.
        }
      }
      if (ring_indices.size() == poly.second.size() + 1) {
        // We never hit the break so we're inside this shape so we're
        // done.
        return ring_indices;
      } else {
        // 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.  Reset the current_ring_index to what it was.
        current_ring_index = ring_indices.front().first;
        ring_indices.clear();
      }
    }
    current_ring_index += 1 + poly.second.size();
  }
  return {};
}

RingIndices outside_multipolygons(
    const point_type_fp& p,
    const nested_multipolygon_type_fp& mp) {
  RingIndices ring_indices;
  size_t current_ring_index = 0;
  for (const auto& poly : mp) {
    auto outside_mp = outside_multipolygon(p, poly.first);
    if (outside_mp.size() == 0) {
      // 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_inner = false;
      for (size_t i = 0; i < poly.second.size(); i++) {
        const auto& inner = poly.second[i];
        auto inside_mp = inside_multipolygon(p, inner);
        if (inside_mp.size() > 0) {
          in_inner = true;
          ring_indices.push_back({current_ring_index + i + 1, inside_mp});
          break;
        }
      }
      if (!in_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 {};
      }
      current_ring_index += 1 + poly.second.size();
    } else {
      // We need to keep out of this outer.
      ring_indices.push_back({current_ring_index, outside_mp});
      // No need to examine the inners which we can't possibly be inside.
      current_ring_index += 1 + poly.second.size();
    }
  }
  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. */
RingIndices PathFindingSurface::in_surface(point_type_fp p) const {
  if (total_keep_in_grown) {
    return inside_multipolygons(p, *total_keep_in_grown);
  } else {
    return outside_multipolygons(p, keep_out_shrunk);
  }
}

bool is_intersecting(const point_type_fp& a, const point_type_fp& b, const ring_type_fp& ring) {
  for (auto current = ring.cbegin(); current + 1 != ring.cend(); current++) {
    if (is_intersecting(a, b, *current, *(current+1))) {
      return true;
    }
  }
  return false;
}

bool is_intersecting(
    const point_type_fp& a, const point_type_fp& b,
    const vector<size_t>& ring_indices,
    const multi_polygon_type_fp& poly_to_search) {
  size_t current_ring = 0;
  size_t current_index = 0;
  for (const auto& poly : poly_to_search) {
    if (ring_indices[current_index] == current_ring) {
      if (is_intersecting(a, b, poly.outer())) {
        return true;
      }
      current_index++;
      if (current_index >= ring_indices.size()) {
        return false; // We've finished our search.
      }
    }
    for (const auto& inner : poly.inners()) {
      current_ring++;
      if (ring_indices[current_index] == current_ring) {
        if (is_intersecting(a, b, inner)) {
          return true;
        }
        current_index++;
        if (current_index >= ring_indices.size()) {
          return false; // We've finished our search.
        }
      }
    }
    current_ring++;
  }
  throw std::logic_error("There were more rings to search than rings.  "
                         "This should never happen.");
}

bool is_intersecting(
    const point_type_fp& a, const point_type_fp& b,
    const RingIndices& ring_indices,
    const nested_multipolygon_type_fp* poly_to_search) {
  size_t current_ring = 0;
  size_t current_index = 0;
  for (const auto& poly : *poly_to_search) {
    if (ring_indices[current_index].first == current_ring) {
      if (is_intersecting(a, b, ring_indices[current_index].second, poly.first)) {
         return true;
      }
      current_index++;
      if (current_index >= ring_indices.size()) {
         return false; // We're finished our search.
      }
    }
    for (const auto& inner : poly.second) {
      current_ring++;
      if (ring_indices[current_index].first == current_ring) {
        if (is_intersecting(a, b, ring_indices[current_index].second, inner)) {
           return true;
        }
        current_index++;
        if (current_index >= ring_indices.size()) {
           return false; // We're finished our search.
        }
      }
    }
    current_ring++;
  }
  throw std::logic_error("There were more rings to search than rings.  "
                         "This should never happen.");
}

// 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 RingIndices& ring_indices) const {
  if (b < a) {
    return in_surface(b, a, ring_indices);
  }
  const auto key = make_pair(a, b);
  auto memoized_result = in_surface_memo.find(key);
  if (memoized_result != in_surface_memo.cend()) {
    return memoized_result->second;
  }
  const nested_multipolygon_type_fp* poly_to_search;
  if (total_keep_in_grown) {
    poly_to_search = &*total_keep_in_grown;
  } else {
    poly_to_search = &keep_out_shrunk;
  }
  auto found_intersection = is_intersecting(a, b, ring_indices, poly_to_search);
  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& end,
                                        const RingIndices& ring_indices,
                                        coordinate_type_fp g_score_current,
                                        const PathLimiter path_limiter,
                                        const point_type_fp& current) const {
  return Neighbors(start, end, current, ring_indices, g_score_current, path_limiter, 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 PathLimiter& path_limiter) const {
  RingIndices ring_indices;
  ring_indices = in_surface(start);
  if (ring_indices.size() == 0) {
    // 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;
  }

  // Connect if a direct connection is possible.  This also takes care
  // of the case where start == goal.
  linestring_type_fp direct_ls;
  direct_ls.push_back(start);
  direct_ls.push_back(goal);
  try {
    if (in_surface(start, goal, ring_indices) &&
        (path_limiter == nullptr || !path_limiter(goal, bg::distance(start, goal)))) {
      return direct_ls;
    }
  } 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, ring_indices, g_score.at(current), path_limiter, 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
27	namespace path_finding {
28
29	using boost::optional;
30	using boost::make_optional;
31
32	Neighbors::iterator Neighbors::iterator::operator++() {	14,293✔
33	do {
34	if (ring_index < 2) {	27,979✔
35	ring_index++;	27,702✔
36	} else {
37	point_index++;	277✔
38	while (ring_index-2 < neighbors->pfs->vertices().size() &&	626✔
39	point_index >= neighbors->pfs->vertices()[ring_index-2].size()) {	566✔
40	ring_index++;	36✔
41	point_index = 0;	36✔
42	}
43	}
44	} while (ring_index < neighbors->pfs->vertices().size()+2 &&	69,808✔
45	!neighbors->is_neighbor(**this));	27,949✔
46	return *this;	194✔
47	}
48
NEW 49	bool Neighbors::iterator::operator!=(const Neighbors::iterator& other) const {	×
50	return ring_index != other.ring_index \|\| point_index != other.point_index;	14,341!
51	}
52
NEW 53	bool Neighbors::iterator::operator==(const Neighbors::iterator& other) const {	×
NEW 54	return !(*this != other);	×
55	}
56
NEW 57	const point_type_fp& Neighbors::iterator::operator*() const {	×
58	if (ring_index == 0) {	28,131!
59	return neighbors->start;	18✔
60	} else if (ring_index == 1) {	28,113!
61	return neighbors->goal;	14,136✔
62	} else {
63	return neighbors->pfs->vertices()[ring_index-2][point_index];	27,954✔
64	}
65	}
66
67	Neighbors::Neighbors(const point_type_fp& start, const point_type_fp& goal,	14,129✔
68	const point_type_fp& current,
69	const RingIndices& ring_indices,
70	coordinate_type_fp g_score_current,
71	const PathLimiter path_limiter,
72	const PathFindingSurface* pfs) :	14,129✔
73	start(start),
74	goal(goal),
75	current(current),
76	ring_indices(ring_indices),
77	g_score_current(g_score_current),
78	path_limiter(path_limiter),
79	pfs(pfs) {}	14,129!
80
81	// Returns a valid neighbor index that is either the one provided or
82	// the next higher valid one.
83	bool Neighbors::is_neighbor(const point_type_fp p) const {	42,078✔
84	if (p == current) {
85	return false;
86	}
87	if (path_limiter != nullptr && path_limiter(p, g_score_current + bg::distance(current, p))) {	41,493!
88	return false;	5,916✔
89	}
90	if (!pfs->in_surface(current, p, ring_indices)) {	7,928✔
91	return false;	7,746✔
92	}
93	return true;
94	}
95
96	Neighbors::iterator Neighbors::begin() const {	14,129!
97	auto ret = iterator(this, 0, 0);
98	if (ret == end()) {
99	// Can't dereferfence the end.
100	return ret;
101	}
102	if (is_neighbor(*ret)) {	14,129✔
103	// This is a valid begin.
104	return ret;
105	} else {
106	// This position is invalid but we can increment to get to a valid one.
107	++ret;	14,111✔
108	return ret;	13✔
109	}
110	}
111
NEW 112	Neighbors::iterator Neighbors::end() const {	×
113	return iterator(this, 2+pfs->vertices().size(), 0);	14,191!
114	}
115
116	PathFindingSurface::PathFindingSurface(const optional<multi_polygon_type_fp>& keep_in,	143✔
117	const multi_polygon_type_fp& keep_out,
118	const coordinate_type_fp tolerance) {	429✔
119	if (!keep_in && keep_out.size() == 0) {	143✔
120	return;
121	}
122	if (keep_in) {	97✔
123	multi_polygon_type_fp total_keep_in = *keep_in - keep_out;	66!
124
125	total_keep_in_grown.emplace();
126	for (const auto& poly : total_keep_in) {	200✔
127	all_vertices.emplace_back(poly.outer().cbegin(), poly.outer().cend());	134!
128	total_keep_in_grown->emplace_back(	134!
129	bg_helpers::buffer_miter(poly.outer(), tolerance),	134!
130	vector<multi_polygon_type_fp>());	134✔
131	for (const auto& inner : poly.inners()) {	453✔
132	all_vertices.emplace_back(inner.cbegin(), inner.cend());	319!
133	// Because the inner is reversed, we need to reverse it so
134	// that the buffer algorithm won't get confused.
135	auto temp_inner = inner;
136	bg::reverse(temp_inner);
137	// tolerance needs to be inverted because growing a shape
138	// shrinks the holes in it.
139	total_keep_in_grown->back().second.push_back(bg_helpers::buffer_miter(temp_inner, -tolerance));	638!
140	}
141	}
142	} else {
143	for (const auto& poly : keep_out){	382✔
144	all_vertices.emplace_back(poly.outer().cbegin(), poly.outer().cend());	351!
145	keep_out_shrunk.emplace_back(	351!
146	bg_helpers::buffer_miter(poly.outer(), -tolerance),	702!
147	vector<multi_polygon_type_fp>());	351✔
148	for (const auto& inner : poly.inners()) {	672✔
149	all_vertices.emplace_back(inner.cbegin(), inner.cend());	321!
150	// Because the inner is reversed, we need to reverse it so
151	// that the buffer algorithm won't get confused.
152	auto temp_inner = inner;
153	bg::reverse(temp_inner);
154	// tolerance needs to be inverted because shrinking a shape
155	// grows the holes in it.
156	keep_out_shrunk.back().second.push_back(bg_helpers::buffer_miter(temp_inner, tolerance));	642!
157	}
158	}
159	}
160
161	for (auto av : all_vertices) {	1,222!
162	sort(av.begin(), av.end());	1,125✔
163	av.erase(std::unique(av.begin(), av.end()), av.end());	1,125✔
164	}
165	}
166
167	vector<size_t> inside_multipolygon(const point_type_fp& p,	277,245✔
168	const multi_polygon_type_fp& mp) {
169	size_t current_ring_index = 0;	277,245✔
170	for (const auto& poly : mp) {	510,196✔
171	if (point_in_ring(p, poly.outer())) {	277,252✔
172	// Might be part of this shape but only if the point isn't in
173	// the inners.
174	vector<size_t> ring_indices{current_ring_index};	44,308!
175	current_ring_index++;	44,308✔
176	for (const auto& inner : poly.inners()) {	44,319✔
177	if (!point_in_ring(p, inner)) {	18✔
178	// We'll have to make sure not to cross this inner.
179	ring_indices.push_back(current_ring_index);	11!
180	current_ring_index++;	11✔
181	} else {
182	break; // We're inside one of the inners so give up.
183	}
184	}
185	if (ring_indices.size() == poly.inners().size() + 1) {	132,924✔
186	// We're inside this shape so we're done.
187	return ring_indices;
188	} else {
189	// We're inside the outer but also inside an inner! It might
190	// be an outer in an inner so we'll ignore this one and keep
191	// searching. Reset the current_ring_index to what it was.
192	current_ring_index = ring_indices.front();	7!
193	ring_indices.clear();
194	}
195	}
196	current_ring_index += 1 + poly.inners().size();	465,902✔
197	}
198	return {};
199	}
200
201	vector<size_t> outside_multipolygon(const point_type_fp& p,	345,109✔
202	const multi_polygon_type_fp& mp) {
203	vector<size_t> ring_indices;
204	size_t current_ring_index = 0;	345,109✔
205	for (const auto& poly : mp) {	656,389✔
206	if (point_in_ring(p, poly.outer())) {	335,982✔
207	// We're inside the outer, maybe we're in an inner? If not, we
208	// aren't outside at all and we'll just give up.
209	bool in_inner = false;
210	for (size_t i = 0; i < poly.inners().size(); i++) {	49,421✔
211	const auto& inner = poly.inners()[i];
212	if (point_in_ring(p, inner)) {	11✔
213	in_inner = true;
214	ring_indices.push_back(current_ring_index + i + 1);	6!
215	break;
216	}
217	}
218	if (!in_inner) {
219	// We're inside the outer but not in any of the inners, so
220	// we're in the shape, but we want to be outside the shape, so
221	// we've failed.
222	return {};
223	}
224	current_ring_index += 1 + poly.inners().size();	12✔
225	} else {
226	// We need to keep out of this outer.
227	ring_indices.push_back(current_ring_index);	311,274!
228	// No need to examine the inners which we can't possibly be inside.
229	current_ring_index += 1 + poly.inners().size();	622,548✔
230	}
231	}
232	return ring_indices;
233	}
234
235	RingIndices inside_multipolygons(	20,496✔
236	const point_type_fp& p,
237	const nested_multipolygon_type_fp& mp){
238	size_t current_ring_index = 0;
239	for (const auto& poly : mp) {	27,224✔
240	// The loop always starts on an outer.
241	auto inside_mp = inside_multipolygon(p, poly.first);	26,887!
242	if (inside_mp.size() > 0) {	26,887✔
243	// Might be part of this shape but only if the point isn't in
244	// the inners.
245	RingIndices ring_indices{{current_ring_index, inside_mp}};	20,607!
246	current_ring_index++;	20,383✔
247	for (const auto& inner : poly.second) {	166,759✔
248	auto outside_mp = outside_multipolygon(p, inner);	146,600!
249	if (outside_mp.size() > 0) {	146,600✔
250	// We'll have to make sure not to cross this inner.
NEW 251	ring_indices.push_back({current_ring_index, outside_mp});	×
252	current_ring_index++;	146,376!
253	} else {
254	break; // We're inside one of the inners so give up.
255	}
256	}
257	if (ring_indices.size() == poly.second.size() + 1) {	61,149✔
258	// We never hit the break so we're inside this shape so we're
259	// done.
260	return ring_indices;	20,159✔
261	} else {
262	// We're inside the outer but also inside an inner! It might
263	// be an outer in an inner so we'll ignore this one and keep
264	// searching. Reset the current_ring_index to what it was.
265	current_ring_index = ring_indices.front().first;	224✔
266	ring_indices.clear();
267	}
268	}
269	current_ring_index += 1 + poly.second.size();	13,456✔
270	}
271	return {};
272	}
273
274	RingIndices outside_multipolygons(	23,913✔
275	const point_type_fp& p,
276	const nested_multipolygon_type_fp& mp) {
277	RingIndices ring_indices;	23,913✔
278	size_t current_ring_index = 0;
279	for (const auto& poly : mp) {	212,711✔
280	auto outside_mp = outside_multipolygon(p, poly.first);	198,498!
281	if (outside_mp.size() == 0) {	198,498✔
282	// We're inside the outer, maybe we're in an inner? If not, we
283	// aren't outside at all and we'll just give up.
284	bool in_inner = false;
285	for (size_t i = 0; i < poly.second.size(); i++) {	293,660✔
286	const auto& inner = poly.second[i];
287	auto inside_mp = inside_multipolygon(p, inner);	250,347!
288	if (inside_mp.size() > 0) {	250,347✔
289	in_inner = true;
290	ring_indices.push_back({current_ring_index + i + 1, inside_mp});	47,826!
291	break;
292	}
293	}
294	if (!in_inner) {
295	// We're inside the outer but not in any of the inners, so
296	// we're in the shape, but we want to be outside the shape, so
297	// we've failed.
298	return {};
299	}
300	current_ring_index += 1 + poly.second.size();	47,826✔
301	} else {
302	// We need to keep out of this outer.
NEW 303	ring_indices.push_back({current_ring_index, outside_mp});	×
304	// No need to examine the inners which we can't possibly be inside.
305	current_ring_index += 1 + poly.second.size();	329,770✔
306	}
307	}
308	return ring_indices;
309	}
310
311	/* Given a point, determine if the point is in the search surface. If
312	so, return a non-default value, otherwise default value. If two
313	points return the same value, there is a path between them in the
314	surface. If not then there cannot be a path between them. The
315	value will actually be a vector of size_t that indicates which
316	rings in the stored polygon should be used for the generated points
317	in the path and also for the collision detection. */
318	RingIndices PathFindingSurface::in_surface(point_type_fp p) const {	44,409✔
319	if (total_keep_in_grown) {	44,409✔
320	return inside_multipolygons(p, *total_keep_in_grown);	20,496✔
321	} else {
322	return outside_multipolygons(p, keep_out_shrunk);	23,913✔
323	}
324	}
325
NEW 326	bool is_intersecting(const point_type_fp& a, const point_type_fp& b, const ring_type_fp& ring) {	×
327	for (auto current = ring.cbegin(); current + 1 != ring.cend(); current++) {	8,971,091!
328	if (is_intersecting(a, b, current, (current+1))) {	4,475,629!
329	return true;
330	}
331	}
332	return false;
333	}
334
335	bool is_intersecting(	37,731✔
336	const point_type_fp& a, const point_type_fp& b,
337	const vector<size_t>& ring_indices,
338	const multi_polygon_type_fp& poly_to_search) {
339	size_t current_ring = 0;
340	size_t current_index = 0;
341	for (const auto& poly : poly_to_search) {	37,757!
342	if (ring_indices[current_index] == current_ring) {	75,514!
343	if (is_intersecting(a, b, poly.outer())) {	37,757✔
344	return true;
345	}
346	current_index++;	28,793✔
347	if (current_index >= ring_indices.size()) {	57,586✔
348	return false; // We've finished our search.
349	}
350	}
351	for (const auto& inner : poly.inners()) {	72✔
352	current_ring++;	46!
353	if (ring_indices[current_index] == current_ring) {	46!
354	if (is_intersecting(a, b, inner)) {	46✔
355	return true;
356	}
357	current_index++;	25✔
358	if (current_index >= ring_indices.size()) {	50!
359	return false; // We've finished our search.
360	}
361	}
362	}
363	current_ring++;	26✔
364	}
365	throw std::logic_error("There were more rings to search than rings. "
NEW 366	"This should never happen.");	×
367	}
368
369	bool is_intersecting(	10,171✔
370	const point_type_fp& a, const point_type_fp& b,
371	const RingIndices& ring_indices,
372	const nested_multipolygon_type_fp* poly_to_search) {
373	size_t current_ring = 0;
374	size_t current_index = 0;
375	for (const auto& poly : *poly_to_search) {	21,713!
376	if (ring_indices[current_index].first == current_ring) {	43,426✔
377	if (is_intersecting(a, b, ring_indices[current_index].second, poly.first)) {	20,587✔
378	return true;
379	}
380	current_index++;	17,630✔
381	if (current_index >= ring_indices.size()) {	35,260✔
382	return false; // We're finished our search.
383	}
384	}
385	for (const auto& inner : poly.second) {	34,104✔
386	current_ring++;	22,562✔
387	if (ring_indices[current_index].first == current_ring) {	45,124✔
388	if (is_intersecting(a, b, ring_indices[current_index].second, inner)) {	17,144✔
389	return true;
390	}
391	current_index++;	11,116✔
392	if (current_index >= ring_indices.size()) {	22,232✔
393	return false; // We're finished our search.
394	}
395	}
396	}
397	current_ring++;	11,542✔
398	}
399	throw std::logic_error("There were more rings to search than rings. "
NEW 400	"This should never happen.");	×
401	}
402
403	// Return true if this edge from a to b is part of the path finding surface.
404	bool PathFindingSurface::in_surface(	25,737✔
405	const point_type_fp& a, const point_type_fp& b,
406	const RingIndices& ring_indices) const {
407	if (b < a) {
408	return in_surface(b, a, ring_indices);	2,941✔
409	}
410	const auto key = make_pair(a, b);
411	auto memoized_result = in_surface_memo.find(key);
412	if (memoized_result != in_surface_memo.cend()) {	22,796✔
413	return memoized_result->second;	12,625✔
414	}
415	const nested_multipolygon_type_fp* poly_to_search;
416	if (total_keep_in_grown) {	10,171✔
417	poly_to_search = &*total_keep_in_grown;
418	} else {
419	poly_to_search = &keep_out_shrunk;	3,626✔
420	}
421	auto found_intersection = is_intersecting(a, b, ring_indices, poly_to_search);	10,171✔
422	in_surface_memo.emplace(key, !found_intersection);	10,171✔
423	return !found_intersection;	10,171✔
424	}
425
426	// Return all possible neighbors of current. A neighbor can be
427	// start, end, or any of the points in all_vertices. But only the
428	// ones that are in_surface are returned.
429	Neighbors PathFindingSurface::neighbors(const point_type_fp& start, const point_type_fp& end,	14,129✔
430	const RingIndices& ring_indices,
431	coordinate_type_fp g_score_current,
432	const PathLimiter path_limiter,
433	const point_type_fp& current) const {
434	return Neighbors(start, end, current, ring_indices, g_score_current, path_limiter, this);	28,258!
435	}
436
437	// Return a path from the start to the current. Always return at
438	// least two points.
439	linestring_type_fp build_path(	7✔
440	point_type_fp current,
441	const unordered_map<point_type_fp, point_type_fp>& came_from) {
442	linestring_type_fp result;
443	while (came_from.count(current)) {	25✔
444	result.push_back(current);	18!
445	current = came_from.at(current);	18✔
446	}
447	result.push_back(current);	7!
448	bg::reverse(result);
449	return result;	7✔
450	}
451
452	optional<linestring_type_fp> PathFindingSurface::find_path(	26,403!
453	const point_type_fp& start, const point_type_fp& goal,
454	const PathLimiter& path_limiter) const {
455	RingIndices ring_indices;	26,403✔
456	ring_indices = in_surface(start);	52,806!
457	if (ring_indices.size() == 0) {	26,403✔
458	// Start is not in the surface.
459	return boost::none;	9,703✔
460	}
461	if (ring_indices != in_surface(goal)) {	33,400!
462	// Either goal is not in the surface or it's in a region unreachable by start.
463	return boost::none;	1,832✔
464	}
465
466	// Connect if a direct connection is possible. This also takes care
467	// of the case where start == goal.
468	linestring_type_fp direct_ls;
469	direct_ls.push_back(start);	14,868!
470	direct_ls.push_back(goal);	14,868!
471	try {
472	if (in_surface(start, goal, ring_indices) &&	14,868!
473	(path_limiter == nullptr \|\| !path_limiter(goal, bg::distance(start, goal)))) {	16,184!
474	return direct_ls;
475	}
476	} catch (GiveUp g) {	1✔
477	return boost::none;
478	}
479
480	// Do astar.
481	priority_queue<pair<coordinate_type_fp, point_type_fp>,
482	vector<pair<coordinate_type_fp, point_type_fp>>,
483	std::greater<pair<coordinate_type_fp, point_type_fp>>> open_set;
484	open_set.emplace(bg::distance(start, goal), start);	14,107!
485	unordered_set<point_type_fp> closed_set;
486	unordered_map<point_type_fp, point_type_fp> came_from;
487	unordered_map<point_type_fp, coordinate_type_fp> g_score; // Empty should be considered infinity.
488	g_score[start] = 0;	14,107✔
489	while (!open_set.empty()) {	14,137✔
490	const auto current = open_set.top().second;	14,136✔
491	open_set.pop();	14,136✔
492	if (current == goal) {
493	// We're done.
494	return make_optional(build_path(current, came_from));	7!
495	}
496	if (closed_set.count(current) > 0) {	14,129!
497	// Skip this because we already "removed it", sort of.
NEW UNCOV 498	continue;	×
499	}
500	try {
501	const auto current_neighbors = neighbors(
502	start, goal, ring_indices, g_score.at(current), path_limiter, current);	42,387!
503	for (const auto& neighbor : current_neighbors) {	14,311✔
504	const auto tentative_g_score = g_score.at(current) + bg::distance(current, neighbor);	364✔
505	if (g_score.count(neighbor) == 0 \|\| tentative_g_score < g_score.at(neighbor)) {	301✔
506	// This path to neighbor is better than any previous one.
507	came_from[neighbor] = current;	64!
508	g_score[neighbor] = tentative_g_score;	64!
509	open_set.emplace(tentative_g_score + bg::distance(neighbor, goal), neighbor);	14,163!
510	}
511	}
512	} catch (GiveUp g) {	14,099✔
513	return boost::none;
514	}
515	// Because we can't delete from the open_set, we'll just marked
516	// items as closed and ignore them later.
517	closed_set.insert(current);
518	}
519	return boost::none;	1✔
520	}
521
522	} //namespace path_finding

pcb2gcode / pcb2gcode / 528534749

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