Deduce rotation conventions used in a software library (Python)

find_rotational_conventions.py

import numpy as np
from typing import Callable, Tuple
import functools

class EulerAnglesOrder:
    NONE = 0
    @staticmethod
    def toStr(v):
        if v != EulerAnglesOrder.NONE:
            return f"R = R({v[0]})R({v[1]})R({v[2]})"
        else:
            return "--"

class DirectionalityConvention:
    NONE = 0
    B2W = 1
    W2B = 2
    @staticmethod
    def toStr(v):
        if v == DirectionalityConvention.B2W:
            return "Body-to-World"
        elif v == DirectionalityConvention.W2B:
            return "World-to-Body"
        else:
            return "--"

class FunctionConvention:
    NONE = 0
    ACTIVE = 1
    PASSIVE = 2
    @staticmethod
    def toStr(v):
        if v == FunctionConvention.ACTIVE:
            return "Active"
        elif v == FunctionConvention.PASSIVE:
            return "Passive"
        else:
            return "--"

class QuatOrderingConvention:
    NONE = 0
    SCALARFIRST = 1
    SCALARLAST = 2
    @staticmethod
    def toStr(v):
        if v == QuatOrderingConvention.SCALARFIRST:
            return "Scalar First"
        elif v == QuatOrderingConvention.SCALARLAST:
            return "Scalar Last"
        else:
            return "--"

class QuatHandednessConvention:
    NONE = 0
    RIGHT = 1
    LEFT = 2
    @staticmethod
    def toStr(v):
        if v == QuatHandednessConvention.RIGHT:
            return "Right-Handed"
        elif v == QuatHandednessConvention.LEFT:
            return "Left-Handed"
        else:
            return "--"

def _directionality_from_mat(R: np.ndarray, axis: str) -> int:
    if axis == "x":
        if R[1, 2] < 0:
            return DirectionalityConvention.B2W
        else:
            return DirectionalityConvention.W2B
    elif axis == "y":
        if R[2, 0] < 0:
            return DirectionalityConvention.B2W
        else:
            return DirectionalityConvention.W2B
    elif axis == "z":
        if R[0, 1] < 0:
            return DirectionalityConvention.B2W
        else:
            return DirectionalityConvention.W2B
    else:
        return DirectionalityConvention.NONE

def find_axis_angle_conventions(libname: str, Rfunc: Callable[[np.ndarray, float], np.ndarray]) -> None:
    x = np.array([1.0, 0.0, 0.0])
    rod_d = _directionality_from_mat(Rfunc(x, 1.0), "x")
    conventionStr = f"""
Axis-Angle Conventions for {libname}:

    Rodrigues Directionality: {DirectionalityConvention.toStr(rod_d)}
"""
    print(conventionStr)

def find_euler_conventions(libname: str, Rfunc: Callable[[float, float, float], np.ndarray]) -> None:
    euler_o_arg = ["-", "-", "-"]
    for i in range(3):
        euler_args = [0.0, 0.0, 0.0]
        euler_args[i] = 1.0
        R = Rfunc(*euler_args)
        if abs(R[0, 0] - 1.0) < 1e-4:
            euler_o_arg[i] = "x"
        elif abs(R[1, 1] - 1.0) < 1e-4:
            euler_o_arg[i] = "y"
        elif abs(R[2, 2] - 1.0) < 1e-4:
            euler_o_arg[i] = "z"

    def rot_comparator(a, b):
        # WARNING: sorting could be incomplete if there are two of the same axis
        idx_a, axis_a = a
        idx_b, axis_b = b
        euler_args = [0.0, 0.0, 0.0]
        euler_args[idx_a] = 1.0
        euler_args[idx_b] = 1.0
        R = Rfunc(*euler_args)
        if axis_a == "x":
            if axis_b == "x":
                return 0
            elif axis_b == "y":
                if abs(R[1, 0]) < 1e-4:
                    return 1
                else:
                    return -1
            else:
                if abs(R[2, 0]) < 1e-4:
                    return 1
                else:
                    return -1
        elif axis_a == "y":
            if axis_b == "x":
                if abs(R[0, 1]) < 1e-4:
                    return 1
                else:
                    return -1
            elif axis_b == "y":
                return 0
            else:
                if abs(R[2, 1]) < 1e-4:
                    return 1
                else:
                    return -1
        elif axis_a == "z":
            if axis_b == "x":
                if abs(R[0, 2]) < 1e-4:
                    return 1
                else:
                    return -1
            elif axis_b == "y":
                if abs(R[1, 2]) < 1e-4:
                    return 1
                else:
                    return -1
            else:
                return 0
        else:
            return 0

    tagged_args = [(i, c) for i, c in enumerate(euler_o_arg)]
    sorted_tagged_args = sorted(
        tagged_args,
        key = functools.cmp_to_key(lambda a, b: rot_comparator(a, b))
    )
    euler_o_mat = [tup[1] for tup in sorted_tagged_args]
    euler_d = _directionality_from_mat(Rfunc(1.0, 0.0, 0.0), euler_o_arg[0])
    conventionStr = f"""
Euler Angle Conventions for {libname}:

    Euler Argument Order: {euler_o_arg}
    Euler Matrix Order:   {EulerAnglesOrder.toStr(euler_o_mat)}
    Euler Directionality: {DirectionalityConvention.toStr(euler_d)}
"""
    print(conventionStr)

def find_quaternion_conventions(
    libname: str,
    Rfunc: Callable[[float, float, float, float], np.ndarray],
    Qfunc: Callable[[Tuple[float, float, float, float], Tuple[float, float, float, float]], Tuple[float, float, float, float]]
) -> None:
    R = Rfunc(1.0, 0.0, 0.0, 0.0)
    if np.allclose(R, np.eye(3)):
        quat_o = QuatOrderingConvention.SCALARFIRST
    else:
        R = Rfunc(0.0, 0.0, 0.0, 1.0)
        if np.allclose(R, np.eye(3)):
            quat_o = QuatOrderingConvention.SCALARLAST
        else:
            quat_o = QuatOrderingConvention.NONE
    x = 0
    if quat_o == QuatOrderingConvention.SCALARFIRST:
        (_, x, _, _) = Qfunc(
            (0.0, 0.0, np.sqrt(3.0) / 2.0, 1.0 / 2.0),
            (0.0, 0.0, 1.0 / 2.0, np.sqrt(3.0) / 2.0)
        )
    elif quat_o == QuatOrderingConvention.SCALARLAST:
        (_, x, _, _) = Qfunc(
            (0.0, np.sqrt(3.0) / 2.0, 1.0 / 2.0, 0.0),
            (0.0, 1.0 / 2.0, np.sqrt(3.0) / 2.0, 0.0)
        )
    if abs(x - 0.5) < 1e-4:
        quat_h = QuatHandednessConvention.RIGHT
    elif abs(x + 0.5) < 1e-4:
        quat_h = QuatHandednessConvention.LEFT
    else:
        quat_h = QuatHandednessConvention.NONE
    quat_d = DirectionalityConvention.NONE
    if quat_o == QuatOrderingConvention.SCALARFIRST:
        quat_d = _directionality_from_mat(Rfunc(np.sqrt(2.0) / 2.0, np.sqrt(2.0) / 2.0, 0.0, 0.0), "x")
    elif quat_o == QuatOrderingConvention.SCALARLAST:
        quat_d = _directionality_from_mat(Rfunc(0.0, np.sqrt(2.0) / 2.0, 0.0, np.sqrt(2.0) / 2.0), "x")
    quat_f = FunctionConvention.NONE
    if quat_d == DirectionalityConvention.B2W:
        quat_f = FunctionConvention.PASSIVE
    elif quat_d == DirectionalityConvention.W2B:
        if quat_o == QuatOrderingConvention.SCALARFIRST:
            R = Rfunc(np.sqrt(2.0) / 2.0, 0.0, 0.0, np.sqrt(2.0) / 2.0)
            if abs(R[0, 1] + 1.0) < 1e-4:
                quat_f = FunctionConvention.PASSIVE
            elif abs(R[0, 1] - 1.0) < 1e-4:
                quat_f = FunctionConvention.ACTIVE
        elif quat_o == QuatOrderingConvention.SCALARLAST:
            R = Rfunc(0.0, 0.0, np.sqrt(2.0) / 2.0, np.sqrt(2.0) / 2.0)
            if abs(R[0, 1] + 1.0) < 1e-4:
                quat_f = FunctionConvention.PASSIVE
            elif abs(R[0, 1] - 1.0) < 1e-4:
                quat_f = FunctionConvention.ACTIVE
    conventionStr = f"""
Quaternion Conventions for {libname}:

    Quaternion Ordering:       {QuatOrderingConvention.toStr(quat_o)}
    Quaternion Handedness:     {QuatHandednessConvention.toStr(quat_h)}
    Quaternion Function:       {FunctionConvention.toStr(quat_f)}
    Quaternion Directionality: {DirectionalityConvention.toStr(quat_d)}
"""
    print(conventionStr)

introstr = """
NOTE: You're running find_rotational_conventions.py standalone, but it's most useful as an import to deduce the rotational conventions of the particular library that you're using.

Running an example from the following source code:
=========
test.py |=================================================================
=========                                                                |
from find_rotational_conventions import (                                |
    find_euler_conventions,                                              |
    find_axis_angle_conventions,                                         |
    find_quaternion_conventions,                                         |
)                                                                        |
import numpy as np                                                       |
from typing import Tuple                                                 |
from geometry import SO3 # https://github.com/goromal/geometry           |
                                                                         |
LIBNAME = "manif-geom-cpp/geometry" # Library being tested               |
                                                                         |
def euler2R(arg1: float, arg2: float, arg3: float) -> np.ndarray:        |
    return SO3.fromEuler(arg1, arg2, arg3).R()                           |
                                                                         |
def axisAngle2R(axis: np.ndarray, angle: float) -> np.ndarray:           |
    return SO3.fromAxisAngle(axis, angle).R()                            |
                                                                         |
def quat2R(q1: float, q2: float, q3: float, q4: float) -> np.ndarray:    |
    return SO3.fromQuat(q1, q2, q3, q4).R()                              |
                                                                         |
def quatComp(                                                            |
    q1: Tuple[float, float, float, float],                               |
    q2: Tuple[float, float, float, float]                                |
) -> Tuple[float, float, float, float]:                                  |
    q = SO3.fromQuat(*q1) * SO3.fromQuat(*q2)                            |
    return (q.w(), q.x(), q.y(), q.z())                                  |
                                                                         |
find_axis_angle_conventions(LIBNAME, axisAngle2R)                        |
find_euler_conventions(LIBNAME, euler2R)                                 |
find_quaternion_conventions(LIBNAME, quat2R, quatComp)                   |
                                                                         |
==========================================================================

With output:
"""

def main():
    from geometry import SO3
    print(introstr)
    LIBNAME = "manif-geom-cpp/geometry"
    def euler2R(arg1: float, arg2: float, arg3: float) -> np.ndarray:
        return SO3.fromEuler(arg1, arg2, arg3).R()
    def axisAngle2R(axis: np.ndarray, angle: float) -> np.ndarray:
        return SO3.fromAxisAngle(axis, angle).R()
    def quat2R(q1: float, q2: float, q3: float, q4: float) -> np.ndarray:
        return SO3.fromQuat(q1, q2, q3, q4).R()
    def quatComp(q1: Tuple[float, float, float, float], q2: Tuple[float, float, float, float]) -> Tuple[float, float, float, float]:
        q = SO3.fromQuat(*q1) * SO3.fromQuat(*q2)
        return (q.w(), q.x(), q.y(), q.z())
    find_axis_angle_conventions(LIBNAME, axisAngle2R)
    find_euler_conventions(LIBNAME, euler2R)
    find_quaternion_conventions(LIBNAME, quat2R, quatComp)

if __name__ == "__main__":
    main()