def distance_from_line(self, position, line): """ distance from line Parameters ---------- position : 2D vector line : np.array Returns ------- True , distance , vector or False , None , None """ line_length = (vec3(line[1]) - vec3(line[0])).length() u = ((position.x - line[0][0]) * (line[1][0] - line[0][0]) + (position.y - line[0][1]) * (line[1][1] - line[0][1])) \ / line_length ** 2 if 0.0 < u < 1.0: # point is tangent to line x = line[0][0] + u * (line[1][0] - line[0][0]) y = line[0][1] + u * (line[1][1] - line[0][1]) vector = position - vec3(x, y) distance = vector.length() return True, distance, vector return False, None, None
def calculate(self, boid): """ calculate boid behavior Parameters ---------- boid Returns ------- steering """ the_world = boid.world others = the_world.boids(boid, 4) speed = boid.velocity.length() local_front = vec3(0, 1) for other in others: local_position = the_world.to_local(boid, other.position) angle = local_position.angle(local_front) if local_position[1] > 0 and angle < pi / 8: if other.velocity.length() < speed: return -boid.localy.scale(speed / boid.max_speed) return vec3()
def calculate(self, boid): the_world = boid.world position = boid.position radius = boid.radius for other in the_world.boids(boid): offset = position - other.position distance = offset.length() radius_ij = radius + other.radius if distance < radius_ij: offset = offset.scale(radius_ij - distance) boid.position += offset wall_found = False checked = [] for obstacle in the_world.obstacles(boid): if obstacle in checked: continue checked.append(obstacle) intersect, distance_to_line, normal = self.distance_from_line( position, obstacle) if intersect and distance_to_line < radius * 1.2: wall_found = True normal = normal.scale(radius * 1.2 - distance_to_line) boid.position += normal if not wall_found: checked = [] for obstacle in the_world.obstacles(boid): if obstacle in checked: continue checked.append(obstacle) for point in (obstacle[0], obstacle[1]): offset = position - vec3(point) distance = offset.length() if distance < radius * 1.2: boid.position += offset.scale(radius * 1.2 - distance) return vec3()
def calculate(self, boid): the_world = boid.world position = boid.position radius = boid.radius for other in the_world.boids(boid): offset = position - other.position distance = offset.length() radius_ij = radius + other.radius if distance < radius_ij: offset = offset.scale(radius_ij - distance) boid.position += offset wall_found = False checked = [] for obstacle in the_world.obstacles(boid): if obstacle in checked: continue checked.append(obstacle) intersect, distance_to_line, normal = self.distance_from_line(position, obstacle) if intersect and distance_to_line < radius * 1.2: wall_found = True normal = normal.scale(radius * 1.2 - distance_to_line) boid.position += normal if not wall_found: checked = [] for obstacle in the_world.obstacles(boid): if obstacle in checked: continue checked.append(obstacle) for point in (obstacle[0], obstacle[1]): offset = position - vec3(point) distance = offset.length() if distance < radius * 1.2: boid.position += offset.scale(radius * 1.2 - distance) return vec3()
def station(start, mainline_speed, side, berths, decision, queue_berths=None): if side == 'left': side = -1 else: side = 1 if queue_berths is None: queue_berths = berths seconds = mainline_speed / PRT.max_acceleration average_ramp_speed = (mainline_speed + 0) / 2 ramp_length = average_ramp_speed * seconds offline_guideway_straight_length = ramp_length * 2 - spline_length * 2 + (berths + queue_berths) * PRT.berth_length online_guideway_straight_length = offline_guideway_straight_length + spline_height * 2 xx, yy = start d1 = decision d1.next = d2 = Diverge() d2.left = s1 = Straight((xx, yy), (xx, yy+online_guideway_straight_length)) d2.right = sp1 = Spline((xx, yy), (xx, yy+spline_height / 2), (xx+(1.8*side), yy+spline_height / 2), (xx+(1.8*side), yy+spline_height)) yy_top = yy+spline_height+offline_guideway_straight_length sp1.next = s2 = Straight((xx+(1.8*side), yy+spline_height), (xx+(1.8*side), yy_top)) s2.destinations = [] for ii in range(0, berths + queue_berths): s2.destinations.append(vec3(xx+(1.8*side), yy_top - (ramp_length - spline_length) - ii * PRT.berth_length)) s2.platform = (vec3(xx+(1.8+PRT.width/2)*side, yy_top - (ramp_length - spline_length) + PRT.berth_length / 2), vec3(xx+(1.8+PRT.width/2+3)*side, yy_top - (ramp_length - spline_length) - berths * PRT.berth_length + PRT.berth_length / 2)) d1.destinations = s2.destinations d1.platform = s2.platform s2.next = sp2 = Spline((xx+(1.8*side), yy_top), (xx+(1.8*side), yy_top+spline_height/2), (xx, yy_top+spline_height/2), (xx, yy_top+spline_height)) return d1, s1, sp2, online_guideway_straight_length
def calculate(self, boid): """ calculate boid behavior Parameters ---------- boid Returns ------- steering """ the_world = boid.world others = the_world.boids(boid, 6.0) separation_distance = 6.0 * boid.velocity.length() / boid.max_speed acceleration = vec3() for other in others: local_position = the_world.to_local(boid, other.position) in_front = local_position[1] > -boid.radius if in_front and local_position.length() < separation_distance: separation = other.position - boid.position force = separation.scale(-1 / max(1e-9,separation.length() ** 2)) # create orthogonal vector in order to make boids avoidance force2 = (-1**randint(0,1))*vec3(-force[1],force[0],0) # force2 = vec3(-force[1],force[0],0) acceleration += 0.5*force2 #3*force2 return acceleration
def calculate(self, boid): """ calculate boid behavior Parameters ---------- boid Returns ------- steering """ the_world = boid.world others = the_world.boids(boid, 4) speed = boid.velocity.length() local_front = vec3(0,1) for other in others: local_position = the_world.to_local(boid, other.position) angle = local_position.angle(local_front) if local_position[1] > 0 and angle < pi / 8: if other.velocity.length() < speed: return -boid.localy.scale(speed / boid.max_speed) return vec3()
def calculate(self, boid): """ calculate boid behavior Parameters ---------- boid Returns ------- steering """ the_world = boid.world others = the_world.boids(boid, 6.0) separation_distance = 6.0 * boid.velocity.length() / boid.max_speed acceleration = vec3() for other in others: local_position = the_world.to_local(boid, other.position) in_front = local_position[1] > -boid.radius if in_front and local_position.length() < separation_distance: separation = other.position - boid.position force = separation.scale(-1 / max(1e-9, separation.length()**2)) # create orthogonal vector in order to make boids avoidance force2 = (-1**randint(0, 1)) * vec3(-force[1], force[0], 0) # force2 = vec3(-force[1],force[0],0) acceleration += 0.5 * force2 #3*force2 return acceleration
def distance_from_line(self, position, line): line_length = (vec3(line[1]) - vec3(line[0])).length() u = ((position.x - line[0][0]) * (line[1][0] - line[0][0]) + (position.y - line[0][1]) * (line[1][1] - line[0][1])) \ / line_length ** 2 if 0.0 < u < 1.0: # point is tangent to line x = line[0][0] + u * (line[1][0] - line[0][0]) y = line[0][1] + u * (line[1][1] - line[0][1]) vector = position - vec3(x, y) distance = vector.length() return True, distance, vector return False, None, None
def calculate(self, boid): """ calculate boid behavior Parameters ---------- boid Returns ------- steering """ waypoints = boid.waypoints if len(waypoints) == 0: boid.arrived = True return vec3() displacement = waypoints[0] - boid.position if displacement.length() < 2: del waypoints[0] desired_velocity = displacement.normalize() * boid.desired_speed steering = desired_velocity - boid.velocity return steering
def calculate(self, boid): waypoints = boid.waypoints if len(waypoints) == 0: boid.arrived = True return vec3() displacement = waypoints[0] - boid.position if displacement.length() < 2: del waypoints[0] desired_velocity = displacement.normalize() * boid.desired_speed return desired_velocity - boid.velocity
def queue_steering_mind(boid): """Sum of all steering vectors, except Separation in some cases. The Separation steering vector will be ignored if any prior steering behavior gave a non-zero acceleration, typically Containment.""" acceleration = vec3() for behavior in boid.behaviors: # if not isinstance(behavior, Separation) or acceleration.length() < 0.0001: acceleration += behavior.calculate(boid) return acceleration
def __init__(self, interval=0.5): Process.__init__(self) self.world = world() self.interval = interval self.manager = None self.manager_args = [] self.waypoints = [] self.position = vec3() self.velocity = vec3() self.localx = vec3(1, 0) self.localy = vec3(0, 1) self.world.add_boid(self) # from Helbing, et al "Self-organizing pedestrian movement" self.max_speed = normalvariate(1.36, 0.26) self.desired_speed = self.max_speed self.radius = normalvariate(self.average_radius, 0.025) / 2 self.intersection = vec3() self.arrived = False self.behaviors = [] self.steering_mind = default_steering_mind self.cancelled = 0
def test_intersection(self, boid, wall, position, vector, method='direct'): # From http://astronomy.swin.edu.au/~pbourke/geometry/lineline2d/ point1, point2 = wall if method == 'direct': VR = vector elif method == 'uniform': ######## version uniform r = uniform(-pi / 12.0, pi / 12.0) v0 = vector.ang0() vl = vector.length() VR = vec3(cos(v0 + r) * vl, sin(v0 + r) * vl, vector.z) elif method == 'gauss': ######## version gaussienne vl = vector.length() r = gauss(vector.ang0(), pi / 12.0) VR = vec3(cos(r) * vl, sin(r) * vl, vector.z) elif method == 'ellipse': ######## version ellipse theta = gauss(vector.ang0(), sqrt(pi / 6.0)) # random angle to test a = vector.length() # create an elipse... b = a / 1.5 # ...to test collision if b=a/1. ellipse =circle e = (sqrt(a**2 - b**2)) / a # ... r = (b**2 / a) / (1.0 + e * cos(theta)) # .... VR = vec3(cos(theta) * r, sin(theta) * r, vector.z) denominator = ((VR.y * (point2[0] - point1[0])) - (VR.x * (point2[1] - point1[1]))) if denominator == 0.0: # parallel or coincident return False, 0.0, None u_a = (VR.x * (point1[1] - position.y) - (VR.y) * (point1[0] - position.x)) / denominator u_b = ((point2[0] - point1[0]) * (point1[1] - position.y) - (point2[1] - point1[1]) * (point1[0] - position.x)) / denominator intersect = 0.0 < u_a < 1.0 and 0.0 < u_b < 1.0 if intersect: intersection = vec3(point1[0] + u_a * (point2[0] - point1[0]), point1[1] + u_a * (point2[1] - point1[1])) boid.intersection = intersection distance_along_check = u_b wall_VR = vec3(point1) - vec3(point2) wall_VR_normal = vec3(-wall_VR.y, wall_VR.x).normalize() boid.intersection_normal = wall_VR_normal normal_point = intersection + wall_VR_normal local_normal_point = boid.world.to_local(boid, normal_point) if local_normal_point.x <= 0.0: direction = 'left' else: direction = 'right' return True, distance_along_check, direction else: return False, 0.0, None
def test_intersection(self, boid, wall, position, vector, method = 'direct'): # From http://astronomy.swin.edu.au/~pbourke/geometry/lineline2d/ point1, point2 = wall if method == 'direct': VR=vector elif method == 'uniform': ######## version uniform r=uniform(-pi/12.0,pi/12.0) v0=vector.ang0() vl=vector.length() VR = vec3(cos(v0+r)*vl,sin(v0+r)*vl,vector.z) elif method == 'gauss': ######## version gaussienne vl=vector.length() r=gauss(vector.ang0(),pi/12.0) VR = vec3(cos(r)*vl,sin(r)*vl,vector.z) elif method == 'ellipse': ######## version ellipse theta = gauss(vector.ang0(),sqrt(pi/6.0)) # random angle to test a = vector.length() # create an elipse... b = a/1.5 # ...to test collision if b=a/1. ellipse =circle e = (sqrt(a**2-b**2))/a # ... r = (b**2/a)/(1.0+e*cos(theta)) # .... VR = vec3(cos(theta)*r,sin(theta)*r,vector.z) denominator = ((VR.y * (point2[0] - point1[0])) - (VR.x * (point2[1] - point1[1]))) if denominator == 0.0: # parallel or coincident return False, 0.0, None u_a = (VR.x * (point1[1] - position.y) - (VR.y) * (point1[0] - position.x)) / denominator u_b = ((point2[0] - point1[0]) * (point1[1] - position.y) - (point2[1] - point1[1]) * (point1[0] - position.x)) / denominator intersect = 0.0 < u_a < 1.0 and 0.0 < u_b < 1.0 if intersect: intersection = vec3(point1[0] + u_a * (point2[0] - point1[0]), point1[1] + u_a * (point2[1] - point1[1])) boid.intersection = intersection distance_along_check = u_b wall_VR = vec3(point1) - vec3(point2) wall_VR_normal = vec3(-wall_VR.y, wall_VR.x).normalize() boid.intersection_normal = wall_VR_normal normal_point = intersection + wall_VR_normal local_normal_point = boid.world.to_local(boid, normal_point) if local_normal_point.x <= 0.0: direction = 'left' else: direction = 'right' return True, distance_along_check, direction else: return False, 0.0, None
def conv_vecarr(vin,dim=3): """ convert vec3 => np.array and np.array => vec3 Parameters ---------- dim : int default (2) """ if isinstance(vin,vec3): if dim == 2: return(np.array((vin[0],vin[1]))) elif dim == 3: return(np.array((vin[0],vin[1],vin[2]))) else : raise AttributeError('incorrect input vec3 or nd array dimension') elif isinstance(vin,np.ndarray): if dim == 2: return(vec3((vin[0],vin[1],0.0))) elif dim == 3: return(vec3((vin[0],vin[1],vin[2]))) else : raise AttributeError('incorrect input vec3 or nd array dimension') else : raise TypeError('vin must be either a vec3 or a np.ndarray instance')
def default_steering_mind(boid): """ Sum all steering vectors. Notes ----- This is the place where all acceleration from all behaviors are summed. """ acceleration = vec3() for behavior in boid.behaviors: acceleration += behavior.calculate(boid) return acceleration
def conv_vecarr(vin,dim=2): """ convert vec3 => np.array and np.array => vec3 Parameters ---------- dim : int default (2) """ if isinstance(vin,vec3): if dim == 2: return(np.array((vin[0],vin[1]))) elif dim == 3: return(np.array((vin[0],vin[1],vin[2]))) else : raise AttributeError('incorrect input vec3 or nd array dimension') elif isinstance(vin,np.ndarray): if dim == 2: return(vec3((vin[0],vin[1],0.0))) elif dim == 3: return(vec3((vin[0],vin[1],vin[2]))) else : raise AttributeError('incorrect input vec3 or nd array dimension') else : raise TypeError('vin must be either a vec3 or a np.ndarray instance')
def __init__(self, interval=0.5,roomId=0, L=[],sim=None): """ boid is initialized in a room of Layout L """ #GeomNetType = np.dtype([('Id',int), # ('time',int), # ('p',float,(1,3)), # ('v',float,(1,3)), # ('a',float,(1,3))]) Person2.npers +=1 Process.__init__(self,sim=sim) self.L = L self.world = world() self.interval = interval self.manager = None self.manager_args = [] self.waypoints = [] self.roomId = roomId self.nextroomId = int(np.floor(uniform(0,self.L.Gr.size()))) while self.nextroomId == self.roomId : # test destination different de l'arrive self.nextroomId = int(np.floor(uniform(0,self.L.Gr.size()))) print "nextroom : ", self.nextroomId self.wp = self.L.waypoint(roomId,self.nextroomId) for tup in self.wp[1:]: self.waypoints.append(vec3(tup)) try: self.position = vec3(L.Gr.pos[roomId]) except: self.position = vec3() # self.old_pos = vec3() self.stuck = 0 self.destination = self.waypoints[0] self.velocity = vec3() self.localx = vec3(1, 0) self.localy = vec3(0, 1) self.world.add_boid(self) # GeomNet = np.vstack((GeomNet,np.array((Nnodes,0,[list(self.position)],[list(self.velocity)],[list(self.velocity)]),dtype=GeomNetType))) # from Helbing, et al "Self-organizing pedestrian movement" self.max_speed = normalvariate(1.36, 0.26) self.desired_speed = self.max_speed self.radius = normalvariate(self.average_radius, 0.025) / 2 self.intersection = vec3() self.arrived = False self.endpoint = False self.behaviors = [] self.steering_mind = default_steering_mind self.cancelled = 0
def truncate(self, max): """ Parameters ---------- max References ---------- "near collision avoidance" inspired from http://people.revoledu.com/kardi/publication/Kouchi2001.pdf """ if self.length() > max: return self.normalize() * max else: return vec3(self)
def default_steering_mind(boid): """ Sum all steering vectors. Parameters ---------- boid Notes ----- This is the place where all acceleration from all behaviors are summed. """ acceleration = vec3() for behavior in boid.behaviors: acceleration += behavior.calculate(boid) return acceleration
def move(self): while True: while self.cancelled: yield passivate, self print "Person.move: activated after being cancelled" checked = [] for zone in self.world.zones(self): if zone not in checked: checked.append(zone) zone(self) acceleration = self.steering_mind(self) acceleration = acceleration.truncate(self.max_acceleration) self.acceleration = acceleration velocity = self.velocity + acceleration * self.interval self.velocity = velocity.truncate(self.max_speed) if velocity.length() > 0.2: # record direction only when we've really had some self.localy = velocity.normalize() self.localx = vec3(self.localy.y, -self.localy.x) self.position = self.position + self.velocity * self.interval self.update() self.world.update_boid(self) if self.arrived: self.arrived = False if self.manager: if self.manager(self, *self.manager_args): yield passivate, self else: yield passivate, self else: yield hold, self, self.interval
def default_steering_mind(boid): """Simple sum of all steering vectors.""" acceleration = vec3() for behavior in boid.behaviors: acceleration += behavior.calculate(boid) return acceleration
def move(self): """ Move the Person """ if self.pdshow: fig = plt.gcf() fig, ax = self.L.showG('w', labels=False, alphacy=0., edges=False, fig=fig) plt.draw() plt.ion() while True: if self.moving: if self.sim.verbose: print 'meca: updt ag ' + self.ID + ' @ ', self.sim.now() # if np.allclose(conv_vecarr(self.destination)[:2],self.L.Gw.pos[47]): # import ipdb # ipdb.set_trace() while self.cancelled: yield passivate, self print "Person.move: activated after being cancelled" checked = [] for zone in self.world.zones(self): if zone not in checked: checked.append(zone) zone(self) # updating acceleration acceleration = self.steering_mind(self) acceleration = acceleration.truncate(self.max_acceleration) self.acceleration = acceleration # updating velocity velocity = self.velocity + acceleration * self.interval self.velocity = velocity.truncate(self.max_speed) if velocity.length() > 0.2: # record direction only when we've really had some self.localy = velocity.normalize() self.localx = vec3(self.localy.y, -self.localy.x) # updating position self.position = self.position + self.velocity * self.interval # self.update() self.position.z = 0 self.world.update_boid(self) self.net.update_pos(self.ID, conv_vecarr(self.position), self.sim.now()) p = conv_vecarr(self.position).reshape(3, 1) v = conv_vecarr(self.velocity).reshape(3, 1) a = conv_vecarr(self.acceleration).reshape(3, 1) # fill panda dataframe 2D trajectory self.df = self.df.append( pd.DataFrame( { 't': pd.Timestamp(self.sim.now(), unit='s'), 'x': p[0], 'y': p[1], 'vx': v[0], 'vy': v[1], 'ax': a[0], 'ay': a[1] }, columns=['t', 'x', 'y', 'vx', 'vy', 'ax', 'ay'])) if self.pdshow: ptmp = np.array([p[:2, 0], p[:2, 0] + v[:2, 0]]) if hasattr(self, 'pl'): self.pl[0].set_data(self.df['x'].tail(1), self.df['y'].tail(1)) self.pla[0].set_data(ptmp[:, 0], ptmp[:, 1]) circle = plt.Circle( (self.df['x'].tail(1), self.df['y'].tail(1)), radius=self.radius, alpha=0.3) ax.add_patch(circle) else: self.pl = ax.plot(self.df['x'].tail(1), self.df['y'].tail(1), 'o', color=self.color, ms=self.radius * 10) self.pla = ax.plot(ptmp[:, 0], ptmp[:, 1], 'r') circle = plt.Circle( (self.df['x'].tail(1), self.df['y'].tail(1)), radius=self.radius, alpha=0.3) ax.add_patch(circle) # try: # fig,ax=plu.displot(p[:2],p[:2]+v[:2],'r') # except: # pass # import ipdb # ipdb.set_trace() plt.draw() plt.pause(0.0001) if 'mysql' in self.save: self.db.writemeca(self.ID, self.sim.now(), p, v, a) if 'txt' in self.save: pyu.writemeca(self.ID, self.sim.now(), p, v, a) # new target when arrived in poi if self.arrived and\ (self.L.pt2ro(self.position) ==\ self.L.Gw.node[self.rooms[1]]['room']): self.arrived = False if self.endpoint: self.endpoint = False self.roomId = self.nextroomId # remove the remaining waypoint which correspond # to current room position del self.waypoints[0] del self.rooms[0] # del self.dlist[0] # # If door lets continue # # # ig destination --> next room # #adjroom = self.L.Gr.neighbors(self.roomId) #Nadjroom = len(adjroom) if self.cdest == 'random': # self.nextroomId = int(np.floor(random.uniform(0,self.L.Gr.size()))) self.nextroomId = random.sample( self.L.Gr.nodes(), 1)[0] # test 1 ) next != actualroom # 2 ) nextroom != fordiden room # 3 ) room not share without another agent while self.nextroomId == self.roomId or ( self.nextroomId in self.forbidroomId ): # or (self.nextroomId in self.sim.roomlist): # self.nextroomId = int(np.floor(random.uniform(0,self.L.Gr.size()))) self.nextroomId = random.sample( self.L.Gr.nodes(), 1)[0] elif self.cdest == 'file': self.room_counter = self.room_counter + 1 if self.room_counter >= self.nb_room: self.room_counter = 0 self.nextroomId = self.room_seq[self.room_counter] self.wait = self.room_wait[self.room_counter] #self.sim.roomlist.append(self.nextroomId) # list of all destiantion of all nodes in object sim self.rooms, wp = self.L.waypointGw( self.roomId, self.nextroomId) # self.dlist = [i in self.L.Gw.ldo for i in self.rooms] for tup in wp[1:]: self.waypoints.append(vec3(tup)) #nextroom = adjroom[k] # print "room : ",self.roomId # print "nextroom : ",self.nextroomId #p_nextroom = self.L.Gr.pos[self.nextroomId] #setdoors1 = self.L.Gr.node[self.roomId]['doors'] #setdoors2 = self.L.Gr.node[nextroom]['doors'] #doorId = np.intersect1d(setdoors1,setdoors2)[0] # # coord door # #unode = self.L.Gs.neighbors(doorId) #p1 = self.L.Gs.pos[unode[0]] #p2 = self.L.Gs.pos[unode[1]] #print p1 #print p2 #pdoor = (np.array(p1)+np.array(p2))/2 self.destination = self.waypoints[0] if self.sim.verbose: print 'meca: ag ' + self.ID + ' wait ' + str( self.wait) #*self.interval) yield hold, self, self.wait else: del self.waypoints[0] del self.rooms[0] # del self.dlist[0] #print "wp : ", self.waypoints if len(self.waypoints) == 1: self.endpoint = True self.destination = self.waypoints[0] #print "dest : ", self.destination else: yield hold, self, self.interval else: # self.update() self.world.update_boid(self) self.net.update_pos(self.ID, conv_vecarr(self.position), self.sim.now()) yield hold, self, self.interval
def __init__(self, ID=0, interval=0.05, roomId=0, L=[], net=Network(), wld=world(), sim=None, moving=True, froom=[], wait=1.0, cdest='random', save=[]): """ Class Person inherits of Simpy.SimulationRT Attributes ---------- ID : float/hex/str/... agent Id interval : float refresh interval of agent mobility roomId : int room ID where agent start when simulation is launched L : pylayers.gis.layout.Layout() Layout instance, in which the agent is moving net : pylayers.network.Network() Network instance, in which network agent are communicating. This is used for fill the true position filed of the graph It must be removed in a further version ( when a proper save instance would be created) wld : pylayers.mobility.transit.world.world() world instance. equivalent to layout but in the pytk framework. TODO : remove in a further version sim : SimPy.Simulation.Simulation.RT() Simulation instance share by all the pylayer project. moving : bool indicate if the agent is moving or not ( relevant for acces poitns) froom : list list of forbiden rooms. wait : float wait time of the agent when he has reach teh desitaion cdest : str method for choosing setination 'random ' of file read save : list list of save option type . It will be removed in a further version ( when a proper save instance would be created) Method ------ Move : make the agent move update : DEPRECATED used for Tkinter plot """ #GeomNetType = np.dtype([('Id',int), # ('time',int), # ('p',float,(1,3)), # ('v',float,(1,3)), # ('a',float,(1,3))]) Person.npers += 1 Process.__init__(self, name='Person_ID' + str(ID), sim=sim) self.ID = ID self.L = L self.world = wld self.interval = interval self.manager = None self.manager_args = [] self.waypoints = [] self.moving = moving self.roomId = roomId self.forbidroomId = froom self.cdest = cdest # choose tdestination type if self.cdest == 'random': self.nextroomId = int(np.floor(uniform(0, self.L.Gr.size()))) while self.nextroomId == self.roomId or ( self.nextroomId in self.forbidroomId ): # or (self.nextroomId in self.sim.roomlist): # test destination different de l'arrive self.nextroomId = int(np.floor(uniform(0, self.L.Gr.size()))) #self.sim.roomlist.append(self.nextroomId) # list of all destiantion of all nodes in object sim elif self.cdest == 'file': cfg = ConfigParser.ConfigParser() cfg.read(pyu.getlong('nodes_destination.ini', 'ini')) self.room_seq = eval(dict(cfg.items(self.ID))['room_seq']) self.room_wait = eval(dict(cfg.items(self.ID))['room_wait']) print 'WARNING: when nodes_destination ini file is read:' print '1) the room initialization starts in the first room of the list, not in the room configured in agent.ini' print '2) forbiden rooms are neglected' self.room_counter = 1 self.nb_room = len(self.room_seq) self.roomId = self.room_seq[0] self.nextroomId = self.room_seq[self.room_counter] self.wait = self.room_wait[self.room_counter] #self.sim.roomlist.append(self.nextroomId) # list of all destiantion of all nodes in object sim self.wp = self.L.waypointGw(self.roomId, self.nextroomId) for tup in self.wp[1:]: self.waypoints.append(vec3(tup)) try: self.position = vec3(L.Gr.pos[self.roomId][0], L.Gr.pos[self.roomId][1]) except: self.position = vec3() # self.old_pos = vec3() self.stuck = 0 self.destination = self.waypoints[0] self.velocity = vec3() self.localx = vec3(1, 0) self.localy = vec3(0, 1) self.world.add_boid(self) # from Helbing, et al "Self-organizing pedestrian movement" self.max_speed = 1.2 #normalvariate(1.0, 0.26) self.desired_speed = self.max_speed self.radius = normalvariate(self.average_radius, 0.025) / 2 self.intersection = vec3() self.arrived = False self.endpoint = False self.behaviors = [] self.steering_mind = default_steering_mind self.cancelled = 0 self.net = net self.wait = wait self.save = save if 'mysql' in self.save: config = ConfigParser.ConfigParser() config.read(pyu.getlong('simulnet.ini', 'ini')) sql_opt = dict(config.items('Mysql')) self.db = Database(sql_opt['host'], sql_opt['user'], sql_opt['passwd'], sql_opt['dbname']) self.date = datetime.datetime.now()
def __init__(self, ID=0, interval=0.05, roomId=-1, L=[], net=Network(), wld=world(), seed=0, sim=None, moving=True, froom=[], wait=1.0, cdest='random', save=[], color='k', pdshow=False): """ Class Person inherits of Simpy.SimulationRT """ #GeomNetType = np.dtype([('Id',int), # ('time',int), # ('p',float,(1,3)), # ('v',float,(1,3)), # ('a',float,(1,3))]) Person.npers += 1 Process.__init__(self, name='Person_ID' + str(ID), sim=sim) self.ID = ID self.color = color self.pdshow = pdshow self.L = L self.world = wld self.interval = interval self.manager = None self.manager_args = [] self.waypoints = [] self.moving = moving # random.seed(seed) if roomId < 0: try: self.roomId = random.sample(self.L.Gr.nodes(), 1)[0] except: raise NameError( 'This error is due to the lack of Gr graph in the Layout argument passed to Person(Object)' ) else: self.roomId = roomId self.forbidroomId = froom self.cdest = cdest # choose tdestination type if self.cdest == 'random': # self.nextroomId = int(np.floor(random.uniform(0,self.L.Gr.size()))) try: self.nextroomId = random.sample(self.L.Gr.nodes(), 1)[0] except: raise NameError( 'This error is due to the lack of Gr graph in the Layout argument passed to Person(Object)' ) while self.nextroomId == self.roomId or ( self.nextroomId in self.forbidroomId ): # or (self.nextroomId in self.sim.roomlist): # test destination different de l'arrive # self.nextroomId = int(np.floor(random.uniform(0,self.L.Gr.size()))) self.nextroomId = random.sample(self.L.Gr.nodes(), 1)[0] #self.sim.roomlist.append(self.nextroomId) # list of all destiantion of all nodes in object sim elif self.cdest == 'file': cfg = ConfigParser.ConfigParser() cfg.read(pyu.getlong('nodes_destination.ini', 'ini')) self.room_seq = eval(dict(cfg.items(self.ID))['room_seq']) self.room_wait = eval(dict(cfg.items(self.ID))['room_wait']) print 'WARNING: when nodes_destination ini file is read:' print '1) the room initialization starts in the first room of the list, not in the room configured in agent.ini' print '2) forbiden rooms are neglected' self.room_counter = 1 self.nb_room = len(self.room_seq) self.roomId = self.room_seq[0] self.nextroomId = self.room_seq[self.room_counter] self.wait = self.room_wait[self.room_counter] #self.sim.roomlist.append(self.nextroomId) # list of all destiantion of all nodes in object sim self.rooms, self.wp = self.L.waypointGw(self.roomId, self.nextroomId) # self.dlist = [i in self.L.Gw.ldo for i in self.rooms] for tup in self.wp[1:]: self.waypoints.append(vec3(tup)) try: self.position = vec3(L.Gr.pos[self.roomId][0], L.Gr.pos[self.roomId][1]) except: self.position = vec3() # self.old_pos = vec3() self.stuck = 0 self.destination = self.waypoints[0] self.arrived_in = False self.velocity = vec3() self.acceleration = vec3() self.localx = vec3(1, 0) self.localy = vec3(0, 1) self.world.add_boid(self) # from Helbing, et al "Self-organizing pedestrian movement" maxspeed = 0.8 self.max_speed = maxspeed #random.normalvariate(maxspeed, 0.1) self.desired_speed = maxspeed self.radius = self.average_radius #random.normalvariate(self.average_radius, 0.025) / 2 self.intersection = vec3() self.arrived = False self.endpoint = False self.behaviors = [] self.steering_mind = default_steering_mind self.cancelled = 0 self.net = net self.wait = wait self.df = pd.DataFrame(columns=['t', 'x', 'y', 'vx', 'vy', 'ax', 'ay']) self.df._metadata = self.ID self.save = save if 'mysql' in self.save: config = ConfigParser.ConfigParser() config.read(pyu.getlong('simulnet.ini', 'ini')) sql_opt = dict(config.items('Mysql')) self.db = Database(sql_opt['host'], sql_opt['user'], sql_opt['passwd'], sql_opt['dbname']) self.date = datetime.datetime.now()
def move(self): """ Move the Person """ if self.pdshow: fig =plt.gcf() fig,ax=self.L.showG('w',labels=False,alphacy=0.,edges=False,fig=fig) plt.draw() plt.ion() while True: if self.moving: if self.sim.verbose: print 'meca: updt ag ' + self.ID + ' @ ',self.sim.now() # if np.allclose(conv_vecarr(self.destination)[:2],self.L.Gw.pos[47]): # import ipdb # ipdb.set_trace() while self.cancelled: yield passivate, self print "Person.move: activated after being cancelled" checked = [] for zone in self.world.zones(self): if zone not in checked: checked.append(zone) zone(self) # updating acceleration acceleration = self.steering_mind(self) acceleration = acceleration.truncate(self.max_acceleration) self.acceleration = acceleration # updating velocity velocity = self.velocity + acceleration * self.interval self.velocity = velocity.truncate(self.max_speed) if velocity.length() > 0.2: # record direction only when we've really had some self.localy = velocity.normalize() self.localx = vec3(self.localy.y, -self.localy.x) # updating position self.position = self.position + self.velocity * self.interval # self.update() self.position.z=0 self.world.update_boid(self) self.net.update_pos(self.ID,conv_vecarr(self.position),self.sim.now()) p=conv_vecarr(self.position).reshape(3,1) v=conv_vecarr(self.velocity).reshape(3,1) a=conv_vecarr(self.acceleration).reshape(3,1) # fill panda dataframe 2D trajectory self.df = self.df.append(pd.DataFrame({'t':pd.Timestamp(self.sim.now(),unit='s'), 'x':p[0], 'y':p[1], 'vx':v[0], 'vy':v[1], 'ax':a[0], 'ay':a[1]}, columns=['t','x','y','vx','vy','ax','ay'])) if self.pdshow: ptmp =np.array([p[:2,0],p[:2,0]+v[:2,0]]) if hasattr(self, 'pl'): self.pl[0].set_data(self.df['x'].tail(1),self.df['y'].tail(1)) self.pla[0].set_data(ptmp[:,0],ptmp[:,1]) else : self.pl = ax.plot(self.df['x'].tail(1),self.df['y'].tail(1),'o',color=self.color,ms=self.radius*10) self.pla = ax.plot(ptmp[:,0],ptmp[:,1],'r') # try: # fig,ax=plu.displot(p[:2],p[:2]+v[:2],'r') # except: # pass # import ipdb # ipdb.set_trace() plt.draw() plt.pause(0.0001) if 'mysql' in self.save: self.db.writemeca(self.ID,self.sim.now(),p,v,a) if 'txt' in self.save: pyu.writemeca(self.ID,self.sim.now(),p,v,a) # new target when arrived in poi if self.arrived and\ (self.L.pt2ro(self.position) ==\ self.L.Gw.node[self.rooms[1]]['room']): self.arrived = False if self.endpoint: self.endpoint=False self.roomId = self.nextroomId # remove the remaining waypoint which correspond # to current room position del self.waypoints[0] del self.rooms[0] # del self.dlist[0] # # If door lets continue # # # ig destination --> next room # #adjroom = self.L.Gr.neighbors(self.roomId) #Nadjroom = len(adjroom) if self.cdest == 'random': # self.nextroomId = int(np.floor(random.uniform(0,self.L.Gr.size()))) self.nextroomId = random.sample(self.L.Gr.nodes(),1)[0] # test 1 ) next != actualroom # 2 ) nextroom != fordiden room # 3 ) room not share without another agent while self.nextroomId == self.roomId or (self.nextroomId in self.forbidroomId):# or (self.nextroomId in self.sim.roomlist): # self.nextroomId = int(np.floor(random.uniform(0,self.L.Gr.size()))) self.nextroomId = random.sample(self.L.Gr.nodes(),1)[0] elif self.cdest == 'file': self.room_counter=self.room_counter+1 if self.room_counter >= self.nb_room: self.room_counter=0 self.nextroomId=self.room_seq[self.room_counter] self.wait=self.room_wait[self.room_counter] #self.sim.roomlist.append(self.nextroomId) # list of all destiantion of all nodes in object sim self.rooms, wp = self.L.waypointGw(self.roomId,self.nextroomId) # self.dlist = [i in self.L.Gw.ldo for i in self.rooms] for tup in wp[1:]: self.waypoints.append(vec3(tup)) #nextroom = adjroom[k] # print "room : ",self.roomId # print "nextroom : ",self.nextroomId #p_nextroom = self.L.Gr.pos[self.nextroomId] #setdoors1 = self.L.Gr.node[self.roomId]['doors'] #setdoors2 = self.L.Gr.node[nextroom]['doors'] #doorId = np.intersect1d(setdoors1,setdoors2)[0] # # coord door # #unode = self.L.Gs.neighbors(doorId) #p1 = self.L.Gs.pos[unode[0]] #p2 = self.L.Gs.pos[unode[1]] #print p1 #print p2 #pdoor = (np.array(p1)+np.array(p2))/2 self.destination = self.waypoints[0] if self.sim.verbose: print 'meca: ag ' + self.ID + ' wait ' + str(self.wait)#*self.interval) yield hold, self, self.wait else: del self.waypoints[0] del self.rooms[0] # del self.dlist[0] #print "wp : ", self.waypoints if len(self.waypoints)==1: self.endpoint=True self.destination = self.waypoints[0] #print "dest : ", self.destination else: yield hold, self, self.interval else: # self.update() self.world.update_boid(self) self.net.update_pos(self.ID,conv_vecarr(self.position),self.sim.now()) yield hold, self, self.interval
def __init__(self, ID = 0, interval=0.05,roomId=0, L=[], net=Network(), wld = world(),sim=None,moving=True,froom=[],wait=1.0,cdest='random',save=[]): """ Class Person inherits of Simpy.SimulationRT Attributes ---------- ID : float/hex/str/... agent Id interval : float refresh interval of agent mobility roomId : int room ID where agent start when simulation is launched L : pylayers.gis.layout.Layout() Layout instance, in which the agent is moving net : pylayers.network.Network() Network instance, in which network agent are communicating. This is used for fill the true position filed of the graph It must be removed in a further version ( when a proper save instance would be created) wld : pylayers.mobility.transit.world.world() world instance. equivalent to layout but in the pytk framework. TODO : remove in a further version sim : SimPy.Simulation.Simulation.RT() Simulation instance share by all the pylayer project. moving : bool indicate if the agent is moving or not ( relevant for acces poitns) froom : list list of forbiden rooms. wait : float wait time of the agent when he has reach teh desitaion cdest : str method for choosing setination 'random ' of file read save : list list of save option type . It will be removed in a further version ( when a proper save instance would be created) Method ------ Move : make the agent move update : DEPRECATED used for Tkinter plot """ #GeomNetType = np.dtype([('Id',int), # ('time',int), # ('p',float,(1,3)), # ('v',float,(1,3)), # ('a',float,(1,3))]) Person.npers +=1 Process.__init__(self,name='Person_ID'+str(ID),sim=sim) self.ID=ID self.L = L self.world = wld self.interval = interval self.manager = None self.manager_args = [] self.waypoints = [] self.moving=moving self.roomId = roomId self.forbidroomId = froom self.cdest = cdest # choose tdestination type if self.cdest == 'random': self.nextroomId = int(np.floor(uniform(0,self.L.Gr.size()))) while self.nextroomId == self.roomId or (self.nextroomId in self.forbidroomId): # or (self.nextroomId in self.sim.roomlist): # test destination different de l'arrive self.nextroomId = int(np.floor(uniform(0,self.L.Gr.size()))) #self.sim.roomlist.append(self.nextroomId) # list of all destiantion of all nodes in object sim elif self.cdest == 'file': cfg = ConfigParser.ConfigParser() cfg.read(pyu.getlong('nodes_destination.ini','ini')) self.room_seq=eval(dict(cfg.items(self.ID))['room_seq']) self.room_wait=eval(dict(cfg.items(self.ID))['room_wait']) print 'WARNING: when nodes_destination ini file is read:' print '1) the room initialization starts in the first room of the list, not in the room configured in agent.ini' print '2) forbiden rooms are neglected' self.room_counter=1 self.nb_room=len(self.room_seq) self.roomId=self.room_seq[0] self.nextroomId=self.room_seq[self.room_counter] self.wait=self.room_wait[self.room_counter] #self.sim.roomlist.append(self.nextroomId) # list of all destiantion of all nodes in object sim self.wp = self.L.waypointGw(self.roomId,self.nextroomId) for tup in self.wp[1:]: self.waypoints.append(vec3(tup) ) try: self.position = vec3(L.Gr.pos[self.roomId][0],L.Gr.pos[self.roomId][1]) except: self.position = vec3() # self.old_pos = vec3() self.stuck = 0 self.destination = self.waypoints[0] self.velocity = vec3() self.localx = vec3(1, 0) self.localy = vec3(0, 1) self.world.add_boid(self) # from Helbing, et al "Self-organizing pedestrian movement" self.max_speed = 1.2#normalvariate(1.0, 0.26) self.desired_speed = self.max_speed self.radius = normalvariate(self.average_radius, 0.025) / 2 self.intersection = vec3() self.arrived = False self.endpoint = False self.behaviors = [] self.steering_mind = default_steering_mind self.cancelled = 0 self.net=net self.wait=wait self.save=save if 'mysql' in self.save: config = ConfigParser.ConfigParser() config.read(pyu.getlong('simulnet.ini','ini')) sql_opt = dict(config.items('Mysql')) self.db = Database(sql_opt['host'],sql_opt['user'],sql_opt['passwd'],sql_opt['dbname']) self.date = datetime.datetime.now()
def to_local(self, boid, point): xx, yy, unused_z = point - boid.position return vec3(boid.localy.y * xx - boid.localy.x * yy, -boid.localx.y * xx + boid.localx.x * yy)
def move(self): """ Move the Agent """ while True: if self.moving: if self.ID == 0: print 'meca update @',self.sim.now() while self.cancelled: yield passivate, self print "Person.move: activated after being cancelled" checked = [] for zone in self.world.zones(self): if zone not in checked: checked.append(zone) zone(self) acceleration = self.steering_mind(self) acceleration = acceleration.truncate(self.max_acceleration) self.acceleration = acceleration velocity = self.velocity + acceleration * self.interval self.velocity = velocity.truncate(self.max_speed) if velocity.length() > 0.2: # record direction only when we've really had some self.localy = velocity.normalize() self.localx = vec3(self.localy.y, -self.localy.x) self.position = self.position + self.velocity * self.interval # self.update() self.world.update_boid(self) self.net.update_pos(self.ID,conv_vecarr(self.position),self.sim.now()) if len(self.save)!=0: p=conv_vecarr(self.position) v=conv_vecarr(self.velocity) a=conv_vecarr(self.acceleration) if 'mysql' in self.save: self.db.writemeca(self.ID,self.sim.now(),p,v,a) if 'txt' in self.save: pyu.writemeca(self.ID,self.sim.now(),p,v,a) if self.arrived: self.arrived = False if self.endpoint: self.endpoint=False #pr = self.sim.roomlist.index(self.nextroomId) #self.sim.roomlist.pop(pr) self.roomId = self.nextroomId # # Si porte on continue # # # ig destination --> next room # #adjroom = self.L.Gr.neighbors(self.roomId) #Nadjroom = len(adjroom) if self.cdest == 'random': # self.nextroomId = int(np.floor(uniform(0,self.L.Gr.size()))) self.nextroomId = sample(self.L.Gr.nodes(),1)[0] # test 1 ) next != actualroom # 2 ) nextroom != fordiden room # 3 ) room not share without another agent while self.nextroomId == self.roomId or (self.nextroomId in self.forbidroomId):# or (self.nextroomId in self.sim.roomlist): # self.nextroomId = int(np.floor(uniform(0,self.L.Gr.size()))) self.nextroomId = sample(self.L.Gr.nodes(),1)[0] elif self.cdest == 'file': self.room_counter=self.room_counter+1 if self.room_counter >= self.nb_room: self.room_counter=0 self.nextroomId=self.room_seq[self.room_counter] self.wait=self.room_wait[self.room_counter] #self.sim.roomlist.append(self.nextroomId) # list of all destiantion of all nodes in object sim wp = self.L.waypointGw(self.roomId,self.nextroomId) for tup in wp[1:]: self.waypoints.append(vec3(tup) ) #nextroom = adjroom[k] # print "room : ",self.roomId # print "nextroom : ",self.nextroomId #p_nextroom = self.L.Gr.pos[self.nextroomId] #setdoors1 = self.L.Gr.node[self.roomId]['doors'] #setdoors2 = self.L.Gr.node[nextroom]['doors'] #doorId = np.intersect1d(setdoors1,setdoors2)[0] # # coord door # #unode = self.L.Gs.neighbors(doorId) #p1 = self.L.Gs.pos[unode[0]] #p2 = self.L.Gs.pos[unode[1]] #print p1 #print p2 #pdoor = (np.array(p1)+np.array(p2))/2 self.destination = self.waypoints[0] #waittime = uniform(0,10) #if self.manager: # if self.manager(self, *self.manager_args): # yield hold , self , waittime #else: # yield hold, self , waittime # self.wait=abs(gauss(50,50)) # self.wait=abs(gauss(1,1)) print 'wait',self.wait*self.interval yield hold, self, self.wait else: del self.waypoints[0] #print "wp : ", self.waypoints if len(self.waypoints)==1: self.endpoint=True self.destination = self.waypoints[0] #print "dest : ", self.destination else: yield hold, self, self.interval else: # self.update() self.world.update_boid(self) self.net.update_pos(self.ID,conv_vecarr(self.position),self.sim.now()) yield hold, self, self.interval
def __init__(self, ID = 0, interval=0.05,roomId=0, L=[], net=Network(), wld = world(),sim=None,moving=True,froom=[],wait=1.0,save=[]): """ boid is initialized in a room of Layout L """ #GeomNetType = np.dtype([('Id',int), # ('time',int), # ('p',float,(1,3)), # ('v',float,(1,3)), # ('a',float,(1,3))]) Person3.npers +=1 Process.__init__(self,name='Person3_ID'+str(ID),sim=sim) self.ID=ID self.L = L self.world = wld self.interval = interval self.manager = None self.manager_args = [] self.waypoints = [] self.moving=moving self.roomId = roomId self.forbidroomId = froom self.nextroomId = int(np.floor(uniform(0,self.L.Gr.size()))) while self.nextroomId == self.roomId or (self.nextroomId in self.forbidroomId) or (self.nextroomId in self.sim.roomlist): # test destination different de l'arrive self.nextroomId = int(np.floor(uniform(0,self.L.Gr.size()))) self.sim.roomlist.append(self.nextroomId) # list of all destiantion of all nodes in object sim self.wp = self.L.waypoint(roomId,self.nextroomId) for tup in self.wp[1:]: self.waypoints.append(vec3(tup) ) try: self.position = vec3(L.Gr.pos[roomId][0],L.Gr.pos[roomId][1]) except: self.position = vec3() # self.old_pos = vec3() self.stuck = 0 self.destination = self.waypoints[0] self.velocity = vec3() self.localx = vec3(1, 0) self.localy = vec3(0, 1) self.world.add_boid(self) # GeomNet = np.vstack((GeomNet,np.array((Nnodes,0,[list(self.position)],[list(self.velocity)],[list(self.velocity)]),dtype=GeomNetType))) # from Helbing, et al "Self-organizing pedestrian movement" self.max_speed = 1.2#normalvariate(1.0, 0.26) self.desired_speed = self.max_speed self.radius = normalvariate(self.average_radius, 0.025) / 2 self.intersection = vec3() self.arrived = False self.endpoint = False self.behaviors = [] self.steering_mind = default_steering_mind self.cancelled = 0 self.net=net self.wait=wait self.save=save if 'mysql' in self.save: config = ConfigParser.ConfigParser() config.read(pyu.getlong('simulnet.ini','ini')) sql_opt = dict(config.items('Mysql')) self.db = Database(sql_opt['host'],sql_opt['user'],sql_opt['passwd'],sql_opt['dbname']) self.date = datetime.datetime.now()
def calculate(self, boid): """ calculate boid behavior Parameters ---------- boid Returns ------- steering """ the_world = boid.world walls = the_world.obstacles(boid) acceleration = vec3() front_intersect = left_intersect = right_intersect = False front_distance = left_distance = right_distance = 10 speed = boid.velocity.length() front_check = FCHK + speed * 0.5 side_check = SCHK + speed * 0.5 front_test = boid.localy.scale(front_check) left_test = (boid.localy - boid.localx).scale(side_check) right_test = (boid.localy + boid.localx).scale(side_check) position = boid.position boid.intersection = None checked = [] df = FCHK + 0.5 dl = SCHK dr = SCHK acceleration = vec3() for wall in walls: # if wall in checked: continue # checked.append(wall) intersect, distance_along_check, direction = self.test_intersection( boid, wall, position, front_test, method='direct') if intersect: if df > distance_along_check: df = distance_along_check if intersect and distance_along_check < front_distance: front_intersect = True front_distance = distance_along_check front_direction = direction # intersect, distance_along_check, direction = self.test_intersection(boid, wall, position, left_test,method = 'direct') # if intersect: # if dl > distance_along_check: # dl = distance_along_check # if not front_intersect and intersect and distance_along_check < left_distance: # left_intersect = True # left_distance = distance_along_check # left_direction = direction # intersect, distance_along_check, direction = self.test_intersection(boid, wall, position, right_test,method = 'direct') # if intersect: # if dr > distance_along_check: # dr = distance_along_check # if not front_intersect and intersect and distance_along_check < right_distance: # right_intersect = True # right_distance = distance_along_check # right_direction = direction # if front_intersect or left_intersect or right_intersect : # break # # print speed if front_intersect: sf = 1 / max(front_distance**2, 1e-9) if front_direction == 'left': acceleration += -vec3(boid.localy.scale(sf).tolist()) acceleration += vec3(boid.localx.scale(sf).tolist()) else: acceleration += -vec3(boid.localy.scale(sf).tolist()) acceleration += -vec3(boid.localx.scale(sf).tolist()) # acceleration += boid.localx.scale(sr) # if left_intersect: # sl = 1 / max(sqrt(left_distance),1e-9) # acceleration += boid.localx.scale(sl) # if right_intersect: # sr = 1 / max(sqrt(right_distance),1e-9) # acceleration += -boid.localx.scale(sr) return acceleration
def move(self): """ Gr : Graph of rooms """ while True: while self.cancelled: yield passivate, self print "Person.move: activated after being cancelled" checked = [] for zone in self.world.zones(self): if zone not in checked: checked.append(zone) zone(self) acceleration = self.steering_mind(self) acceleration = acceleration.truncate(self.max_acceleration) self.acceleration = acceleration velocity = self.velocity + acceleration * self.interval self.velocity = velocity.truncate(self.max_speed) if velocity.length() > 0.2: # record direction only when we've really had some self.localy = velocity.normalize() self.localx = vec3(self.localy.y, -self.localy.x) self.position = self.position + self.velocity * self.interval self.update() self.world.update_boid(self) if self.arrived: self.arrived = False if self.endpoint: self.endpoint=False self.roomId = self.nextroomId # # Si porte on continue # # # ig destination --> next room # #adjroom = self.L.Gr.neighbors(self.roomId) #Nadjroom = len(adjroom) self.nextroomId = int(np.floor(uniform(0,self.L.Gr.size()))) while (self.nextroomId == self.roomId) or (self.nextroomId in self.forbid) : # test destination different de l'arrive self.nextroomId = int(np.floor(uniform(0,self.L.Gr.size()))) wp = self.L.waypoint(self.roomId,self.nextroomId) for tup in wp[1:]: self.waypoints.append(vec3(tup)) #nextroom = adjroom[k] print "room : ",self.roomId print "nextroom : ",self.nextroomId #p_nextroom = self.L.Gr.pos[self.nextroomId] #setdoors1 = self.L.Gr.node[self.roomId]['doors'] #setdoors2 = self.L.Gr.node[nextroom]['doors'] #doorId = np.intersect1d(setdoors1,setdoors2)[0] # # coord door # #unode = self.L.Gs.neighbors(doorId) #p1 = self.L.Gs.pos[unode[0]] #p2 = self.L.Gs.pos[unode[1]] #print p1 #print p2 #pdoor = (np.array(p1)+np.array(p2))/2 self.destination = self.waypoints[0] #waittime = uniform(0,10) #if self.manager: # if self.manager(self, *self.manager_args): # yield hold , self , waittime #else: # yield hold, self , waittime yield hold, self, 50*abs(gauss(5,2.5)) else: del self.waypoints[0] #print "wp : ", self.waypoints if len(self.waypoints)==1: self.endpoint=True self.destination = self.waypoints[0] #print "dest : ", self.destination else: yield hold, self, self.interval
def calculate(self, boid): the_world = boid.world walls = the_world.obstacles(boid) acceleration = vec3() front_intersect = left_intersect = right_intersect = False front_distance = left_distance = right_distance = 30000 speed = boid.velocity.length() front_check = 0.1 + speed * 0.5 side_check = 0.1 + speed * 0.5 front_test = boid.localy.scale(front_check) left_test = (boid.localy - boid.localx).scale(side_check) right_test = (boid.localy + boid.localx).scale(side_check) position = boid.position boid.intersection = None checked = [] for wall in walls: if wall in checked: continue checked.append(wall) intersect, distance_along_check, direction = self.test_intersection(boid, wall, position, front_test,method = 'gauss') # if intersect: # pdb.set_trace() if intersect and distance_along_check < front_distance: front_intersect = True front_distance = distance_along_check front_direction = direction intersect, distance_along_check, direction = self.test_intersection(boid, wall, position, left_test,method = 'direct') if not front_intersect and intersect and distance_along_check < left_distance: left_intersect = True left_distance = distance_along_check left_direction = direction intersect, distance_along_check, direction = self.test_intersection(boid, wall, position, right_test,method = 'direct') if not front_intersect and intersect and distance_along_check < right_distance: right_intersect = True right_distance = distance_along_check right_direction = direction if front_intersect or left_intersect or right_intersect : break # print speed # # parabolic speed d_no_influ = 0.1 # m repuls = boid.velocity.length() #/ boid.max_speed # speed = (repuls/(d_no_influ**2)*min(distance_along_check,d_no_influ)**2 - 2*repuls/(d_no_influ)*min(distance_along_check,d_no_influ) + repuls) #/ boid.max_speed # speed = max (1.2*boid.max_speed, 1.0/(sqrt(2*pi*d_no_influ**2))*exp(-repuls**2/(2**d_no_influ**2))) speed = max (boid.max_speed, 3.0/max(0.0001,(1.*repuls))) # speed = boid.velocity.length() / boid.max_speed # ORIGINAL CODE if front_intersect: if front_direction == 'left': acceleration = -boid.localx.scale(speed) else: acceleration = boid.localx.scale(speed) elif left_intersect: acceleration = boid.localx.scale(speed) elif right_intersect: acceleration = -boid.localx.scale(speed) else: acceleration = vec3() return acceleration
def truncate(self, max): if self.length() > max: return self.normalize() * max else: return vec3(self)
def calculate(self, boid): """ calculate boid behavior Parameters ---------- boid Returns ------- steering """ the_world = boid.world walls = the_world.obstacles(boid) acceleration = vec3() front_intersect = left_intersect = right_intersect = False front_distance = left_distance = right_distance = 30000 speed = boid.velocity.length() front_check = FCHK + speed * 0.5 side_check = SCHK + speed * 0.5 front_test = boid.localy.scale(front_check) left_test = (boid.localy - boid.localx).scale(side_check) right_test = (boid.localy + boid.localx).scale(side_check) position = boid.position boid.intersection = None checked = [] df = FCHK+0.5 dl = SCHK dr = SCHK for wall in walls: if wall in checked: continue checked.append(wall) intersect, distance_along_check, direction = self.test_intersection(boid, wall, position, front_test,method = 'gauss') if df > distance_along_check: df = distance_along_check if intersect and distance_along_check < front_distance: front_intersect = True front_distance = distance_along_check front_direction = direction intersect, distance_along_check, direction = self.test_intersection(boid, wall, position, left_test,method = 'direct') if dl > distance_along_check: dl = distance_along_check if not front_intersect and intersect and distance_along_check < left_distance: left_intersect = True left_distance = distance_along_check left_direction = direction intersect, distance_along_check, direction = self.test_intersection(boid, wall, position, right_test,method = 'direct') if dr > distance_along_check: dr = distance_along_check if not front_intersect and intersect and distance_along_check < right_distance: right_intersect = True right_distance = distance_along_check right_direction = direction # if front_intersect or left_intersect or right_intersect : # break # print speed # # parabolic speed d_no_influ = 1.0#0.3 # m repuls = boid.velocity.length() #/ boid.max_speed # speed = (repuls/(d_no_influ**2)*min(distance_along_check,d_no_influ)**2 - 2*repuls/(d_no_influ)*min(distance_along_check,d_no_influ) + repuls) #/ boid.max_speed # speed = max (1.2*boid.max_speed, 1.0/(sqrt(2*pi*d_no_influ**2))*exp(-repuls**2/(2**d_no_influ**2))) speed = max (boid.max_speed, d_no_influ/max(0.0001,(1.*repuls))) speed = boid.velocity.length() / boid.max_speed # ORIGINAL CODE # sf = max (boid.max_speed, speed*d_no_influ/max(0.0001,(1.*df))) # sl = max (boid.max_speed, speed*d_no_influ/max(0.0001,(1.*dl))) # sr = max (boid.max_speed, speed*d_no_influ/max(0.0001,(1.*dr))) sf = 1 / max(df**2,1e-9) sl = 1 / max(dl**2,1e-9) sr = 1 / max(dr**2,1e-9) acceleration = vec3() if front_intersect: if front_direction == 'left': acceleration = boid.localx.scale(sl) acceleration = -boid.localy.scale(sf) else: acceleration = boid.localx.scale(sr) acceleration = -boid.localy.scale(sf) # if left_intersect: # acceleration = boid.localx.scale(sl) # if right_intersect: # acceleration = boid.localx.scale(sr) return acceleration
def move(self): """ Move the Agent """ while True: if self.moving: if self.ID == 0: print 'meca update @', self.sim.now() while self.cancelled: yield passivate, self print "Person.move: activated after being cancelled" checked = [] for zone in self.world.zones(self): if zone not in checked: checked.append(zone) zone(self) acceleration = self.steering_mind(self) acceleration = acceleration.truncate(self.max_acceleration) self.acceleration = acceleration velocity = self.velocity + acceleration * self.interval self.velocity = velocity.truncate(self.max_speed) if velocity.length() > 0.2: # record direction only when we've really had some self.localy = velocity.normalize() self.localx = vec3(self.localy.y, -self.localy.x) self.position = self.position + self.velocity * self.interval # self.update() self.world.update_boid(self) self.net.update_pos(self.ID, conv_vecarr(self.position)) if len(self.save) != 0: p = conv_vecarr(self.position) v = conv_vecarr(self.velocity) a = conv_vecarr(self.acceleration) if 'mysql' in self.save: self.db.writemeca(self.ID, self.sim.now(), p, v, a) if 'txt' in self.save: pyu.writemeca(self.ID, self.sim.now(), p, v, a) if self.arrived: self.arrived = False if self.endpoint: self.endpoint = False #pr = self.sim.roomlist.index(self.nextroomId) #self.sim.roomlist.pop(pr) self.roomId = self.nextroomId # # Si porte on continue # # # ig destination --> next room # #adjroom = self.L.Gr.neighbors(self.roomId) #Nadjroom = len(adjroom) if self.cdest == 'random': self.nextroomId = int( np.floor(uniform(0, self.L.Gr.size()))) # test 1 ) next != actualroom # 2 ) nextroom != fordiden room # 3 ) room not share without another agent while self.nextroomId == self.roomId or ( self.nextroomId in self.forbidroomId ): # or (self.nextroomId in self.sim.roomlist): self.nextroomId = int( np.floor(uniform(0, self.L.Gr.size()))) elif self.cdest == 'file': self.room_counter = self.room_counter + 1 if self.room_counter >= self.nb_room: self.room_counter = 0 self.nextroomId = self.room_seq[self.room_counter] self.wait = self.room_wait[self.room_counter] #self.sim.roomlist.append(self.nextroomId) # list of all destiantion of all nodes in object sim wp = self.L.waypointGw(self.roomId, self.nextroomId) for tup in wp[1:]: self.waypoints.append(vec3(tup)) #nextroom = adjroom[k] # print "room : ",self.roomId # print "nextroom : ",self.nextroomId #p_nextroom = self.L.Gr.pos[self.nextroomId] #setdoors1 = self.L.Gr.node[self.roomId]['doors'] #setdoors2 = self.L.Gr.node[nextroom]['doors'] #doorId = np.intersect1d(setdoors1,setdoors2)[0] # # coord door # #unode = self.L.Gs.neighbors(doorId) #p1 = self.L.Gs.pos[unode[0]] #p2 = self.L.Gs.pos[unode[1]] #print p1 #print p2 #pdoor = (np.array(p1)+np.array(p2))/2 self.destination = self.waypoints[0] #waittime = uniform(0,10) #if self.manager: # if self.manager(self, *self.manager_args): # yield hold , self , waittime #else: # yield hold, self , waittime # self.wait=abs(gauss(50,50)) # self.wait=abs(gauss(1,1)) print 'wait', self.wait * self.interval yield hold, self, self.wait else: del self.waypoints[0] #print "wp : ", self.waypoints if len(self.waypoints) == 1: self.endpoint = True self.destination = self.waypoints[0] #print "dest : ", self.destination else: yield hold, self, self.interval else: # self.update() self.world.update_boid(self) self.net.update_pos(self.ID, conv_vecarr(self.position)) yield hold, self, self.interval
def calculate(self, boid): the_world = boid.world walls = the_world.obstacles(boid) acceleration = vec3() front_intersect = left_intersect = right_intersect = False front_distance = left_distance = right_distance = 30000 speed = boid.velocity.length() front_check = 0.1 + speed * 0.5 side_check = 0.1 + speed * 0.5 front_test = boid.localy.scale(front_check) left_test = (boid.localy - boid.localx).scale(side_check) right_test = (boid.localy + boid.localx).scale(side_check) position = boid.position boid.intersection = None checked = [] for wall in walls: if wall in checked: continue checked.append(wall) intersect, distance_along_check, direction = self.test_intersection( boid, wall, position, front_test, method='gauss') # if intersect: # pdb.set_trace() if intersect and distance_along_check < front_distance: front_intersect = True front_distance = distance_along_check front_direction = direction intersect, distance_along_check, direction = self.test_intersection( boid, wall, position, left_test, method='direct') if not front_intersect and intersect and distance_along_check < left_distance: left_intersect = True left_distance = distance_along_check left_direction = direction intersect, distance_along_check, direction = self.test_intersection( boid, wall, position, right_test, method='direct') if not front_intersect and intersect and distance_along_check < right_distance: right_intersect = True right_distance = distance_along_check right_direction = direction if front_intersect or left_intersect or right_intersect: break # print speed # # parabolic speed d_no_influ = 0.1 # m repuls = boid.velocity.length() #/ boid.max_speed # speed = (repuls/(d_no_influ**2)*min(distance_along_check,d_no_influ)**2 - 2*repuls/(d_no_influ)*min(distance_along_check,d_no_influ) + repuls) #/ boid.max_speed # speed = max (1.2*boid.max_speed, 1.0/(sqrt(2*pi*d_no_influ**2))*exp(-repuls**2/(2**d_no_influ**2))) speed = max(boid.max_speed, 3.0 / max(0.0001, (1. * repuls))) # speed = boid.velocity.length() / boid.max_speed # ORIGINAL CODE if front_intersect: if front_direction == 'left': acceleration = -boid.localx.scale(speed) else: acceleration = boid.localx.scale(speed) elif left_intersect: acceleration = boid.localx.scale(speed) elif right_intersect: acceleration = -boid.localx.scale(speed) else: acceleration = vec3() return acceleration
def copy(self): return vec3(self)
def __init__(self, ID = 0, interval=0.05,roomId=-1, L=[], net=Network(), wld = world(),seed=0,sim=None,moving=True,froom=[],wait=1.0,cdest='random', save=[],color='k',pdshow=False): """ Class Person inherits of Simpy.SimulationRT """ #GeomNetType = np.dtype([('Id',int), # ('time',int), # ('p',float,(1,3)), # ('v',float,(1,3)), # ('a',float,(1,3))]) Person.npers +=1 Process.__init__(self,name='Person_ID'+str(ID),sim=sim) self.ID=ID self.color=color self.pdshow=pdshow self.L = L self.world = wld self.interval = interval self.manager = None self.manager_args = [] self.waypoints = [] self.moving=moving # random.seed(seed) if roomId < 0: try : self.roomId = random.sample(self.L.Gr.nodes(),1)[0] except: raise NameError('This error is due to the lack of Gr graph in the Layout argument passed to Person(Object)') else: self.roomId = roomId self.forbidroomId = froom self.cdest = cdest # choose tdestination type if self.cdest == 'random': # self.nextroomId = int(np.floor(random.uniform(0,self.L.Gr.size()))) try : self.nextroomId = random.sample(self.L.Gr.nodes(),1)[0] except: raise NameError('This error is due to the lack of Gr graph in the Layout argument passed to Person(Object)') while self.nextroomId == self.roomId or (self.nextroomId in self.forbidroomId): # or (self.nextroomId in self.sim.roomlist): # test destination different de l'arrive # self.nextroomId = int(np.floor(random.uniform(0,self.L.Gr.size()))) self.nextroomId = random.sample(self.L.Gr.nodes(),1)[0] #self.sim.roomlist.append(self.nextroomId) # list of all destiantion of all nodes in object sim elif self.cdest == 'file': cfg = ConfigParser.ConfigParser() cfg.read(pyu.getlong('nodes_destination.ini','ini')) self.room_seq=eval(dict(cfg.items(self.ID))['room_seq']) self.room_wait=eval(dict(cfg.items(self.ID))['room_wait']) print 'WARNING: when nodes_destination ini file is read:' print '1) the room initialization starts in the first room of the list, not in the room configured in agent.ini' print '2) forbiden rooms are neglected' self.room_counter=1 self.nb_room=len(self.room_seq) self.roomId=self.room_seq[0] self.nextroomId=self.room_seq[self.room_counter] self.wait=self.room_wait[self.room_counter] #self.sim.roomlist.append(self.nextroomId) # list of all destiantion of all nodes in object sim self.rooms, self.wp = self.L.waypointGw(self.roomId,self.nextroomId) # self.dlist = [i in self.L.Gw.ldo for i in self.rooms] for tup in self.wp[1:]: self.waypoints.append(vec3(tup)) try: self.position = vec3(L.Gr.pos[self.roomId][0],L.Gr.pos[self.roomId][1]) except: self.position = vec3() # self.old_pos = vec3() self.stuck = 0 self.destination = self.waypoints[0] self.arrived_in = False self.velocity = vec3() self.acceleration = vec3() self.localx = vec3(1, 0) self.localy = vec3(0, 1) self.world.add_boid(self) # from Helbing, et al "Self-organizing pedestrian movement" maxspeed = 0.8 self.max_speed = maxspeed#random.normalvariate(maxspeed, 0.1) self.desired_speed = maxspeed self.radius = self.average_radius#random.normalvariate(self.average_radius, 0.025) / 2 self.intersection = vec3() self.arrived = False self.endpoint = False self.behaviors = [] self.steering_mind = default_steering_mind self.cancelled = 0 self.net=net self.wait=wait self.df = pd.DataFrame(columns=['t','x','y','vx','vy','ax','ay']) self.df._metadata = self.ID self.save=save if 'mysql' in self.save: config = ConfigParser.ConfigParser() config.read(pyu.getlong('simulnet.ini','ini')) sql_opt = dict(config.items('Mysql')) self.db = Database(sql_opt['host'],sql_opt['user'],sql_opt['passwd'],sql_opt['dbname']) self.date = datetime.datetime.now()
def calculate(self, boid): """ calculate boid behavior Parameters ---------- boid Returns ------- steering """ the_world = boid.world walls = the_world.obstacles(boid) acceleration = vec3() front_intersect = left_intersect = right_intersect = False front_distance = left_distance = right_distance = 10 speed = boid.velocity.length() front_check = FCHK + speed * 0.5 side_check = SCHK + speed * 0.5 front_test = boid.localy.scale(front_check) left_test = (boid.localy - boid.localx).scale(side_check) right_test = (boid.localy + boid.localx).scale(side_check) position = boid.position boid.intersection = None checked = [] df = FCHK+0.5 dl = SCHK dr = SCHK acceleration = vec3() for wall in walls: # if wall in checked: continue # checked.append(wall) intersect, distance_along_check, direction = self.test_intersection(boid, wall, position, front_test,method = 'direct') if intersect: if df > distance_along_check: df = distance_along_check if intersect and distance_along_check < front_distance: front_intersect = True front_distance = distance_along_check front_direction = direction # intersect, distance_along_check, direction = self.test_intersection(boid, wall, position, left_test,method = 'direct') # if intersect: # if dl > distance_along_check: # dl = distance_along_check # if not front_intersect and intersect and distance_along_check < left_distance: # left_intersect = True # left_distance = distance_along_check # left_direction = direction # intersect, distance_along_check, direction = self.test_intersection(boid, wall, position, right_test,method = 'direct') # if intersect: # if dr > distance_along_check: # dr = distance_along_check # if not front_intersect and intersect and distance_along_check < right_distance: # right_intersect = True # right_distance = distance_along_check # right_direction = direction # if front_intersect or left_intersect or right_intersect : # break # # print speed if front_intersect: sf = 1 / max(front_distance**2,1e-9) if front_direction == 'left': acceleration += -boid.localy.scale(sf) acceleration += boid.localx.scale(sf) else: acceleration += -boid.localy.scale(sf) acceleration += -boid.localx.scale(sf) # acceleration += boid.localx.scale(sr) # if left_intersect: # sl = 1 / max(sqrt(left_distance),1e-9) # acceleration += boid.localx.scale(sl) # if right_intersect: # sr = 1 / max(sqrt(right_distance),1e-9) # acceleration += -boid.localx.scale(sr) return acceleration