-
Notifications
You must be signed in to change notification settings - Fork 0
/
fix_paths_sjg.py
82 lines (77 loc) · 2.97 KB
/
fix_paths_sjg.py
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
import sys
from math import sqrt, pi, atan2, sin, cos
small_set = [2,4,8,9,12,13,15,18]
dirs = []
for i in small_set: # list(range(18 + 1)):
for j in small_set: # list(range(18 + 1)):
dirs += ["data/%s/t%d_min/v%d/" % (sys.argv[1], i, j)]
for d in dirs:
print(d)
for prefix in ["", "b_"]:
agents = dict()
with open(d + prefix + "agent_paths.csv", "r") as og:
with open(d + prefix + "fixed_paths.csv", "w") as fix:
for line in og:
line = line.split(",")
id_ = int(line[0])
gid = int(line[1])
x = float(line[2])
y = float(line[3])
vx = float(line[4])
vy = float(line[5])
r = float(line[6])
t = float(line[7])
if id_ not in agents:
agents[id_] = [
{
"id": id_,
"x": x,
"y": y,
"ori": atan2(vy, vx),
"r": r,
"t": t,
}
]
else:
s = sqrt(vx * vx + vy * vy)
alphaO = .8 + .2 * (1 - s)
if (s < .02): alphaO = 1
nvx = vx / s
nvy = vy / s
# npx = ???
# npy = ???
# score = ???
ordiff = atan2(vy, vx) - agents[id_][-1]["ori"]
if (ordiff > pi):
ordiff -= 2 * pi
if (ordiff < -pi):
ordiff += 2 * pi
ori = (1.0 - alphaO) * ordiff + agents[id_][-1]["ori"]
if (ori > pi):
ori -= 2 * pi
if (ori < -pi):
ori += 2 * pi
agents[id_] += [
{
"id": id_,
"x": x,
"y": y,
"ori": ori,
"r": r,
"t": t,
}
]
last = agents[id_][-1]
fix.write(
"%d,%d,%f,%f,%f,%f,%f,%f\n"
% (
last["id"],
last["id"],
last["x"],
last["y"],
cos(last["ori"]),
sin(last["ori"]),
last["r"],
last["t"],
)
)