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