31 #include "GraphDefinition.h"
34 GraphDefinition::GraphDefinition(
36 unsigned int edge_count,
43 m_edge_count(edge_count),
44 m_bIsturnRestrictOn(false),
45 m_bIsGraphConstructed(false) {
49 construct_graph(edges, has_rcost, directed);
50 m_bIsGraphConstructed =
true;
54 GraphDefinition::~GraphDefinition(
void) { }
58 void GraphDefinition::init() {
61 isStartVirtual =
false;
68 void GraphDefinition::deleteall() {
69 m_vecEdgeVector.clear();
79 double GraphDefinition::construct_path(int64_t ed_id, int64_t v_pos) {
80 if(parent[ed_id].ed_ind[v_pos] == -1) {
84 pelement.vertex_id = cur_edge->m_lStartNode;
85 pelement.cost = cur_edge->m_dCost;
87 pelement.vertex_id = cur_edge->m_lEndNode;
88 pelement.cost = cur_edge->m_dReverseCost;
90 pelement.edge_id = cur_edge->m_lEdgeID;
92 m_vecPath.push_back(pelement);
95 double ret = construct_path(parent[ed_id].ed_ind[v_pos], parent[ed_id].v_pos[v_pos]);
99 pelement.vertex_id = cur_edge->m_lStartNode;
100 pelement.cost = m_dCost[ed_id].endCost - ret;
101 ret = m_dCost[ed_id].endCost;
103 pelement.vertex_id = cur_edge->m_lEndNode;
104 pelement.cost = m_dCost[ed_id].startCost - ret;
105 ret = m_dCost[ed_id].startCost;
107 pelement.edge_id = cur_edge->m_lEdgeID;
109 m_vecPath.push_back(pelement);
116 double GraphDefinition::getRestrictionCost(
121 auto edge_id = new_edge.m_lEdgeID;
122 if(m_ruleTable.find(edge_id) == m_ruleTable.end()) {
125 std::vector<Rule> vecRules = m_ruleTable[edge_id];
126 auto totalRule = vecRules.size();
127 auto st_edge_ind = edge_ind;
128 for(
size_t ruleIndex = 0; ruleIndex < totalRule; ruleIndex++) {
130 auto total_edge = vecRules[ruleIndex].precedencelist.size();
131 int64_t v_pos = (isStart?0:1);
132 edge_ind = st_edge_ind;
133 for(
size_t i = 0; i < total_edge; i++) {
138 if(vecRules[ruleIndex].precedencelist[i] != m_vecEdgeVector[edge_ind].m_lEdgeID) {
142 auto parent_ind = parent[edge_ind].ed_ind[v_pos];
143 v_pos = parent[edge_ind].v_pos[v_pos];
144 edge_ind = parent_ind;
147 cost += vecRules[ruleIndex].cost;
154 void GraphDefinition::explore(
158 LongVector &vecIndex,
159 std::priority_queue<PDP, std::vector<PDP>,
160 std::greater<PDP> > &que) {
162 double extCost = 0.0;
166 for(i = 0; i < vecIndex.size(); i++) {
167 new_edge = m_vecEdgeVector[vecIndex[i]];
169 if(m_bIsturnRestrictOn) {
170 extCost = getRestrictionCost(cur_edge.m_lEdgeIndex, new_edge, isStart);
172 if(new_edge.m_lStartNode == cur_node) {
173 if(new_edge.m_dCost >= 0.0) {
177 totalCost = m_dCost[cur_edge.m_lEdgeIndex].endCost + new_edge.m_dCost + extCost;
179 totalCost = m_dCost[cur_edge.m_lEdgeIndex].startCost + new_edge.m_dCost + extCost;
180 if(totalCost < m_dCost[vecIndex[i]].endCost) {
181 m_dCost[vecIndex[i]].endCost = totalCost;
182 parent[new_edge.m_lEdgeIndex].v_pos[0] = (isStart?0:1);
183 parent[new_edge.m_lEdgeIndex].ed_ind[0] = cur_edge.m_lEdgeIndex;
184 que.push(std::make_pair(totalCost, std::make_pair(new_edge.m_lEdgeIndex,
true)));
188 if(new_edge.m_dReverseCost >= 0.0)
192 totalCost = m_dCost[cur_edge.m_lEdgeIndex].endCost + new_edge.m_dReverseCost + extCost;
194 totalCost = m_dCost[cur_edge.m_lEdgeIndex].startCost + new_edge.m_dReverseCost + extCost;
195 if(totalCost < m_dCost[vecIndex[i]].startCost)
197 m_dCost[vecIndex[i]].startCost = totalCost;
198 parent[new_edge.m_lEdgeIndex].v_pos[1] = (isStart?0:1);
199 parent[new_edge.m_lEdgeIndex].ed_ind[1] = cur_edge.m_lEdgeIndex;
200 que.push(std::make_pair(totalCost, std::make_pair(new_edge.m_lEdgeIndex,
false)));
211 GraphDefinition::add_virtual_vertices(
int start_edge_id,
double start_part,
int end_edge_id,
double end_part,
212 int64_t &start_vertex, int64_t &end_vertex)
214 GraphEdgeInfo *start_edge_info = &m_vecEdgeVector[m_mapEdgeId2Index[start_edge_id]];
217 m_dStartpart = start_part;
218 m_dEndPart = end_part;
219 m_lStartEdgeId = start_edge_id;
220 m_lEndEdgeId = end_edge_id;
222 if(start_part == 0.0)
224 start_vertex = start_edge_info->m_lStartNode;
226 else if(start_part == 1.0)
228 start_vertex = start_edge_info->m_lEndNode;
232 isStartVirtual =
true;
233 m_lStartEdgeId = start_edge_id;
234 start_vertex = max_node_id + 1;
236 start_edge.id = max_edge_id + 1;
238 start_edge.source = start_vertex;
239 start_edge.reverse_cost = -1.0;
240 if(start_edge_info->m_dCost >= 0.0)
242 start_edge.target = start_edge_info->m_lEndNode;
243 start_edge.cost = (1.0 - start_part) * start_edge_info->m_dCost;
247 if(start_edge_info->m_dReverseCost >= 0.0)
249 start_edge.id = max_edge_id + 1;
251 start_edge.target = start_edge_info->m_lStartNode;
252 start_edge.cost = start_part * start_edge_info->m_dReverseCost;
258 GraphEdgeInfo *end_edge_info = &m_vecEdgeVector[m_mapEdgeId2Index[end_edge_id]];
263 end_vertex = end_edge_info->m_lStartNode;
265 else if(end_part == 1.0)
267 end_vertex = end_edge_info->m_lEndNode;
272 m_lEndEdgeId = end_edge_id;
273 end_vertex = max_node_id + 1;
275 end_edge.id = max_edge_id + 1;
277 end_edge.target = end_vertex;
278 end_edge.reverse_cost = -1.0;
279 if(end_edge_info->m_dCost >= 0.0)
281 end_edge.source = end_edge_info->m_lStartNode;
282 end_edge.cost = end_part * end_edge_info->m_dCost;
286 if(end_edge_info->m_dReverseCost >= 0.0)
288 end_edge.source = end_edge_info->m_lEndNode;
289 end_edge.id = max_edge_id + 1;
290 end_edge.cost = (1.0 - end_part) * end_edge_info->m_dReverseCost;
301 GraphDefinition::set_restrictions(
302 int64_t start_vertex,
304 std::vector<PDVI> &ruleList) {
306 auto total_rule = ruleList.size();
307 LongVector vecsource;
309 for(
size_t i = 0; i < total_rule; i++)
312 rule.cost = ruleList[i].first;
313 auto seq_cnt = ruleList[i].second.size();
314 for(
size_t j = 1; j < seq_cnt; j++) {
315 rule.precedencelist.push_back(ruleList[i].second[j]);
317 auto dest_edge_id = ruleList[i].second[0];
318 if(m_ruleTable.find(dest_edge_id) != m_ruleTable.end()) {
319 m_ruleTable[dest_edge_id].push_back(rule);
321 std::vector<Rule> temprules;
323 temprules.push_back(rule);
324 m_ruleTable.insert(std::make_pair(dest_edge_id, temprules));
328 if(seq_cnt == 2 && ruleList[i].second[1] == m_lStartEdgeId) {
329 vecsource = m_mapNodeId2Edge[start_vertex];
330 for(kk = 0; kk < vecsource.size(); kk++) {
331 rule.precedencelist.clear();
332 rule.precedencelist.push_back(m_vecEdgeVector[vecsource[kk]].m_lEdgeID);
333 m_ruleTable[dest_edge_id].push_back(rule);
339 if(m_ruleTable.find(m_lEndEdgeId) != m_ruleTable.end()) {
340 std::vector<Rule> tmpRules = m_ruleTable[m_lEndEdgeId];
341 vecsource = m_mapNodeId2Edge[end_vertex];
342 for(kk = 0; kk < vecsource.size(); kk++) {
343 m_ruleTable.insert(std::make_pair(m_vecEdgeVector[vecsource[kk]].m_lEdgeID, tmpRules));
347 m_bIsturnRestrictOn =
true;
352 int GraphDefinition:: my_dijkstra(int64_t start_vertex, int64_t end_vertex,
354 std::ostringstream &log) {
356 std::priority_queue<PDP, std::vector<PDP>, std::greater<PDP> > que;
357 parent.resize(m_edge_count + 1);
358 m_dCost.resize(m_edge_count + 1);
361 for (
auto &dcost : m_dCost) {
362 dcost.startCost = 1e15;
363 dcost.endCost = 1e15;
366 if(m_mapNodeId2Edge.find(start_vertex) == m_mapNodeId2Edge.end())
368 log <<
"Source Not Found";
372 if(m_mapNodeId2Edge.find(end_vertex) == m_mapNodeId2Edge.end())
374 log <<
"Destination Not Found";
378 LongVector vecsource = m_mapNodeId2Edge[start_vertex];
381 for (
auto &s : vecsource)
384 if(cur_edge->m_lStartNode == start_vertex)
386 if(cur_edge->m_dCost >= 0.0)
388 m_dCost[cur_edge->m_lEdgeIndex].endCost= cur_edge->m_dCost;
389 parent[cur_edge->m_lEdgeIndex].v_pos[0] = -1;
390 parent[cur_edge->m_lEdgeIndex].ed_ind[0] = -1;
391 que.push(std::make_pair(cur_edge->m_dCost, std::make_pair(cur_edge->m_lEdgeIndex,
true)));
394 if(cur_edge->m_dReverseCost >= 0.0)
396 m_dCost[cur_edge->m_lEdgeIndex].startCost = cur_edge->m_dReverseCost;
397 parent[cur_edge->m_lEdgeIndex].v_pos[1] = -1;
398 parent[cur_edge->m_lEdgeIndex].ed_ind[1] = -1;
399 que.push(std::make_pair(cur_edge->m_dReverseCost, std::make_pair(cur_edge->m_lEdgeIndex,
false)));
404 int64_t cur_node = -1;
406 while(!que.empty()) {
407 PDP cur_pos = que.top();
409 int cured_index = cur_pos.second.first;
410 cur_edge = &m_vecEdgeVector[cured_index];
412 if(cur_pos.second.second) {
414 cur_node = cur_edge->m_lEndNode;
415 if(cur_edge->m_dCost < 0.0)
417 if(cur_node == end_vertex)
419 explore(cur_node, *cur_edge,
true, cur_edge->m_vecEndConnedtedEdge, que);
422 cur_node = cur_edge->m_lStartNode;
423 if(cur_edge->m_dReverseCost < 0.0)
425 if(cur_node == end_vertex)
427 explore(cur_node, *cur_edge,
false, cur_edge->m_vecStartConnectedEdge, que);
430 if(cur_node != end_vertex) {
431 if(m_lStartEdgeId == m_lEndEdgeId) {
432 if(get_single_cost(1000.0, path, path_count)) {
436 log <<
"Path Not Found";
442 if(cur_node == cur_edge->m_lStartNode) {
443 total_cost = m_dCost[cur_edge->m_lEdgeIndex].startCost;
444 construct_path(cur_edge->m_lEdgeIndex, 1);
446 total_cost = m_dCost[cur_edge->m_lEdgeIndex].endCost;
447 construct_path(cur_edge->m_lEdgeIndex, 0);
450 pelement.vertex_id = end_vertex;
451 pelement.edge_id = -1;
453 m_vecPath.push_back(pelement);
455 if(m_lStartEdgeId == m_lEndEdgeId)
457 if(get_single_cost(total_cost, path, path_count))
464 *path_count = m_vecPath.size();
466 for(
size_t i = 0; i < *path_count; i++)
468 (*path)[i].vertex_id = m_vecPath[i].vertex_id;
469 (*path)[i].edge_id = m_vecPath[i].edge_id;
470 (*path)[i].cost = m_vecPath[i].cost;
474 (*path)[0].vertex_id = -1;
475 (*path)[0].edge_id = m_lStartEdgeId;
479 *path_count = *path_count - 1;
480 (*path)[*path_count - 1].edge_id = m_lEndEdgeId;
488 bool GraphDefinition::get_single_cost(
double total_cost,
path_element_t **path,
size_t *path_count)
490 GraphEdgeInfo *start_edge_info = &m_vecEdgeVector[m_mapEdgeId2Index[m_lStartEdgeId]];
491 if(m_dEndPart >= m_dStartpart)
493 if(start_edge_info->m_dCost >= 0.0 && start_edge_info->m_dCost * (m_dEndPart - m_dStartpart) <= total_cost)
497 (*path)[0].vertex_id = -1;
498 (*path)[0].edge_id = m_lStartEdgeId;
499 (*path)[0].cost = start_edge_info->m_dCost * (m_dEndPart - m_dStartpart);
506 if(start_edge_info->m_dReverseCost >= 0.0 && start_edge_info->m_dReverseCost * (m_dStartpart - m_dEndPart) <= total_cost)
510 (*path)[0].vertex_id = -1;
511 (*path)[0].edge_id = m_lStartEdgeId;
512 (*path)[0].cost = start_edge_info->m_dReverseCost * (m_dStartpart - m_dEndPart);
523 bool GraphDefinition::construct_graph(
edge_t* edges,
bool has_reverse_cost,
bool directed) {
525 for(i = 0; i < m_edge_count; i++) {
532 if (has_reverse_cost) {
533 if (edges[i].cost < 0) {
534 auto tmp_v = edges[i].source;
535 edges[i].source = edges[i].target;
536 edges[i].target = tmp_v;
537 edges[i].cost = edges[i].reverse_cost;
538 edges[i].reverse_cost = -1.0;
545 if (!has_reverse_cost) {
546 edges[i].reverse_cost = -1.0;
553 if (!directed && !has_reverse_cost) {
554 edges[i].reverse_cost = edges[i].cost;
555 }
else if (!directed && has_reverse_cost) {
556 if (edges[i].reverse_cost < 0) {
557 edges[i].reverse_cost = edges[i].cost;
559 edges[i].reverse_cost = std::min(edges[i].reverse_cost, edges[i].cost);
565 m_bIsGraphConstructed =
true;
575 if(firstEdge.m_dReverseCost >= 0.0)
576 firstEdge.m_vecStartConnectedEdge.push_back(secondEdge.m_lEdgeIndex);
577 if(firstEdge.m_lStartNode == secondEdge.m_lStartNode)
579 if(secondEdge.m_dReverseCost >= 0.0)
580 secondEdge.m_vecStartConnectedEdge.push_back(firstEdge.m_lEdgeIndex);
584 if(secondEdge.m_dCost >= 0.0)
585 secondEdge.m_vecEndConnedtedEdge.push_back(firstEdge.m_lEdgeIndex);
590 if(firstEdge.m_dCost >= 0.0)
591 firstEdge.m_vecEndConnedtedEdge.push_back(secondEdge.m_lEdgeIndex);
592 if(firstEdge.m_lEndNode == secondEdge.m_lStartNode)
594 if(secondEdge.m_dReverseCost >= 0.0)
595 secondEdge.m_vecStartConnectedEdge.push_back(firstEdge.m_lEdgeIndex);
599 if(secondEdge.m_dCost >= 0.0)
600 secondEdge.m_vecEndConnedtedEdge.push_back(firstEdge.m_lEdgeIndex);
609 bool GraphDefinition::addEdge(
edge_t edgeIn)
612 Long2LongMap::iterator itMap = m_mapEdgeId2Index.find(edgeIn.id);
613 if(itMap != m_mapEdgeId2Index.end())
618 edgeIn.source, edgeIn.target,edgeIn.cost, edgeIn.reverse_cost);
619 newEdge.m_vecStartConnectedEdge.clear();
620 newEdge.m_vecEndConnedtedEdge.clear();
621 newEdge.m_vecRestrictedEdge.clear();
622 newEdge.m_lEdgeID = edgeIn.id;
623 newEdge.m_lEdgeIndex = m_vecEdgeVector.size();
624 newEdge.m_lStartNode = edgeIn.source;
625 newEdge.m_lEndNode = edgeIn.target;
626 newEdge.m_dCost = edgeIn.cost;
627 newEdge.m_dReverseCost = edgeIn.reverse_cost;
628 if(edgeIn.id > max_edge_id) {
629 max_edge_id = edgeIn.id;
632 if(newEdge.m_lStartNode > max_node_id) { max_node_id = newEdge.m_lStartNode;
635 if(newEdge.m_lEndNode > max_node_id) { max_node_id = newEdge.m_lEndNode;
639 Long2LongVectorMap::iterator itNodeMap = m_mapNodeId2Edge.find(edgeIn.source);
640 if(itNodeMap != m_mapNodeId2Edge.end())
644 long lEdgeCount = itNodeMap->second.size();
646 for(lEdgeIndex = 0; lEdgeIndex < lEdgeCount; lEdgeIndex++)
648 long lEdge = itNodeMap->second.at(lEdgeIndex);
649 connectEdge(newEdge, m_vecEdgeVector[lEdge],
true);
655 itNodeMap = m_mapNodeId2Edge.find(edgeIn.target);
656 if(itNodeMap != m_mapNodeId2Edge.end())
660 long lEdgeCount = itNodeMap->second.size();
662 for(lEdgeIndex = 0; lEdgeIndex < lEdgeCount; lEdgeIndex++)
664 long lEdge = itNodeMap->second.at(lEdgeIndex);
665 connectEdge(newEdge, m_vecEdgeVector[lEdge],
false);
672 m_mapNodeId2Edge[edgeIn.source].push_back(newEdge.m_lEdgeIndex);
673 m_mapNodeId2Edge[edgeIn.target].push_back(newEdge.m_lEdgeIndex);
677 m_mapEdgeId2Index.insert(std::make_pair(newEdge.m_lEdgeID, m_vecEdgeVector.size()));
678 m_vecEdgeVector.push_back(newEdge);