Extract pt

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()