Source code for sym.ops.pose2.lie_group_ops

# -----------------------------------------------------------------------------
# This file was autogenerated by symforce from template:
#     ops/CLASS/lie_group_ops.py.jinja
# Do NOT modify by hand.
# -----------------------------------------------------------------------------

# ruff: noqa: PLR0915, F401, PLW0211, PLR0914

from __future__ import annotations

import math
import typing as T

import numpy

import sym


class LieGroupOps(object):
    """
    Python LieGroupOps implementation for :py:class:`symforce.geo.pose2.Pose2`.
    """

    @staticmethod
    def from_tangent(vec: numpy.ndarray, epsilon: float) -> sym.Pose2:
        # Total ops: 2

        # Input arrays
        if vec.shape == (3,):
            vec = vec.reshape((3, 1))
        elif vec.shape != (3, 1):
            raise IndexError(
                "vec is expected to have shape (3, 1) or (3,); instead had shape {}".format(
                    vec.shape
                )
            )

        # Intermediate terms (0)

        # Output terms
        _res = sym.Pose2.from_storage(
            [math.cos(vec[0, 0]), math.sin(vec[0, 0]), vec[1, 0], vec[2, 0]]
        )
        return _res

    @staticmethod
    def to_tangent(a: sym.Pose2, epsilon: float) -> numpy.ndarray:
        # Total ops: 3

        # Input arrays
        _a = a.data

        # Intermediate terms (0)

        # Output terms
        _res = numpy.zeros(3)
        _res[0] = math.atan2(_a[1], _a[0] + math.copysign(epsilon, _a[0]))
        _res[1] = _a[2]
        _res[2] = _a[3]
        return _res

    @staticmethod
    def retract(a: sym.Pose2, vec: numpy.ndarray, epsilon: float) -> sym.Pose2:
        # Total ops: 10

        # Input arrays
        _a = a.data
        if vec.shape == (3,):
            vec = vec.reshape((3, 1))
        elif vec.shape != (3, 1):
            raise IndexError(
                "vec is expected to have shape (3, 1) or (3,); instead had shape {}".format(
                    vec.shape
                )
            )

        # Intermediate terms (2)
        _tmp0 = math.sin(vec[0, 0])
        _tmp1 = math.cos(vec[0, 0])

        # Output terms
        _res = sym.Pose2.from_storage(
            [
                _a[0] * _tmp1 - _a[1] * _tmp0,
                _a[0] * _tmp0 + _a[1] * _tmp1,
                _a[2] + vec[1, 0],
                _a[3] + vec[2, 0],
            ]
        )
        return _res

    @staticmethod
    def local_coordinates(a: sym.Pose2, b: sym.Pose2, epsilon: float) -> numpy.ndarray:
        # Total ops: 11

        # Input arrays
        _a = a.data
        _b = b.data

        # Intermediate terms (1)
        _tmp0 = _a[0] * _b[0] + _a[1] * _b[1]

        # Output terms
        _res = numpy.zeros(3)
        _res[0] = math.atan2(_a[0] * _b[1] - _a[1] * _b[0], _tmp0 + math.copysign(epsilon, _tmp0))
        _res[1] = -_a[2] + _b[2]
        _res[2] = -_a[3] + _b[3]
        return _res

    @staticmethod
    def interpolate(a: sym.Pose2, b: sym.Pose2, alpha: float, epsilon: float) -> sym.Pose2:
        # Total ops: 24

        # Input arrays
        _a = a.data
        _b = b.data

        # Intermediate terms (4)
        _tmp0 = _a[0] * _b[0] + _a[1] * _b[1]
        _tmp1 = alpha * math.atan2(
            _a[0] * _b[1] - _a[1] * _b[0], _tmp0 + math.copysign(epsilon, _tmp0)
        )
        _tmp2 = math.sin(_tmp1)
        _tmp3 = math.cos(_tmp1)

        # Output terms
        _res = sym.Pose2.from_storage(
            [
                _a[0] * _tmp3 - _a[1] * _tmp2,
                _a[0] * _tmp2 + _a[1] * _tmp3,
                _a[2] + alpha * (-_a[2] + _b[2]),
                _a[3] + alpha * (-_a[3] + _b[3]),
            ]
        )
        return _res