Advertisement
Guest User

Untitled

a guest
Feb 19th, 2020
117
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. import struct
  2. import binascii
  3. import sys
  4. import math
  5.  
  6. def get_angles(v_a, v_b):
  7.     ax, ay, az = v_a
  8.     bx, by, bz = v_b
  9.  
  10.     az_k = math.atan2(bx - ax, by - ay)
  11.     if az_k < 0:
  12.         az_k += 2 * math.pi
  13.  
  14.     az = (az_k * 180) / math.pi
  15.  
  16.     el_k = math.sqrt(math.pow(bx - ax, 2) + math.pow(by - ay, 2))
  17.     el = math.atan((bz - az) / el_k) * 180 / math.pi
  18.  
  19.     return (az, el)
  20.  
  21. ax, ay, az = [float(n) for n in sys.argv[1:4]]
  22. bx, by, bz = [struct.unpack(">f", binascii.unhexlify(s))[0] for s in sys.argv[4:7]]
  23.  
  24. print(ax, ay, az)
  25. print(bx, by, bz)
  26.  
  27. az, el = get_angles((ax, ay, az), (bx, by, bz))
  28.  
  29. el_s = binascii.hexlify(struct.pack(">f", el))
  30.  
  31. if len(el_s) < 8:
  32.     el_s = "0" + el_s
  33.  
  34. az_s = binascii.hexlify(struct.pack(">f", az))
  35. if len(az_s) < 8:
  36.     az_s = "0" + az_s
  37.  
  38.  
  39.  
  40. print("el:", el, el_s)
  41. print("az:", az, az_s)
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement