Corrects for physical stick rotation

axis_rotation.py

import math
import gremlin


t16000 = gremlin.input_devices.JoystickDecorator(
    "T.16000M",
    72331530,
    "Default"
)


def _deg2rad(angle):
    """Returns radians representation of the provided angle in degrees.

    :param angle value in degrees
    :return angle value in radians
    """
    return angle * (math.pi / 180.0)


def _rad2deg(angle):
    """Returns degree representation of the provided angle in radians.

    :param angle value in radians
    :return angle value in degrees
    """
    return angle * (180.0 / math.pi)


def _max_length(angle):
    """Returns the maximum length of a vector for the given angle.

    :param angle of the vector in radians
    :return maximum length of a vector of the given angle
    """
    angle = abs(angle)
    if 0 <= angle < math.pi/4:
        return math.sqrt(1 + math.tan(angle)**2)
    elif math.pi/4 <= angle < 0.75*math.pi:
        return math.sqrt(1 + (1.0/math.tan(angle)) ** 2)
    else:
        return math.sqrt(1 + math.tan(angle) ** 2)


def _rotate(x, y, angle):
    """Returns vector values after rotation.

    :param x value of the vector
    :param y value of the vector
    :param angle rotation angle in radians
    :return x and y values of the rotated vector
    """
    x_prime = x * math.cos(angle) - y * math.sin(angle)
    y_prime = x * math.sin(angle) + y * math.cos(angle)
    return x_prime, y_prime


class AxisRotationFilter:

    """Applies corrections for a physically rotated stick."""

    def __init__(self, angle):
        """Creates a new instance with the provided rotation angle.

        :param angle the physical rotation angle of the stick in degrees
        """
        self._rotation_angle = _deg2rad(angle)
        self._basis_x = _rotate(1.0, 0.0, self._rotation_angle)
        self._basis_y = _rotate(0.0, 1.0, self._rotation_angle)

    def process(self, x, y):
        """Applies corrections for the physical rotation to the inputs.

        :param x X-axis deflection value
        :param y Y-axis deflection value
        :return corrected x and y axis values
        """
        # Obtain angles of the current input, physical as well as virtual
        physical_angle = math.atan2(y, x)
        virtual_angle = self._rotation_angle + physical_angle

        # Scale factor between virtual and physical vector
        scale = _max_length(virtual_angle) / _max_length(physical_angle)
        return (
            (self._basis_x[0] * x + self._basis_y[0] * y) * scale,
            (self._basis_x[1] * x + self._basis_y[1] * y) * scale
        )


arf = AxisRotationFilter(-15)

g_last_x = 0.0
g_last_y = 0.0


def set_ax(vjoy):
    xp, yp = arf.process(g_last_x, g_last_y)
    vjoy[1].axis(1).value = xp
    vjoy[1].axis(2).value = yp


@t16000.axis(1)
def ax1(event, vjoy):
    global g_last_x
    g_last_x = event.value
    set_ax(vjoy)


@t16000.axis(2)
def ax2(event, vjoy):
    global g_last_y
    g_last_y = event.value
    set_ax(vjoy)