From bef7301a24a939dfa77171b5b6095fafe1659eaa Mon Sep 17 00:00:00 2001 From: Lucas Marais Date: Thu, 23 Mar 2023 15:50:58 +0100 Subject: [PATCH] Extract pt --- Position-Robot/extractcoord.py | 47 ++++++++++++++++++++++++++++++++++ 1 file changed, 47 insertions(+) diff --git a/Position-Robot/extractcoord.py b/Position-Robot/extractcoord.py index f57c7a7..2ccb9fb 100644 --- a/Position-Robot/extractcoord.py +++ b/Position-Robot/extractcoord.py @@ -16,6 +16,7 @@ class getpos(): self.background = (0,0,0) self.Done = True self.coords = [] + self.origin = [[0,0],[3000,0],[3000,2000],[0,3000]] self.pos = np.array([]) while self.Done : @@ -47,5 +48,51 @@ class getpos(): print(x,y) + def unit_vector(vector): + """ Returns the unit vector of the vector. """ + return vector / np.linalg.norm(vector) + + def angle_between(v1, v2): + + v1_u = unit_vector(v1) + v2_u = unit_vector(v2) + return np.arccos(np.clip(np.dot(v1_u, v2_u), -1.0, 1.0)) + + + + def findpos(self): + for i in range(4): + print(angle_between(np.array(board[i-1])-np.array(board[i]), point - np.array(board[i]))*180/np.pi) + print(angle_between(np.array(board[i+1])-np.array(board[i]), point - np.array(board[i]))*180/np.pi) + + + + +####################################################################################### +a = (164, 672) +b = (600, 60) +c = (995, 72) +d = (1421, 669) + + +point = (726, 231) + +board = [a, b, c, d, a, b, c, d] + +def unit_vector(vector): + """ Returns the unit vector of the vector. """ + return vector / np.linalg.norm(vector) + +def angle_between(v1, v2): + + v1_u = unit_vector(v1) + v2_u = unit_vector(v2) + return np.arccos(np.clip(np.dot(v1_u, v2_u), -1.0, 1.0)) + +for i in range(4): + print(angle_between(np.array(board[i-1])-np.array(board[i]), point - np.array(board[i]))*180/np.pi) + print(angle_between(np.array(board[i+1])-np.array(board[i]), point - np.array(board[i]))*180/np.pi) + + extract = getpos()