-
Notifications
You must be signed in to change notification settings - Fork 1
/
controllerUtils.py
85 lines (69 loc) · 2.6 KB
/
controllerUtils.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
83
84
85
# this file contains general functions used by controllers to perceive the track
from numpy import linspace, sin, cos, pi
# used for loading tracks
import pickle
from os import scandir
import os, os.path
#
def getDistanceReadings(car, track, numDistSensors):
percepts = [0] * numDistSensors
for i, n in enumerate(linspace(-pi/2, pi/2, numDistSensors)):
nearest = float('inf')
for l in track.trackLines:
t = intersectsAt((car.x, car.y), (cos(car.dir+n), sin(car.dir+n)), l)
if t > 0 and t < nearest:
nearest = t
percepts[i] = nearest
return percepts
# functions used in line intersection calculation
def mag(v):
return (v[0]**2 + v[1]**2)**0.5
def dot(a, b):
return a[0]*b[0] + a[1]*b[1]
def comp(a, b): # project a onto b (assumes b has unit length)
return dot(a, b)/(b[0]**2 + b[1]**2)
# takes a point (2d), a direction (2d), and a track.Line
def intersectsAt(p, d, l):
# normalize d
ddenom = mag(d)
d = (d[0]/ddenom, d[1]/ddenom)
# get L normal
lX = l.p[0][0] - l.p[1][0]
lY = l.p[0][1] - l.p[1][1]
llength = (lX**2 + lY**2)**0.5
if llength == 0:
# print('bad')
return float('inf')
lN = (-lY/llength, lX/llength)
# calculate t
disp = (l.p[0][0] - p[0], l.p[0][1] - p[1])
denom = dot(d, lN)
if denom == 0:
return float('inf')
t = dot(disp, lN)/denom
# see if t is valid
'''def proj(a, b): # project a onto b
dist = dot(a, b)/(b[0]**2 + b[1]**2)
return (b[0]*dist, b[1]*dist)'''
'''p0p = proj(disp, d)
p1p = proj((disp[0] - lX, disp[1] - lY), d)'''
# p0p = mag(proj((l.p[0][0] - p[0], l.p[0][1] - p[1]), d))
# p1p = mag(proj((l.p[1][0] - p[0], l.p[1][1] - p[1]), d))
p0p = comp((l.p[0][0] - p[0], l.p[0][1] - p[1]), d)
p1p = comp((l.p[1][0] - p[0], l.p[1][1] - p[1]), d)
# print('{} - {} - {}'.format(p0p, t, p1p))
if t >= min(p0p, p1p) and t <= max(p0p, p1p):
return t
else:
return float('inf')
# Load all tracks in a specified path (like "tracks/."). The tqdm argument allows you to track the progress by passing the tqdm function
def load_tracks(path, tqdm=lambda some_list: some_list):
def safe_load(file):
with open(file, 'rb') as f:
meat = pickle.load(f)
return meat
total_tracks = len([name for name in os.listdir(path) if os.path.isfile(os.path.join(path, name))])
all_tracks = [safe_load(track) for track in tqdm(scandir(path), total=total_tracks) if track.is_file()]
for track in all_tracks:
track.updateTrackLines()
return all_tracks