Advertisement
ronAmit

Untitled

Mar 13th, 2022
920
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 0.98 KB | None | 0 0
  1.  
  2.     # find the closest mid-lane point to the agent centroid
  3.     dists_to_mid_points = np.linalg.norm(centroid - lanes_mid_points, axis=1)
  4.     i_min_dist_to_mid = dists_to_mid_points.argmin()
  5.     mid_point = lanes_mid_points[i_min_dist_to_mid]
  6.  
  7.     # disqualify the point if there is any left-lane or right-lane point closer to mid_point than the centroid
  8.     min_dist_to_left = np.min(np.linalg.norm(mid_point - lanes_left_points, axis=1))
  9.     min_dist_to_right = np.min(np.linalg.norm(mid_point - lanes_right_points, axis=1))
  10.  
  11.     dist_to_centroid = np.linalg.norm(mid_point - centroid)
  12.  
  13.     if dist_to_centroid > min_dist_to_left or dist_to_centroid > min_dist_to_right:
  14.         if verbose:
  15.             print(f'Agent {agent_name} discarded, dist_to_centroid: {dist_to_centroid},'
  16.                   f' min_dist_to_left: {min_dist_to_left}, min_dist_to_right: {min_dist_to_right}')
  17.         return False
  18.     if verbose:
  19.         print(f'Agent {agent_name} is OK')
  20.     return True
  21.  
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement