def calcAngle(p, obstacles):
viserSortie(p)
dt, j = -1, 0
xp, yp = p.x(), p.y()
O = []
for i in range(len(obstacles)):
Test, Sol = intersectionCercle(p, i, obstacles)
if Test:
O.append([i, Sol])
if len(O) == 0:
pass
else:
for i in range(len(O)):
j, Sol = O[i]
dt1, dt2 = distancePoints((xp, yp), Sol[1]), distancePoints(
(xp, yp), Sol[1])
if dt1 < dt or dt2 < dt or dt == -1:
dt = min(dt1, dt2)
k = i
l = O[k][0]
xc, yc = obstacles[l][0]
r = obstacles[l][1]
dx, dy = xp - xc, yp - yc
if distancePoints((xp, yp), (xc, yc)) <= r + 3 * rp:
if dx == 0:
p.thetaDeg(180)
if dy <= 0:
p.theta = getAngle(dy, -dx)
if dy > 0:
p.theta = getAngle(-dy, dx)
else:
dy = (yc + r + rp - yp if yp > yc else yc - r - rp - yp)
p.theta = getAngle(-dx, dy)
def viserSortie(p):
x, yi, ys = 0, exits[0][0], exits[0][1]
if yi + p.rayonPropre <= p.y() <= ys - p.rayonPropre:
p.thetaDeg(180)
if p.y() > ys - p.rayonPropre:
p.theta = getAngle(x - p.x(), ys - p.rayonPropre - p.y())
if p.y() < yi + p.rayonPropre:
p.theta = getAngle(x - p.x(), yi + p.rayonPropre - p.y())
def shift(self, x, angle):
#donner l'angle en degrés
angle = angle * np.pi / 180
nx = self.x() + x * np.cos(self.theta - angle)
ny = self.y() + x * np.sin(self.theta - angle)
self.angle = getAngle(nx, ny)
self.rayon = distancePoints([0, 0], [nx, ny])
def __init__(self,
x,
y,
theta=180,
rayonPropre=rp,
blocked=False,
bloquant=-1):
self.rayon = distancePoints([0, 0], [x, y])
self.angle = getAngle(x, y) # we use polar coordinates
self.theta = theta * np.pi / 180 # direction in which person walks
self.rayonPropre = rayonPropre # radius of a person
self.blocked = blocked # can not move
self.bloquant = bloquant # blocks someone
def avancer(self, x):
nx = self.x() + x * np.cos(self.theta)
ny = self.y() + x * np.sin(self.theta)
self.angle = getAngle(nx, ny)
self.rayon = distancePoints([0, 0], [nx, ny])