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 gizmo(self):
pygame.draw.line(
self.screen, Color.Yellow,
world_to_pixels(HALFWAY, SCREEN_HEIGHT, PPM),
world_to_pixels(HALFWAY - vec2(ROAD_WIDTH, 0), SCREEN_HEIGHT, PPM))
if self.distance_goal < np.pi * RADIUS_INNER:
# 2nd part : Turn right
pygame.draw.line(self.screen, Color.Magenta,
world_to_pixels(C2, SCREEN_HEIGHT, PPM),
world_to_pixels(S2_POINT, SCREEN_HEIGHT, PPM))
theta = Util.angle_direct(
Util.normalize(S2_POINT - C2),
Util.normalize(self.car.body.position - 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
pygame.draw.line(self.screen, Color.Red,
world_to_pixels(C2, SCREEN_HEIGHT, PPM),
world_to_pixels(h, SCREEN_HEIGHT, PPM))
tangent = Util.rotate(Util.normalize(C2 - h),
90.0) # tangent to the circle
pygame.draw.line(self.screen, Color.Yellow,
world_to_pixels(h, SCREEN_HEIGHT, PPM),
world_to_pixels(h + tangent, SCREEN_HEIGHT, PPM))
else:
# 1st part : Turn Left
pygame.draw.line(self.screen, Color.Magenta,
world_to_pixels(C1, SCREEN_HEIGHT, PPM),
world_to_pixels(S1_POINT, SCREEN_HEIGHT, PPM))
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
pygame.draw.line(self.screen, Color.Red,
world_to_pixels(C1, SCREEN_HEIGHT, PPM),
world_to_pixels(h, SCREEN_HEIGHT, PPM))
tangent = Util.rotate(Util.normalize(C1 - h),
-90.0) # tangent to the circle
pygame.draw.line(self.screen, Color.Yellow,
world_to_pixels(h, SCREEN_HEIGHT, PPM),
world_to_pixels(h + tangent, SCREEN_HEIGHT, PPM))
def distance_goal(self):
theta = Util.angle_direct(
Util.normalize(S_POINT - CENTER_POINT),
Util.normalize(self.car.body.position - CENTER_POINT))
if theta < 0:
theta = 180.0 + (180.0 + theta)
theta = Util.deg_to_rad(theta)
phi = 2 * np.pi - theta
d2g = RADIUS_INNER * phi
if self.inside_goal:
return 0.
d2g = np.abs(d2g)
return d2g
def distance_goal(self):
pos = self.car.body.position
if (pos.y < HALFWAY.y
and pos.x < HALFWAY.x) or (pos.x < HALFWAY.x - ROAD_WIDTH):
# 2nd part : Turn Right
theta = Util.angle_direct(Util.normalize(pos - C2),
Util.normalize(S2_POINT - C2))
else:
# 1st part : Turn Left
theta = Util.angle_direct(Util.normalize(S1_POINT - C1),
Util.normalize(pos - C1))
if theta < 0:
theta = 180.0 + (180.0 + theta)
theta = Util.deg_to_rad(theta)
phi = 2 * np.pi - theta
d2g = RADIUS_INNER * phi
if self.inside_goal:
return 0.
d2g = np.abs(d2g)
return d2g
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
def gizmo(self):
pygame.draw.line(
self.screen, Color.Magenta,
world_to_pixels(CENTER_POINT, SCREEN_HEIGHT, PPM),
world_to_pixels(S_POINT + vec2(ROAD_WIDTH, 0), SCREEN_HEIGHT, PPM))
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
pygame.draw.line(self.screen, Color.Red,
world_to_pixels(CENTER_POINT, SCREEN_HEIGHT, PPM),
world_to_pixels(h, SCREEN_HEIGHT, PPM))
tangent = Util.rotate(Util.normalize(CENTER_POINT - h),
-90.0) # tangent to the circle
pygame.draw.line(self.screen, Color.Yellow,
world_to_pixels(h, SCREEN_HEIGHT, PPM),
world_to_pixels(h + tangent, SCREEN_HEIGHT, PPM))