示例#1
0
    def think(self, dt, view, debugsurface):
        if not self.goal:  # have no goal?
            return
        # debugsurface.fill((255, 0, 0, 100))
        ccourse = False
        last = self.position
        for i in xrange(self.waypoint_len()):
            for o in view.pedestrians:
                if linesegdist2(last, self.waypoint(i).position, o.position) <= (self.radius + o.radius) ** 2:
                    ccourse = True
                    break
            last = self.waypoint(i).position
            if ccourse:
                break

        gb = graphbuilder.GraphBuilder(self.speed, self.turningspeed)

        safe_distance = self.radius + 1  # some margin is nice
        start = gb.node(self.position, self.angle)
        end = gb.node(self.goal, None)

        if graphbuilder.free_path(self.position, self.goal, view, safe_distance):
            gb.connect(self.position, self.goal)
        else:
            for i in xrange(self.NODES / 2):  # global planning
                newpos = Vec2d(640 * random.random(), 480 * random.random())
                for pos in gb.positions():
                    if graphbuilder.free_path(Vec2d(*pos), newpos, view, safe_distance):
                        gb.connect(pos, newpos)
                        pygame.draw.aaline(debugsurface, (0, 255, 0, 255), pos, newpos)

            for i in xrange(self.NODES - self.NODES / 2):  # some extra local points to handle the crowd
                newpos = self.position + Vec2d(
                    (2 * random.random() - 1) * self.view_range, (2 * random.random() - 1) * self.view_range
                )
                for pos in gb.positions():
                    if graphbuilder.free_path(Vec2d(*pos), newpos, view, safe_distance):
                        gb.connect(pos, newpos)
                        pygame.draw.aaline(debugsurface, (0, 255, 0, 255), pos, newpos)

            for p in gb.positions():
                pygame.draw.circle(debugsurface, (0, 0, 0), map(int, p), 2, 0)

        nodes = gb.all_nodes()
        result = astar.shortest_path(start, end, nodes)
        if result.success:
            result.path = [tuple(self.position)]
            for i in result.indices:
                if result.path[-1] != nodes[i].position:
                    result.path.append(nodes[i].position)

        if ccourse is True:
            self.waypoint_clear()

        if result.success is True and (self.waypoint_len() == 0 or result.total_cost < self.cdist):
            self.waypoint_clear()
            for p in result.path:
                self.waypoint_push(Vec2d(*p))
            self.cdist = result.total_cost
示例#2
0
文件: maze.py 项目: freider/keiro
    def init(self):
        if self.parameter is None:
            self.parameter = 10

        self.agent.position = Vec2d(200, 200)
        self.agent.goal = Vec2d(400, 200)
        self.agent.angle = 0

        shapes = [
            [
                Vec2d(100, 100),
                Vec2d(100, 380),
                Vec2d(500, 380),
                Vec2d(500, 100),
                Vec2d(100, 100),
            ],
            [
                Vec2d(300, 100),
                Vec2d(300, 250),
            ],
            [
                Vec2d(200, 250),
                Vec2d(400, 250),
            ]
        ]

        for shape in shapes:
            last = shape[0]
            for point in shape:
                self.world.add_obstacle(obstacle.Line(last, point))
                last = point

        for m in xrange(self.parameter):
            good = False
            u = RandomWalkingAvoider(random_seed=self.random.random())

            # generate random positions for pedestrians
            # that are not inside obstacles...
            while not good:
                init_position = Vec2d(
                    self.random.randrange(100 + u.radius + 1, 500 - u.radius - 1),
                    self.random.randrange(100 + u.radius + 1, 380 - u.radius - 1),
                )
                good = init_position.distance_to(self.agent.position) > 20
                for shape in shapes:
                    last = shape[0]
                    for point in shape:
                        if linesegdist2(
                            last, point,
                            init_position
                        ) < u.radius ** 2:
                            good = False
                            break
                        last = point
                    if not good:
                        break

            u.position = init_position
            self.world.add_unit(u)
示例#3
0
文件: arty.py 项目: pombredanne/keiro
 def freeprob_turn(self, position, a1, a2, view, starttime):
     """ Probability that a turn started at starttime at position between angles a1 and a2 will be collision free """
     dur = abs(angle_diff(a1, a2)) / self.turningspeed
     for p in view.pedestrians:
         p1 = p.position
         p2 = self.future_position(p, dur)  # extrapolate
         if linesegdist2(p1, p2, position) < (self.radius + p.radius + self.FREEMARGIN) ** 2:
             self.freeprob_fail_pedestrian = p
             return 0
     return 1
示例#4
0
文件: arty.py 项目: freider/keiro
    def turn_safeness(self, position, a1, a2, view, starttime):
        """ Probability that a turn is collision free

            Started at starttime at position between
            angles a1 and a2.
        """
        dur = abs(angle_diff(a1, a2)) / self.turningspeed
        for p in view.pedestrians:
            # extrapolate to start of turn
            p1 = self.future_position(p, starttime)
            # extrapolate to end of turn
            p2 = self.future_position(p, starttime + dur)
            safedist2 = (self.radius + p.radius + self.FREEMARGIN) ** 2
            if linesegdist2(p1, p2, position) < safedist2:
                self.safeness_fail_pedestrian = p
                return 0
        return 1
示例#5
0
    def think(self, dt, view, debugsurface):        
        if not self.goal: #have no goal?
            return

        for p in view.pedestrians:
            pygame.draw.circle(debugsurface, pygame.Color("green"), map(int, p.position), int(p.radius+2), 2)
            #pygame.draw.aaline(debugsurface, pygame.Color("yellow"), map(int, self.position), map(int, p.position))

        #generating static voronoi points (only performed once)
        if len(self.staticVPoints) == 0:
            for o in view.obstacles:
                already = False
                for p in self.staticVPoints:
                    if o.p1 == p:
                        already = True
                        break
                if not already:
                    self.staticVPoints.append(o.p1)
                already = False
                for p in self.staticVPoints:
                    if o.p2 == p:
                        already = True
                        break
                if not already:
                    self.staticVPoints.append(o.p2)

                obstacle_length = o.p1.distance_to(o.p2)
                num_midpoints = int(obstacle_length/(7.5*self.radius))
                for m in xrange(num_midpoints):
                    self.staticVPoints.append(o.p1+(o.p2-o.p1)*(m+1)/(num_midpoints+1))

        #debugsurface.fill((255, 0, 0, 100))
        ccourse = False
        last = self.position
        for i in xrange(self.waypoint_len()):
            for o in view.pedestrians:
                if linesegdist2(last, self.waypoint(i).position, o.position) <= (self.radius + o.radius + self.FREEMARGIN)**2:
                    ccourse = True
                    break
            last = self.waypoint(i).position
            if ccourse: break

        gb = graphbuilder.SimpleGraphBuilder()
        
        safe_distance = self.radius + self.FREEMARGIN #some margin is nice
        
        diff = self.goal - self.position # in order to ignore obstacles just beyond the goal
        if graphbuilder.free_path(self.position, self.goal - diff.norm()*self.radius, view, safe_distance):
            gb.connect(self.position, self.goal)
        else:
            vPoints = [self.position, self.goal]
            for o in view.pedestrians:
                vPoints.append(o.position)
            for vP in self.staticVPoints:
                #if(self.position.distance_to(vP) <= self.view_range):
                    vPoints.append(vP)
            
            if len(vPoints):
                vD = computeVoronoiDiagram(vPoints)
                vDNodes = vD[0]
                vDEdges = vD[2]
                for e in vDEdges: # build the voronoi map
                    if((e[1] != -1) and (e[2] != -1)): #indicates line to infinity
                        if graphbuilder.free_path_obstacles_only(Vec2d(*vDNodes[e[1]]), Vec2d(*vDNodes[e[2]]), view, safe_distance):
                            gb.connect(vDNodes[e[1]], vDNodes[e[2]])
                            pygame.draw.aaline(debugsurface, (0,255,0,255), vDNodes[e[1]], vDNodes[e[2]])

                for pos in gb.positions(): # get off the voronoi map
                    diff = Vec2d(*pos) - self.position
                    if(graphbuilder.free_path(self.position + diff.norm()*self.radius, Vec2d(*pos), view, safe_distance) and (diff.length() <= self.view_range)):
                        #so agent is not blocked by pedestrian next to it
                        gb.connect(self.position, pos)
                        pygame.draw.aaline(debugsurface, (0,255,0,255), self.position, pos)
                    diff = self.goal - Vec2d(*pos)
                    if(graphbuilder.free_path(self.goal, Vec2d(*pos), view, 0) and (diff.length() <= self.view_range)):
                        gb.connect(self.goal, pos)
                        pygame.draw.aaline(debugsurface, (0,255,0,255), self.goal, pos)

        
        start = gb.node(self.position, self.angle)
        end = gb.node(self.goal, None)
        nodes = gb.all_nodes()
        for p in gb.positions():
            pygame.draw.circle(debugsurface, (0,0,0), map(int, p), 2, 0)

        result = astar.shortest_path(start, end, nodes)
        if result.success:
            result.path = [tuple(self.position)]
            for i in result.indices:
                if result.path[-1] != nodes[i].position: # gets rid of duplicate pathpoints (?)
                    result.path.append(nodes[i].position)
    
        if ccourse is True:
            self.waypoint_clear()
            
        if result.success is True and (self.waypoint_len() == 0 or result.total_cost < self.cdist):
            self.waypoint_clear()
            for p in result.path:
                self.waypoint_push(Vec2d(*p))
            self.cdist = result.total_cost
示例#6
0
文件: roadmap.py 项目: freider/keiro
    def think(self, dt, view, debugsurface):
        if not self.goal:  # have no goal?
            return
        #debugsurface.fill((255, 0, 0, 100))
        ccourse = False
        last_pos = self.position
        for i in xrange(self.waypoint_len()):
            for pedestrian in view.pedestrians:
                safe_dist2 = (self.radius +
                              pedestrian.radius +
                              self.FREEMARGIN) ** 2
                closest_dist2 = linesegdist2(
                    last_pos,
                    self.waypoint(i).position,
                    pedestrian.position
                )
                if closest_dist2 <= safe_dist2:
                    ccourse = True
                    break
            last_pos = self.waypoint(i).position
            if ccourse:
                break

        gb = graphbuilder.GraphBuilder(self.speed, self.turningspeed)

        safe_distance = self.radius + self.FREEMARGIN  # some margin is nice
        start = gb.node(self.position, self.angle)
        end = gb.node(self.goal, None)

        if graphbuilder.free_path(
            self.position,
            self.goal,
            view,
            safe_distance
        ):
            gb.connect(self.position, self.goal)
        else:
            # using half of the points for global planning
            world_size = view.world_bounds[1::2]
            for i in xrange(self.NODES / 2):
                newpos = Vec2d(
                    world_size[0] * self.random.random(),
                    world_size[1] * self.random.random()
                )
                for pos in gb.positions():
                    if graphbuilder.free_path(
                        Vec2d(*pos), newpos, view, safe_distance
                    ):
                        gb.connect(pos, newpos)
                        debugsurface.line(pos, newpos, "green")

            # some extra local points (within view range) to handle the crowd
            for i in xrange(self.NODES - self.NODES / 2):
                random_offset = Vec2d(
                    (2 * self.random.random() - 1) * self.view_range,
                    (2 * self.random.random() - 1) * self.view_range
                )
                newpos = self.position + random_offset
                for pos in gb.positions():
                    if graphbuilder.free_path(
                        Vec2d(*pos), newpos, view, safe_distance
                    ):
                        gb.connect(pos, newpos)
                        debugsurface.line(pos, newpos, "green")

            for p in gb.positions():
                debugsurface.circle(p, 2, "black", 0)

        nodes = gb.all_nodes()
        result = astar.shortest_path(start, end, nodes)
        if result.success:
            result.path = [tuple(self.position)]
            for i in result.indices:
                if result.path[-1] != nodes[i].position:
                    result.path.append(nodes[i].position)

        if ccourse is True:
            self.waypoint_clear()

        # replace current waypoints if we have a better path
        # FIXME: self.cdist is not updated to take into account current
        # position which will penalize the current solution over
        # the last unjustly
        if result.success and (self.waypoint_len() == 0 or
                               result.total_cost < self.cdist):
            self.waypoint_clear()
            for p in result.path:
                self.waypoint_push(Vec2d(*p))
            self.cdist = result.total_cost