def distance_goal(self):
# Starting point
s = None
if self.env_name == 'RaceCircle':
s = vec2(c.x + r, c.y)
elif self.env_name == 'RaceCircle_v2':
s = vec2(c.x - r, c.y)
# Orthogonal projection to the circle
theta = None
if self.env_name == 'RaceCircle':
theta = Util.angle_direct(Util.normalize(s - c),
Util.normalize(self.body.position - c))
elif self.env_name == 'RaceCircle_v2':
theta = Util.angle_indirect(Util.normalize(s - c),
Util.normalize(self.body.position - c))
if theta < 0:
theta = 180.0 + (180.0 + theta)
theta = Util.deg_to_rad(theta)
phi = 2 * np.pi - theta
d2g = r * phi
# Boundary reaching goal
if self.distance and np.abs(d2g - self.distance) >= 10.0:
return 0.
# res = round(np.abs(d2g), 2)
res = np.abs(d2g)
return res
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 ReportFixture(self, fixture, point, normal, fraction):
if fixture.type != 2:
return 1.0
self.hit = True
self.fixtures.append(fixture)
self.points.append(vec2(point))
self.normals.append(vec2(normal))
return 1.0
def ReportFixture(self, fixture, point, normal, fraction):
self.fixture = fixture
self.point = vec2(point)
self.normal = vec2(normal)
self.hit = True # flag to inform raycast hit an object
self.fraction = fraction
# You will get this error: "TypeError: Swig director type mismatch in output value of type 'float32'"
# without returning a value
return fraction
def __init__(self, render=False, fixed_ur_timestep=False, num_agents=1):
debug_homing_simple.xprint(color=PRINT_GREEN, msg='Creating Environment, Physics Setup')
self.render = render
self.fixed_ur_timestep = fixed_ur_timestep
# -------------------- Pygame Setup ----------------------
pygame.init()
self.delta_time = 1.0 / TARGET_FPS # 0.016666
self.fps = TARGET_FPS
self.accumulator = 0
self.screen = None
if self.render:
self.screen = pygame.display.set_mode((SCREEN_WIDTH, SCREEN_HEIGHT), 0, 32)
pygame.display.set_caption('Testbed Homing Simple')
self.clock = pygame.time.Clock()
self.myfont = pygame.font.SysFont("monospace", 15)
# -------------------- Environment and PyBox2d World Setup ----------------------
# Goal positions
goal1 = (100, 100)
goal2 = (SCREEN_WIDTH - goal1[0], SCREEN_HEIGHT - goal1[1]) # 1180, 620
self.goals_pixels = [goal1, goal2]
goal1_vec = pixels_to_world(goal1) # b2Vec2(5,31)
goal2_vec = vec2(SCREEN_WIDTH / PPM - goal1_vec.x, SCREEN_HEIGHT / PPM - goal1_vec.y) # b2Vec2(59,5)
self.goals_vec = [goal1_vec, goal2_vec]
# Create the world
self.world = world(gravity=(0, 0), doSleep=True)
# Border of the map
self.border = Border(screen=self.screen, world=self.world)
# Agents
self.numAgents = num_agents # total numbers of agents in the simulation
self.agents = []
for j in range(num_agents):
start_pos = pixels_to_world(self.goals_pixels[1]) # start from goal 2
toGoal = Util.normalize(pixels_to_world(self.goals_pixels[0]) - start_pos)
forward = vec2(0, 1)
angleDeg = Util.angle_indirect(forward, toGoal)
angle = Util.deg_to_rad(angleDeg)
angle = -angle # start by looking at goal1
a = AgentHomingSimple(screen=self.screen, world=self.world, x=start_pos.x, y=start_pos.y, angle=angle,
radius=1.5, id=j, goals=self.goals_vec, num_agents=num_agents)
self.agents.append(a)
# Affect list of agents to each agent
for agent in self.agents:
agent.agents_list = self.agents
def draw(self):
vertices = [(self.body.transform * v) * PPM
for v in self.fixture.shape.vertices]
vertices = [(v[0], SCREEN_HEIGHT - v[1]) for v in vertices]
pygame.draw.polygon(self.screen, Color.Red, vertices)
p = self.body.GetWorldPoint(
localPoint=(0, self.height)) # upper point of the box
forward = self.body.GetWorldVector(
(0, 1)) # transform.forward of this box
pygame.draw.line(self.screen, Color.White, world_to_pixels(vec2(p)),
world_to_pixels(vec2(p) + forward))
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 __init__(self,
screen=None,
world=None,
x=0,
y=0,
angle=0,
radius=1.25,
screen_height=None,
screen_width=None,
ppm=20.0,
env_name='RaceCircleLeft'):
self.screen = screen
self.world = world
self.screen_height = screen_height
self.screen_width = screen_width
self.ppm = ppm
self.env_name = env_name
self.radius = radius
self.body = self.world.CreateDynamicBody(position=(x, y),
userData=self,
angle=angle)
self.fixture = self.body.CreateCircleFixture(radius=radius,
density=1,
friction=0,
restitution=0)
self.color = Color.Red
# Speed specs
self.max_forward_speed = 100 # 50
self.max_drive_force = 300 # can reach maximum speed in 2.833 s (0-100) # 180
self.turn_torque = 72 # achieved max_angular_velocity of 3.13 (~pi) rad/s in 1.7s
self.speed = 0.
# Proximity Sensors
self.raycast_length = 4.0
self.raycastSafeColor = Color.Green
self.raycastDangerColor = Color.Red
front_middle = vec2(0, 1)
front_right = vec2(np.sqrt(2) / 2, np.sqrt(2) / 2)
front_left = vec2(-np.sqrt(2) / 2, np.sqrt(2) / 2)
self.raycast_vectors = (
front_left, front_middle, front_right
) # Store raycast direction vector for each raycast
self.num_sensors = len(self.raycast_vectors)
self.sensors = np.ones(
self.num_sensors
) * self.raycast_length # store the value of each sensors
self.collision = False
def inside_goal(self):
if self.env_name == 'RaceCircle':
M1 = vec2(np.cos(3 * (np.pi / 12)) * 11 + 18, 18)
M2 = vec2(36, 18)
M3 = vec2(36, np.sin(3 * (np.pi / 12)) * 11 + 18)
M4 = vec2(
np.cos(3 * (np.pi / 12)) * 11 + 18,
np.sin(3 * (np.pi / 12)) * 11 + 18)
pos = self.body.position
if M1.x <= pos.x <= M2.x and M2.y <= pos.y <= M3.y:
return True
else:
return False
elif self.env_name == 'RaceCircle_v2':
M1 = vec2(-np.cos(3 * (np.pi / 12)) * 11 + 18, 18)
M2 = vec2(0, 18)
M3 = vec2(0, np.sin(3 * (np.pi / 12)) * 11 + 18)
M4 = vec2(-np.cos(3 * (np.pi / 12)) * 11 + 18,
np.sin(3 * (np.pi / 12)) * 11 + 18)
pos = self.body.position
if M2.x <= pos.x <= M1.x and M2.y <= pos.y <= M3.y:
return True
else:
return False
else:
return None
def init_car(self, angle=None):
if self.car is not None:
self.car.destroy()
if angle is None:
# XXX: make random switch
# angle = np.random() * math.pi * 2.
p = self.checkpoints[1] - self.checkpoints[0]
angle = math.atan2(p.y, p.x) - math.pi / 2.
self.car = TDCar(
self.world,
position=b2.vec2(self.checkpoints[0].x, self.checkpoints[0].y),
angle=angle,
tire_kwargs=dict(
dimensions=(0.2, 0.8),
max_forward_speed=40.0,
max_backward_speed=-10.0,
max_drive_force=280.,
),
density=0.08,
rays=self.rays,
)
# TODO: make front-wheel / rear drive switch
# self.car.tires[0].max_drive_force = self.car.tires[1].max_drive_force = 0
# self.car.tires[2].max_drive_force = self.car.tires[3].max_drive_force = 0
while (self.checkpoints[self.car.next_checkpoint] -
self.car.body.worldCenter).length < self.checkpoint_radius:
self.car.next_checkpoint += 1
def add_pucks(self):
for i in range(0, len(self.cond['sls'])):
# Give each a unique name
objname = 'o' + str(i + 1)
# Create the body
b = self.world.CreateDynamicBody(
position=(self.cond['sls'][i]['x'], self.cond['sls'][i]['y']),
linearDamping=0.05,
fixedRotation=True,
userData={
'name': objname,
'bodyType': 'dynamic'
})
b.linearVelocity = vec2(self.cond['svs'][i]['x'],
self.cond['svs'][i]['y'])
# Add the the shape 'fixture'
circle = b.CreateCircleFixture(radius=BALL_RADIUS,
density=self.cond['mass'][i],
friction=0.05,
restitution=0.98)
b.mass = self.cond['mass'][i]
# Add it to our list of dynamic objects
self.bodies.append(b)
# Add a named entry in the data for this object
# init self.data by intial condition
self.data = self.initial_data(self.bodies)
def _save_state_timeline(timeline):
"""
Alternative way of saving a state, called as a method of a timeline
"""
shapes = get_shapes(timeline.vehicle)
timeline.vehicle_states.append(shapes)
tracker = vec2(timeline.vehicle.tracker)
timeline.tracker_states.append(tracker)
def update_bodies(self, cond):
for i, objname in enumerate(self.bodies_names):
self.bodies[i].position = (cond['sls'][i]['x'], cond['sls'][i]['y'])
self.bodies[i].linearDamping = 0.05
self.bodies[i].linearVelocity = vec2(cond['svs'][i]['x'], cond['svs'][i]['y'])
self.bodies[i].mass = cond['mass'][i]
self.bodies[i].fixtures[0].density = cond['mass'][i]
def pixels_to_world(p, screen_height, ppm):
"""
Convert pygame pixels coordinates to box2d coordinates
:param p: pygame pixels
:param screen_height: height of the screen
:param ppm: pixel per meter
:return: box2d vector
"""
return vec2(p[0] / ppm, (screen_height - p[1]) / ppm)
def draw(self):
for triangle in self.triangle_list:
triangle.draw()
self.wall.draw()
# self.goal.draw()
self.inner_circle.draw()
# Goal square
M1 = vec2(np.cos(3*(np.pi/12)) * 11 + 18, 18)
M2 = vec2(36, 18)
M3 = vec2(36, np.sin(3*(np.pi/12)) * 11 + 18)
M4 = vec2(np.cos(3*(np.pi/12)) * 11 + 18, np.sin(3*(np.pi/12)) * 11 + 18)
pygame.draw.line(self.screen, Color.Orange, world_to_pixels(M1), world_to_pixels(M2))
pygame.draw.line(self.screen, Color.Orange, world_to_pixels(M2), world_to_pixels(M3))
pygame.draw.line(self.screen, Color.Orange, world_to_pixels(M3), world_to_pixels(M4))
pygame.draw.line(self.screen, Color.Orange, world_to_pixels(M4), world_to_pixels(M1))
def setup(self, training=True, random_agent=False):
# Update the agent's flag
self.training = training
if random_agent: # can't train when random
self.training = False
self.random_agent = random_agent
# Create agent's brain
self.brain = DQN(input_size=self.input_size,
action_size=len(list(Action)),
id=self.id,
training=self.training,
random_agent=self.random_agent,
**homing_simple_hyperparams)
# Initial position : start from goal 2
start_pos = self.goals[1]
self.body.position = start_pos
self.distance = self.distance_goal()
# Initial orientation : start by looking at goal 1
toGoal = Util.normalize(pixels_to_world(self.goals[0]) - start_pos)
forward = vec2(0, 1)
angleDeg = Util.angle_indirect(forward, toGoal)
angle = Util.deg_to_rad(angleDeg)
angle = -angle
self.body.angle = angle
self.orientation = self.orientation_goal()
# Initial state
observation = np.asarray([self.distance, self.orientation])
# observation = np.asarray([self.orientation])
self.state = self.brain.preprocess(observation)
# Goals
self.current_goal_index = 0
# Meeting with Agents
self.elapsed_timestep_meetings = np.zeros(self.num_agents)
self.start_timestep_meetings = np.zeros(self.num_agents)
# Event features
self.goal_reached_count = 0
self.start_time = 0.0
self.start_timestep = 0
self.elapsed_time = 0.00
self.elapsed_timestep = 0
self.prev_shaping = None
self.done = False
debug_homing_simple.printEvent(color=PrintColor.PRINT_CYAN,
agent=self,
event_message="agent is ready")
def reset(self):
self._destroy()
self.world.contactListener_keepref = RaceContactListener()
self.world.contactListener = self.world.contactListener_keepref
observation = []
self.prev_shaping = None
self.car.collision = False
self.goal_reached = False
self.car.speed = 0.
self.timesteps = 0
# Teleport car to initial position
start_pos = vec2(CENTER_POINT.x, CENTER_POINT.y + RADIUS_INNER +
(ROAD_WIDTH / 2)) # starting point
self.car.body.position = start_pos
self.car.body.angle = Util.deg_to_rad(90)
self.car.kill_motion() # reset car velocity
# Distance to goal (debug purpose)
self.d2g = self.distance_goal
# Orientation in lane
self.orientation = self.orientation_lane
observation.append(self.orientation_lane)
# Distance in lane
distance_lane_norm = self.normalize_distance(self.distance_lane,
_min=self.car.radius,
_max=ROAD_WIDTH -
self.car.radius)
observation.append(distance_lane_norm)
# Speed
speed_norm = Util.min_max_normalization_m1_1(
0., _min=0., _max=self.car.max_forward_speed)
observation.append(speed_norm)
# Angular velocity
angular_vel_norm = Util.min_max_normalization_m1_1(0.,
_min=-3.5,
_max=3.5)
observation.append(angular_vel_norm)
# Sensor's value
self.car.read_sensors()
for sensor in self.car.sensors:
normed_sensor = self.normalize_sensors_value(sensor)
observation.append(normed_sensor)
# Go 1 step further to apply the environment reset
self.world.Step(PHYSICS_TIME_STEP, VEL_ITERS, POS_ITERS)
self.world.ClearForces()
state = np.array(observation, dtype=np.float32)
return state
def rotate(vec, angle):
"""
Rotate the vector in degrees. (clockwise base)
-> v = Vector(100, 0)
-> v.rotate(45)
[70.71067811865476, 70.71067811865474]
"""
angle = math.radians(angle)
return vec2((vec.x * math.cos(angle)) - (vec.y * math.sin(angle)),
(vec.y * math.cos(angle)) + (vec.x * math.sin(angle)))
def update_bodies(self, cond):
for i in range(0, len(cond['sls'])):
objname = 'o' + str(i + 1)
self.bodies[i].position = (cond['sls'][i]['x'],
cond['sls'][i]['y'])
self.bodies[i].linearDamping = 0.05
self.bodies[i].linearVelocity = vec2(cond['svs'][i]['x'],
cond['svs'][i]['y'])
self.bodies[i].mass = cond['mass'][i]
self.bodies[i].fixtures[0].density = cond['mass'][i]
def shift_scale_revert(vertices, shift):
"""
Take vertices and shift them, scale to pixel values and revert y coordinate
for pygame drawing. Remember to pass a list of vertices even if one element.
Return vertices as tuples (not vec2 anymore)
"""
for i, vert in enumerate(vertices):
vert = vec2(vert) + shift
vert = [int(pos * PPM) for pos in vert]
vert[1] = SCREEN_HEIGHT - vert[1]
vertices[i] = tuple(vert)
return vertices
def normalize(vector):
"""
Normalize a vector
:param vector: box2d vector
:return: normalized vector
"""
length = vector.length
if length == 0.:
invLength = 0.
else:
invLength = 1.0 / length
return vec2(vector.x * invLength, vector.y * invLength)
def draw(self):
position = self.body.transform * self.fixture.shape.pos * PPM
position = (position[0], SCREEN_HEIGHT - position[1])
pygame.draw.circle(self.screen, self.color, [int(x) for x in position],
int(self.car_radius * PPM))
current_forward_normal = self.body.GetWorldVector((0, 1))
pygame.draw.line(
self.screen, Color.White, world_to_pixels(self.body.worldCenter),
world_to_pixels(self.body.worldCenter +
current_forward_normal * self.car_radius))
# Draw raycasts
for i in range(self.num_sensors):
v = self.body.GetWorldVector(self.raycast_vectors[i])
p1 = self.body.worldCenter + v * self.car_radius
p2 = p1 + v * self.sensors[i] # self.raycast_length
if self.sensors[i] <= self.raycast_length / 2:
ray_color = self.raycastDangerColor
else:
ray_color = self.raycastSafeColor
pygame.draw.line(self.screen, ray_color, world_to_pixels(p1),
world_to_pixels(p2))
# DEBUG gizmo --------------------------------------------------------------------------------------------------
# Starting point
s = None
if self.env_name == 'RaceCircle':
s = vec2(c.x + r, c.y)
pygame.draw.line(self.screen, Color.Magenta, world_to_pixels(c),
world_to_pixels(s + vec2(7, 0)))
elif self.env_name == 'RaceCircle_v2':
s = vec2(c.x - r, c.y)
pygame.draw.line(self.screen, Color.Magenta, world_to_pixels(c),
world_to_pixels(s + vec2(-7, 0)))
# Orthogonal projection to the circle
ph = None
if self.env_name == 'RaceCircle':
theta = Util.angle_direct(Util.normalize(s - c),
Util.normalize(self.body.position - c))
theta = Util.deg_to_rad(theta)
ph = vec2(r * np.cos(theta) + c.x, r * np.sin(theta) + c.y)
elif self.env_name == 'RaceCircle_v2':
theta = Util.angle_direct(Util.normalize(s - c),
Util.normalize(self.body.position - c))
theta = Util.deg_to_rad(theta)
ph = vec2(-r * np.cos(theta) + c.x, -r * np.sin(theta) + c.y)
pygame.draw.line(self.screen, Color.Red, world_to_pixels(c),
world_to_pixels(ph))
# Tangent to the circle
tangent = None
if self.env_name == 'RaceCircle':
tangent = Util.rotate(Util.normalize(c - ph), -90.0)
elif self.env_name == 'RaceCircle_v2':
tangent = Util.rotate(Util.normalize(c - ph), 90.0)
pygame.draw.line(self.screen, Color.Yellow, world_to_pixels(ph),
world_to_pixels(ph + tangent))
def draw_history(history, timelines, index):
screen.fill(bkg_color)
first = timelines[0]
tracker = history.timelines[first].tracker_states[index]
shift = vec2(40, 20) - tracker
drawing_func(history.terrain, shift=shift, color=def_color)
for i, time in enumerate(reversed(timelines)):
#objects = history.timelines[time].vehicle_states[index]
objects = history.get_shapes(index=index, timeline=time)
drawing_func(objects, shift=shift, color=obj_colors[i])
# Update the screen
pygame.display.flip()
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))
def save_state(self, vehicle, timename='timeline'):
"""
Save current state of vehicle bodies. Can also call as a method of a
timeline without the vehicle argument.
Arguments:
vehicle: Object with list of dynamic bodies in vehicle.bodies
timename: Name of the timeline
"""
if timename not in self.timelines:
self.new_timeline(vehicle, timename)
shapes = get_shapes(vehicle)
self.timelines[timename].vehicle_states.append(shapes)
tracker = vec2(vehicle.tracker)
self.timelines[timename].tracker_states.append(tracker)
def orientation_lane(self):
"""
Get agent orientation in lane
"""
# Starting point
s = None
if self.env_name == 'RaceCircle':
s = vec2(c.x + r, c.y)
elif self.env_name == 'RaceCircle_v2':
s = vec2(c.x - r, c.y)
# Orthogonal projection to the circle
ph = None
if self.env_name == 'RaceCircle':
theta = Util.angle_direct(Util.normalize(s - c),
Util.normalize(self.body.position - c))
theta = Util.deg_to_rad(theta)
ph = vec2(r * np.cos(theta) + c.x, r * np.sin(theta) + c.y)
elif self.env_name == 'RaceCircle_v2':
theta = Util.angle_direct(Util.normalize(s - c),
Util.normalize(self.body.position - c))
theta = Util.deg_to_rad(theta)
ph = vec2(-r * np.cos(theta) + c.x, -r * np.sin(theta) + c.y)
# Tangent to the circle
tangent = None
if self.env_name == 'RaceCircle':
tangent = Util.rotate(Util.normalize(c - ph), -90.0)
elif self.env_name == 'RaceCircle_v2':
tangent = Util.rotate(Util.normalize(c - ph), 90.0)
forward = Util.normalize(self.body.GetWorldVector((0, 1)))
orientation = Util.angle_indirect(forward, tangent) / 180.0
# orientation = round(orientation, 2) # only 3 decimals
return orientation
def _gen_rough(self, roughness):
SEG_LENGTH = 15
nn = self.length // SEG_LENGTH
self.n_segments = nn
self.seg_lengths = [0] * nn
self.seg_angles = [0] * nn
self.seg_positions = [0] * nn
prev_pos = vec2(0, 20) # starting coordinates
for i in range(nn):
self.seg_lengths[i] = SEG_LENGTH
angle = random.uniform(-0.1, 0.1) * roughness
self.seg_angles[i] = angle
length = SEG_LENGTH * math.cos(angle)
height = SEG_LENGTH * math.sin(angle)
self.seg_positions[i] = prev_pos + (.5 * length, .5 * height)
prev_pos += (length, height)
self.spawn = (SEG_LENGTH, 30)
self.generated = True
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 process(self):
for (entity,
(physics,
explodable)) in self.world.get_components(Physics, Explodable):
if self._should_explode(explodable):
hit_already = set()
for i in range(NUM_RAYS):
angle = math.radians((i / NUM_RAYS) * 360)
ray_dir = vec2(math.sin(angle), math.cos(angle))
bomber = self.world.component_for_entity(
explodable.planter, Bomber)
if bomber.bombrange == 'lg':
BLAST_RADIUS = 3
else:
BLAST_RADIUS = 2
ray_end = physics.body.position + BLAST_RADIUS * ray_dir
callback = RayCastClosestCallback()
self.world.pworld.RayCast(callback, physics.body.position,
ray_end)
if callback.fixture and callback.fixture.body.userData not in hit_already:
hit_already.add(callback.fixture.body.userData)
self.world.msg_bus.add(
DamageMessage(entity,
callback.fixture.body.userData,
bomber.damage))
# explosion_image = pygame.image.load("assets/explosion.png")
# explosion = self.world.create_entity()
# explosion_body = self.world.pworld.CreateStaticBody(position=physics.body.position)
# self.world.add_component(explosion, Renderable(image=explosion_image))
# self.world.add_component(explosion, Physics(body=explosion_body))
x, y = physics.body.position
x *= self.world.PPM
y *= self.world.PPM
y = self.world.RESOLUTION[1] - y
bomber = self.world.component_for_entity(
explodable.planter, Bomber)
explosion_anim.run((x, y), self.screen, bomber.bombrange)
bomber.used -= 1
self.world.to_delete.add(entity)
def _get_in_range(self, ent, elem_type):
"""Return collection of elems of elem_type close to enemy"""
collection = set()
physics = self.world.component_for_entity(ent, Physics)
NUM_RAYS = 64
RADIUS = 5
for i in range(NUM_RAYS):
angle = math.radians((i / NUM_RAYS) * 360)
ray_dir = vec2(math.sin(angle), math.cos(angle))
ray_end = physics.body.position + RADIUS * ray_dir
callback = RayCastClosestCallback()
self.world.pworld.RayCast(callback, physics.body.position,
ray_end)
if callback.fixture:
try:
visible_ent = callback.fixture.body.userData
if self.world.has_component(visible_ent, elem_type):
collection.add(visible_ent)
except KeyError:
pass
return collection
TIME_STEP=1.0/TARGET_FPS
# --- pybox2d world setup ---
# Create the world
world=world(gravity=(0,-10),doSleep=True)
# And a static body to hold the ground shape
ground_body=world.CreateStaticBody(
position=(0,0),
shapes=polygonShape(box=(70,1)),
)
for idx, poly in enumerate(polys):
print idx
vecs = [vec2(p[0], HT-p[1]) / PPM for p in poly]
mn = sum(vecs, vec2(0,0)) / len(vecs)
if mn[1] > 17:
continue
try:
ps = polygonShape(vertices=[1.02 * (v-mn) for v in vecs])
except:
vecs.reverse()
try:
ps = polygonShape(vertices=[v-mn for v in vecs])
except:
continue
body=world.CreateDynamicBody(position=mn, angle=0, damping=0.01)
body.CreatePolygonFixture(shape=ps, density=0.1, friction=10.0)
print body.massData
def __init__(self, time):
super(Move, self).__init__(time)
self.fv = b2.vec2(random.randrange(-45, 45), random.randrange(-45, 45))