raddbg.py

import math

def arr_rad_to_deg(a):
	for i in range(len(a)):
		a[i] = (a[i] / (2 * math.pi)) * 360

def arr_deg_to_rad(a):
	for i in range(len(a)):
		a[i] = (a[i] / 360) * (2 * math.pi)

def arr_rad_to_quat(a):
	if len(a) < 4:
		a.append(0.0)
	cy = math.cos(a[2] * 0.5)
	sy = math.sin(a[2] * 0.5)
	cp = math.cos(a[1] * 0.5)
	sp = math.sin(a[1] * 0.5)
	cr = math.cos(a[0] * 0.5)
	sr = math.sin(a[0] * 0.5)
	a[0] = cr * cp * cy + sr * sp * sy
	a[1] = sr * cp * cy - cr * sp * sy
	a[2] = cr * sp * cy + sr * cp * sy
	a[3] = cr * cp * sy - sr * sp * cy

def arr_normalize_deg(a):
	for i in range(len(a)):
		a[i] %= 360
		if a[i] < 0:
			a[i] += 360

# def arr_quat_to_rad(a):
#     arr = [0.0, 0.0, 0.0]

#     # roll (x-axis rotation)
#     sinr_cosp = 2 * (a[3] * a[0] + a[1] * a[2])
#     cosr_cosp = 1 - 2 * (a[0] * a[0] + a[1] * a[1])
#     arr[0] = math.atan2(sinr_cosp, cosr_cosp)

#     # pitch (y-axis rotation)
#     sinp = math.sqrt(1 + 2 * (a[3] * a[1] - a[0] * a[2]))
#     cosp = math.sqrt(1 - 2 * (a[3] * a[1] - a[0] * a[2]))
#     arr[1] = 2 * math.atan2(sinp, cosp) - math.pi / 2

#     # yaw (z-axis rotation)
#     siny_cosp = 2 * (a[3] * a[2] + a[0] * a[1])
#     cosy_cosp = 1 - 2 * (a[1] * a[1] + a[2] * a[2])
#     arr[2] = math.atan2(siny_cosp, cosy_cosp)

#     return arr

def arr_quat_to_mat3(m, q):
	sqrt2 = math.sqrt(2)
	q0 = sqrt2 * q[0]
	q1 = sqrt2 * q[1]
	q2 = sqrt2 * q[2]
	q3 = sqrt2 * q[3]

	qda = q0 * q1
	qdb = q0 * q2
	qdc = q0 * q3
	qaa = q1 * q1
	qab = q1 * q2
	qac = q1 * q3
	qbb = q2 * q2
	qbc = q2 * q3
	qcc = q3 * q3

	m[(0 * 3) + 0] = 1.0 - qbb - qcc
	m[(0 * 3) + 1] = qdc + qab
	m[(0 * 3) + 2] = -qdb + qac

	m[(1 * 3) + 0] = -qdc + qab
	m[(1 * 3) + 1] = 1.0 - qaa - qcc
	m[(1 * 3) + 2] = qda + qbc

	m[(2 * 3) + 0] = qdb + qac
	m[(2 * 3) + 1] = -qda + qbc
	m[(2 * 3) + 2] = 1.0 - qaa - qbb

def arr_mat3_normalized_to_eul2(mat, eul1, eul2):
	cy = math.hypot(mat[(0 * 3) + 0], mat[(0 * 3) + 1])

	epsilon = 1.192092896e-07
	if cy > 16.0 * epsilon:

		eul1[0] = math.atan2(mat[(1 * 3) + 2], mat[(2 * 3) + 2])
		eul1[1] = math.atan2(-mat[(0 * 3) + 2], cy)
		eul1[2] = math.atan2(mat[(0 * 3) + 1], mat[(0 * 3) + 0])

		eul2[0] = math.atan2(-mat[(1 * 3) + 2], -mat[(2 * 3) + 2])
		eul2[1] = math.atan2(-mat[(0 * 3) + 2], -cy)
		eul2[2] = math.atan2(-mat[(0 * 3) + 1], -mat[(0 * 3) + 0])
	else:
		eul1[0] = math.atan2(-mat[(2 * 3) + 1], mat[(1 * 3) + 1])
		eul1[1] = math.atan2(-mat[(0 * 3) + 2], cy)
		eul1[2] = 0.0

		eul2[0] = eul1[0]
		eul2[1] = eul1[1]
		eul2[2] = eul1[2]

def arr_mat3_normalized_to_eul(eul, mat):
	eul1 = [0.0, 0.0, 0.0]
	eul2 = [0.0, 0.0, 0.0]

	arr_mat3_normalized_to_eul2(mat, eul1, eul2)

	# return best, which is just the one with lowest values it in
	if (math.fabs(eul1[0]) + math.fabs(eul1[1]) + math.fabs(eul1[2])) > (math.fabs(eul2[0]) + math.fabs(eul2[1]) + math.fabs(eul2[2])):
		eul[0] = eul2[0]
		eul[1] = eul2[1]
		eul[2] = eul2[2]
	else:
		eul[0] = eul1[0]
		eul[1] = eul1[1]
		eul[2] = eul1[2]

def arr_quat_to_rad(a):
	unit_mat = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
	arr_quat_to_mat3(unit_mat, a)
	if len(a) == 4:
		del a[-1]
	arr_mat3_normalized_to_eul(a, unit_mat)

a = [2048, 3839, 2048]
print(a)
a = [x * 90.0 / 1024.0 for x in a]
print(a)
arr_deg_to_rad(a)
print(a)
arr_rad_to_quat(a)
print(a)
arr_quat_to_rad(a)
# a = arr_quat_to_rad(a)
print(a)
arr_rad_to_deg(a)
print(a)