angle = f(x,y)

PAR 152/Yolo V3/TensorFlow-2.x-YOLOv3-master/MCL.py

@@ -10,7 +10,7 @@ import matplotlib.pyplot as plt
 
 import numpy as np
 import random as rd
-from math import cos, sin, tan
+from math import cos, sin, tan, atan
 
 
 #yolo = Load_Yolo_model()
@@ -243,6 +243,18 @@ def get_position(boxes,commande,pos):
     
     return pos_calc, pos, cone_x, cone_y
     
+def orientation_voiture(x,y):
+    x1,y1 = x-centre_x, y-centre_y
+    if x<0:
+        x2,y2 = x1+diametre/2, y1
+        theta = atan(y2/x2)
+        return theta-3.1415/2
+    else:
+        x2,y2 = x1-diametre/2, y1
+        theta = atan(y2/x2)
+        return theta-3.1415/2
+        
+
 if __name__ == "__main__":
     # detection = [[2.18939087e+03, 1.48177417e+03, 2.86449023e+03, 2.54427783e+03, 9.66490030e-01, 0.00000000e+00], 
     #              [4.22634644e+02, 1.44312561e+03, 1.08822302e+03, 2.31131567e+03, 9.36378121e-01, 0.00000000e+00],