def Points_Intersection_DC(D, C):
"""Return all of 2 intersections point if the right D collied the circle C
Args:
D (tuple): D=(a,b) with y=ax+c
C (tuple): C=(xc,yc,r) : the circle whith center (xc,yc) and radius r
Returns:
Vector: P1,P2 all of solution
Explanation :
We know the equation of C : ((xc-x)²+(yc-y)²=r²) and the equation of D : y=a*x+b
"""
(a, b) = D
(xc, yc, r) = C
#equation Ax²+Bx+C=0
A1 = (1 + a**2)
B1 = 2 * (a * (b - yc) - xc)
#C1=(b-yc)**2-r**2
C1 = xc**2 + yc**2 + b**2 - 2 * b * yc - r**2
ret = solv_polynome(A1, B1, C1, False)
if (ret): #si il est bien défini
(x1, x2) = ret
y1 = a * x1 + b
y2 = a * x2 + b
#y1=math.sqrt(r**2-(x1-xc)**2)+yc
#y2=math.sqrt(r**2-(x2-xc)**2)+yc
return (Vector(x1, y1), Vector(x2, y2))
else:
return None
def Points_Intersection_SC(A, B, C):
"""Return all of 2 intersections point the circle C collied with AB
Args:
A : Vector
B : Vector
C : typle (xc,yc,r) circle radius : r and circle center in (xc,yc)
Returns:
list Vector: P1,P2 all of solution
"""
V = A.add(B.x_scal(-1))
V.setNorm(1)
r = Points_Intersection_DC(droite(V, A), C)
P = []
if (r):
(P1, P2) = r
A_ = Vector(min(A.x, B.x), min(A.y, B.y))
B_ = Vector(max(A.x, B.x), max(A.y, B.y))
if (A_.x < P1.x < B_.x and A_.y < P1.y <
B_.y): #si P1 est dans le carre de diagonale A_ B_
P.append(P1)
if (A_.x < P2.x < B_.x and A_.y < P2.y < B_.y):
P.append(P2)
if (len(P) == 0):
return None
else:
return P
else:
return None
def zoom_auto(self):
"""set new zoom
"""
#recherche du barycenter des objets et des drones
center=Vector(0,0)
for drone in self.environment.drones:
center.x+=drone.position.x
center.y+=drone.position.y
for obj in self.environment.objects:
center.x+=obj.center.x
center.y+=obj.center.y
center=center.x_scal(1/(self.environment.nb_objects+ self.environment.nb_drones))
self.center=center
#recherche du points le plus loin du centre
radius=0
for drone in self.environment.drones:
if(self.center.distance(drone.position)>radius):
radius=self.center.distance(drone.position)
for obj in self.environment.objects:
if(self.center.distance(obj.center)>radius):
radius=self.center.distance(obj.center)
self.radius=radius*1.5
#def du zoom
self.zoom=min(self.size)/2*1/self.radius
def __init__(self,environment:Environment, eventDisplay):
threading.Thread.__init__(self)
"""need one Surface and one simulation"""
pygame.init()
self.screen=pygame.display.set_mode((1080,720), pygame.RESIZABLE) #taille modifiable
self.environment=environment
self.eventDisplay=eventDisplay
self.size=self.screen.get_size()
self.background=(20,20,150) #never use yet
self.running = True
#definition du zoom
self.center=Vector(0,0)
self.radius=0
self.zoom=1
self.zoom_auto() #à # si vous voulez pas utiliser le zoom auto
self.clock= pygame.time.Clock()
#sauvegarde des events
#pour déplacer le centre, clique gauche continue
self.maintien_clique_gauche=False
self.position_souris_avant=Vector(0,0)
self.maintien_clique_droit=False
self.new_clique_Object=[]
self.pos_souris=[]
self.displayRadar=True
def load(self, name: str):
"""load a save of environment with json compatible
Args:
name (str): name of the folder
Returns:
[type]: [description]
"""
if name[-5:] != ".json":
name += ".json"
try:
f = open(name, 'r')
dic = json.load(f)
except:
print("Unable to open file, invalid file!")
print("nom : ", name)
return None
try:
drones = []
for drone in dic['drones']:
position = Vector(drone["position"]["x"],
drone["position"]["y"])
vitesse = Vector(drone["speed"]["vx"], drone["speed"]["vy"])
radius = drone["radius"]
color = (drone["color"][0], drone["color"][1],
drone["color"][2])
name = drone["name"]
drones.append(Drone(position, vitesse, radius, name, color))
objs = []
for obj in dic['objects']:
points = []
for point in obj["points"]:
points.append(Vector(point["x"], point["y"]))
objs.append(Object(points))
try:
points = []
for point in dic['goal']["points"]:
points.append(Vector(point["x"], point["y"]))
self.goal = Object(points)
except:
self.goal = None
self.objects = objs
self.drones = drones
self.nb_drones = len(drones)
self.nb_objects = len(objs)
except:
print("invalid file type!")
return None
def IA(self, **kwargs):
"""create one specific IA
"""
dt = kwargs.get('dt', None)
if (self.arrive):
self.next_speed.setNorm(0)
self.color = (0, 0, 0)
else:
if (self.Dt > 5):
self.Dt = 0
if (self.goal != None):
self.next_speed.setCap(
Vector(self.goal.x -
self.next_position.x, self.goal.y -
self.next_position.y).cap() +
(2 * random.random() - 1) * math.pi / 4)
else:
self.next_speed = self.speed
if (min(self.radar.rays) < 2):
if (self.speed.norm_2() < 0.1):
self.next_speed.setNorm(0.1)
else:
self.next_speed.setNorm(self.speed.norm_2() * 0.9)
else:
if (self.speed.norm_2() < 1):
self.next_speed.setNorm(self.speed.norm_2() * 1.1)
else:
self.next_speed.setNorm(1)
self.Dt += dt
if (not self.arrive):
self.T += dt
def __init__(self, position: Vector):
super().__init__(position,
Vector(0.5, 0),
0.2,
name="R2D2",
color=(50, 50, 100))
self.radar = Radar(
10,
[0, math.pi / 2, math.pi, -math.pi / 2, math.pi / 6, -math.pi / 6])
def update_screen(self):
pygame.draw.rect(self.screen, self.background, (0,0)+self.size) #recrée un fond
#dessine l'absice et l'ordonnée
pygame.draw.line(self.screen, (255,0,0),self.offset((0,-1e4)), self.offset((0, 1e4)))
pygame.draw.line(self.screen, (255,0,0),self.offset((-1e4,0)), self.offset((1e4, 0)))
#on dessine le but :
if(self.environment.goal_has_def()):
P=[]
for p in self.environment.goal.list_Points:
P.append(self.offset_Point(p.x_scal(self.zoom)))
pygame.draw.polygon(self.screen, (255,0,0), P,0)
#dessine les obstacles
for obj in self.environment.objects:
points=obj.list_Points
P=[]
for point in points:
P.append(self.offset_Point(point.x_scal(self.zoom)))
pygame.draw.polygon(self.screen, (255,255,255), P,7)
#draw all drones by circle
for drone in self.environment.drones:
pygame.draw.circle(self.screen, drone.color, self.offset_Point(drone.position.x_scal(self.zoom)), drone.radius*self.zoom)
#draw radar
if(self.displayRadar):
for i in range(drone.radar.nb_rays):
ray=Vector(1,0)
ray.setCap(drone.radar.angles_[i]+drone.speed.cap())
ray.setNorm(drone.radar.rays[i])
pygame.draw.line(self.screen, (0,200,0), self.offset_Point(drone.position.x_scal(self.zoom)),
self.offset_Point(drone.position.add(ray).x_scal(self.zoom)), 1)
#drow speed vector
pygame.draw.line(self.screen, (143,202,90), self.offset_Point(drone.position.x_scal(self.zoom)),
self.offset_Point(drone.position.add(drone.speed).x_scal(self.zoom)), 2)
if(self.eventDisplay.pause):
police = pygame.font.Font(None,60)
texte = police.render("Pause",True,pygame.Color("#FFFF00"))
a,b=texte.get_size()
self.screen.blit(texte, (self.size[0]/2-a/2, (self.size[1]-b)/3))
#on dessine le polygone en cours
if(len(self.new_clique_Object)>0):
P=[]
for point in self.new_clique_Object:
P.append(self.offset_Point(point.x_scal(self.zoom)))
P.append(self.pos_souris)
pygame.draw.lines(self.screen, (255,255,255), False, P,2)
def Point_Intersection_SS(A, B, C, D):
"""Return intersection point if the segment AB collied with CD
Args:
A (Vector): [description]
B (Vector): [description]
C (Vector): [description]
D (Vector): [description]
"""
P = Point_Intersection_DD(droite(A.add(B.x_scal(-1)), A),
droite(C.add(D.x_scal(-1)), C))
if (P):
A_ = Vector(min(A.x, B.x), min(A.y, B.y))
B_ = Vector(max(A.x, B.x), max(A.y, B.y))
C_ = Vector(min(C.x, D.x), min(C.y, D.y))
D_ = Vector(max(C.x, D.x), max(C.y, D.y))
if (A_.x < P.x < B_.x and A_.y < P.y < B_.y and C_.x < P.x < D_.x
and C_.y < P.y < D_.y):
return P
else:
return None
else:
return None
def __updateRadar(self):
list_drones = []
for drone in self.nearDrones:
pos = drone.position.add(self.position.x_scal(-1))
pos.setCap(pos.cap() - self.speed.cap())
list_drones.append(Drone(pos, Vector(0, 0), drone.radius))
list_objects = []
for obj in self.nearObjects:
P = []
for point in obj.list_Points:
pos = point.add(self.position.x_scal(-1))
pos.setCap(pos.cap() - self.speed.cap())
P.append(pos)
list_objects.append(Object(P))
self.radar.update(list_Objects=list_objects, list_Drones=list_drones)
def Point_Intersection_DD(D1, D2):
"""Calcul the intersection point between D1 and D2
Args:
D1 (tuple): (a1,b1) y=a1*x+b1
D2 (tuple): (a2,b2) y=a2*x+b2
Returns:
Vector:
"""
(a1, b1) = D1
(a2, b2) = D2
if (a1 == a2): #si les deux droites sont //
return None
x = (b2 - b1) / (a1 - a2)
y = a1 * x + b1
return Vector(x, y)
def update(self, list_Objects: list = [], list_Drones: list = []) -> None:
"""Update the Radar : with the two lists, this simulates a radar like a Lidar
Args:
list_Objects (list, Object): Object. Defaults to [].
list_Drones (list, Drone): Drone. Defaults to [].
"""
for I in range(self.nb_rays):
tetha_i = self.angles_[I] #angle of Ith rays
vdir = Vector(1, 0)
vdir.setCap(tetha_i)
A = Vector(0, 0)
B = vdir.copy()
B.setNorm(self.ranges_[I]) #radius of the Ith rays
D0 = droite(vdir, A)
list_points = [B]
for obj in list_Objects:
if (len(obj.list_Points) > 1):
C = obj.list_Points[0]
for i in range(1, len(obj.list_Points)):
D = obj.list_Points[i]
P = Point_Intersection_SS(A, B, C, D)
C = D
if (P):
list_points.append(P)
D = obj.list_Points[0]
P = Point_Intersection_SS(A, B, C, D)
if (P):
list_points.append(P)
for drone in list_Drones:
if (drone.position.norm_2() > 0.1):
r = Points_Intersection_SC(
A, B,
(drone.position.x, drone.position.y, drone.radius))
if (r):
for p in r:
list_points.append(p)
list_distance = [point.norm_2() for point in list_points]
self.rays[I] = min(list_distance)
def IA(self, **kwargs):
dt = kwargs.get('dt', None)
if (self.arrive):
self.next_speed.setCap(self.speed.cap() + math.pi / 12 * dt)
self.color = (255, random.randint(0, 255), random.randint(0, 255))
else:
if (self.Dt > 5):
self.Dt = 0
if (self.goal != None):
self.next_speed.setCap(
Vector(self.goal.x -
self.next_position.x, self.goal.y -
self.next_position.y).cap() +
(2 * random.random() - 1) * math.pi / 6)
else:
self.next_speed = self.speed
self.Dt += dt
if (not self.arrive):
self.T += dt
def inv_offsetPoint(self,a):
(x,y)=self.inv_offset(a)
return Vector(x,y)
def process_event(self, event:pygame.event):
##utilisation du zoom
if(event.type == pygame.QUIT):
self.running=False
if(event.type == pygame.MOUSEBUTTONDOWN): #si on clique avec la souris
if(event.button==1): #clique gauche
self.maintien_clique_gauche=True
x, y = event.pos #position de la souris
self.position_souris_avant=Vector(x,y) #sauvegarde
if(event.button==3): #clique droit
self.maintien_clique_droit=True
x, y = event.pos #position de la souris
#p_y=
self.new_clique_Object.append(self.inv_offsetPoint((x,y)).x_scal(1/self.zoom))
if(event.button==4): #Molette souris haut
self.zoom+=5 #on zoom
if(event.button==5): #Molette souris bas
self.zoom-=5 #on dezoom
if(self.zoom<1):
self.zoom=1
if(event.type == pygame.MOUSEBUTTONUP): # si on declique
if(event.button==1): #clique gauche
self.maintien_clique_gauche=False
if(event.button==3): #clique droit
self.maintien_clique_droit=False
if(event.type==pygame.MOUSEMOTION): #si la souris bouge
self.pos_souris=event.pos
if(self.maintien_clique_gauche): #si le clique gauche est tjrs enfoncé
x, y = event.pos #position souris
delta=self.position_souris_avant.add(Vector(x,y).x_scal(-1)) #delta=avant-après
self.center=self.center.add(delta.x_scal(-1)) #centre=centre-delta
self.position_souris_avant=Vector(x,y)
if(event.type==pygame.KEYDOWN): #si on apuye sur une touche clavier
if(event.key==pygame.K_SPACE): #espace
if(self.eventDisplay.pause): #si on était en pause on enlève
self.eventDisplay.pause=False
else: #si on était pas en pause on met pause
self.eventDisplay.pause=True
if(event.key==const.K_q):
self.stop()
if(event.key==const.K_a):
if(self.displayRadar):
self.displayRadar=False
else:
self.displayRadar=True
if(event.key==const.K_PLUS or event.key==const.K_KP_PLUS):
self.eventDisplay.coefTime*=1.8
if(event.key==const.K_MINUS or event.key==const.K_KP_MINUS):
self.eventDisplay.coefTime*=1/2
if(event.key == const.K_ESCAPE): #on appuye sur echap => annule le polygone
self.new_clique_Object=[]
if(event.key == const.K_RETURN): #sur enter on confirme le polygone
if(len(self.new_clique_Object)>1):
self.environment.addObject(self.new_clique_Object)
self.new_clique_Object=[]
if(event.type == pygame.QUIT):
self.stop()
else:
if (self.speed.norm_2() < 1):
self.next_speed.setNorm(self.speed.norm_2() * 1.1)
else:
self.next_speed.setNorm(1)
self.Dt += dt
if (not self.arrive):
self.T += dt
if __name__ == '__main__':
if (True):
drone1 = MyDrone(Vector(0, 0))
drone2 = Drone(Vector(1, 1), Vector(0.5, 0.5), 0.2)
drone3 = Drone(Vector(0, 1), Vector(0, -0.5), 0.2)
drone4 = Drone(Vector(4, 1), Vector(0, 0.5), 0.2)
drone5 = Drone(Vector(4, 0), Vector(0.5, 0), 0.2)
drone6 = Drone(Vector(2, 1), Vector(0.5, 0), 0.2)
#creation des obstacles, liste des coins
obj = Object([
Vector(5, 5),
Vector(3, 5),
Vector(3, 3),
Vector(5, 3),
Vector(7, 5)
])
obj1 = Object([Vector(-10, 5), Vector(-13, 5), Vector(-5, 3)])
class Display():
"""this class use pygame to display the simulation"""
def __init__(self,environment:Environment, eventDisplay):
threading.Thread.__init__(self)
"""need one Surface and one simulation"""
pygame.init()
self.screen=pygame.display.set_mode((1080,720), pygame.RESIZABLE) #taille modifiable
self.environment=environment
self.eventDisplay=eventDisplay
self.size=self.screen.get_size()
self.background=(20,20,150) #never use yet
self.running = True
#definition du zoom
self.center=Vector(0,0)
self.radius=0
self.zoom=1
self.zoom_auto() #à # si vous voulez pas utiliser le zoom auto
self.clock= pygame.time.Clock()
#sauvegarde des events
#pour déplacer le centre, clique gauche continue
self.maintien_clique_gauche=False
self.position_souris_avant=Vector(0,0)
self.maintien_clique_droit=False
self.new_clique_Object=[]
self.pos_souris=[]
self.displayRadar=True
def zoom_auto(self):
"""set new zoom
"""
#recherche du barycenter des objets et des drones
center=Vector(0,0)
for drone in self.environment.drones:
center.x+=drone.position.x
center.y+=drone.position.y
for obj in self.environment.objects:
center.x+=obj.center.x
center.y+=obj.center.y
center=center.x_scal(1/(self.environment.nb_objects+ self.environment.nb_drones))
self.center=center
#recherche du points le plus loin du centre
radius=0
for drone in self.environment.drones:
if(self.center.distance(drone.position)>radius):
radius=self.center.distance(drone.position)
for obj in self.environment.objects:
if(self.center.distance(obj.center)>radius):
radius=self.center.distance(obj.center)
self.radius=radius*1.5
#def du zoom
self.zoom=min(self.size)/2*1/self.radius
def process_event(self, event:pygame.event):
##utilisation du zoom
if(event.type == pygame.QUIT):
self.running=False
if(event.type == pygame.MOUSEBUTTONDOWN): #si on clique avec la souris
if(event.button==1): #clique gauche
self.maintien_clique_gauche=True
x, y = event.pos #position de la souris
self.position_souris_avant=Vector(x,y) #sauvegarde
if(event.button==3): #clique droit
self.maintien_clique_droit=True
x, y = event.pos #position de la souris
#p_y=
self.new_clique_Object.append(self.inv_offsetPoint((x,y)).x_scal(1/self.zoom))
if(event.button==4): #Molette souris haut
self.zoom+=5 #on zoom
if(event.button==5): #Molette souris bas
self.zoom-=5 #on dezoom
if(self.zoom<1):
self.zoom=1
if(event.type == pygame.MOUSEBUTTONUP): # si on declique
if(event.button==1): #clique gauche
self.maintien_clique_gauche=False
if(event.button==3): #clique droit
self.maintien_clique_droit=False
if(event.type==pygame.MOUSEMOTION): #si la souris bouge
self.pos_souris=event.pos
if(self.maintien_clique_gauche): #si le clique gauche est tjrs enfoncé
x, y = event.pos #position souris
delta=self.position_souris_avant.add(Vector(x,y).x_scal(-1)) #delta=avant-après
self.center=self.center.add(delta.x_scal(-1)) #centre=centre-delta
self.position_souris_avant=Vector(x,y)
if(event.type==pygame.KEYDOWN): #si on apuye sur une touche clavier
if(event.key==pygame.K_SPACE): #espace
if(self.eventDisplay.pause): #si on était en pause on enlève
self.eventDisplay.pause=False
else: #si on était pas en pause on met pause
self.eventDisplay.pause=True
if(event.key==const.K_q):
self.stop()
if(event.key==const.K_a):
if(self.displayRadar):
self.displayRadar=False
else:
self.displayRadar=True
if(event.key==const.K_PLUS or event.key==const.K_KP_PLUS):
self.eventDisplay.coefTime*=1.8
if(event.key==const.K_MINUS or event.key==const.K_KP_MINUS):
self.eventDisplay.coefTime*=1/2
if(event.key == const.K_ESCAPE): #on appuye sur echap => annule le polygone
self.new_clique_Object=[]
if(event.key == const.K_RETURN): #sur enter on confirme le polygone
if(len(self.new_clique_Object)>1):
self.environment.addObject(self.new_clique_Object)
self.new_clique_Object=[]
if(event.type == pygame.QUIT):
self.stop()
def offset(self, a): #def décalage par rapport au centre de la fenètre
x,y=a
x=x+self.center.x+self.size[0]/2
y=-y+self.center.y+self.size[1]/2
return (x,y)
def inv_offset(self, a): #inversion du décalage par rapport au centre de la fenetre
x,y=a
x=x-(self.center.x+self.size[0]/2)
y=-y+self.center.y+self.size[1]/2
return (x,y)
def inv_offsetPoint(self,a):
(x,y)=self.inv_offset(a)
return Vector(x,y)
def offset_Point(self, p): #espèce de sur-définition
return self.offset((p.x, p.y))
def update_screen(self):
pygame.draw.rect(self.screen, self.background, (0,0)+self.size) #recrée un fond
#dessine l'absice et l'ordonnée
pygame.draw.line(self.screen, (255,0,0),self.offset((0,-1e4)), self.offset((0, 1e4)))
pygame.draw.line(self.screen, (255,0,0),self.offset((-1e4,0)), self.offset((1e4, 0)))
#on dessine le but :
if(self.environment.goal_has_def()):
P=[]
for p in self.environment.goal.list_Points:
P.append(self.offset_Point(p.x_scal(self.zoom)))
pygame.draw.polygon(self.screen, (255,0,0), P,0)
#dessine les obstacles
for obj in self.environment.objects:
points=obj.list_Points
P=[]
for point in points:
P.append(self.offset_Point(point.x_scal(self.zoom)))
pygame.draw.polygon(self.screen, (255,255,255), P,7)
#draw all drones by circle
for drone in self.environment.drones:
pygame.draw.circle(self.screen, drone.color, self.offset_Point(drone.position.x_scal(self.zoom)), drone.radius*self.zoom)
#draw radar
if(self.displayRadar):
for i in range(drone.radar.nb_rays):
ray=Vector(1,0)
ray.setCap(drone.radar.angles_[i]+drone.speed.cap())
ray.setNorm(drone.radar.rays[i])
pygame.draw.line(self.screen, (0,200,0), self.offset_Point(drone.position.x_scal(self.zoom)),
self.offset_Point(drone.position.add(ray).x_scal(self.zoom)), 1)
#drow speed vector
pygame.draw.line(self.screen, (143,202,90), self.offset_Point(drone.position.x_scal(self.zoom)),
self.offset_Point(drone.position.add(drone.speed).x_scal(self.zoom)), 2)
if(self.eventDisplay.pause):
police = pygame.font.Font(None,60)
texte = police.render("Pause",True,pygame.Color("#FFFF00"))
a,b=texte.get_size()
self.screen.blit(texte, (self.size[0]/2-a/2, (self.size[1]-b)/3))
#on dessine le polygone en cours
if(len(self.new_clique_Object)>0):
P=[]
for point in self.new_clique_Object:
P.append(self.offset_Point(point.x_scal(self.zoom)))
P.append(self.pos_souris)
pygame.draw.lines(self.screen, (255,255,255), False, P,2)
def run(self):
t1=t0=time.time() #save time
T=[]
while(not self.eventDisplay.stop):
for event in pygame.event.get(): #pécho les events
self.process_event(event) #travail event
self.update_screen() #modifie la fenètre
pygame.display.flip() #update
self.size=self.screen.get_size() #reupdate size
self.clock.tick(30)
#time.sleep(max(1/30-time.time()-t0,0))
self.eventDisplay.dt=time.time()-t0
t0=time.time()
if(len(T)==100):
# print('mean fps',1/statistics.mean(T))
T=[]
T.append(self.eventDisplay.dt)
def stop(self):
self.eventDisplay.stop=True
pygame.quit()
droite(C.add(D.x_scal(-1)), C))
if (P):
A_ = Vector(min(A.x, B.x), min(A.y, B.y))
B_ = Vector(max(A.x, B.x), max(A.y, B.y))
C_ = Vector(min(C.x, D.x), min(C.y, D.y))
D_ = Vector(max(C.x, D.x), max(C.y, D.y))
if (A_.x < P.x < B_.x and A_.y < P.y < B_.y and C_.x < P.x < D_.x
and C_.y < P.y < D_.y):
return P
else:
return None
else:
return None
if __name__ == '__main__':
A = Vector(-5, -5)
B = Vector(5, 5)
C = Vector(5 + 2, -5 + 2)
D = Vector(-5 + 2, 5 + 2)
P = Point_Intersection_SS(A, B, C, D)
plt.plot([A.x, B.x], [A.y, B.y], 'r')
plt.plot([C.x, D.x], [C.y, D.y], 'b')
if (P):
plt.plot([P.x], [P.y], 'ok')
plt.show()
def droneGoal(self, i: int):
"""check if the drone i achieve the goal"""
distance = self.environment.drones[i].next_position.distance(
self.environment.goal.center)
if (distance < self.environment.drones[i].get_radius() +
self.environment.goal.radius):
"""si il y a collision entre les cercles du drone et de l'objet, on regarde si il y a une vrai collision"""
#//https://openclassrooms.com/fr/courses/1374826-theorie-des-collisions/1375352-formes-plus-complexes page fermé :/
A = self.environment.goal.list_Points[0]
for l in range(1, len(self.environment.goal.list_Points)):
B = self.environment.goal.list_Points[l]
if (self.__collisionDroiteCercle(
A, B, self.environment.drones[i].next_position,
self.environment.drones[i].radius)
): #si le robot colisionne avec la ligne
##on verifie si le drone ne s'éloigne pas de la ligne
return True
#on calcul le point d'intersection entre V et (AB)
P = intersection(A, Vector(A.x - B.x, A.y - B.y),
self.environment.drones[i].next_position,
self.environment.drones[i].next_speed)
if (P == None):
return False
else:
x = self.environment.drones[i].next_position.x
y = self.environment.drones[i].next_position.y
vx = self.environment.drones[i].next_speed.x
vy = self.environment.drones[i].next_speed.y
(a, b, c) = droite(A, Vector(A.x - B.x, A.y - B.y))
if (x < P.x):
if (b == 0):
return vx > 0
a, c = -a / b, -c / b
if (y < a * x +
c): #si le point est sous la droite
return vx > 0 #si il se dirige vers la droite
else:
return vx < 0
else:
if (b == 0):
return vx < 0
a, c = -a / b, -c / b
if (y < a * x +
c): #si le point est sous la droite
return vx < 0 #si il se dirige vers la droite
else:
return vx > 0
A = B
return self.__collisionDroiteCercle(
A, self.environment.goal.list_Points[0],
self.environment.drones[i].next_position,
self.environment.drones[i].radius)
else:
return False