github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-02-11 04:45:24 -05:00
Code Issues Packages Projects Releases Wiki Activity

Optimize calc_nearest_index

Browse Source
This commit is contained in:
Alexis Paques
2018-07-12 13:26:18 +02:00
parent 8652a4b7d1
commit 8f6a30b924
4 changed files with 16 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
PathTracking/rear_wheel_feedback/rear_wheel_feedback.py
View File

@@ -83,12 +83,14 @@ def calc_nearest_index(state, cx, cy, cyaw):
dx = [state.x - icx for icx in cx]
dy = [state.y - icy for icy in cy]
d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)]
d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]
mind = min(d)
ind = d.index(mind)
mind = math.sqrt(mind)
dxl = cx[ind] - state.x
dyl = cy[ind] - state.y