505207050

Build # 505207050

Build Type

github

Committed by GitHub

Commit Message

Merge 0fa0d5221 into 83f02e360

Pull Request Pull Request #519: Refactor PathSurface to compute fewer vertices

Run Details

4612 of 7939 branches covered (58.09%)

Branch coverage included in aggregate %.

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

1 existing line 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);
      }
    }
    // Collect just the relevant vertices into an array.
    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	// Collect just the relevant vertices into an array.
144	all_vertices.clear();
145	all_vertices.push_back(begin);	6,217!
146	all_vertices.push_back(end);	6,217!
147	for (const auto& point : base->all_vertices) {	9,464,938✔
148	if (!bg::equals(begin, point) && !bg::equals(end, point)) {	9,458,721✔
149	all_vertices.push_back(point);	9,458,204!
150	}
151	}
152	distance.resize(all_vertices.size());	12,434!
153	}	6,217✔
154
155	const point_type_fp& get_point_by_index(const size_t index) const {
156	return all_vertices[index];	4!
157	}
158
159	size_t points_num() const {
160	return all_vertices.size();	2,566!
161	}
162
163	// Returns true if there is at least some path from begin to end.
164	bool has_path() {
165	return total_keep_in_grown.size() > 0;	6,217✔
166	}
167
168	bool in_surface(const size_t& a_index, const size_t& b_index) const {	793✔
169	if (a_index > b_index) {	793✔
170	return in_surface(b_index, a_index);	41✔
171	}
172	auto a_b_hash = a_index * points_num() + b_index;	752✔
173	auto memoized_result = in_surface_memo.find(a_b_hash);
174	if (memoized_result != in_surface_memo.cend()) {	752✔
175	return memoized_result->second;	19✔
176	}
177	const auto& point_a = get_point_by_index(a_index);
178	const auto& point_b = get_point_by_index(b_index);
179	for (const auto& poly : total_keep_in_grown) {	789✔
180	const auto& ring = poly.outer();
181	for (auto current = ring.cbegin(); current + 1 != ring.cend(); current++) {	404,017✔
182	if (is_intersecting(point_a, point_b, current, (current+1))) {	202,109✔
183	in_surface_memo.emplace(a_b_hash, false);	467✔
184	return false;
185	}
186	}
187	for (const auto& ring : poly.inners()) {	305✔
188	for (auto current = ring.cbegin(); current + 1 != ring.cend(); current++) {	35,377✔
189	if (is_intersecting(point_a, point_b, current, (current+1))) {	17,774✔
190	in_surface_memo.emplace(a_b_hash, false);	210✔
191	return false;
192	}
193	}
194	}
195	}
196	in_surface_memo.emplace(a_b_hash, true);	56✔
197	return true;	56✔
198	}
199	// The distance map is a vertex-to-distance mapping.
200	vector<coordinate_type_fp> distance;
201	const PathLimiter path_limiter;
202	private:
203	mutable std::unordered_map<size_t, bool> in_surface_memo;
204	multi_polygon_type_fp total_keep_in_grown;
205	vector<point_type_fp> all_vertices;
206	};
207
208	struct path_surface_traversal_catetory:
209	public boost::vertex_list_graph_tag,
210	public boost::incidence_graph_tag {};
211
212	class OutEdgeIteratorImpl;
213	class VertexIteratorImpl;
214
215	} // namespace path_finding
216
217	namespace boost {
218
219	template <> struct graph_traits<path_finding::PathSurface> {
220	typedef size_t vertex_descriptor;
221	typedef std::pair<vertex_descriptor, vertex_descriptor> edge_descriptor;
222	typedef boost::undirected_tag directed_category;
223	typedef boost::disallow_parallel_edge_tag edge_parallel_category;
224	typedef path_finding::path_surface_traversal_catetory traversal_category;
225
226	typedef path_finding::VertexIteratorImpl vertex_iterator;
227	typedef path_finding::OutEdgeIteratorImpl out_edge_iterator;
228	typedef size_t degree_size_type;
229	typedef size_t vertices_size_type;
230
231	typedef void in_edge_iterator;
232	typedef void edge_iterator;
233	typedef void edges_size_type;
234	};
235	} // namespace boost
236
237	namespace path_finding {
238
239	typedef boost::graph_traits<PathSurface>::vertex_descriptor Vertex;
240	typedef boost::graph_traits<PathSurface>::edge_descriptor Edge;
241	typedef boost::graph_traits<PathSurface>::vertex_iterator VertexIterator;
242	typedef boost::graph_traits<PathSurface>::out_edge_iterator OutEdgeIterator;
243	typedef boost::graph_traits<PathSurface>::degree_size_type DegreeSizeType;
244	typedef boost::graph_traits<PathSurface>::vertices_size_type VerticesSizeType;
245
246	class OutEdgeIteratorImpl : public boost::forward_iterator_helper<OutEdgeIterator, Edge, std::ptrdiff_t, Edge*, Edge> {
247	public:
248	OutEdgeIteratorImpl()
249	: source(0),	1,814✔
250	target(0) {}	907✔
251	OutEdgeIteratorImpl(const PathSurface& path_surface, const Vertex& source, const Vertex& target)
252	: path_surface(&path_surface),	1,834✔
253	source(source),	1,834✔
254	target(target) {	3,668✔
255	move_to_valid_target();	917✔
256	}
257	Edge operator*() const {
258	return std::make_pair(source, target);	144✔
259	}
260	void operator++() {
261	target++;	72✔
262	move_to_valid_target();	72✔
263	}
264	bool operator==(const OutEdgeIterator& other) const {
265	return path_surface == other.path_surface	86✔
266	&& source == other.source	86!
267	&& target == other.target;	172!
268	}
269
270	private:
271	// Only advances if the current edge isn't in the surface or it's 0 length.
272	void move_to_valid_target() {	1,906✔
273	while (true) {
274	if (target >= path_surface->points_num()) {	7,496✔
275	break; // already fell off the end
276	}
277	if (target == source) {	2,817✔
278	target++; // don't self-edge for sure	917✔
279	continue;	917✔
280	}
281	if (path_surface->path_limiter &&	2,783✔
282	path_surface->path_limiter(path_surface->get_point_by_index(target),	1,786✔
283	path_surface->distance[source] +	5,358!
284	bg::distance(path_surface->get_point_by_index(source),
285	path_surface->get_point_by_index(target)))) {
286	target++; // This path would be too long.	245✔
287	continue;	245✔
288	}
289	if (!path_surface->in_surface(source, target)) {	752✔
290	target++; // segment is not in the allowed surface	680✔
291	continue;	680✔
292	}
293	break;
294	}
295	}	1,003✔
296	const PathSurface* path_surface;
297	Vertex source;
298	Vertex target;
299	};
300
301	class VertexIteratorImpl : public boost::forward_iterator_helper<VertexIterator, Vertex, std::ptrdiff_t, Vertex*, Vertex> {
302	public:
303	VertexIteratorImpl()
304	: current(0) {}	×
305	VertexIteratorImpl(const Vertex current)
306	: current(current) {}
307	Vertex operator*() const {	×
308	return current;	×
309	}
310	void operator++() {	×
311	current++;	×
312	}	×
313	bool operator==(const VertexIterator& other) const {	×
314	return current == other.current;	×
315	}
316
317	private:
318	Vertex current;
319	};
320
321	std::pair<VertexIterator, VertexIterator> vertices(const PathSurface& path_surface) {	×
322	return std::make_pair(0, path_surface.points_num());	×
323	}
324
325	std::pair<OutEdgeIterator, OutEdgeIterator> out_edges(const Vertex& vertex, const PathSurface& path_surface) {	917✔
326	return std::make_pair(OutEdgeIteratorImpl(path_surface, vertex, 0), OutEdgeIteratorImpl(path_surface, vertex, path_surface.points_num()));	14✔
327	}
328
329	DegreeSizeType out_degree(Vertex v, const PathSurface& graph) {	×
330	DegreeSizeType result = 0;
331
332	std::pair<OutEdgeIterator, OutEdgeIterator> edges = out_edges(v, graph);	×
333	for (OutEdgeIterator i = edges.first; i != edges.second; i++) {	×
334	++result;	×
335	}
336
337	return result;	×
338	}
339
340	Vertex source(Edge edge, const PathSurface&) {	×
341	return edge.first;	×
342	}
343
344	Vertex target(Edge edge, const PathSurface&) {	×
345	return edge.second;	×
346	}
347
348	VerticesSizeType num_vertices(const PathSurface& path_surface) {	×
349	return path_surface.points_num();	×
350	}
351
352	} // namespace path_finding
353
354	namespace boost {
355
356	template <> struct vertex_property_type<path_finding::PathSurface> {
357	typedef boost::graph_traits<path_finding::PathSurface>::vertex_descriptor type;
358	};
359
360	} // namespace boost
361
362	namespace path_finding {
363
364	class PathSurfaceHeuristic: public boost::astar_heuristic<PathSurface, coordinate_type_fp> {	907!
365	public:
366	PathSurfaceHeuristic(shared_ptr<PathSurface> path_surface, const Vertex& goal)
367	: path_surface(path_surface),	907✔
368	goal(goal) {}	907!
369
370	int operator()(Vertex v) {	939✔
371	return bg::distance(path_surface->get_point_by_index(v),
372	path_surface->get_point_by_index(goal));	1,878✔
373	}
374
375	private:
376	const shared_ptr<PathSurface> path_surface;
377	const Vertex goal;
378	};
379
380	struct FoundGoal {};
381
382	struct AstarGoalVisitor : public boost::default_astar_visitor {
383	AstarGoalVisitor(Vertex goal)
384	: goal(goal) {}
385
386	void examine_vertex(Vertex u, const PathSurface&) {	×
387	if (u == goal) {	×
388	throw FoundGoal();	×
389	}
390	}	×
391
392	private:
393	Vertex goal;
394	};
395
396	const std::shared_ptr<const PathFindingSurface> create_path_finding_surface(	1,042✔
397	const optional<multi_polygon_type_fp>& keep_in,
398	const multi_polygon_type_fp& keep_out,
399	const coordinate_type_fp tolerance) {
400	boost::function_requires<boost::VertexListGraphConcept<PathSurface>>();
401	boost::function_requires<boost::IncidenceGraphConcept<PathSurface>>();
402	return make_shared<PathFindingSurface>(keep_in, keep_out, tolerance);	1,042✔
403	}
404
405	boost::optional<linestring_type_fp> find_path(	6,914!
406	const std::shared_ptr<const PathFindingSurface> path_finding_surface,
407	const point_type_fp& start, const point_type_fp& end,
408	PathLimiter path_limiter) {
409	linestring_type_fp direct_ls;
410	direct_ls.push_back(start);	6,914!
411	direct_ls.push_back(end);	6,914!
412	try {
413	if (!path_finding_surface->total_keep_in_grown) {	6,914✔
414	if (!bg::intersects(direct_ls, path_finding_surface->keep_out_shrunk)) {	3!
415	return direct_ls;
416	}
417	} else {
418	if (bg::covered_by(direct_ls, *(path_finding_surface->total_keep_in_grown)) &&	6,911✔
419	(path_limiter == nullptr \|\| !path_limiter(end, bg::distance(start, end)))) {	7,626!
420	return direct_ls;
421	}
422	}
423	} catch (GiveUp g) {	×
424	return boost::none;
425	}
426	auto path_surface = make_shared<PathSurface>(path_finding_surface,
427	start, end,
428	path_limiter);
429	if (!path_surface->has_path()) {	6,217✔
430	return boost::none;	5,310✔
431	}
432	// The predecessor map is a vertex-to-vertex mapping.
433	vector<Vertex> predecessor(path_surface->points_num());	907!
434
435	// The weight map is the length of each segment.
436	auto weight_pmap = boost::make_function_property_map<Edge>(
437	[&path_surface](const Edge& edge) {	144✔
438	return bg::distance(path_surface->get_point_by_index(edge.first),	144✔
439	path_surface->get_point_by_index(edge.second));	288✔
440	});
441
442	// The weight map is the length of each segment.
443	auto vertex_index_pmap = boost::typed_identity_property_map<Vertex>();
444	try {
445	astar_search(*path_surface, 0, PathSurfaceHeuristic(path_surface, 1), // The end point is always made spot number 1 in the PathSurface.	1,814!
446	boost::weight_map(weight_pmap)
447	.predecessor_map(&predecessor[0])	907✔
448	.distance_map(&path_surface->distance[0])	907✔
449	.vertex_index_map(vertex_index_pmap)
450	.visitor(AstarGoalVisitor(1)));	907!
451	} catch (FoundGoal f) {	8✔
452	linestring_type_fp result;
453	for (int i = 1; i != 0; i = predecessor[i]) {	22✔
454	bg::append(result, path_surface->get_point_by_index(i));	9!
455	}
456	bg::append(result, path_surface->get_point_by_index(0));
457	bg::reverse(result);
458	return make_optional(result);
459	} catch (GiveUp g) {	903✔
460	return boost::none;
461	}
UNCOV 462	return boost::none;	×
463	}
464
465	} //namespace path_finding

pcb2gcode / pcb2gcode / 505207050

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