PGROUTING  3.1
pgr_withPoints.cpp
Go to the documentation of this file.
1 /*PGR-GNU*****************************************************************
2 File: pgr_withPoints.cpp
3 
4 Generated with Template by:
5 Copyright (c) 2015 pgRouting developers
6 Mail: project@pgrouting.org
7 
8 Function's developer:
9 Copyright (c) 2015 Celia Virginia Vergara Castillo
10 Mail:
11 
12 ------
13 
14 This program is free software; you can redistribute it and/or modify
15 it under the terms of the GNU General Public License as published by
16 the Free Software Foundation; either version 2 of the License, or
17 (at your option) any later version.
18 
19 This program is distributed in the hope that it will be useful,
20 but WITHOUT ANY WARRANTY; without even the implied warranty of
21 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
22 GNU General Public License for more details.
23 
24 You should have received a copy of the GNU General Public License
25 along with this program; if not, write to the Free Software
26 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
27 
28  ********************************************************************PGR-GNU*/
29 
30 
32 
33 #include <sstream>
34 #include <deque>
35 #include <set>
36 #include <vector>
37 #include <string>
38 #include <algorithm>
39 #include <cassert>
40 
41 #include "cpp_common/pgr_assert.h"
42 
43 namespace pgrouting {
44 
45 
46 std::ostream& operator<<(
47  std::ostream &os, const Pg_points_graph &g) {
48  for (const auto p : g.m_points) {
49  os << p.pid << "\t"
50  << p.edge_id << "\t"
51  << p.fraction << "\t"
52  << p.side << "\n";
53  }
54  return os;
55 }
56 
57 
58 
59 std::vector<Point_on_edge_t>
61  return m_points;
62 }
63 
64 std::vector<pgr_edge_t>
66  return m_edges_of_points;
67 }
68 
69 
70 
72  std::vector<Point_on_edge_t> p_points,
73  std::vector<pgr_edge_t> p_edges_of_points,
74  bool p_normal,
75  char p_driving_side,
76  bool p_directed) :
77  m_points(p_points),
78  m_o_points(p_points),
79  m_edges_of_points(p_edges_of_points),
80  m_driving_side(p_driving_side),
81  m_directed(p_directed) {
82  if (!p_normal) {
83  reverse_sides();
84  }
85  if (!m_directed) {
86  m_driving_side = 'b';
87  }
88  check_points();
90  log << "constructor";
91 }
92 
93 void
95  for (auto &point : m_points) {
96  if (point.side == 'r') {
97  point.side = 'l';
98  } else if (point.side == 'l') {
99  point.side = 'r';
100  }
101  point.fraction = 1 - point.fraction;
102  }
103  if (m_driving_side == 'r') {
104  m_driving_side = 'l';
105  } else if (m_driving_side == 'l') {
106  m_driving_side = 'r';
107  }
108 }
109 
110 
111 void
113  log << "original points" << *this;
114  /*
115  * deleting duplicate points
116  */
117  std::sort(m_points.begin(), m_points.end(),
118  [](const Point_on_edge_t &a, const Point_on_edge_t &b)
119  -> bool {
120  if (a.pid != b.pid) return a.pid < b.pid;
121  if (a.edge_id != b.edge_id) return a.edge_id < b.edge_id;
122  if (a.fraction != b.fraction) return a.fraction < b.fraction;
123  return a.side < b.side;
124  });
125  log << "after sorting" << *this;
126  auto last = std::unique(m_points.begin(), m_points.end(),
127  [](const Point_on_edge_t &a, const Point_on_edge_t &b) {
128  return a.pid == b.pid &&
129  a.edge_id == b.edge_id &&
130  a.fraction == b.fraction &&
131  a.side == b.side;
132  });
133  m_points.erase(last, m_points.end());
134  size_t total_points = m_points.size();
135 
136  log << "after deleting repetitions" << *this;
137  log << "We have " << total_points << " different points";
138 
139  last = std::unique(m_points.begin(), m_points.end(),
140  [](const Point_on_edge_t &a, const Point_on_edge_t &b) {
141  return a.pid == b.pid;
142  });
143  m_points.erase(last, m_points.end());
144  log << "after deleting points with same id" << *this;
145 
146  if (m_points.size() != total_points) {
147  error << "Unexpected point(s) with same pid"
148  << " but different edge/fraction/side combination found.";
149  }
150 }
151 
152 
153 int64_t
154 Pg_points_graph::get_edge_id(int64_t pid) const {
155  auto point_ptr = std::find_if(
156  m_points.begin(), m_points.end(),
157  [&pid](const Point_on_edge_t &point)
158  {return pid == -point.pid;});
159  return point_ptr != m_points.end() ?
160  point_ptr->edge_id :
161  -1;
162 }
163 
164 const pgr_edge_t*
165 Pg_points_graph::get_edge_data(int64_t eid) const {
166  auto e_itr = std::find_if(
167  m_edges_of_points.begin(), m_edges_of_points.end(),
168  [&eid](const pgr_edge_t &edge)
169  {return eid == edge.id;});
170  return e_itr == m_edges_of_points.end()?
171  nullptr : &(*e_itr);
172 }
173 
174 
175 void
177  Path &path) const {
178  /*
179  * There is no path nothing to do
180  */
181  if (path.empty()) return;
182 
183  Path newPath(path.start_id(), path.end_id());
184  auto edge_id = path.start_id() < 0?
185  get_edge_id(path.start_id()) :
186  -1;
187 
188  for (auto pathstop : path) {
189  /*
190  * skip points (no details)
191  * except if ithe point its the starting point
192  */
193  if (!((pathstop.node == path.start_id())
194  || (pathstop.node > 0))) {
195  continue;
196  }
197 
198  /*
199  * Change costs only when the node is not:
200  * - start_id
201  * - directly connected to start_id
202  */
203  if (pathstop.node != path.start_id()) {
204  auto edge_ptr = get_edge_data(pathstop.edge);
205  /*
206  * edge found
207  * and its not the edge directly connected to start_id()
208  */
209  if (edge_ptr
210  && edge_ptr->id != edge_id) {
211  pathstop.cost = pathstop.node == edge_ptr->source?
212  edge_ptr->cost :
213  edge_ptr->reverse_cost;
214  }
215  }
216 
217  /*
218  * add to the new path
219  */
220  newPath.push_back(pathstop);
221  }
222 
223  path = newPath;
224 }
225 
226 
227 
228 Path
230  Path path) const {
231  /*
232  * There is no path nothing to do
233  */
234  if (path.empty()) return path;
235 
236  Path newPath(path.start_id(), path.end_id());
237  double cost = 0.0;
238  for (const auto &pathstop : path) {
239  if ((pathstop.node == path.start_id())
240  || (pathstop.node == path.end_id())
241  || (pathstop.node > 0)) {
242  newPath.push_back(pathstop);
243  if (pathstop.node != path.end_id()) cost = 0.0;
244  continue;
245  }
246  cost += pathstop.cost;
247  }
248 
249  newPath[0].cost = newPath[1].agg_cost;
250  for (unsigned int i = 1; i < newPath.size() - 2; ++i) {
251  /* newPath[i] has: node, edge, cost, agg_cost
252  * pgr_type_t has: id, source, target, cost, reverse_cost
253  *
254  * find the edge where the pathstop.edge == edge.id */
255 
256  int64_t edge_to_find = newPath[i].edge;
257  auto edge_ptr = std::find_if(
258  m_edges_of_points.begin(), m_edges_of_points.end(),
259  [&edge_to_find](const pgr_edge_t &edge)
260  {return edge_to_find == edge.id;});
261  if (edge_ptr != m_edges_of_points.end()) {
262  newPath[i].cost = edge_ptr->target == newPath[i+1].node ?
263  edge_ptr->cost : edge_ptr->reverse_cost;
264  }
265  }
266  newPath[newPath.size()-2].cost += cost;
267 
268  return newPath;
269 }
270 
271 
272 void
274  const std::vector< Point_on_edge_t > &points,
275  const int64_t &start_pid,
276  const int64_t &end_pid,
277  Path &path) {
278  if (path.empty()) return;
279  path.start_id(start_pid);
280  path.end_id(end_pid);
281 
282  for (auto &path_stop : path) {
283  for (const auto point : points) {
284  if (point.vertex_id == path_stop.node) {
285  path_stop.node = -point.pid;
286  break;
287  }
288  }
289  }
290 }
291 
292 void
294  const std::vector< Point_on_edge_t > &points,
295  Path &path) {
296  /*
297  * There is no path nothing to do
298  */
299  if (path.empty()) return;
300  /* from, to:
301  * * are constant along the path
302  * */
303  int64_t start_vid = path.start_id();
304  int64_t end_vid = path.end_id();
305 
306  int64_t start_pid = 0;
307  int64_t end_pid = 0;
308 
309  for (auto &p : points) {
310  if (p.vertex_id == start_vid) {
311  start_pid = -p.pid;
312  }
313  if (p.vertex_id == end_vid) {
314  end_pid = -p.pid;
315  }
316  }
317  adjust_pids(points, start_pid, end_pid, path);
318 }
319 
320 
321 
322 std::vector<pgr_edge_t>
324  return m_new_edges;
325 }
326 
327 void
329  for (const auto &point : m_points) {
330  log << "point: "
331  << point.pid << "\t"
332  << point.edge_id << "\t"
333  << point.fraction << "\t"
334  << point.side << "\t"
335  << point.vertex_id << "\n";
336  }
337 
338  int64_t vertex_id = 1;
339  std::vector< Point_on_edge_t > new_points;
340  for (const auto edge : m_edges_of_points) {
341  std::set< Point_on_edge_t, pointCompare> points_on_edge;
342  for (const auto point : m_points) {
343  if (edge.id == point.edge_id) {
344  points_on_edge.insert(point);
345  log << "working points: "
346  << point.pid << "\t"
347  << point.edge_id << "\t"
348  << point.fraction << "\t"
349  << point.side << "\t"
350  << point.vertex_id << "\n";
351  }
352  }
353  if (points_on_edge.empty()) {
354  error << "For some reason didn't find a point belonging to the edge"
355  << ", must be an error\n";
356  return;
357  }
358 #if 0
359  log << "breaking: \n"
360  << edge.id << "\t"
361  << edge.source << "\t"
362  << edge.target << "\t"
363  << edge.cost << "\t"
364  << edge.reverse_cost << "\n";
365 #endif
366  int64_t prev_target = edge.source;
367  int64_t prev_rtarget = edge.source;
368  double prev_fraction = 0;
369  double prev_rfraction = 0;
370  double agg_cost = 0;
371  double agg_rcost = 0;
372  double last_cost = 0;
373  double last_rcost = 0;
374  std::vector<Point_on_edge_t> the_points(
375  points_on_edge.begin(), points_on_edge.end());
376 
377  for (auto &point : the_points) {
378  /* the point either has
379  * vertex_id = source
380  * vertex_id = target
381  * vertex_id = -newnumber
382  */
383  log << "\npid"
384  << point.pid
385  << "\teid" << point.edge_id
386  << "/t" << point.fraction
387  << "\t" << point.side << "\n";
388  if (point.fraction < 0 || point.fraction > 1) {
389  error << "For some reason an invalid fraction was accepted,"
390  << " must be an error\n";
391  return;
392  }
393  if (point.fraction == 0) {
394  log << "Point's vertex_id = source" << edge.source << "\n";
395  point.vertex_id = edge.source;
396  }
397  if (point.fraction == 1) {
398  log << "point's vertex_id = target" << edge.target << "\n";
399  point.vertex_id = edge.target;
400  }
401  if (point.fraction > 0 && point.fraction < 1) {
402  log << "vertex_id of the point is " << -point.pid << "\n";
403  point.vertex_id = -point.pid;
404  ++vertex_id;
405  }
406  new_points.push_back(point);
407 
408  double deltaFraction = point.fraction - prev_fraction;
409  double deltarFraction = point.fraction - prev_rfraction;
410  if ((edge.cost < 0 || edge.reverse_cost < 0)
411  || m_driving_side == 'b'
412  || point.side == 'b') {
413  log << "Edge is one way "
414  << " or driving side is both or point side is both\n";
415  log << "Edge is one way: "
416  << (edge.cost < 0 || edge.reverse_cost < 0)
417  << "\n";
418  log << "driving side: " << m_driving_side << "\n";
419  log << "point side: " << point.side << "\n";
420  if (point.fraction > 0 && point.fraction < 1) {
421  if (edge.cost >= 0) {
422  last_cost = deltaFraction * edge.cost;
423  pgr_edge_t new_edge = {
424  edge.id,
425  prev_target,
426  point.vertex_id,
427  last_cost,
428  -1};
429  m_new_edges.push_back(new_edge);
430  log << "new_edge("
431  << "id, source, target, cost, reverse_cost) = ("
432  << new_edge.id << "\t"
433  << new_edge.source << "\t"
434  << new_edge.target << "\t"
435  << new_edge.cost << "\t"
436  << new_edge.reverse_cost << ")\n";
437  }
438  if (edge.reverse_cost >= 0) {
439  last_rcost = deltarFraction * edge.reverse_cost;
440  pgr_edge_t new_edge = {
441  edge.id,
442  prev_target,
443  point.vertex_id,
444  -1,
445  last_rcost};
446  m_new_edges.push_back(new_edge);
447  log << "new_edge("
448  << "id, source, target, cost, reverse_cost) = ("
449  << new_edge.id << "\t"
450  << new_edge.source << "\t"
451  << new_edge.target << "\t"
452  << new_edge.cost << "\t"
453  << new_edge.reverse_cost << ")\n";
454  }
455  }
456  prev_target = point.vertex_id;
457  prev_fraction = point.fraction;
458  agg_cost += last_cost;
459 
460  prev_rtarget = point.vertex_id;
461  prev_rfraction = point.fraction;
462  agg_rcost += last_rcost;
463  continue;
464  }
465 
466  pgassert(edge.cost > 0 && edge.reverse_cost > 0);
467  pgassert(point.side != 'b');
468 
469  if (m_driving_side == point.side) {
470  log << "two way and driving side == the side of the point\n";
471  log << "Breaking (source, target) when its not the extreme\n";
472  if (point.fraction > 0 && point.fraction < 1) {
473  last_cost = deltaFraction * edge.cost;
474  pgr_edge_t new_edge = {
475  edge.id, prev_target, point.vertex_id, last_cost, -1};
476  m_new_edges.push_back(new_edge);
477  log << "new_edge("
478  << "id, source, target, cost, reverse_cost) = ("
479  << new_edge.id << "\t"
480  << new_edge.source << "\t"
481  << new_edge.target << "\t"
482  << new_edge.cost << "\t"
483  << new_edge.reverse_cost << ")\n";
484  }
485  prev_target = point.vertex_id;
486  prev_fraction = point.fraction;
487  agg_cost += last_cost;
488  continue;
489  }
490 
491  log << "\ntwo way and driving side != the side of the point";
492  if (point.fraction > 0 && point.fraction < 1) {
493  last_rcost = deltarFraction * edge.reverse_cost;
494  pgr_edge_t new_edge = {
495  edge.id,
496  prev_rtarget,
497  point.vertex_id,
498  -1,
499  last_rcost};
500  m_new_edges.push_back(new_edge);
501  log << "\nnew_edge(id, source, target, cost, reverse_cost) = ("
502  << new_edge.id << "\t"
503  << new_edge.source << "\t"
504  << new_edge.target << "\t"
505  << new_edge.cost << "\t"
506  << new_edge.reverse_cost << ")\n";
507  }
508  prev_rtarget = point.vertex_id;
509  prev_rfraction = point.fraction;
510  agg_rcost += last_rcost;
511  }
512 
513  { // the last segments
514  pgr_edge_t new_edge = {
515  edge.id,
516  prev_target,
517  edge.target,
518  (edge.cost - agg_cost),
519  -1};
520  m_new_edges.push_back(new_edge);
521  log << "last edge: (id, source, target, cost, reverse_cost) = ("
522  << new_edge.id << "\t"
523  << new_edge.source << "\t"
524  << new_edge.target << "\t"
525  << new_edge.cost << "\t"
526  << new_edge.reverse_cost << ")\n";
527 
528  new_edge = {edge.id , prev_rtarget, edge.target,
529  -1, (edge.reverse_cost - agg_rcost)};
530  m_new_edges.push_back(new_edge);
531  log << "last edge: (id, source, target, cost, reverse_cost) = ("
532  << new_edge.id << "\t"
533  << new_edge.source << "\t"
534  << new_edge.target << "\t"
535  << new_edge.cost << "\t"
536  << new_edge.reverse_cost << ")\n";
537  }
538  }
539 
540  m_points = new_points;
541  for (const auto &point : m_points) {
542  log << "point: "
543  << point.pid << "\t"
544  << point.edge_id << "\t"
545  << point.fraction << "\t"
546  << point.side << "\t"
547  << point.vertex_id << "\n";
548  }
549 }
550 
551 } // namespace pgrouting
void eliminate_details_dd(Path &path) const
std::vector< Point_on_edge_t > m_points
float8 cost
Definition: trsp.h:45
int64_t get_edge_id(int64_t pid) const
int64 target
Definition: trsp.h:44
double reverse_cost
Definition: pgr_edge_t.h:42
Definition: trsp.h:41
int64_t source
Definition: pgr_edge_t.h:39
std::vector< pgr_edge_t > edges_of_points() const
int64_t target
Definition: pgr_edge_t.h:40
std::ostringstream log
Stores the hint information.
Definition: pgr_messages.h:81
std::ostream & operator<<(std::ostream &log, const Basic_vertex &v)
std::vector< pgr_edge_t > m_edges_of_points
std::vector< pgr_edge_t > new_edges() const
Path eliminate_details(Path path) const
void push_back(Path_t data)
std::vector< Point_on_edge_t > points() const
int64 source
Definition: trsp.h:43
std::ostringstream error
Stores the error information.
Definition: pgr_messages.h:85
#define pgassert(expr)
Uses the standard assert syntax.
Definition: pgr_assert.h:94
int64_t id
Definition: pgr_edge_t.h:38
std::vector< Point_on_edge_t > m_o_points
double cost
Definition: pgr_edge_t.h:41
std::vector< pgr_edge_t > m_new_edges
const pgr_edge_t * get_edge_data(int64_t eid) const
int64_t end_id() const
bool empty() const
Book keeping class for swapping orders between vehicles.
Assertions Handling.
int64 id
Definition: trsp.h:42
int64_t start_id() const
float8 reverse_cost
Definition: trsp.h:46
void adjust_pids(const std::vector< Point_on_edge_t > &points, Path &path)