Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- import struct
- import binascii
- import sys
- import math
- def get_angles(v_a, v_b):
- ax, ay, az = v_a
- bx, by, bz = v_b
- az_k = math.atan2(bx - ax, by - ay)
- if az_k < 0:
- az_k += 2 * math.pi
- az = (az_k * 180) / math.pi
- el_k = math.sqrt(math.pow(bx - ax, 2) + math.pow(by - ay, 2))
- el = math.atan((bz - az) / el_k) * 180 / math.pi
- return (az, el)
- ax, ay, az = [float(n) for n in sys.argv[1:4]]
- bx, by, bz = [struct.unpack(">f", binascii.unhexlify(s))[0] for s in sys.argv[4:7]]
- print(ax, ay, az)
- print(bx, by, bz)
- az, el = get_angles((ax, ay, az), (bx, by, bz))
- el_s = binascii.hexlify(struct.pack(">f", el))
- if len(el_s) < 8:
- el_s = "0" + el_s
- az_s = binascii.hexlify(struct.pack(">f", az))
- if len(az_s) < 8:
- az_s = "0" + az_s
- print("el:", el, el_s)
- print("az:", az, az_s)
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement