yostlabs 2025.1.3.4__py3-none-any.whl

yostlabs/tss3/consts.py

@@ -0,0 +1,24 @@
+THREESPACE_HEADER_STATUS_BIT_POS = 0
+THREESPACE_HEADER_TIMESTAMP_BIT_POS = 1
+THREESPACE_HEADER_ECHO_BIT_POS = 2
+THREESPACE_HEADER_CHECKSUM_BIT_POS = 3
+THREESPACE_HEADER_SERIAL_BIT_POS = 4
+THREESPACE_HEADER_LENGTH_BIT_POS = 5
+THREESPACE_HEADER_NUM_BITS = 6
+
+THREESPACE_HEADER_STATUS_BIT = (1 << THREESPACE_HEADER_STATUS_BIT_POS)
+THREESPACE_HEADER_TIMESTAMP_BIT = (1 << THREESPACE_HEADER_TIMESTAMP_BIT_POS)
+THREESPACE_HEADER_ECHO_BIT = (1 << THREESPACE_HEADER_ECHO_BIT_POS)
+THREESPACE_HEADER_CHECKSUM_BIT = (1 << THREESPACE_HEADER_CHECKSUM_BIT_POS)
+THREESPACE_HEADER_SERIAL_BIT = (1 << THREESPACE_HEADER_SERIAL_BIT_POS)
+THREESPACE_HEADER_LENGTH_BIT = (1 << THREESPACE_HEADER_LENGTH_BIT_POS)
+
+FIRMWARE_VALID_BIT = 0x01
+
+PASSIVE_CALIBRATE_GYRO = (1 << 0)
+PASSIVE_CALIBRATE_MAG_REF = (1 << 1)
+
+STREAMING_MAX_HZ = 2000
+
+THREESPACE_OUTPUT_MODE_ASCII = 1
+THREESPACE_OUTPUT_MODE_BINARY = 2

yostlabs/tss3/eepts.py

@@ -0,0 +1,228 @@
+from dataclasses import dataclass, field, fields
+from typing import ClassVar
+
+YL_SENSOR_BIT_GYRO: int = 1
+YL_SENSOR_BIT_ACCEL: int = 2
+YL_SENSOR_BIT_MAG: int = 4
+YL_SENSOR_BIT_BARO: int = 8
+YL_SENSOR_BIT_TEMP: int = 16
+YL_SENSOR_BIT_GPS: int = 32
+
+@dataclass
+class YL_EEPTS_INPUT_DATA:
+    gyro_data: list[float] = None  # XYZ in radians/sec. only needs to be float32
+    accel_data: list[float] = None # XYZ in G force units(9.8 m/s^2). only needs to be float32
+    mag_data: list[float] = None  # XYZ in Gauss. only needs to be float32
+    orient_data: list[float] = None # XYZW Optional, may not be here if YL_BUILTIN_ORIENT is false
+    barometer_data: float = 0  # in hPa. only needs to be float32
+    temperature: float = 0  # in degrees C. only needs to be float32
+    gps_longitude: float = 0  # in decimal degrees (positive east, negative west). needs to be float64
+    gps_latitude: float = 0  # in decimal degrees (positive north, negative south). needs to be float64
+    gps_altitude: float = 0  # in meters. only needs to be float32
+    timestamp: int = 0 # in microseconds. uint32_t
+
+#Locomotion Modes
+YL_LOCOMOTION_IDLE: int = 0
+YL_LOCOMOTION_WALKING: int = 1
+YL_LOCOMOTION_JOGGING: int = 2
+YL_LOCOMOTION_RUNNING: int = 3
+YL_LOCOMOTION_CRAWLING: int = 4
+YL_LOCOMOTION_OTHER: int = 6
+YL_LOCOMOTION_UNKNOWN: int = 5
+
+#Sensor Locations
+YL_SENSOR_RSHOULDER: int = 5
+YL_SENSOR_BACK: int = 3
+YL_SENSOR_RHAND: int = 4
+YL_SENSOR_WAIST: int = 2
+YL_SENSOR_CHEST: int = 1
+YL_SENSOR_UNKNOWN: int = 0
+
+LocomotionModes = {
+    YL_LOCOMOTION_IDLE: "Idle",
+    YL_LOCOMOTION_WALKING: "Walking",
+    YL_LOCOMOTION_JOGGING: "Jogging",
+    YL_LOCOMOTION_RUNNING: "Running",
+    YL_LOCOMOTION_CRAWLING: "Crawling",
+    YL_LOCOMOTION_OTHER: "Other",
+    YL_LOCOMOTION_UNKNOWN: "Unknown"
+}
+
+SensorLocations = {
+    YL_SENSOR_RSHOULDER: "RShoulder",
+    YL_SENSOR_BACK: "Back",
+    YL_SENSOR_RHAND: "RHand",
+    YL_SENSOR_WAIST: "Waist",
+    YL_SENSOR_CHEST: "Chest",
+    YL_SENSOR_UNKNOWN: "Unknown"
+}
+
+PresetMotionWR = 0
+PresetMotionWRC = 1
+
+PresetHandDisabled = 0
+PresetHandEnabled = 1
+PresetHandOnly = 2
+
+PresetHeadingDynamic = 0
+PresetHeadingStatic = 1
+
+@dataclass
+class YL_EEPTS_OUTPUT_DATA:
+
+    segment_count: int = 0                  #number of segments (a segment refers to a single step action)
+    timestamp: int = 0                      #in microseconds, set to end of last estimated segment
+    estimated_gps_longitude: float = 0      #DDMM.MMMM (positive east, negative west)
+    estimated_gps_latitude: float = 0       #DDMM.MMMM (positive north, negative south)
+    estimated_gps_altitude: float = 0       #in meters
+    estimated_heading_angle: float = 0      #in degrees (0 north, 90 east, 180 south, 270 west)
+    estimated_distance_travelled: float = 0 #in meters, since last reset (A reset is a start command)
+    estimated_distance_x: float = 0         #in meters since last update, along positive east/negative west (An update is a read)
+    estimated_distance_y: float = 0         #in meters, since last update, along positive north/negative south
+    estimated_distance_z: float = 0         #in meters, since last update, along positive up/negative down
+    estimated_locomotion_mode: int = 0      #0=idle, 1=walking, etc...
+    estimated_receiver_location: int = 0    #0=unknown, 1=chest, etc...
+    last_event_confidence: float = 1
+    overall_confidence: float = 1
+
+    LOCMOTION_DICT: ClassVar[dict[int, str]] = { YL_LOCOMOTION_IDLE : "Idle", YL_LOCOMOTION_WALKING : "Walking", YL_LOCOMOTION_JOGGING : "Jogging", YL_LOCOMOTION_RUNNING : "Running", YL_LOCOMOTION_CRAWLING: "Crawling", YL_LOCOMOTION_OTHER: "Other", YL_LOCOMOTION_UNKNOWN: "Unknown" } 
+    LOCATION_DICT: ClassVar[dict[int, str]] = { YL_SENSOR_UNKNOWN : "Unknown", YL_SENSOR_CHEST : "Chest", YL_SENSOR_WAIST : "Waist", YL_SENSOR_BACK : "Back", YL_SENSOR_RHAND : "RHand", YL_SENSOR_RSHOULDER : "RShoulder" }
+
+    def clone(self, other: "YL_EEPTS_OUTPUT_DATA"):
+        self.segment_count = other.segment_count
+        self.timestamp = other.timestamp
+        self.estimated_gps_longitude = other.estimated_gps_longitude
+        self.estimated_gps_latitude = other.estimated_gps_latitude
+        self.estimated_gps_altitude = other.estimated_gps_altitude
+        self.estimated_heading_angle = other.estimated_heading_angle
+        self.estimated_distance_travelled = other.estimated_distance_travelled
+        self.estimated_distance_x = other.estimated_distance_x
+        self.estimated_distance_y = other.estimated_distance_y
+        self.estimated_distance_z = other.estimated_distance_z
+        self.estimated_locomotion_mode = other.estimated_locomotion_mode
+        self.estimated_receiver_location = other.estimated_receiver_location
+        self.last_event_confidence = other.last_event_confidence
+        self.overall_confidence = other.overall_confidence
+
+    def get_locomotion_string(self):
+        return YL_EEPTS_OUTPUT_DATA.LOCMOTION_DICT[self.estimated_locomotion_mode]
+    
+    def get_location_string(self):
+        return YL_EEPTS_OUTPUT_DATA.LOCATION_DICT[self.estimated_receiver_location]
+
+    def __str__(self):
+        return f"{self.segment_count},{self.timestamp}," \
+               f"{self.estimated_gps_longitude},{self.estimated_gps_latitude}," \
+               f"{self.estimated_gps_altitude},{self.estimated_heading_angle}," \
+               f"{self.estimated_distance_travelled},{self.estimated_distance_x}," \
+               f"{self.estimated_distance_y},{self.estimated_distance_z}," \
+               f"{self.estimated_locomotion_mode},{self.estimated_receiver_location}"
+    
+    def print_fancy(self):
+        print("Segment Count:", self.segment_count)
+        print("Timestamp:", self.timestamp)
+        print("Longitude:", self.estimated_gps_longitude)
+        print("Latitude:", self.estimated_gps_latitude)
+        print("Altitude:", self.estimated_gps_altitude)
+        print("Heading:", self.estimated_heading_angle)
+        print("Distance Travelled:", self.estimated_distance_travelled)
+        print("Delta_X:", self.estimated_distance_x)
+        print("Delta_Y:", self.estimated_distance_y)
+        print("Delta_Z:", self.estimated_distance_z)
+        print("Locomotion:", self.get_locomotion_string())
+        print("Sensor Location:", self.get_location_string())
+        print("Confidence:", self.last_event_confidence)
+        print("Overall Confidence:", self.overall_confidence)
+
+@dataclass
+class SensorData:
+    accel_x : float
+    accel_y : float
+    accel_z : float
+
+    gyro_x : float
+    gyro_y : float
+    gyro_z : float
+
+    mag_x : float
+    mag_y : float
+    mag_z : float
+
+    accel : list[float] = field(init=False, default_factory=list)
+    gyro : list[float] = field(init=False, default_factory=list)
+    mag : list[float] = field(init=False, default_factory=list)
+
+    def __post_init__(self):
+        self.accel = [self.accel_x, self.accel_y, self.accel_z]
+        self.gyro = [self.gyro_x, self.gyro_y, self.gyro_z]
+        self.mag = [self.mag_x, self.mag_y, self.mag_z]
+    
+    def __str__(self):
+        return f"{self.accel_x},{self.accel_y},{self.accel_z}," \
+               f"{self.gyro_x},{self.gyro_y}, {self.gyro_z}," \
+               f"{self.mag_x},{self.mag_y},{self.mag_z}"
+
+    def print_fancy(self):
+        print("Accel:", self.accel)
+        print("Gyro:", self.gyro)
+        print("Mag:", self.mag)
+
+@dataclass
+class DebugMessage:
+    level: int = 0
+    module: int = 0
+    msg: str = ""
+
+    def get_display_str(self):
+        return f"Level: {self.level} Module: {self.module}    {self.msg}"
+
+@dataclass
+class Segment:
+    #Output Structure
+    segment_count: int = 0
+    timestamp: int = 0
+    estimated_gps_longitude: float = 0
+    estimated_gps_latitude: float = 0
+    estimated_gps_altitude: float = 0
+    estimated_heading_angle: float = 0
+    estimated_distance_travelled: float = 0
+    estimated_distance_x: float = 0
+    estimated_distance_y: float = 0
+    estimated_distance_z: float = 0
+    estimated_locomotion_mode: int = 0
+    estimated_receiver_location: int = 0
+    last_event_confidence: float = 0
+    overall_confidence: float = 0
+
+    #Segment Info Structure
+    start_global_index: int = 0
+    end_global_index: int = 0
+    f_start_index_offset: float = 0
+    f_end_index_offset: float = 0
+    len: int = 0
+    f_len: float = 0
+    dir: int = 0
+
+    #Debug Messages:
+    debug_msgs: list[DebugMessage] = field(default_factory=lambda: [])
+    
+    @classmethod
+    def from_only_output_obj(cls, output: YL_EEPTS_OUTPUT_DATA):
+        new_obj = cls()
+        output_fields = [f for f in dir(output) if not f.startswith('__') and not callable(getattr(output, f))]
+        for field in output_fields: #Can NOT just assign. That will cause unintentional problems if ever saving segment info
+            setattr(new_obj, field, getattr(output, field))
+
+        return new_obj
+    
+    def __str__(self):
+        s = ""
+        for field in fields(self):
+            attr = getattr(self, field.name)
+            if field.name == "estimated_locomotion_mode":
+                s += f"{field.name}: {LocomotionModes[attr]}\n"
+            elif field.name == "estimated_receiver_location":
+                s += f"{field.name}: {SensorLocations[attr]}\n"
+            else:
+                s += f"{field.name}: {attr}\n"
+        return s

yostlabs/tss3/parser.py

@@ -0,0 +1,258 @@
+from yostlabs.tss3.api import *
+import math
+
+class ThreespaceBufferInputStream(ThreespaceInputStream):
+    """
+    Default Input Stream for the binary parser.
+    Ignores timeout since this is only used synchronously for now.
+    """
+
+    def __init__(self):
+        self.buffer = bytearray()
+
+    """Reads specified number of bytes."""
+    def read(self, num_bytes) -> bytes:
+        num_bytes = min(len(self.buffer), num_bytes)
+        result = self.buffer[:num_bytes]
+        del self.buffer[:num_bytes]
+        #self.buffer = self.buffer[num_bytes:]
+        return result
+    
+    def read_all(self):
+        return self.read(self.length)
+
+    def read_until(self, expected: bytes) -> bytes:
+        if expected not in self.buffer:
+            return self.read_all()
+        
+        length = self.buffer.index(expected) + len(expected)
+        result = self.buffer[:length]
+        del self.buffer[:length]
+        #self.buffer = self.buffer[length:]
+        raise result
+
+    """Allows reading without removing the data from the buffer"""
+    def peek(self, num_bytes) -> bytes:
+        num_bytes = min(len(self.buffer), num_bytes)
+        return self.buffer[:num_bytes]
+    
+    def peek_until(self, expected: bytes, max_length=None) -> bytes:
+        if expected in self.buffer: #Read until the expected
+            length = self.buffer.index(expected) + len(expected)
+            if max_length is not None and length > max_length:
+                length = max_length
+            return self.buffer[:length]
+        
+        #There is no expected, so read as far as possible
+        length = len(self.buffer)
+        if max_length is not None:
+            length = min(length, max_length)
+        return self.buffer[:length]
+    
+    def readline(self) -> bytes:
+        return self.read_until(b"\n")
+    
+    def peekline(self, max_length=None) -> bytes:
+        return self.peek_until(b"\n", max_length=max_length)    
+    
+    def insert(self, data: bytes):
+        self.buffer.extend(data)
+
+    @property
+    def length(self) -> int:
+        return len(self.buffer)
+    
+    @property
+    def timeout(self) -> float:
+        raise NotImplementedError()
+
+    @timeout.setter
+    def timeout(self, timeout: float):
+        raise NotImplementedError()    
+
+class ThreespaceBinaryParser:
+    """
+    A class that can be used to parse a stream of binary data
+    that could contain multiple different command responses and validates
+    the responses to handle misalignment/data corruption.
+
+    Requires all expected responses to have the same header enabled.
+
+    The header should contain the cmd_echo, checksum, and data_length fields
+    for full functionality. The lack of any of those fields could limit functionality
+    of the parser.
+
+    If cmd_echo is missing, only one command can be registered with the binary parser as it has no way
+    of knowing what of verifying what the current incoming response is.
+
+    If checksum is missing, data integrity can not be checked.
+
+    If data_length is missing, commands that do not return a static length may cause blocking operations
+    or errors while parsing. (These are planned to be fixed in a future version)
+
+    NOTE: For speed, a custom implementation will be better then this parser class. This parser handles allowing
+    multiple commands as well as data validation and misalignment correction. It also formats the data into the ThreespaceCmdResult
+    response type. The overhead added because of all these additional checks/calculations can add a significant amount of time
+    to processing binary data compared to just reading a known amount of data and instantly unpacking it to a tuple in the desired format.
+    """
+
+    COMMAND_EXCEPTIONS = [84, 177] #getStreamingBatch and fileReadBytes need additional info and so need registered via the 
+
+    def __init__(self, data_stream: ThreespaceInputStream = None, verbose=False):
+        """
+        Parameters
+        ----------
+        data_stream - (optional) The data stream to use with the Binary Parser. If not supplied, will default to a new ThreespaceBufferInputStream
+        """
+        self.data_stream = data_stream
+        self.registered_commands: dict[int,ThreespaceCommand] = {}
+
+        if self.data_stream is None:
+            self.data_stream = ThreespaceBufferInputStream()
+        self.header_info = None
+
+        self.__parsing_header: ThreespaceHeader = None  #Used to optimize preventing reading to much by cacheing the header seperately from the cmd data
+        self.__parsing_command: ThreespaceCommand = None
+        self.__parsing_msg_length: int = None           #Used seperately from the __parsing_header so can handle msg lengths that are static without modifying the header
+
+        self.misaligned = False
+        self.verbose = verbose
+
+    def register_command(self, cmd: int|ThreespaceCommand, **kwargs):
+        """
+        Registers the given cmd number/cmd with the binary parser.
+
+        Some commands may require additional information:
+        stream_slots - list[int] Required when registering command 84 (getStreamingBatch) a list of command numbers that are being streamed.
+        read_size - 'auto' or int Required when registering a command that requires a given length such as fileReadBytes. If 'auto' will use the header length to determine length.
+        """
+        if isinstance(cmd, int):
+            cmd = threespaceCommandGet(cmd)
+            if cmd is None:
+                raise ValueError(f"Invalid Cmd {cmd}")
+            
+        if cmd.info.num in self.registered_commands:
+            return False
+        
+        #These command types are special and need additional info
+        if cmd.info.num == THREESPACE_FILE_READ_BYTES_COMMAND_NUM:
+            if "read_size" not in kwargs:
+                raise ValueError("Missing arguement 'read_size' when registering the fileReadBytes command with the binary parser")
+            raise NotImplementedError("The fileReadBytes command has yet to be implemented for the ThreespaceBinaryParser")
+        elif cmd.info.num == THREESPACE_GET_STREAMING_BATCH_COMMAND_NUM:
+            if "stream_slots" not in kwargs:
+                raise ValueError("Missing arguement 'stream_slots' when registering the getStreamingBatch command with the binary parser")
+            cmd = ThreespaceGetStreamingBatchCommand(kwargs['stream_slots'])
+
+        self.registered_commands[cmd.info.num] = cmd
+        return True
+
+    def unregister_command(self, cmd: int|ThreespaceCommand):
+        if cmd.info.num not in self.registered_commands:
+            return False
+        del self.registered_commands[cmd.info.num]
+        return True
+
+    def set_header(self, header_info: ThreespaceHeaderInfo):
+        self.header_info = header_info
+
+    def insert_data(self, data: bytes):
+        """
+        Add the given data to the default ThreespaceBufferInputStream.
+        This method will raise an exception if used on a different type of InputStream
+        """
+        if not isinstance(self.data_stream, ThreespaceBufferInputStream):
+            raise Exception("Insert data with Binary Parser only valid when using the default data_stream")
+        self.data_stream.insert(data)
+    
+    def parse_message(self) -> ThreespaceCmdResult:
+        if self.__parsing_header is None:
+            self.__parse_header()
+        
+        if self.__parsing_command is None:
+            return None
+        
+        return self.__parse_command()
+
+    def __parse_header(self):
+        if self.data_stream.length < self.header_info.size:
+            return
+        
+        header = self.data_stream.peek(self.header_info.size)
+        header = ThreespaceHeader.from_bytes(header, self.header_info)
+
+        cmd_found = False
+        if self.header_info.echo_enabled: #Search for the command to parse
+            for command in self.registered_commands.values():
+                if header.echo != command.info.num: continue
+
+                #Command matches! Attempt to parse
+                self.__parsing_command = command
+                self.__parsing_header = header
+
+                cmd_found = True
+        else: #Can only parse one command
+            if len(self.registered_commands) > 1:
+                raise Exception("Only one command type can be parsed when the 'cmd echo' is not enabled in the header")
+            self.__parsing_command = list(self.registered_commands.values())[0]
+            self.__parsing_header = header
+            cmd_found = True
+
+        if cmd_found:
+            if self.header_info.length_enabled:
+                self.__parsing_msg_length = self.__parsing_header.length
+            else:
+                self.__parsing_msg_length = self.__parsing_command.info.out_size
+            return
+
+        #This header is not related to any command, so it needs skipped
+        if self.verbose and not self.misaligned:
+            print("Unexpected header:", header)
+        self.misaligned = True
+        self.data_stream.read(1)
+
+    def __peek_checksum(self):
+        header_len = len(self.__parsing_header.raw_binary)
+        data = self.data_stream.peek(header_len + self.__parsing_msg_length)[header_len:]
+        checksum = sum(data) % 256
+        return checksum == self.__parsing_header.checksum
+
+    def __parse_command(self):
+        #Not enough data to parse yet
+        if self.data_stream.length < self.header_info.size + self.__parsing_msg_length:
+            return None
+        
+        if self.header_info.checksum_enabled and not math.isnan(self.__parsing_msg_length): #Can validate checksum before parsing
+            print("Pre validating checksum")
+            if not self.__peek_checksum():
+                #Data corruption/Misalignment error
+                if self.verbose and not self.misaligned:
+                    print("Checksum mismatch for command", self.__parsing_command.info.num)
+                self.misaligned = True
+                self.data_stream.read(1)
+                self.__parsing_command = None
+                self.__parsing_header = None
+                return None
+        
+        #Header and pre validation checksum checks out! Now just parse the actual command result and return it
+        header = self.__parsing_header
+        self.data_stream.read(len(header.raw_binary)) #Skip these bytes since they are already parsed
+        result, raw = self.__parsing_command.read_command(self.data_stream)
+
+        #Validate checksum if couldn't pre-validate due to unknown message length
+        if math.isnan(self.__parsing_msg_length) and self.header_info.checksum_enabled:
+            checksum = sum(raw) % 256
+            if checksum != header.checksum:
+                if self.verbose and not self.misaligned:
+                    print("Checksum mismatch for command", self.__parsing_command.info.num)
+                self.misaligned = True
+                self.__parsing_command = None
+                self.__parsing_header = None
+                return None
+
+        #Reset and return
+        self.__parsing_header = None
+        self.__parsing_command = None
+        self.misaligned = False
+        return ThreespaceCmdResult(result, header, data_raw_binary=raw)
+        

yostlabs/tss3/quaternion.py

@@ -0,0 +1,166 @@
+import math
+
+def vec_len(vector: list[float|int]):
+    return sum(v ** 2 for v in vector) ** 0.5
+
+def vec_dot(a: list[float], b: list[float]):
+    return sum(a[i] * b[i] for i in range(len(a)))
+
+def vec_cross(a: list[float], b: list[float]):
+    cross = [0, 0, 0]
+    cross[0] = a[1] * b[2] - a[2] * b[1]
+    cross[1] = a[2] * b[0] - a[0] * b[2]
+    cross[2] = a[0] * b[1] - a[1] * b[0]
+    return cross
+
+def vec_normalize(vec: list[float]):
+    l = vec_len(vec)
+    if l == 0:
+        return vec
+    return [v / l for v in vec]
+
+def quat_mul(a: list[float], b: list[float]):
+    out = [0, 0, 0, 0]
+    x = 0; y = 1; z = 2; w = 3
+    out[w] = a[w]*b[w] - a[x]*b[x] - a[y]*b[y] - a[z]*b[z]
+    out[x] = a[w]*b[x] + a[x]*b[w] + a[y]*b[z] - a[z]*b[y]
+    out[y] = a[w]*b[y] + a[y]*b[w] + a[z]*b[x] - a[x]*b[z]
+    out[z] = a[w]*b[z] + a[z]*b[w] + a[x]*b[y] - a[y]*b[x]
+    return out
+
+#Rotates quaternion b by quaternion a, it does not combine them
+def quat_rotate(a: list[float], b: list[float]):
+    inv = a.copy()
+    inv[0] *= -1
+    inv[1] *= -1
+    inv[2] *= -1
+    axis = [b[0], b[1], b[2], 0]
+    halfway = quat_mul(a, axis)
+    final = quat_mul(halfway, inv)
+    return [*final[:3], b[3]]
+
+def quat_inverse(quat: list[float]):
+    return [-quat[0], -quat[1], -quat[2], quat[3]]
+
+def quat_rotate_vec(quat: list[float], vec: list[float]):
+    inv = quat_inverse(quat)
+    tmp = [vec[0], vec[1], vec[2], 0]
+    halfway = quat_mul(quat, tmp)
+    final = quat_mul(halfway, inv)
+    return [final[0], final[1], final[2]]
+
+def is_right_handed(order: str, negations: list[bool]):
+    right_handed = order.lower() in ("xzy", "yxz", "zyx")
+    for i in range(3):
+        if negations[i]:
+            right_handed = not right_handed
+    return right_handed
+
+def axis_to_unit_vector(axis: str):
+    axis = axis.lower()
+    if axis == 'x' or axis == 0: return [1, 0, 0]
+    if axis == 'y' or axis == 1: return [0, 1, 0]
+    if axis == 'z' or axis == 2: return [0, 0, 1]
+
+def angles_to_quaternion(angles: list[float], order: str, degrees=True):
+    quat = [0, 0, 0, 1]
+    for i in range(len(angles)):
+        axis = order[i]
+        angle = angles[i]
+        if degrees:
+            angle = math.radians(angle)
+        unit_vec = axis_to_unit_vector(axis)
+        w = math.cos(angle / 2)
+        imaginary = math.sin(angle / 2)
+        unit_vec = [v * imaginary for v in unit_vec]
+        angle_quat = [*unit_vec, w]
+        #print(f"{axis} {angle} {angle_quat}")
+        quat = quat_mul(quat, angle_quat)
+    return quat
+
+def quat_from_axis_angle(axis: list[float], angle: float):
+    imaginary = math.sin(angle / 2)
+    quat = [imaginary * v for v in axis]
+    quat.append(math.cos(angle / 2))
+    return quat
+
+def quat_from_two_vectors(forward: list[float], down: list[float]):
+    """
+    This function requires two orthogonal vectors to work
+    """
+    forward_reference = [0, 0, 1]
+    down_reference = [0, -1, 0]
+
+    forward = vec_normalize(forward)
+    down = vec_normalize(down)
+
+    #Create the first rotation to align the forward axis
+    axis_of_rotation = vec_cross(forward_reference, forward)
+    axis_of_rotation = vec_normalize(axis_of_rotation)
+    if not any(abs(v) > 0 for v in axis_of_rotation):
+        axis_of_rotation = down_reference #This is just a direct 180 degree rotation around any orthogonal axis, so just use the down_ref
+    dot = min(1, max(-1, vec_dot(forward_reference, forward)))
+    angle1 = math.acos(dot)
+    imaginary = math.sin(angle1/2)
+    quat = [v * imaginary for v in axis_of_rotation] #XYZ
+    quat.append(math.cos(angle1/2)) #W
+
+    #Update the reference to figure out where it is after the turn
+    down_reference = quat_rotate_vec(quat, down_reference)
+
+    #find the rotation to make the remaining reference align with its given value
+    axis_of_rotation = vec_cross(down_reference, down)
+    axis_of_rotation = vec_normalize(axis_of_rotation)
+    if not any(abs(v) > 0 for v in axis_of_rotation):
+        axis_of_rotation = forward #Rotate along the final forward vector until up is aligned
+    dot = min(1, max(-1, vec_dot(down_reference, down)))
+    angle2 = math.acos(dot)
+    imaginary = math.sin(angle2/2)
+    rotation = [v * imaginary for v in axis_of_rotation] #XYZ
+    rotation.append(math.cos(angle2/2)) #W
+
+    quat = quat_mul(rotation, quat)
+    return quat
+
+def quaternion_to_3x3_rotation_matrix(quat):
+    """
+    Convert a quaternion in form x, y, z, w to a rotation matrix
+    """
+    x, y, z, w = quat
+
+    fTx = 2.0 * x
+    fTy = 2.0 * y
+    fTz = 2.0 * z
+    fTwx = fTx * w
+    fTwy = fTy * w
+    fTwz = fTz * w
+    fTxx = fTx * x
+    fTxy = fTy * x
+    fTxz = fTz * x
+    fTyy = fTy * y
+    fTyz = fTz * y
+    fTzz = fTz * z
+
+    out = [0] * 9
+    out[0] = 1.0-(fTyy+fTzz)
+    out[1] = fTxy-fTwz
+    out[2] = fTxz+fTwy
+    out[3] = fTxy+fTwz
+    out[4] = 1.0-(fTxx+fTzz)
+    out[5] = fTyz-fTwx
+    out[6] = fTxz-fTwy
+    out[7] = fTyz + fTwx
+    out[8] = 1.0 - (fTxx+fTyy)
+
+    return [
+        [out[0], out[1], out[2]],
+        [out[3], out[4], out[5]],
+        [out[6], out[7], out[8]],
+    ]
+
+def quaternion_global_to_local(quat, vec):
+    inverse = quat_inverse(quat)
+    return quat_rotate_vec(inverse, vec)
+
+def quaternion_local_to_global(quat, vec):
+    return quat_rotate_vec(quat, vec)