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 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, 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 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):
""" 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 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 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 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 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 default_steering_mind(boid):
"""Simple sum of all steering vectors."""
acceleration = vec3()
for behavior in boid.behaviors:
acceleration += behavior.calculate(boid)
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 = 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
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 copy(self):
return vec3(self)
