예제 #1
0
    def testLength(self):
        self.assertAlmostEqual(length(Vector(0.0, 0.0)), 0.0, geom_places)
        self.assertAlmostEqual(length(Vector(1.0, 1.0)), 2**0.5, geom_places)

        v = normalize(Vector(0.23, 1.45))
        self.assertAlmostEqual(length(v * 23.145), 23.145)
        self.assertAlmostEqual(length(v / 23.145), 1.0/23.145)
예제 #2
0
    def testLength(self):
        self.assertAlmostEqual(length(Vector(0.0, 0.0)), 0.0, geom_places)
        self.assertAlmostEqual(length(Vector(1.0, 1.0)), 2**0.5, geom_places)

        v = normalize(Vector(0.23, 1.45))
        self.assertAlmostEqual(length(v * 23.145), 23.145)
        self.assertAlmostEqual(length(v / 23.145), 1.0 / 23.145)
예제 #3
0
 def point_is_visible(self, p, obstacles):
     if not self.point_in_fov(p):
         return False
     try:
         ray = Ray(self.pos, p - self.pos)
     except NormalizationError:
         ray = Ray(self.pos, Vector(1.0, 0.0))
     i = first_intersection(ray, obstacles)
     return (i is None) or (length(i - self.pos) > length(p - self.pos))
예제 #4
0
    def update_vel(self, delta_time, desired_vel):
        dv = desired_vel - self.vel
        if length(dv) < self.max_accel * delta_time:
            self.vel = desired_vel
        else:
            self.vel += normalize(dv) * self.max_accel * delta_time

        if length(self.vel) > self.max_vel:
            self.vel = self.max_vel * normalize(self.vel)
예제 #5
0
    def calc_desired_velocity(self, bots, obstacles, targets):
        vel = self.vel
        if self.movement != Movement.Accel:
            vel = Vector(0, 0)
        for inter in bots:
            if (not KNOW_BOT_POSITIONS) and dist(inter.real.pos, self.pos) > self.max_sensing_distance:
                continue
            force = -potential.gradient(potential.morse(r0=2 * BOT_RADIUS, k=2.5, a=4.0),
                                        dist(inter.real.pos, self.pos),
                                        self.pos - inter.real.pos,
                                        self.radius + inter.virtual.radius)
            vel += _FORCE_SENSITIVITY * force

        for target in targets:
            force = -potential.gradient(potential.linear(k=-2.0),
                                        dist(target, self.pos),
                                        target - self.pos,
                                        0)
            vel += _FORCE_SENSITIVITY * force

        for obstacle in obstacles:
            if obstacle.distance(self.pos) <= self.max_sensing_distance:
                force = -potential.gradient(potential.inverse_quadratic(k=1.0),
                                            obstacle.distance(self.pos),
                                            obstacle.repulsion_dir(self.pos),
                                            OBSTACLE_CLEARANCE + self.radius)
                vel += _FORCE_SENSITIVITY * force

        if self.movement == Movement.Dir:
            if length(vel) > 0:
                vel = normalize(vel)
        return vel
예제 #6
0
    def update_vel(self, delta_time, desired_vel):
        ang = signed_angle(self.dir, desired_vel)
        if abs(ang) < MIN_ROTATION_ANGLE:
            ang = 0.0
        ang = ROTATION_GAIN * ang
        abs_vel = min(length(desired_vel), self.max_vel)

        # don't move forward when doing a sharp turn;
        # this seems sensible and also prevents bots from
        # slamming into obstacles when trying to move away
        #
        # !this only makes sense with ROTATION_GAIN = 1.0!
        abs_vel *= cos(signed_angle(desired_vel, self.dir))
        if abs_vel < 0:
            abs_vel = 0

        # these are the velocities wheels would get
        # if they didn't have to accelerate smoothly
        target_lvel = abs_vel - 0.5 * self.width * ang
        target_rvel = abs_vel + 0.5 * self.width * ang
        #self.lvel = target_lvel
        #self.rvel = target_rvel
        if abs(self.lvel - target_lvel) < delta_time * BOT_ACCEL_CAP:
            self.lvel = target_lvel
        else:
            self.lvel += copysign(BOT_ACCEL_CAP, target_lvel - self.lvel) * delta_time
        if abs(self.rvel - target_rvel) < delta_time * BOT_ACCEL_CAP:
            self.rvel = target_rvel
        else:
            self.rvel += copysign(BOT_ACCEL_CAP, target_rvel - self.rvel) * delta_time
        # cap velocity
        v = max(self.lvel, self.rvel)
        if v > self.max_vel:
            self.lvel *= self.max_vel / v
            self.rvel *= self.max_vel / v
예제 #7
0
 def draw(self, screen, field):
     if graphics.DRAW_SENSOR_RAYS:
         if ROTATE_SENSORS:
             ang = self.get_heading_angle()
         else:
             ang = 0.0
         for s, d in zip(self.sensors, self.distances):
             r = s.get_ray(self.pos, ang)
             graphics.draw_line(screen, field, (115, 115, 200),
                                r.orig,
                                r.orig + r.dir * d,
                                1)
     if COLLISION_CHECK and self.collision:
         # draw small purple circle indicating collision state
         graphics.draw_circle(screen, field, (255, 0, 255),
                              self.pos,
                              0.5 * BOT_RADIUS)
         # draw projections of virtual velocity onto sensor rays
         if graphics.DRAW_SENSOR_RAYS:
             vel_ang = signed_angle(self.virtual_vel_before_check, Vector(0.0, 1.0))
             abs_vel = length(self.virtual_vel_before_check)
             if ROTATE_SENSORS:
                 ang = self.get_heading_angle()
             else:
                 ang = 0.0
             for s in self.sensors:
                 r = s.get_ray(self.pos, ang)
                 c = cos(signed_angle(self.virtual_vel_before_check, r.dir))
                 proj = c * abs_vel;
                 if proj < 0:
                     continue
                 graphics.draw_line(screen, field, (115, 200, 200),
                                    r.orig,
                                    r.orig + r.dir * proj,
                                    2)
         v = Vector(0.0, 0.0)
         try:
             v = 0.5 * normalize(self.virtual_vel_before_check)
         except ZeroDivisionError:
             pass
         # draw virtual velocity vector that was picked before collision check
         graphics.draw_line(screen, field, (0, 200, 0),
                            self.pos,
                            self.pos + v,
                            1)
예제 #8
0
 def update_state(self, delta_time, movement_sigma):
     if self.pos_fun is not None:
         self.time += delta_time
         self.pos = self.pos_fun(self.time)
         v = self.vel_fun(self.time, delta_time)
         self.vel = length(v)
         try:
             self.dir = normalize(v)
         except ZeroDivisionError:
             # keep the previous value of dir
             pass
     else: # bot is user-controlled
         if abs(self.vel) > self.max_vel:
             self.vel = copysign(self.max_vel, self.vel)
         if self.vel < 0:
             self.vel = 0
         self.pos += self.dir * self.vel * delta_time
     self.shape.center = self.pos
예제 #9
0
 def draw(self, screen, field):
     if graphics.DRAW_SENSOR_RAYS:
         if ROTATE_SENSORS:
             ang = self.get_heading_angle()
         else:
             ang = 0.0
         for s, d in zip(self.sensors, self.distances):
             r = s.get_ray(self.pos, ang)
             graphics.draw_line(screen, field, (115, 115, 200), r.orig,
                                r.orig + r.dir * d, 1)
     if COLLISION_CHECK and self.collision:
         # draw small purple circle indicating collision state
         graphics.draw_circle(screen, field, (255, 0, 255), self.pos,
                              0.5 * BOT_RADIUS)
         # draw projections of virtual velocity onto sensor rays
         if graphics.DRAW_SENSOR_RAYS:
             vel_ang = signed_angle(self.virtual_vel_before_check,
                                    Vector(0.0, 1.0))
             abs_vel = length(self.virtual_vel_before_check)
             if ROTATE_SENSORS:
                 ang = self.get_heading_angle()
             else:
                 ang = 0.0
             for s in self.sensors:
                 r = s.get_ray(self.pos, ang)
                 c = cos(signed_angle(self.virtual_vel_before_check, r.dir))
                 proj = c * abs_vel
                 if proj < 0:
                     continue
                 graphics.draw_line(screen, field, (115, 200, 200), r.orig,
                                    r.orig + r.dir * proj, 2)
         v = Vector(0.0, 0.0)
         try:
             v = 0.5 * normalize(self.virtual_vel_before_check)
         except ZeroDivisionError:
             pass
         # draw virtual velocity vector that was picked before collision check
         graphics.draw_line(screen, field, (0, 200, 0), self.pos,
                            self.pos + v, 1)
예제 #10
0
 def point_in_fov(self, p):
     if length(p - self.pos) > self.visibility_radius:
         return False
     d = p - self.pos
     ang = atan2(cross(self.real_dir, d), dot(self.real_dir, d))
     return -0.5 * self.visibility_fov < ang < 0.5 * self.visibility_fov
예제 #11
0
    def calc_desired_velocity(self, bots, obstacles, targets):
        vel = self.real_vel
        self.collision_delta_time = length(vel) / BOT_ACCEL_CAP
        if self.movement != Movement.Accel:
            vel = Vector(0, 0)
        for inter in bots:
            if (not KNOW_BOT_POSITIONS) and dist(inter.real.pos, self.pos) > self.max_sensing_distance:
                continue
            force = -potential.gradient(potential.morse(r0=2 * BOT_RADIUS, k=2.5, a=4.0),
                                        dist(inter.real.pos, self.pos),
                                        self.pos - inter.real.pos,
                                        self.radius + inter.virtual.radius)
            vel += _FORCE_SENSITIVITY * force

        for target in targets:
            force = -potential.gradient(potential.linear(k=-10.0),
                                        dist(target, self.pos),
                                        target - self.pos,
                                        0)
            vel += _FORCE_SENSITIVITY * force


        if ROTATE_SENSORS:
            ang = self.get_heading_angle()
        else:
            ang = 0.0
        self.distances = []
        for s in self.sensors:
            d = s.get_distance(self.pos, ang, obstacles)
            self.distances.append(d)
            if d < self.max_sensing_distance:
                force = -potential.gradient(potential.inverse_quadratic(k=1.5),
                                            d,
                                            -s.get_ray(self.pos, ang).dir,
                                            OBSTACLE_CLEARANCE)
                vel += _FORCE_SENSITIVITY * force * self.obstacle_force_coeff

        if self.movement == Movement.Dir:
            if length(vel) > 0:
                vel = normalize(vel)

        # TODO: get max_vel from corresponding Model
        if length(vel) > BOT_VEL_CAP:
            vel = normalize(vel) * BOT_VEL_CAP

        self.virtual_vel_before_check = vel
        if COLLISION_CHECK:
            # check for possible collisions:
            # stop moving forward if an obstacle is nearby
            collision = False
            abs_vel = length(vel)
            if ROTATE_SENSORS:
                ang = self.get_heading_angle()
            else:
                ang = 0.0
            for cur_d, s in zip(self.distances, self.sensors):
                if cur_d == s.max_distance:
                    continue
                r = s.get_ray(self.pos, ang)
                c = cos(signed_angle(vel, r.dir))
                new_d = cur_d - c * abs_vel * self.collision_delta_time
                if new_d < CRITICAL_DIST and new_d < cur_d:
                    collision = True
                    break
            if not collision:
                # separate check for possible collision with another bot
                for inter in bots:
                    # don't count collsions with self
                    if inter.virtual is self:
                        continue
                    new_d = dist(inter.real.pos + self.collision_delta_time * inter.real.vel,
                                 self.pos + self.collision_delta_time * vel)
                    new_d -= (self.radius + inter.real.radius)
                    cur_d = dist(inter.real.pos, self.pos)
                    cur_d -= (self.radius + inter.real.radius)
                    if d <= CRITICAL_DIST and new_d < cur_d:
                        collision = True
                        break
            if collision:
                vel = normalize(vel) * CRITICAL_VEL
                self.collision = True
            else:
                self.collision = False

        return vel
예제 #12
0
    def calc_desired_velocity(self, bots, obstacles, targets):
        vel = self.real_vel
        self.collision_delta_time = length(vel) / BOT_ACCEL_CAP
        if self.movement != Movement.Accel:
            vel = Vector(0, 0)
        for inter in bots:
            if (not KNOW_BOT_POSITIONS) and dist(
                    inter.real.pos, self.pos) > self.max_sensing_distance:
                continue
            force = -potential.gradient(
                potential.morse(r0=2 * BOT_RADIUS, k=2.5, a=4.0),
                dist(inter.real.pos, self.pos), self.pos - inter.real.pos,
                self.radius + inter.virtual.radius)
            vel += _FORCE_SENSITIVITY * force

        for target in targets:
            force = -potential.gradient(potential.linear(
                k=-10.0), dist(target, self.pos), target - self.pos, 0)
            vel += _FORCE_SENSITIVITY * force

        if ROTATE_SENSORS:
            ang = self.get_heading_angle()
        else:
            ang = 0.0
        self.distances = []
        for s in self.sensors:
            d = s.get_distance(self.pos, ang, obstacles)
            self.distances.append(d)
            if d < self.max_sensing_distance:
                force = -potential.gradient(potential.inverse_quadratic(k=1.5),
                                            d, -s.get_ray(self.pos, ang).dir,
                                            OBSTACLE_CLEARANCE)
                vel += _FORCE_SENSITIVITY * force * self.obstacle_force_coeff

        if self.movement == Movement.Dir:
            if length(vel) > 0:
                vel = normalize(vel)

        # TODO: get max_vel from corresponding Model
        if length(vel) > BOT_VEL_CAP:
            vel = normalize(vel) * BOT_VEL_CAP

        self.virtual_vel_before_check = vel
        if COLLISION_CHECK:
            # check for possible collisions:
            # stop moving forward if an obstacle is nearby
            collision = False
            abs_vel = length(vel)
            if ROTATE_SENSORS:
                ang = self.get_heading_angle()
            else:
                ang = 0.0
            for cur_d, s in zip(self.distances, self.sensors):
                if cur_d == s.max_distance:
                    continue
                r = s.get_ray(self.pos, ang)
                c = cos(signed_angle(vel, r.dir))
                new_d = cur_d - c * abs_vel * self.collision_delta_time
                if new_d < CRITICAL_DIST and new_d < cur_d:
                    collision = True
                    break
            if not collision:
                # separate check for possible collision with another bot
                for inter in bots:
                    # don't count collsions with self
                    if inter.virtual is self:
                        continue
                    new_d = dist(
                        inter.real.pos +
                        self.collision_delta_time * inter.real.vel,
                        self.pos + self.collision_delta_time * vel)
                    new_d -= (self.radius + inter.real.radius)
                    cur_d = dist(inter.real.pos, self.pos)
                    cur_d -= (self.radius + inter.real.radius)
                    if d <= CRITICAL_DIST and new_d < cur_d:
                        collision = True
                        break
            if collision:
                vel = normalize(vel) * CRITICAL_VEL
                self.collision = True
            else:
                self.collision = False

        return vel