PGROUTING  2.6
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_normal(p_normal),
81  m_driving_side(p_driving_side),
82  m_directed(p_directed) {
83  if (!p_normal) {
84  reverse_sides();
85  }
86  if (!m_directed) {
87  m_driving_side = 'b';
88  }
89  check_points();
91  log << "constructor";
92 }
93 
94 void
96  for (auto &point : m_points) {
97  if (point.side == 'r') {
98  point.side = 'l';
99  } else if (point.side == 'l') {
100  point.side = 'r';
101  }
102  point.fraction = 1 - point.fraction;
103  }
104  if (m_driving_side == 'r') {
105  m_driving_side = 'l';
106  } else if (m_driving_side == 'l') {
107  m_driving_side = 'r';
108  }
109 }
110 
111 
112 void
114  log << "original points" << *this;
115  /*
116  * deleting duplicate points
117  */
118  std::sort(m_points.begin(), m_points.end(),
119  [](const Point_on_edge_t &a, const Point_on_edge_t &b)
120  -> bool {
121  if (a.pid != b.pid) return a.pid < b.pid;
122  if (a.edge_id != b.edge_id) return a.edge_id < b.edge_id;
123  if (a.fraction != b.fraction) return a.fraction < b.fraction;
124  return a.side < b.side;
125  });
126  log << "after sorting" << *this;
127  auto last = std::unique(m_points.begin(), m_points.end(),
128  [](const Point_on_edge_t &a, const Point_on_edge_t &b) {
129  return a.pid == b.pid &&
130  a.edge_id == b.edge_id &&
131  a.fraction == b.fraction &&
132  a.side == b.side;
133  });
134  m_points.erase(last, m_points.end());
135  size_t total_points = m_points.size();
136 
137  log << "after deleting repetitions" << *this;
138  log << "We have " << total_points << " different points";
139 
140  last = std::unique(m_points.begin(), m_points.end(),
141  [](const Point_on_edge_t &a, const Point_on_edge_t &b) {
142  return a.pid == b.pid;
143  });
144  m_points.erase(last, m_points.end());
145  log << "after deleting points with same id" << *this;
146 
147  if (m_points.size() != total_points) {
148  error << "Unexpected point(s) with same pid"
149  << " but different edge/fraction/side combination found.";
150  }
151 }
152 
153 
154 int64_t
155 Pg_points_graph::get_edge_id(int64_t pid) const {
156  auto point_ptr = std::find_if(
157  m_points.begin(), m_points.end(),
158  [&pid](const Point_on_edge_t &point)
159  {return pid == -point.pid;});
160  return point_ptr != m_points.end() ?
161  point_ptr->edge_id :
162  -1;
163 }
164 
165 const pgr_edge_t*
166 Pg_points_graph::get_edge_data(int64_t eid) const {
167  auto e_itr = std::find_if(
168  m_edges_of_points.begin(), m_edges_of_points.end(),
169  [&eid](const pgr_edge_t &edge)
170  {return eid == edge.id;});
171  return e_itr == m_edges_of_points.end()?
172  nullptr : &(*e_itr);
173 }
174 
175 
176 void
178  Path &path) const {
179  /*
180  * There is no path nothing to do
181  */
182  if (path.empty()) return;
183 
184  Path newPath(path.start_id(), path.end_id());
185  auto edge_id = path.start_id() < 0?
186  get_edge_id(path.start_id()) :
187  -1;
188 
189  for (auto pathstop : path) {
190  /*
191  * skip points (no details)
192  * except if ithe point its the starting point
193  */
194  if (!((pathstop.node == path.start_id())
195  || (pathstop.node > 0))) {
196  continue;
197  }
198 
199  /*
200  * Change costs only when the node is not:
201  * - start_id
202  * - directly connected to start_id
203  */
204  if (pathstop.node != path.start_id()) {
205  auto edge_ptr = get_edge_data(pathstop.edge);
206  /*
207  * edge found
208  * and its not the edge directly connected to start_id()
209  */
210  if (edge_ptr
211  && edge_ptr->id != edge_id) {
212  pathstop.cost = pathstop.node == edge_ptr->source?
213  edge_ptr->cost :
214  edge_ptr->reverse_cost;
215  }
216  }
217 
218  /*
219  * add to the new path
220  */
221  newPath.push_back(pathstop);
222  }
223 
224  path = newPath;
225 }
226 
227 
228 
229 Path
231  Path path) const {
232  /*
233  * There is no path nothing to do
234  */
235  if (path.empty()) return path;
236 
237  Path newPath(path.start_id(), path.end_id());
238  double cost = 0.0;
239  for (const auto &pathstop : path) {
240  if ((pathstop.node == path.start_id())
241  || (pathstop.node == path.end_id())
242  || (pathstop.node > 0)) {
243  newPath.push_back(pathstop);
244  if (pathstop.node != path.end_id()) cost = 0.0;
245  continue;
246  }
247  cost += pathstop.cost;
248  }
249 
250  newPath[0].cost = newPath[1].agg_cost;
251  for (unsigned int i = 1; i < newPath.size() - 2; ++i) {
252  /* newPath[i] has: node, edge, cost, agg_cost
253  * pgr_type_t has: id, source, target, cost, reverse_cost
254  *
255  * find the edge where the pathstop.edge == edge.id */
256 
257  int64_t edge_to_find = newPath[i].edge;
258  auto edge_ptr = std::find_if(
259  m_edges_of_points.begin(), m_edges_of_points.end(),
260  [&edge_to_find](const pgr_edge_t &edge)
261  {return edge_to_find == edge.id;});
262  if (edge_ptr != m_edges_of_points.end()) {
263  newPath[i].cost = edge_ptr->target == newPath[i+1].node ?
264  edge_ptr->cost : edge_ptr->reverse_cost;
265  }
266  }
267  newPath[newPath.size()-2].cost += cost;
268 
269  return newPath;
270 }
271 
272 
273 void
275  const std::vector< Point_on_edge_t > &points,
276  const int64_t &start_pid,
277  const int64_t &end_pid,
278  Path &path) {
279  if (path.empty()) return;
280  path.start_id(start_pid);
281  path.end_id(end_pid);
282 
283  for (auto &path_stop : path) {
284  for (const auto point : points) {
285  if (point.vertex_id == path_stop.node) {
286  path_stop.node = -point.pid;
287  break;
288  }
289  }
290  }
291 }
292 
293 void
295  const std::vector< Point_on_edge_t > &points,
296  Path &path) {
297  /*
298  * There is no path nothing to do
299  */
300  if (path.empty()) return;
301  /* from, to:
302  * * are constant along the path
303  * */
304  int64_t start_vid = path.start_id();
305  int64_t end_vid = path.end_id();
306 
307  int64_t start_pid = 0;
308  int64_t end_pid = 0;
309 
310  for (auto &p : points) {
311  if (p.vertex_id == start_vid) {
312  start_pid = -p.pid;
313  }
314  if (p.vertex_id == end_vid) {
315  end_pid = -p.pid;
316  }
317  }
318  adjust_pids(points, start_pid, end_pid, path);
319 }
320 
321 
322 
323 std::vector<pgr_edge_t>
325  return m_new_edges;
326 }
327 
328 void
330  for (const auto &point : m_points) {
331  log << "point: "
332  << point.pid << "\t"
333  << point.edge_id << "\t"
334  << point.fraction << "\t"
335  << point.side << "\t"
336  << point.vertex_id << "\n";
337  }
338 
339  int64_t vertex_id = 1;
340  std::vector< Point_on_edge_t > new_points;
341  for (const auto edge : m_edges_of_points) {
342  std::set< Point_on_edge_t, pointCompare> points_on_edge;
343  for (const auto point : m_points) {
344  if (edge.id == point.edge_id) {
345  points_on_edge.insert(point);
346  log << "working points: "
347  << point.pid << "\t"
348  << point.edge_id << "\t"
349  << point.fraction << "\t"
350  << point.side << "\t"
351  << point.vertex_id << "\n";
352  }
353  }
354  if (points_on_edge.empty()) {
355  error << "For some reason didn't find a point belonging to the edge"
356  << ", must be an error\n";
357  return;
358  }
359 #if 0
360  log << "breaking: \n"
361  << edge.id << "\t"
362  << edge.source << "\t"
363  << edge.target << "\t"
364  << edge.cost << "\t"
365  << edge.reverse_cost << "\n";
366 #endif
367  int64_t prev_target = edge.source;
368  int64_t prev_rtarget = edge.source;
369  double prev_fraction = 0;
370  double prev_rfraction = 0;
371  double agg_cost = 0;
372  double agg_rcost = 0;
373  double last_cost = 0;
374  double last_rcost = 0;
375  std::vector<Point_on_edge_t> the_points(
376  points_on_edge.begin(), points_on_edge.end());
377 
378  for (auto &point : the_points) {
379  /* the point either has
380  * vertex_id = source
381  * vertex_id = target
382  * vertex_id = -newnumber
383  */
384  log << "\npid"
385  << point.pid
386  << "\teid" << point.edge_id
387  << "/t" << point.fraction
388  << "\t" << point.side << "\n";
389  if (point.fraction < 0 || point.fraction > 1) {
390  error << "For some reason an invalid fraction was accepted,"
391  << " must be an error\n";
392  return;
393  }
394  if (point.fraction == 0) {
395  log << "Point's vertex_id = source" << edge.source << "\n";
396  point.vertex_id = edge.source;
397  }
398  if (point.fraction == 1) {
399  log << "point's vertex_id = target" << edge.target << "\n";
400  point.vertex_id = edge.target;
401  }
402  if (point.fraction > 0 && point.fraction < 1) {
403  log << "vertex_id of the point is " << -point.pid << "\n";
404  point.vertex_id = -point.pid;
405  ++vertex_id;
406  }
407  new_points.push_back(point);
408 
409  double deltaFraction = point.fraction - prev_fraction;
410  double deltarFraction = point.fraction - prev_rfraction;
411  if ((edge.cost < 0 || edge.reverse_cost < 0)
412  || m_driving_side == 'b'
413  || point.side == 'b') {
414  log << "Edge is one way "
415  << " or driving side is both or point side is both\n";
416  log << "Edge is one way: "
417  << (edge.cost < 0 || edge.reverse_cost < 0)
418  << "\n";
419  log << "driving side: " << m_driving_side << "\n";
420  log << "point side: " << point.side << "\n";
421  if (point.fraction > 0 && point.fraction < 1) {
422  if (edge.cost >= 0) {
423  last_cost = deltaFraction * edge.cost;
424  pgr_edge_t new_edge = {
425  edge.id,
426  prev_target,
427  point.vertex_id,
428  last_cost,
429  -1};
430  m_new_edges.push_back(new_edge);
431  log << "new_edge("
432  << "id, source, target, cost, reverse_cost) = ("
433  << new_edge.id << "\t"
434  << new_edge.source << "\t"
435  << new_edge.target << "\t"
436  << new_edge.cost << "\t"
437  << new_edge.reverse_cost << ")\n";
438  }
439  if (edge.reverse_cost >= 0) {
440  last_rcost = deltarFraction * edge.reverse_cost;
441  pgr_edge_t new_edge = {
442  edge.id,
443  prev_target,
444  point.vertex_id,
445  -1,
446  last_rcost};
447  m_new_edges.push_back(new_edge);
448  log << "new_edge("
449  << "id, source, target, cost, reverse_cost) = ("
450  << new_edge.id << "\t"
451  << new_edge.source << "\t"
452  << new_edge.target << "\t"
453  << new_edge.cost << "\t"
454  << new_edge.reverse_cost << ")\n";
455  }
456  }
457  prev_target = point.vertex_id;
458  prev_fraction = point.fraction;
459  agg_cost += last_cost;
460 
461  prev_rtarget = point.vertex_id;
462  prev_rfraction = point.fraction;
463  agg_rcost += last_rcost;
464  continue;
465  }
466 
467  pgassert(edge.cost > 0 && edge.reverse_cost > 0);
468  pgassert(point.side != 'b');
469 
470  if (m_driving_side == point.side) {
471  log << "two way and driving side == the side of the point\n";
472  log << "Breaking (source, target) when its not the extreme\n";
473  if (point.fraction > 0 && point.fraction < 1) {
474  last_cost = deltaFraction * edge.cost;
475  pgr_edge_t new_edge = {
476  edge.id, prev_target, point.vertex_id, last_cost, -1};
477  m_new_edges.push_back(new_edge);
478  log << "new_edge("
479  << "id, source, target, cost, reverse_cost) = ("
480  << new_edge.id << "\t"
481  << new_edge.source << "\t"
482  << new_edge.target << "\t"
483  << new_edge.cost << "\t"
484  << new_edge.reverse_cost << ")\n";
485  }
486  prev_target = point.vertex_id;
487  prev_fraction = point.fraction;
488  agg_cost += last_cost;
489  continue;
490  }
491 
492  log << "\ntwo way and driving side != the side of the point";
493  if (point.fraction > 0 && point.fraction < 1) {
494  last_rcost = deltarFraction * edge.reverse_cost;
495  pgr_edge_t new_edge = {
496  edge.id,
497  prev_rtarget,
498  point.vertex_id,
499  -1,
500  last_rcost};
501  m_new_edges.push_back(new_edge);
502  log << "\nnew_edge(id, source, target, cost, reverse_cost) = ("
503  << new_edge.id << "\t"
504  << new_edge.source << "\t"
505  << new_edge.target << "\t"
506  << new_edge.cost << "\t"
507  << new_edge.reverse_cost << ")\n";
508  }
509  prev_rtarget = point.vertex_id;
510  prev_rfraction = point.fraction;
511  agg_rcost += last_rcost;
512  }
513 
514  { // the last segments
515  pgr_edge_t new_edge = {
516  edge.id,
517  prev_target,
518  edge.target,
519  (edge.cost - agg_cost),
520  -1};
521  m_new_edges.push_back(new_edge);
522  log << "last edge: (id, source, target, cost, reverse_cost) = ("
523  << new_edge.id << "\t"
524  << new_edge.source << "\t"
525  << new_edge.target << "\t"
526  << new_edge.cost << "\t"
527  << new_edge.reverse_cost << ")\n";
528 
529  new_edge = {edge.id , prev_rtarget, edge.target,
530  -1, (edge.reverse_cost - agg_rcost)};
531  m_new_edges.push_back(new_edge);
532  log << "last edge: (id, source, target, cost, reverse_cost) = ("
533  << new_edge.id << "\t"
534  << new_edge.source << "\t"
535  << new_edge.target << "\t"
536  << new_edge.cost << "\t"
537  << new_edge.reverse_cost << ")\n";
538  }
539  }
540 
541  m_points = new_points;
542  for (const auto &point : m_points) {
543  log << "point: "
544  << point.pid << "\t"
545  << point.edge_id << "\t"
546  << point.fraction << "\t"
547  << point.side << "\t"
548  << point.vertex_id << "\n";
549  }
550 }
551 
552 } // namespace pgrouting
std::vector< Point_on_edge_t > m_points
float8 cost
Definition: trsp.h:35
double reverse_cost
Definition: pgr_edge_t.h:63
Definition: trsp.h:31
int64_t source
Definition: pgr_edge_t.h:60
int64_t target
Definition: pgr_edge_t.h:61
std::ostringstream log
Stores the hint information.
Definition: pgr_messages.h:102
std::ostream & operator<<(std::ostream &log, const Basic_vertex &v)
std::vector< pgr_edge_t > m_edges_of_points
int64_t get_edge_id(int64_t pid) const
long id
Definition: trsp.h:32
Path eliminate_details(Path path) const
std::vector< pgr_edge_t > edges_of_points() const
void push_back(Path_t data)
int64_t end_id() const
std::ostringstream error
Stores the error information.
Definition: pgr_messages.h:106
#define pgassert(expr)
Uses the standard assert syntax.
Definition: pgr_assert.h:81
int64_t id
Definition: pgr_edge_t.h:59
std::vector< Point_on_edge_t > m_o_points
double cost
Definition: pgr_edge_t.h:62
std::vector< pgr_edge_t > m_new_edges
int64_t start_id() const
std::vector< pgr_edge_t > new_edges() const
void eliminate_details_dd(Path &path) const
bool empty() const
Book keeping class for swapping orders between vehicles.
Definition: basic_edge.cpp:28
Assertions Handling.
long target
Definition: trsp.h:34
const pgr_edge_t * get_edge_data(int64_t eid) const
std::vector< Point_on_edge_t > points() const
float8 reverse_cost
Definition: trsp.h:36
long source
Definition: trsp.h:33
void adjust_pids(const std::vector< Point_on_edge_t > &points, Path &path)