In [1430]:

    

import logging
import itertools
import numpy as np

import shapely.geometry

import scipy.spatial
import pandas

import matplotlib
import descartes
import rtree
logging.basicConfig()
logger = logging.getLogger("person")
logger.setLevel(logging.INFO)


    

In [1431]:

    

# Number of persons
N = 10
WALLWIDTH = 0.1

# initial position
x = np.random.rand(N)
y = np.random.rand(N)

#intial speed
u = np.random.rand(N) - 0.5
v = np.random.rand(N) - 0.5

# thresholds (things within this distance are annoying)
threshold = np.random.rand(N) * 0.2


    

In [1432]:

    

# generate shapely objects
geom = [shapely.geometry.Point(x_i,y_i) for x_i,y_i in zip(x,y)]
# create a dataframe
persons = pandas.DataFrame(data=dict(x=x, y=y, u=u, v=v, geom=geom, threshold=threshold))


    

In [1433]:

    

# create a wall
shell = np.array([[0-WALLWIDTH,0-WALLWIDTH], [0-WALLWIDTH, 1+WALLWIDTH], [1+WALLWIDTH, 1+WALLWIDTH], [1+WALLWIDTH, 0-WALLWIDTH], [0-WALLWIDTH,0-WALLWIDTH]])
hole = np.array([[0,0], [1,0],[1,1], [0,1],  [0,0]]) # should be ccw 
obstructions = shapely.geometry.Polygon(shell=shell, 
                                        holes=[hole])


    

In [1434]:

    

# plot the configuration
fig, ax = plt.subplots()
q = ax.quiver(*map(np.array, [persons['x'],persons['y'],persons['u'],persons['v']]))
ax.plot(*obstructions.interiors[0].xy, linestyle='-')
ax.add_patch(descartes.PolygonPatch(obstructions, alpha=0.3))
ax.autoscale()
#for i, row in persons.iterrows():
#    ax.annotate(str(i), (row['x'], row['y']), xytext=(row['x']+0.05, row['y']+0.05))


    

In [1435]:

    

# create an index for nearest neighbour search
index = rtree.Rtree()
for i, row in persons.iterrows():
    index.add(i, coordinates=(row['x'], row['y'], row['x'], row['y']), obj=row)


    

In [1436]:

    

# look for forces
def update_forces(persons, obstructions, ax=None):
    index = rtree.Rtree()
    for i, row in persons.iterrows():
        index.add(i, coordinates=(row['x'], row['y'], row['x'], row['y']), obj=row)
    for i, person in persons.iterrows():
        geom = person['geom']
        seq = obstructions.interiors[0].nearest_points(geom)
        coords = np.array(seq)[:,:2]
        dist = shapely.geometry.Point(coords[0,0], coords[0,1]).distance(geom)
        u = 0
        v = 0
        if dist < person['threshold']:
            
            if ax:
                line = matplotlib.lines.Line2D(coords[:,0], coords[:,1])
                ax.add_artist(line)
            # move in oposite direction
            spd = 1/dist if dist > 0 else 0.1
            spd = min(spd, 0.1)
            spd = 0.1
            dx = (coords[1,0] - coords[0,0])
            dy = (coords[1,1] - coords[0,1])
            direction = np.arctan2(dy, dx)
            u += np.cos(direction)*spd
            v += np.sin(direction)*spd
        for nn in index.nearest(geom.bounds, objects=True, num_results=2):
            dist = nn.object['geom'].distance(geom)
            if dist < person['threshold']:
                if ax:
                    line = matplotlib.lines.Line2D([person['x'], nn.object['x']], 
                                                [person['y'], nn.object['y']], 
                                                color='red' ,alpha=0.3) 
                    ax.add_artist(line)
                spd = 1/dist if dist > 0 else 0.1
                spd = min(spd, 0.1)
                spd = 0.1
                dx = (nn.object['x'] - geom.x)
                dy = (nn.object['y'] - geom.y)
                direction = np.arctan2(dy, dx)
                u += np.cos(direction)*spd
                v += np.sin(direction)*spd
        person['u'] = u
        person['v'] = v
        persons.ix[i] = person


    

In [1437]:

    

def move(persons, dt=1.0):
    persons['x'] = persons['x'] + persons['u']*dt
    persons['y'] = persons['y'] + persons['v']*dt
    geom = [shapely.geometry.Point(x_i,y_i) for x_i,y_i in zip(persons['x'],persons['y'])]
    persons['geom'] = geom


    

In [1464]:

    

update_forces(persons, obstructions, ax=ax)
move(persons, dt=0.1)
     
q.set_UVC(U=np.array(persons['u']), V=np.array(persons['v']))
offsets = np.array(persons[['x','y']])
q.set_offsets(offsets)
ax.autoscale_view()
fig


    

    
Out[1464]:












    
Out[1464]:












    
Out[1464]:

In [1439]:

    

np.cos(0)


    

    
Out[1439]:





1.0

In [1439]: