diff --git a/PAR 152/positionnement.py b/PAR 152/positionnement.py new file mode 100644 index 0000000000000000000000000000000000000000..0445014b10ba916a59a4b33b55975dc1c2581a2f --- /dev/null +++ b/PAR 152/positionnement.py @@ -0,0 +1,149 @@ +from scipy.cluster.hierarchy import linkage, fcluster, dendrogram +from numpy import array +import numpy as np +from math import sqrt +import matplotlib.pyplot as plt + +carte = [] #carte contenant les coordonnées (x,y) de chaque plot, l'indince i correspond au plot i +carte = np.array(carte) + + + +boxes = [array([ 71.55448151, 48.70854568, 117.95120239, 128.82731628, + 0.96357524, 0. ]), array([265.82519531, 63.02380371, 317.72790527, 161.43673706, + 0.95850408, 0. ]), array([204.24989319, 124.79711151, 320.40344238, 312.65106201, + 0.95731342, 0. ]), array([392.64465332, 120.61515808, 507. , 307.8104248 , + 0.95594758, 0. ]), array([467.19827271, 83.70771027, 507. , 204.51119995, + 0.95114303, 0. ]), array([183.88371277, 18.13962936, 212.38748169, 72.65458679, + 0.94486743, 0. ]), array([467.60531616, 21.23407555, 501.08724976, 79.89296722, + 0.93922979, 0. ]), array([ 39.37908936, 126.7710495 , 137.76670837, 320.09881592, + 0.93714899, 0. ]), array([346.33688354, 74.00056458, 411.95550537, 180.61778259, + 0.9355219 , 0. ]), array([198.56393433, 60.01593781, 251.088974 , 148.07209778, + 0.93453765, 0. ]), array([107.80426025, 17.43326759, 135.90390015, 67.50372314, + 0.92372549, 0. ]), array([226.01693726, 20.78704262, 255.77336121, 71.31395721, + 0.92241275, 0. ]), array([ 0.73022616, 43.94894028, 50.41823196, 118.34261322, + 0.92166394, 0. ]), array([413.37960815, 21.73137093, 447.26065063, 77.86586761, + 0.91958499, 0. ]), array([135.58837891, 52.80698395, 189.51263428, 134.50894165, + 0.91850674, 0. ]), array([366.38388062, 22.89640999, 395.97808838, 77.14238739, + 0.91489226, 0. ]), array([23.01332664, 17.67047882, 51.43401337, 67.86820221, 0.91442037, + 0. ]), array([154.2374115 , 88.37498474, 231.43321228, 213.48825073, + 0.9137339 , 0. ]), array([ 22.97482491, 88.09642029, 71.89588165, 214.76116943, + 0.90880054, 0. ]), array([63.18217087, 15.96899509, 91.19147491, 69.62237549, 0.90012252, + 0. ]), array([12.51589012, 22.42526627, 51.88586807, 94.58940887, 0.38236487, + 0. ])] +milieu = 2 +F = 500 +h_reel = 0.7 +y0 = (boxes[milieu][0] + boxes[milieu][2])/2 + +seuil = 0.5 + +def distance(pt1,pt2): + return sqrt((pt1[0]-pt2[0])**2 + (pt1[1]-pt2[1])**2) + + +def boite2coord(x1,y1,x2,y2): + + d = F*h_reel/abs(y1-y2) + + y_r = ((x1+x2)/2 - y0)*d/F + sin_theta= y_r/d + x_r = d*(1-sin_theta**2)**0.5 + + return x_r,y_r + +def changement_referentiel(coord_voiture, coord_plot): + theta = coord_voiture[2] + x_v = coord_plot[0] - coord_voiture[0] + y_v = coord_plot[1] - coord_voiture[1] + x = x_v*np.cos(theta) + y_v*np.sin(theta) + y = x_v*np.sin(theta) + y_v*np.cos(theta) + return [x,y] + + + + +def get_groupe_max(cluster): + occ = [0 for i in range(max(cluster)+1)] + print(occ) + for i in cluster: + occ[i] += 1 + return np.argmax(occ) + +def association(cone, old_pos_cone): + dist = [] + for pt in old_pos_cone: + dist.append(distance(cone,pt)) + res = np.argmin(dist) + if dist[res]<seuil: + return res + else: + return None + + + +def identification_cones(old_pos, old_pos_cone, new_pos_cone, ind_cone): + #Regrouper les cones par deux si possible + + correspondance = [-1 for i in range(len(new_pos_cone))] + + for i in range(len(new_pos_cone)): + cone = association(new_pos_cone[i], old_pos_cone) + if cone != None: + correspondance[i] = cone + + liste_ind_cone = [-1]*len(new_pos_cone) + for i in range(len(new_pos_cone)): + if correspondance[i] != -1: + liste_ind_cone[i] = ind_cone[correspondance[i]] + + new_pos = positionnement(new_pos_cone, liste_ind_cone) #permet de renvoyer ls cones + + for i in range(len(liste_ind_cone)): + if liste_ind_cone[i] == -1: + liste_ind_cone[i] = identification_new(new_pos, i) + + new_pos = positionnement(new_pos_cone, liste_ind_cone) + + return new_pos, liste_ind_cone + + + + +def identification_new(pos, pos_cone): + + new_pos_cone = changement_referentiel(pos, pos_cone) + + dist = [] + for cone in carte: + dist.append(distance(cone,new_pos_cone)) + res = np.argmin(dist) + + return res + + + +def positionnement(new_pos_cone, liste_ind_cone): + pass + + + + + + +if __name__ == '__main__': + + #initialisation + pos = [0,0,0] + old_pos_cone = [] + ind_cone = [] + + #boucle + pos_cone = [] + for i in boxes: + pos_cone = boite2coord(i[0],i[1],i[2],i[3]) + pos, liste_ind_cone = identification_cones(pos, old_pos_cone, pos_cone, ind_cone) + old_pos_cone = pos_cone + + + \ No newline at end of file