-
Notifications
You must be signed in to change notification settings - Fork 1
/
conflict_avoidance_table.cpp
128 lines (116 loc) · 4.75 KB
/
conflict_avoidance_table.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
#include "conflict_avoidance_table.h"
void ConflictAvoidanceTable::addNode(const Node &node) {
auto tuple = std::make_tuple(node.i, node.j, node.g);
if (nodeAgentsCount.find(tuple) == nodeAgentsCount.end()) {
nodeAgentsCount[tuple] = 1;
} else {
++nodeAgentsCount[tuple];
}
}
void ConflictAvoidanceTable::addEdge(const Node &node, const Node &prev) {
auto tuple = std::make_tuple(prev.i, prev.j, node.i, node.j, node.g);
if (edgeAgentsCount.find(tuple) == edgeAgentsCount.end()) {
edgeAgentsCount[tuple] = 1;
} else {
++edgeAgentsCount[tuple];
}
}
void ConflictAvoidanceTable::addAgentPath(const std::list<Node>::const_iterator& start,
const std::list<Node>::const_iterator& end) {
for (auto it = start; it != end; ++it) {
addNode(*it);
if (it != start && *it != *std::prev(it)) {
addEdge(*it, *std::prev(it));
}
}
}
void ConflictAvoidanceTable::removeNode(const Node &node) {
auto tuple = std::make_tuple(node.i, node.j, node.g);
if (nodeAgentsCount[tuple] == 1) {
nodeAgentsCount.erase(tuple);
} else {
--nodeAgentsCount[tuple];
}
}
void ConflictAvoidanceTable::removeEdge(const Node &node, const Node &prev) {
auto tuple = std::make_tuple(prev.i, prev.j, node.i, node.j, node.g);
if (edgeAgentsCount[tuple] == 1) {
edgeAgentsCount.erase(tuple);
} else {
--edgeAgentsCount[tuple];
}
}
void ConflictAvoidanceTable::removeAgentPath(const std::list<Node>::const_iterator& start,
const std::list<Node>::const_iterator& end) {
for (auto it = start; it != end; ++it) {
removeNode(*it);
if (it != start && *it != *std::prev(it)) {
removeEdge(*it, *std::prev(it));
}
}
}
int ConflictAvoidanceTable::getAgentsCount(const Node &node) const {
int res = 0;
auto tuple = std::make_tuple(node.i, node.j, node.g);
if (nodeAgentsCount.find(tuple) != nodeAgentsCount.end()) {
res = nodeAgentsCount.at(tuple);
}
return res;
}
int ConflictAvoidanceTable::getFirstSoftConflict(const Node & node, int startTime, int endTime) const {
auto nodeIt = nodeAgentsCount.lower_bound(std::make_tuple(node.i, node.j, startTime));
if (nodeIt != nodeAgentsCount.end() && std::get<0>(nodeIt->first) == node.i
&& std::get<1>(nodeIt->first) == node.j && std::get<2>(nodeIt->first) <= endTime) {
return std::get<2>(nodeIt->first);
}
return -1;
}
int ConflictAvoidanceTable::getFutureConflictsCount(const Node & node, int time) const {
int res = 0;
auto it = nodeAgentsCount.upper_bound(std::make_tuple(node.i, node.j, time));
for (; it != nodeAgentsCount.end() && std::get<0>(it->first) == node.i
&& std::get<1>(it->first) == node.j; ++it) {
res += it->second;
}
return res;
}
void ConflictAvoidanceTable::getSoftConflictIntervals(std::vector<std::pair<int, int>> &res,
const Node & node, const Node &prevNode,
int startTime, int endTime, bool binary) const {
std::map<int, int> agentsCount;
auto nodeIt = nodeAgentsCount.lower_bound(std::make_tuple(node.i, node.j, startTime));
auto nodeEnd = nodeAgentsCount.upper_bound(std::make_tuple(node.i, node.j, endTime));
for (nodeIt; nodeIt != nodeEnd; ++nodeIt) {
agentsCount[std::get<2>(nodeIt->first)] = nodeIt->second;
}
auto edgeIt = edgeAgentsCount.lower_bound(std::make_tuple(node.i, node.j, prevNode.i, prevNode.j, startTime));
auto edgeEnd = edgeAgentsCount.upper_bound(std::make_tuple(node.i, node.j, prevNode.i, prevNode.j, endTime));
for (edgeIt; edgeIt != edgeEnd; ++edgeIt) {
int time = std::get<4>(edgeIt->first);
if (agentsCount.find(time) == agentsCount.end()) {
agentsCount[std::get<4>(edgeIt->first)] = 0;
}
agentsCount[std::get<4>(edgeIt->first)] += edgeIt->second;
}
int count = 0, prevTime = startTime - 1, beg = -1;
for (auto it = agentsCount.begin(); it != agentsCount.end(); ++it) {
int time = it->first;
if (time > prevTime + 1 || count == 0 || (!binary && it->second != count)) {
if (beg != -1) {
res.push_back(std::make_pair(beg, count));
}
if (time > prevTime + 1) {
res.push_back(std::make_pair(prevTime + 1, 0));
}
beg = time;
count = it->second;
}
prevTime = time;
}
if (beg != -1) {
res.push_back(std::make_pair(beg, count));
}
if (prevTime < endTime) {
res.push_back(std::make_pair(prevTime + 1, 0));
}
}