Beispiel #1
0
    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
Beispiel #2
0
    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
Beispiel #3
0
 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
Beispiel #4
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
Beispiel #6
0
    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))
Beispiel #7
0
    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))
Beispiel #8
0
    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
Beispiel #9
0
    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
Beispiel #10
0
    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
Beispiel #11
0
 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)
Beispiel #13
0
    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]
Beispiel #14
0
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)
Beispiel #15
0
    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")
Beispiel #17
0
    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
Beispiel #18
0
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)))
Beispiel #19
0
 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
Beispiel #21
0
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)
Beispiel #22
0
    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()
Beispiel #24
0
    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)
Beispiel #26
0
    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
Beispiel #28
0
    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
Beispiel #29
0
    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)
Beispiel #30
0
 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
Beispiel #32
0
 def __init__(self, time):
     super(Move, self).__init__(time)
     self.fv = b2.vec2(random.randrange(-45, 45), random.randrange(-45, 45))