import numpy as np
import os
def euler_to_quat(a):
a = np.deg2rad(a)
cy = np.cos(a[1] * 0.5)
sy = np.sin(a[1] * 0.5)
cr = np.cos(a[0] * 0.5)
sr = np.sin(a[0] * 0.5)
cp = np.cos(a[2] * 0.5)
sp = np.sin(a[2] * 0.5)
q = np.zeros(4)
q[0] = cy * cr * cp + sy * sr * sp
q[1] = cy * sr * cp - sy * cr * sp
q[3] = cy * cr * sp + sy * sr * cp
q[2] = sy * cr * cp - cy * sr * sp
return q
def format_line(name, value, unit=''):
"""
Formats a line e.g.
{Name:} {value}{unit}
"""
name += ':'
if isinstance(value, (float, np.ndarray)):
value = f'{value:{0}.{4}}'
return f'{name.ljust(40)}{value}{unit}'
def save_arrays(path, a_dict):
"""
:param path: Output path
:param a_dict: A dict containing the name of the array as key.
"""
path = path.rstrip('/')
if not os.path.isdir(path):
os.mkdir(path)
if len(os.listdir(path)) == 0:
folder_number = '000'
else:
folder_number = str(int(max(os.listdir(path))) + 1).zfill(3)
os.mkdir(f'{path}/{folder_number}')
for key in a_dict:
np.save(f'{path}/{folder_number}/{key}.npy', a_dict[key])
