def orientation_lane(self): """ Get agent orientation in lane """ pos = self.car.body.position if self.distance_goal < np.pi * RADIUS_INNER: # 2nd part : Turn right theta = Util.angle_direct(Util.normalize(S2_POINT - C2), Util.normalize(pos - C2)) theta = Util.deg_to_rad(theta) h = vec2(-RADIUS_INNER * np.cos(theta) + C2.x, -RADIUS_INNER * np.sin(theta) + C2.y) # orthogonal projection tangent = Util.rotate(Util.normalize(C2 - h), 90.0) # tangent to the circle else: # 1st part : Turn Left theta = Util.angle_direct( Util.normalize(S1_POINT - C1), Util.normalize(self.car.body.position - C1)) theta = Util.deg_to_rad(theta) h = vec2(RADIUS_INNER * np.cos(theta) + C1.x, RADIUS_INNER * np.sin(theta) + C1.y) # orthogonal projection tangent = Util.rotate(Util.normalize(C1 - h), -90.0) # tangent to the circle forward = Util.normalize(self.car.body.GetWorldVector((0, 1))) orientation = Util.angle_indirect(forward, tangent) / 180.0 return orientation
def orientation_lane(self): """ Get agent orientation in lane """ theta = Util.angle_direct( Util.normalize(S_POINT - CENTER_POINT), Util.normalize(self.car.body.position - CENTER_POINT)) theta = Util.deg_to_rad(theta) h = vec2(RADIUS_INNER * np.cos(theta) + CENTER_POINT.x, RADIUS_INNER * np.sin(theta) + CENTER_POINT.y) # orthogonal projection tangent = Util.rotate(Util.normalize(CENTER_POINT - h), -90.0) # tangent to the circle forward = Util.normalize(self.car.body.GetWorldVector((0, 1))) orientation = Util.angle_indirect(forward, tangent) / 180.0 return orientation