508492256

Build Type

push

github

Committed by GitHub

Commit Message

Merge pull request #519 from eyal0/better_path_finding

Run Details

4612 of 7939 branches covered (58.09%)

Branch coverage included in aggregate %.

10 of 10 new or added lines in 1 file covered. (100.0%)

19 existing lines in 1 file now uncovered.

3163 of 3416 relevant lines covered (92.59%)

32790.94 hits per line

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

74.73

/path_finding.cpp

#include <vector>
using std::vector;

#include <unordered_map>
using std::unordered_map;

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

#include "path_finding.hpp"

#include "geometry.hpp"
#include "bg_operators.hpp"
#include "bg_helpers.hpp"

namespace path_finding {

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

using std::shared_ptr;
using std::make_shared;


class PathFindingSurface {
 public:
  PathFindingSurface(const optional<multi_polygon_type_fp>& keep_in,
                     const multi_polygon_type_fp& keep_out,
                     const coordinate_type_fp tolerance)
    : tolerance(tolerance) {
    if (!keep_in && keep_out.size() == 0) {
      return;
    }
    if (keep_in) {
      multi_polygon_type_fp total_keep_in;
      total_keep_in = *keep_in;
      total_keep_in = total_keep_in - keep_out;

      for (const auto& poly : total_keep_in) {
        for (const auto& point : poly.outer()) {
          all_vertices.push_back(point);
        }
        for (const auto& inner : poly.inners()) {
          for (const auto& point : inner) {
            all_vertices.push_back(point);
          }
        }
      }
      total_keep_in_grown = {bg_helpers::buffer_miter(total_keep_in, tolerance)};
    } else {
      for (const auto& poly : keep_out) {
        for (const auto& point : poly.outer()) {
          all_vertices.push_back(point);
        }
        for (const auto& inner : poly.inners()) {
          for (const auto& point : inner) {
            all_vertices.push_back(point);
          }
        }
      }
      keep_out_shrunk = bg_helpers::buffer(keep_out, -tolerance);
    }

    sort(all_vertices.begin(),
         all_vertices.end(),
         [](const point_type_fp& a, const point_type_fp& b) {
           return std::tie(a.x(), a.y()) < std::tie(b.x(), b.y());
         });
    all_vertices.erase(
        std::unique(all_vertices.begin(),
                    all_vertices.end(),
                    [](const point_type_fp& a, const point_type_fp& b) {
                      return std::tie(a.x(), a.y()) == std::tie(b.x(), b.y());
                    }),
        all_vertices.end());
    for (const auto& v : all_vertices) {
      bg::expand(all_vertices_box, v);
    }
  }
  optional<multi_polygon_type_fp> total_keep_in_grown;
  multi_polygon_type_fp keep_out_shrunk;
  const coordinate_type_fp tolerance;
  vector<point_type_fp> all_vertices;
  box_type_fp all_vertices_box = {{INFINITY, INFINITY}, {-INFINITY, -INFINITY}};
};

inline bool is_intersecting(const point_type_fp& p0, const point_type_fp& p1,
                            const point_type_fp& p2, const point_type_fp& p3) {
  const coordinate_type_fp s10_x = p1.x() - p0.x();
  const coordinate_type_fp s10_y = p1.y() - p0.y();
  const coordinate_type_fp s32_x = p3.x() - p2.x();
  const coordinate_type_fp s32_y = p3.y() - p2.y();

  coordinate_type_fp denom = s10_x * s32_y - s32_x * s10_y;
  if (denom == 0) {
    return false; // Collinear
  }
  const bool denomPositive = denom > 0;

  const coordinate_type_fp s02_x = p0.x() - p2.x();
  const coordinate_type_fp s02_y = p0.y() - p2.y();
  const coordinate_type_fp s_numer = s10_x * s02_y - s10_y * s02_x;
  if ((s_numer < 0) == denomPositive) {
    return false; // No collision
  }

  const coordinate_type_fp t_numer = s32_x * s02_y - s32_y * s02_x;
  if ((t_numer < 0) == denomPositive) {
    return false; // No collision
  }

  if (((s_numer > denom) == denomPositive) || ((t_numer > denom) == denomPositive)) {
    return false; // No collision
  }
  return true;
}

class PathSurface {
 public:
  PathSurface(const std::shared_ptr<const PathFindingSurface>& base, const point_type_fp begin, const point_type_fp end,
              PathLimiter path_limiter)
      : path_limiter(path_limiter) {
    multi_polygon_type_fp new_total_keep_in_grown;
    const multi_polygon_type_fp* new_total_keep_in_grown_ptr;
    if (!base->total_keep_in_grown) {
      box_type_fp bounding_box = bg::return_envelope<box_type_fp>(begin);
      bg::expand(bounding_box, end);
      bg::expand(bounding_box, base->all_vertices_box);
      bounding_box = bg::return_buffer<box_type_fp>(bounding_box, base->tolerance);
      bg::convert(bounding_box, new_total_keep_in_grown);
      new_total_keep_in_grown = new_total_keep_in_grown - base->keep_out_shrunk;
      new_total_keep_in_grown_ptr = &new_total_keep_in_grown;
    } else {
      new_total_keep_in_grown_ptr = &*base->total_keep_in_grown;
    }
    total_keep_in_grown = {};
    for (const auto& poly : *new_total_keep_in_grown_ptr) {
      if (bg::covered_by(begin, poly) && bg::covered_by(end, poly)) {
        total_keep_in_grown.push_back(poly);
      }
    }
    all_vertices.clear();
    all_vertices.push_back(begin);
    all_vertices.push_back(end);
    for (const auto& point : base->all_vertices) {
      if (!bg::equals(begin, point) && !bg::equals(end, point)) {
        all_vertices.push_back(point);
      }
    }
    distance.resize(all_vertices.size());
  }

  const point_type_fp& get_point_by_index(const size_t index) const {
    return all_vertices[index];
  }

  size_t points_num() const {
    return all_vertices.size();
  }

  // Returns true if there is at least some path from begin to end.
  bool has_path() {
    return total_keep_in_grown.size() > 0;
  }

  bool in_surface(const size_t& a_index, const size_t& b_index) const {
    if (a_index > b_index) {
      return in_surface(b_index, a_index);
    }
    auto a_b_hash = a_index * points_num() + b_index;
    auto memoized_result = in_surface_memo.find(a_b_hash);
    if (memoized_result != in_surface_memo.cend()) {
      return memoized_result->second;
    }
    const auto& point_a = get_point_by_index(a_index);
    const auto& point_b = get_point_by_index(b_index);
    for (const auto& poly : total_keep_in_grown) {
      const auto& ring = poly.outer();
      for (auto current = ring.cbegin(); current + 1 != ring.cend(); current++) {
        if (is_intersecting(point_a, point_b, *current, *(current+1))) {
          in_surface_memo.emplace(a_b_hash, false);
          return false;
        }
      }
      for (const auto& ring : poly.inners()) {
        for (auto current = ring.cbegin(); current + 1 != ring.cend(); current++) {
          if (is_intersecting(point_a, point_b, *current, *(current+1))) {
            in_surface_memo.emplace(a_b_hash, false);
            return false;
          }
        }
      }
    }
    in_surface_memo.emplace(a_b_hash, true);
    return true;
  }
  // The distance map is a vertex-to-distance mapping.
  vector<coordinate_type_fp> distance;
  const PathLimiter path_limiter;
 private:
  mutable std::unordered_map<size_t, bool> in_surface_memo;
  multi_polygon_type_fp total_keep_in_grown;
  vector<point_type_fp> all_vertices;
};

struct path_surface_traversal_catetory:
    public boost::vertex_list_graph_tag,
    public boost::incidence_graph_tag {};

class OutEdgeIteratorImpl;
class VertexIteratorImpl;

} // namespace path_finding

namespace boost {

template <> struct graph_traits<path_finding::PathSurface> {
  typedef size_t vertex_descriptor;
  typedef std::pair<vertex_descriptor, vertex_descriptor> edge_descriptor;
  typedef boost::undirected_tag directed_category;
  typedef boost::disallow_parallel_edge_tag edge_parallel_category;
  typedef path_finding::path_surface_traversal_catetory traversal_category;

  typedef path_finding::VertexIteratorImpl vertex_iterator;
  typedef path_finding::OutEdgeIteratorImpl out_edge_iterator;
  typedef size_t degree_size_type;
  typedef size_t vertices_size_type;

  typedef void in_edge_iterator;
  typedef void edge_iterator;
  typedef void edges_size_type;
};
} // namespace boost

namespace path_finding {

typedef boost::graph_traits<PathSurface>::vertex_descriptor Vertex;
typedef boost::graph_traits<PathSurface>::edge_descriptor Edge;
typedef boost::graph_traits<PathSurface>::vertex_iterator VertexIterator;
typedef boost::graph_traits<PathSurface>::out_edge_iterator OutEdgeIterator;
typedef boost::graph_traits<PathSurface>::degree_size_type DegreeSizeType;
typedef boost::graph_traits<PathSurface>::vertices_size_type VerticesSizeType;

class OutEdgeIteratorImpl : public boost::forward_iterator_helper<OutEdgeIterator, Edge, std::ptrdiff_t, Edge*, Edge> {
 public:
  OutEdgeIteratorImpl()
      : source(0),
        target(0) {}
  OutEdgeIteratorImpl(const PathSurface& path_surface, const Vertex& source, const Vertex& target)
      : path_surface(&path_surface),
        source(source),
        target(target) {
    move_to_valid_target();
  }
  Edge operator*() const {
    return std::make_pair(source, target);
  }
  void operator++() {
      target++;
      move_to_valid_target();
  }
  bool operator==(const OutEdgeIterator& other) const {
    return path_surface == other.path_surface
        && source == other.source
        && target == other.target;
  }

 private:
  // Only advances if the current edge isn't in the surface or it's 0 length.
  void move_to_valid_target() {
    while (true) {
      if (target >= path_surface->points_num()) {
        break; // already fell off the end
      }
      if (target == source) {
        target++; // don't self-edge for sure
        continue;
      }
      if (path_surface->path_limiter &&
          path_surface->path_limiter(path_surface->get_point_by_index(target),
                                     path_surface->distance[source] +
                                     bg::distance(path_surface->get_point_by_index(source),
                                                  path_surface->get_point_by_index(target)))) {
        target++; // This path would be too long.
        continue;
      }
      if (!path_surface->in_surface(source, target)) {
        target++; // segment is not in the allowed surface
        continue;
      }
      break;
    }
  }
  const PathSurface* path_surface;
  Vertex source;
  Vertex target;
};

class VertexIteratorImpl : public boost::forward_iterator_helper<VertexIterator, Vertex, std::ptrdiff_t, Vertex*, Vertex> {
 public:
  VertexIteratorImpl()
      : current(0) {}
  VertexIteratorImpl(const Vertex current)
      : current(current) {}
  Vertex operator*() const {
    return current;
  }
  void operator++() {
      current++;
  }
  bool operator==(const VertexIterator& other) const {
    return current == other.current;
  }

 private:
  Vertex current;
};

std::pair<VertexIterator, VertexIterator> vertices(const PathSurface& path_surface) {
  return std::make_pair(0, path_surface.points_num());
}

std::pair<OutEdgeIterator, OutEdgeIterator> out_edges(const Vertex& vertex, const PathSurface& path_surface) {
  return std::make_pair(OutEdgeIteratorImpl(path_surface, vertex, 0), OutEdgeIteratorImpl(path_surface, vertex, path_surface.points_num()));
}

DegreeSizeType out_degree(Vertex v, const PathSurface& graph) {
  DegreeSizeType result = 0;

  std::pair<OutEdgeIterator, OutEdgeIterator> edges = out_edges(v, graph);
  for (OutEdgeIterator i = edges.first; i != edges.second; i++) {
    ++result;
  }

  return result;
}

Vertex source(Edge edge, const PathSurface&) {
  return edge.first;
}

Vertex target(Edge edge, const PathSurface&) {
  return edge.second;
}

VerticesSizeType num_vertices(const PathSurface& path_surface) {
  return path_surface.points_num();
}

} // namespace path_finding

namespace boost {

template <> struct vertex_property_type<path_finding::PathSurface> {
  typedef boost::graph_traits<path_finding::PathSurface>::vertex_descriptor type;
};

} // namespace boost

namespace path_finding {

class PathSurfaceHeuristic: public boost::astar_heuristic<PathSurface, coordinate_type_fp> {
 public:
  PathSurfaceHeuristic(shared_ptr<PathSurface> path_surface, const Vertex& goal)
      : path_surface(path_surface),
        goal(goal) {}

  int operator()(Vertex v) {
    return bg::distance(path_surface->get_point_by_index(v),
                        path_surface->get_point_by_index(goal));
  }

 private:
  const shared_ptr<PathSurface> path_surface;
  const Vertex goal;
};

struct FoundGoal {};

struct AstarGoalVisitor : public boost::default_astar_visitor {
  AstarGoalVisitor(Vertex goal)
      : goal(goal) {}

  void examine_vertex(Vertex u, const PathSurface&) {
    if (u == goal) {
      throw FoundGoal();
    }
  }

 private:
  Vertex goal;
};

const std::shared_ptr<const PathFindingSurface> create_path_finding_surface(
    const optional<multi_polygon_type_fp>& keep_in,
    const multi_polygon_type_fp& keep_out,
    const coordinate_type_fp tolerance) {
  boost::function_requires<boost::VertexListGraphConcept<PathSurface>>();
  boost::function_requires<boost::IncidenceGraphConcept<PathSurface>>();
  return make_shared<PathFindingSurface>(keep_in, keep_out, tolerance);
}

boost::optional<linestring_type_fp> find_path(
    const std::shared_ptr<const PathFindingSurface> path_finding_surface,
    const point_type_fp& start, const point_type_fp& end,
    PathLimiter path_limiter) {
  linestring_type_fp direct_ls;
  direct_ls.push_back(start);
  direct_ls.push_back(end);
  try {
    if (!path_finding_surface->total_keep_in_grown) {
      if (!bg::intersects(direct_ls, path_finding_surface->keep_out_shrunk)) {
        return direct_ls;
      }
    } else {
      if (bg::covered_by(direct_ls, *(path_finding_surface->total_keep_in_grown)) &&
          (path_limiter == nullptr || !path_limiter(end, bg::distance(start, end)))) {
        return direct_ls;
      }
    }
  } catch (GiveUp g) {
    return boost::none;
  }
  auto path_surface = make_shared<PathSurface>(path_finding_surface,
                                               start, end,
                                               path_limiter);
  if (!path_surface->has_path()) {
    return boost::none;
  }
  // The predecessor map is a vertex-to-vertex mapping.
  vector<Vertex> predecessor(path_surface->points_num());

  // The weight map is the length of each segment.
  auto weight_pmap = boost::make_function_property_map<Edge>(
      [&path_surface](const Edge& edge) {
        return bg::distance(path_surface->get_point_by_index(edge.first),
                            path_surface->get_point_by_index(edge.second));
      });

  // The weight map is the length of each segment.
  auto vertex_index_pmap = boost::typed_identity_property_map<Vertex>();
  try {
    astar_search(*path_surface, 0, PathSurfaceHeuristic(path_surface, 1), // The end point is always made spot number 1 in the PathSurface.
                 boost::weight_map(weight_pmap)
                 .predecessor_map(&predecessor[0])
                 .distance_map(&path_surface->distance[0])
                 .vertex_index_map(vertex_index_pmap)
                 .visitor(AstarGoalVisitor(1)));
  } catch (FoundGoal f) {
    linestring_type_fp result;
    for (int i = 1; i != 0; i = predecessor[i]) {
      bg::append(result, path_surface->get_point_by_index(i));
    }
    bg::append(result, path_surface->get_point_by_index(0));
    bg::reverse(result);
    return make_optional(result);
  } catch (GiveUp g) {
    return boost::none;
  }
  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 <boost/graph/adjacency_list.hpp>
8	#include <boost/graph/astar_search.hpp>
9	#include <boost/optional.hpp>
10
11	#include "path_finding.hpp"
12
13	#include "geometry.hpp"
14	#include "bg_operators.hpp"
15	#include "bg_helpers.hpp"
16
17	namespace path_finding {
18
19	using boost::optional;
20	using boost::make_optional;
21
22	using std::shared_ptr;
23	using std::make_shared;
24
25
26	class PathFindingSurface {
27	public:
28	PathFindingSurface(const optional<multi_polygon_type_fp>& keep_in,	1,042✔
29	const multi_polygon_type_fp& keep_out,
30	const coordinate_type_fp tolerance)
31	: tolerance(tolerance) {	2,084✔
32	if (!keep_in && keep_out.size() == 0) {	1,042!
33	return;
34	}
35	if (keep_in) {	1,042✔
36	multi_polygon_type_fp total_keep_in;
37	total_keep_in = *keep_in;
38	total_keep_in = total_keep_in - keep_out;	2,078!
39
40	for (const auto& poly : total_keep_in) {	2,555✔
41	for (const auto& point : poly.outer()) {	150,676✔
42	all_vertices.push_back(point);	149,160!
43	}
44	for (const auto& inner : poly.inners()) {	2,261✔
45	for (const auto& point : inner) {	59,954✔
46	all_vertices.push_back(point);	59,209!
47	}
48	}
49	}
50	total_keep_in_grown = {bg_helpers::buffer_miter(total_keep_in, tolerance)};	2,078!
51	} else {
52	for (const auto& poly : keep_out) {	6✔
53	for (const auto& point : poly.outer()) {	18✔
54	all_vertices.push_back(point);	15!
55	}
56	for (const auto& inner : poly.inners()) {	5✔
57	for (const auto& point : inner) {	12✔
58	all_vertices.push_back(point);	10!
59	}
60	}
61	}
62	keep_out_shrunk = bg_helpers::buffer(keep_out, -tolerance);	6!
63	}
64
65	sort(all_vertices.begin(),	1,042✔
66	all_vertices.end(),
67	[](const point_type_fp& a, const point_type_fp& b) {
68	return std::tie(a.x(), a.y()) < std::tie(b.x(), b.y());
69	});
70	all_vertices.erase(
71	std::unique(all_vertices.begin(),
72	all_vertices.end(),
73	[](const point_type_fp& a, const point_type_fp& b) {
74	return std::tie(a.x(), a.y()) == std::tie(b.x(), b.y());
75	}),	1,042✔
76	all_vertices.end());
77	for (const auto& v : all_vertices) {	207,170✔
78	bg::expand(all_vertices_box, v);
79	}
80	}
81	optional<multi_polygon_type_fp> total_keep_in_grown;
82	multi_polygon_type_fp keep_out_shrunk;
83	const coordinate_type_fp tolerance;
84	vector<point_type_fp> all_vertices;
85	box_type_fp all_vertices_box = {{INFINITY, INFINITY}, {-INFINITY, -INFINITY}};
86	};
87
88	inline bool is_intersecting(const point_type_fp& p0, const point_type_fp& p1,	219,883✔
89	const point_type_fp& p2, const point_type_fp& p3) {
90	const coordinate_type_fp s10_x = p1.x() - p0.x();	219,883✔
91	const coordinate_type_fp s10_y = p1.y() - p0.y();	219,883✔
92	const coordinate_type_fp s32_x = p3.x() - p2.x();	219,883✔
93	const coordinate_type_fp s32_y = p3.y() - p2.y();	219,883✔
94
95	coordinate_type_fp denom = s10_x * s32_y - s32_x * s10_y;	219,883✔
96	if (denom == 0) {	219,883✔
97	return false; // Collinear
98	}
99	const bool denomPositive = denom > 0;	219,715✔
100
101	const coordinate_type_fp s02_x = p0.x() - p2.x();	219,715✔
102	const coordinate_type_fp s02_y = p0.y() - p2.y();	219,715✔
103	const coordinate_type_fp s_numer = s10_x * s02_y - s10_y * s02_x;	219,715✔
104	if ((s_numer < 0) == denomPositive) {	219,715✔
105	return false; // No collision
106	}
107
108	const coordinate_type_fp t_numer = s32_x * s02_y - s32_y * s02_x;	120,610✔
109	if ((t_numer < 0) == denomPositive) {	120,610✔
110	return false; // No collision
111	}
112
113	if (((s_numer > denom) == denomPositive) \|\| ((t_numer > denom) == denomPositive)) {	67,861✔
114	return false; // No collision	67,184✔
115	}
116	return true;
117	}
118
119	class PathSurface {
120	public:
121	PathSurface(const std::shared_ptr<const PathFindingSurface>& base, const point_type_fp begin, const point_type_fp end,	6,217✔
122	PathLimiter path_limiter)
123	: path_limiter(path_limiter) {	12,434!
124	multi_polygon_type_fp new_total_keep_in_grown;
125	const multi_polygon_type_fp* new_total_keep_in_grown_ptr;
126	if (!base->total_keep_in_grown) {	6,217✔
127	box_type_fp bounding_box = bg::return_envelope<box_type_fp>(begin);
128	bg::expand(bounding_box, end);
129	bg::expand(bounding_box, base->all_vertices_box);	2✔
130	bounding_box = bg::return_buffer<box_type_fp>(bounding_box, base->tolerance);	4!
131	bg::convert(bounding_box, new_total_keep_in_grown);
132	new_total_keep_in_grown = new_total_keep_in_grown - base->keep_out_shrunk;	4!
133	new_total_keep_in_grown_ptr = &new_total_keep_in_grown;
134	} else {
135	new_total_keep_in_grown_ptr = &*base->total_keep_in_grown;
136	}
137	total_keep_in_grown = {};	6,217✔
138	for (const auto& poly : *new_total_keep_in_grown_ptr) {	171,384✔
139	if (bg::covered_by(begin, poly) && bg::covered_by(end, poly)) {	170,769✔
140	total_keep_in_grown.push_back(poly);	907!
141	}
142	}
143	all_vertices.clear();
144	all_vertices.push_back(begin);	6,217!
145	all_vertices.push_back(end);	6,217!
146	for (const auto& point : base->all_vertices) {	9,464,938✔
147	if (!bg::equals(begin, point) && !bg::equals(end, point)) {	9,458,721✔
148	all_vertices.push_back(point);	9,458,204!
149	}
150	}
151	distance.resize(all_vertices.size());	12,434!
152	}	6,217✔
153
154	const point_type_fp& get_point_by_index(const size_t index) const {
155	return all_vertices[index];	4!
156	}
157
158	size_t points_num() const {
159	return all_vertices.size();	2,566!
160	}
161
162	// Returns true if there is at least some path from begin to end.
163	bool has_path() {
164	return total_keep_in_grown.size() > 0;	6,217✔
165	}
166
167	bool in_surface(const size_t& a_index, const size_t& b_index) const {	793✔
168	if (a_index > b_index) {	793✔
169	return in_surface(b_index, a_index);	41✔
170	}
171	auto a_b_hash = a_index * points_num() + b_index;	752✔
172	auto memoized_result = in_surface_memo.find(a_b_hash);
173	if (memoized_result != in_surface_memo.cend()) {	752✔
174	return memoized_result->second;	19✔
175	}
176	const auto& point_a = get_point_by_index(a_index);
177	const auto& point_b = get_point_by_index(b_index);
178	for (const auto& poly : total_keep_in_grown) {	789✔
179	const auto& ring = poly.outer();
180	for (auto current = ring.cbegin(); current + 1 != ring.cend(); current++) {	404,017✔
181	if (is_intersecting(point_a, point_b, current, (current+1))) {	202,109✔
182	in_surface_memo.emplace(a_b_hash, false);	467✔
183	return false;
184	}
185	}
186	for (const auto& ring : poly.inners()) {	305✔
187	for (auto current = ring.cbegin(); current + 1 != ring.cend(); current++) {	35,377✔
188	if (is_intersecting(point_a, point_b, current, (current+1))) {	17,774✔
189	in_surface_memo.emplace(a_b_hash, false);	210✔
190	return false;
191	}
192	}
193	}
194	}
195	in_surface_memo.emplace(a_b_hash, true);	56✔
196	return true;	56✔
197	}
198	// The distance map is a vertex-to-distance mapping.
199	vector<coordinate_type_fp> distance;
200	const PathLimiter path_limiter;
201	private:
202	mutable std::unordered_map<size_t, bool> in_surface_memo;
203	multi_polygon_type_fp total_keep_in_grown;
204	vector<point_type_fp> all_vertices;
205	};
206
207	struct path_surface_traversal_catetory:
208	public boost::vertex_list_graph_tag,
209	public boost::incidence_graph_tag {};
210
211	class OutEdgeIteratorImpl;
212	class VertexIteratorImpl;
213
214	} // namespace path_finding
215
216	namespace boost {
217
218	template <> struct graph_traits<path_finding::PathSurface> {
219	typedef size_t vertex_descriptor;
220	typedef std::pair<vertex_descriptor, vertex_descriptor> edge_descriptor;
221	typedef boost::undirected_tag directed_category;
222	typedef boost::disallow_parallel_edge_tag edge_parallel_category;
223	typedef path_finding::path_surface_traversal_catetory traversal_category;
224
225	typedef path_finding::VertexIteratorImpl vertex_iterator;
226	typedef path_finding::OutEdgeIteratorImpl out_edge_iterator;
227	typedef size_t degree_size_type;
228	typedef size_t vertices_size_type;
229
230	typedef void in_edge_iterator;
231	typedef void edge_iterator;
232	typedef void edges_size_type;
233	};
234	} // namespace boost
235
236	namespace path_finding {
237
238	typedef boost::graph_traits<PathSurface>::vertex_descriptor Vertex;
239	typedef boost::graph_traits<PathSurface>::edge_descriptor Edge;
240	typedef boost::graph_traits<PathSurface>::vertex_iterator VertexIterator;
241	typedef boost::graph_traits<PathSurface>::out_edge_iterator OutEdgeIterator;
242	typedef boost::graph_traits<PathSurface>::degree_size_type DegreeSizeType;
243	typedef boost::graph_traits<PathSurface>::vertices_size_type VerticesSizeType;
244
245	class OutEdgeIteratorImpl : public boost::forward_iterator_helper<OutEdgeIterator, Edge, std::ptrdiff_t, Edge*, Edge> {
246	public:
247	OutEdgeIteratorImpl()
248	: source(0),	1,814✔
249	target(0) {}	907✔
250	OutEdgeIteratorImpl(const PathSurface& path_surface, const Vertex& source, const Vertex& target)
251	: path_surface(&path_surface),	1,834✔
252	source(source),	1,834✔
253	target(target) {	3,668✔
254	move_to_valid_target();	917✔
255	}
256	Edge operator*() const {
257	return std::make_pair(source, target);	144✔
258	}
259	void operator++() {
260	target++;	72✔
261	move_to_valid_target();	72✔
262	}
263	bool operator==(const OutEdgeIterator& other) const {
264	return path_surface == other.path_surface	86✔
265	&& source == other.source	86!
266	&& target == other.target;	172!
267	}
268
269	private:
270	// Only advances if the current edge isn't in the surface or it's 0 length.
271	void move_to_valid_target() {	1,906✔
272	while (true) {
273	if (target >= path_surface->points_num()) {	7,496✔
274	break; // already fell off the end
275	}
276	if (target == source) {	2,817✔
277	target++; // don't self-edge for sure	917✔
278	continue;	917✔
279	}
280	if (path_surface->path_limiter &&	2,783✔
281	path_surface->path_limiter(path_surface->get_point_by_index(target),	1,786✔
282	path_surface->distance[source] +	5,358!
283	bg::distance(path_surface->get_point_by_index(source),
284	path_surface->get_point_by_index(target)))) {
285	target++; // This path would be too long.	245✔
286	continue;	245✔
287	}
288	if (!path_surface->in_surface(source, target)) {	752✔
289	target++; // segment is not in the allowed surface	680✔
290	continue;	680✔
291	}
292	break;
293	}
294	}	1,003✔
295	const PathSurface* path_surface;
296	Vertex source;
297	Vertex target;
298	};
299
300	class VertexIteratorImpl : public boost::forward_iterator_helper<VertexIterator, Vertex, std::ptrdiff_t, Vertex*, Vertex> {
301	public:
302	VertexIteratorImpl()
UNCOV 303	: current(0) {}	×
304	VertexIteratorImpl(const Vertex current)
305	: current(current) {}
UNCOV 306	Vertex operator*() const {	×
UNCOV 307	return current;	×
308	}
UNCOV 309	void operator++() {	×
310	current++;	×
UNCOV 311	}	×
UNCOV 312	bool operator==(const VertexIterator& other) const {	×
313	return current == other.current;	×
314	}
315
316	private:
317	Vertex current;
318	};
319
320	std::pair<VertexIterator, VertexIterator> vertices(const PathSurface& path_surface) {	×
UNCOV 321	return std::make_pair(0, path_surface.points_num());	×
322	}
323
324	std::pair<OutEdgeIterator, OutEdgeIterator> out_edges(const Vertex& vertex, const PathSurface& path_surface) {	917✔
325	return std::make_pair(OutEdgeIteratorImpl(path_surface, vertex, 0), OutEdgeIteratorImpl(path_surface, vertex, path_surface.points_num()));	14✔
326	}
327
328	DegreeSizeType out_degree(Vertex v, const PathSurface& graph) {	×
329	DegreeSizeType result = 0;
330
UNCOV 331	std::pair<OutEdgeIterator, OutEdgeIterator> edges = out_edges(v, graph);	×
UNCOV 332	for (OutEdgeIterator i = edges.first; i != edges.second; i++) {	×
UNCOV 333	++result;	×
334	}
335
UNCOV 336	return result;	×
337	}
338
339	Vertex source(Edge edge, const PathSurface&) {	×
340	return edge.first;	×
341	}
342
343	Vertex target(Edge edge, const PathSurface&) {	×
UNCOV 344	return edge.second;	×
345	}
346
347	VerticesSizeType num_vertices(const PathSurface& path_surface) {	×
UNCOV 348	return path_surface.points_num();	×
349	}
350
351	} // namespace path_finding
352
353	namespace boost {
354
355	template <> struct vertex_property_type<path_finding::PathSurface> {
356	typedef boost::graph_traits<path_finding::PathSurface>::vertex_descriptor type;
357	};
358
359	} // namespace boost
360
361	namespace path_finding {
362
363	class PathSurfaceHeuristic: public boost::astar_heuristic<PathSurface, coordinate_type_fp> {	907!
364	public:
365	PathSurfaceHeuristic(shared_ptr<PathSurface> path_surface, const Vertex& goal)
366	: path_surface(path_surface),	907✔
367	goal(goal) {}	907!
368
369	int operator()(Vertex v) {	939✔
370	return bg::distance(path_surface->get_point_by_index(v),
371	path_surface->get_point_by_index(goal));	1,878✔
372	}
373
374	private:
375	const shared_ptr<PathSurface> path_surface;
376	const Vertex goal;
377	};
378
379	struct FoundGoal {};
380
381	struct AstarGoalVisitor : public boost::default_astar_visitor {
382	AstarGoalVisitor(Vertex goal)
383	: goal(goal) {}
384
UNCOV 385	void examine_vertex(Vertex u, const PathSurface&) {	×
UNCOV 386	if (u == goal) {	×
UNCOV 387	throw FoundGoal();	×
388	}
UNCOV 389	}	×
390
391	private:
392	Vertex goal;
393	};
394
395	const std::shared_ptr<const PathFindingSurface> create_path_finding_surface(	1,042✔
396	const optional<multi_polygon_type_fp>& keep_in,
397	const multi_polygon_type_fp& keep_out,
398	const coordinate_type_fp tolerance) {
399	boost::function_requires<boost::VertexListGraphConcept<PathSurface>>();
400	boost::function_requires<boost::IncidenceGraphConcept<PathSurface>>();
401	return make_shared<PathFindingSurface>(keep_in, keep_out, tolerance);	1,042✔
402	}
403
404	boost::optional<linestring_type_fp> find_path(	6,914!
405	const std::shared_ptr<const PathFindingSurface> path_finding_surface,
406	const point_type_fp& start, const point_type_fp& end,
407	PathLimiter path_limiter) {
408	linestring_type_fp direct_ls;
409	direct_ls.push_back(start);	6,914!
410	direct_ls.push_back(end);	6,914!
411	try {
412	if (!path_finding_surface->total_keep_in_grown) {	6,914✔
413	if (!bg::intersects(direct_ls, path_finding_surface->keep_out_shrunk)) {	3!
414	return direct_ls;
415	}
416	} else {
417	if (bg::covered_by(direct_ls, *(path_finding_surface->total_keep_in_grown)) &&	6,911✔
418	(path_limiter == nullptr \|\| !path_limiter(end, bg::distance(start, end)))) {	7,626!
419	return direct_ls;
420	}
421	}
UNCOV 422	} catch (GiveUp g) {	×
423	return boost::none;
424	}
425	auto path_surface = make_shared<PathSurface>(path_finding_surface,
426	start, end,
427	path_limiter);
428	if (!path_surface->has_path()) {	6,217✔
429	return boost::none;	5,310✔
430	}
431	// The predecessor map is a vertex-to-vertex mapping.
432	vector<Vertex> predecessor(path_surface->points_num());	907!
433
434	// The weight map is the length of each segment.
435	auto weight_pmap = boost::make_function_property_map<Edge>(
436	[&path_surface](const Edge& edge) {	144✔
437	return bg::distance(path_surface->get_point_by_index(edge.first),	144✔
438	path_surface->get_point_by_index(edge.second));	288✔
439	});
440
441	// The weight map is the length of each segment.
442	auto vertex_index_pmap = boost::typed_identity_property_map<Vertex>();
443	try {
444	astar_search(*path_surface, 0, PathSurfaceHeuristic(path_surface, 1), // The end point is always made spot number 1 in the PathSurface.	1,814!
445	boost::weight_map(weight_pmap)
446	.predecessor_map(&predecessor[0])	907✔
447	.distance_map(&path_surface->distance[0])	907✔
448	.vertex_index_map(vertex_index_pmap)
449	.visitor(AstarGoalVisitor(1)));	907!
450	} catch (FoundGoal f) {	8✔
451	linestring_type_fp result;
452	for (int i = 1; i != 0; i = predecessor[i]) {	22✔
453	bg::append(result, path_surface->get_point_by_index(i));	9!
454	}
455	bg::append(result, path_surface->get_point_by_index(0));
456	bg::reverse(result);
457	return make_optional(result);
458	} catch (GiveUp g) {	903✔
459	return boost::none;
460	}
UNCOV 461	return boost::none;	×
462	}
463
464	} //namespace path_finding

pcb2gcode / pcb2gcode / 508492256

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