github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-04-22 03:00:22 -04:00
Code Issues Packages Projects Releases Wiki Activity

Replaced sqrt(x**2+y**2) with hypot in PathTracking/pure_pursuit/pure_pursuit.py

Browse Source
This commit is contained in:
Guillaume Jacquenot
2019-12-07 22:16:12 +01:00
parent 41bcdf8573
commit e500f65d7f
1 changed files with 3 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
PathTracking/pure_pursuit/pure_pursuit.py
View File

@@ -77,7 +77,7 @@ def calc_distance(state, point_x, point_y):
dx = state.rear_x - point_x
dy = state.rear_y - point_y
return math.sqrt(dx ** 2 + dy ** 2)
return math.hypot(dx, dy)
def calc_target_index(state, cx, cy):
@@ -88,7 +88,7 @@ def calc_target_index(state, cx, cy):
# search nearest point index
dx = [state.rear_x - icx for icx in cx]
dy = [state.rear_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)]
ind = d.index(min(d))
old_nearest_point_index = ind
else:
@@ -110,7 +110,7 @@ def calc_target_index(state, cx, cy):
while Lf > L and (ind + 1) < len(cx):
dx = cx[ind] - state.rear_x
dy = cy[ind] - state.rear_y
L = math.sqrt(dx ** 2 + dy ** 2)
L = math.hypot(dx, dy)
ind += 1
return ind