# -----------------------------------------------------------------------------
# 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
[docs]
class LieGroupOps(object):
"""
Python LieGroupOps implementation for :py:class:`symforce.geo.unit3.Unit3`.
"""
[docs]
@staticmethod
def retract(a: sym.Unit3, vec: numpy.ndarray, epsilon: float) -> sym.Unit3:
# Total ops: 80
# Input arrays
_a = a.data
if vec.shape == (2,):
vec = vec.reshape((2, 1))
elif vec.shape != (2, 1):
raise IndexError(
"vec is expected to have shape (2, 1) or (2,); instead had shape {}".format(
vec.shape
)
)
# Intermediate terms (22)
_tmp0 = _a[0] - 1
_tmp1 = _tmp0**2
_tmp2 = _a[1] ** 2
_tmp3 = _a[2] + epsilon * math.copysign(1, _a[2])
_tmp4 = _tmp3**2
_tmp5 = _tmp2 + _tmp4
_tmp6 = 2 / (_tmp1 + _tmp5)
_tmp7 = max(
0,
-(
0.0
if _a[2] ** 2 + _tmp2 - 10 * epsilon * math.copysign(1, _a[0]) == 0
else math.copysign(1, _a[2] ** 2 + _tmp2 - 10 * epsilon * math.copysign(1, _a[0]))
),
)
_tmp8 = 1 - _tmp7
_tmp9 = math.sqrt(epsilon + vec[0, 0] ** 2 + vec[1, 0] ** 2)
_tmp10 = math.cos(_tmp9)
_tmp11 = _tmp9 + epsilon * math.copysign(1, _tmp9)
_tmp12 = math.sin(_tmp11) / _tmp11
_tmp13 = _tmp12 * vec[1, 0]
_tmp14 = _tmp6 * _tmp8
_tmp15 = _tmp0 * _tmp14
_tmp16 = _tmp12 * vec[0, 0]
_tmp17 = _tmp10 * _tmp15
_tmp18 = 2 / _tmp5
_tmp19 = _a[1] * _tmp3
_tmp20 = _tmp14 * _tmp19
_tmp21 = _tmp18 * _tmp19 * _tmp7
# Output terms
_res = sym.Unit3.from_storage(
[
_a[1] * _tmp15 * _tmp16
+ _tmp10 * (_tmp7 + _tmp8 * (-_tmp1 * _tmp6 + 1))
- _tmp13 * _tmp15 * _tmp3,
-_a[1] * _tmp17
+ _tmp13 * (-_tmp20 - _tmp21)
+ _tmp16 * (-_tmp7 * (-_tmp18 * _tmp2 + 1) - _tmp8 * (-_tmp2 * _tmp6 + 1)),
_tmp13 * (_tmp7 * (-_tmp18 * _tmp4 + 1) + _tmp8 * (-_tmp4 * _tmp6 + 1))
+ _tmp16 * (_tmp20 + _tmp21)
- _tmp17 * _tmp3,
]
)
return _res
[docs]
@staticmethod
def local_coordinates(a: sym.Unit3, b: sym.Unit3, epsilon: float) -> numpy.ndarray:
# Total ops: 78
# Input arrays
_a = a.data
_b = b.data
# Intermediate terms (18)
_tmp0 = _a[2] + epsilon * math.copysign(1, _a[2])
_tmp1 = _a[1] * _tmp0
_tmp2 = _a[1] ** 2
_tmp3 = max(
0,
-(
0.0
if _a[2] ** 2 + _tmp2 - 10 * epsilon * math.copysign(1, _a[0]) == 0
else math.copysign(1, _a[2] ** 2 + _tmp2 - 10 * epsilon * math.copysign(1, _a[0]))
),
)
_tmp4 = 1 - _tmp3
_tmp5 = _a[0] - 1
_tmp6 = _tmp5**2
_tmp7 = _tmp0**2
_tmp8 = _tmp2 + _tmp7
_tmp9 = 2 / (_tmp6 + _tmp8)
_tmp10 = _tmp4 * _tmp9
_tmp11 = _tmp1 * _tmp10
_tmp12 = 2 / _tmp8
_tmp13 = _tmp1 * _tmp12 * _tmp3
_tmp14 = _tmp10 * _tmp5
_tmp15 = _b[0] * _tmp14
_tmp16 = min(
1 - epsilon,
max(
epsilon - 1,
-_a[1] * _b[1] * _tmp14
+ _b[0] * (_tmp3 + _tmp4 * (-_tmp6 * _tmp9 + 1))
- _b[2] * _tmp0 * _tmp14,
),
)
_tmp17 = math.acos(_tmp16) / math.sqrt(1 - _tmp16**2)
# Output terms
_res = numpy.zeros(2)
_res[0] = _tmp17 * (
_a[1] * _tmp15
+ _b[1] * (-_tmp3 * (-_tmp12 * _tmp2 + 1) - _tmp4 * (-_tmp2 * _tmp9 + 1))
+ _b[2] * (_tmp11 + _tmp13)
)
_res[1] = _tmp17 * (
_b[1] * (-_tmp11 - _tmp13)
+ _b[2] * (_tmp3 * (-_tmp12 * _tmp7 + 1) + _tmp4 * (-_tmp7 * _tmp9 + 1))
- _tmp0 * _tmp15
)
return _res
[docs]
@staticmethod
def interpolate(a: sym.Unit3, b: sym.Unit3, alpha: float, epsilon: float) -> sym.Unit3:
# Total ops: 111
# Input arrays
_a = a.data
_b = b.data
# Intermediate terms (34)
_tmp0 = _a[0] - 1
_tmp1 = _tmp0**2
_tmp2 = _a[1] ** 2
_tmp3 = _a[2] + epsilon * math.copysign(1, _a[2])
_tmp4 = _tmp3**2
_tmp5 = _tmp2 + _tmp4
_tmp6 = 2 / (_tmp1 + _tmp5)
_tmp7 = max(
0,
-(
0.0
if _a[2] ** 2 + _tmp2 - 10 * epsilon * math.copysign(1, _a[0]) == 0
else math.copysign(1, _a[2] ** 2 + _tmp2 - 10 * epsilon * math.copysign(1, _a[0]))
),
)
_tmp8 = 1 - _tmp7
_tmp9 = _tmp7 + _tmp8 * (-_tmp1 * _tmp6 + 1)
_tmp10 = _a[1] * _tmp3
_tmp11 = _tmp6 * _tmp8
_tmp12 = _tmp10 * _tmp11
_tmp13 = 2 / _tmp5
_tmp14 = _tmp10 * _tmp13 * _tmp7
_tmp15 = -_tmp12 - _tmp14
_tmp16 = _tmp0 * _tmp11
_tmp17 = _tmp16 * _tmp3
_tmp18 = _tmp7 * (-_tmp13 * _tmp4 + 1) + _tmp8 * (-_tmp4 * _tmp6 + 1)
_tmp19 = -_b[0] * _tmp17 + _b[1] * _tmp15 + _b[2] * _tmp18
_tmp20 = _a[1] * _tmp16
_tmp21 = min(1 - epsilon, max(epsilon - 1, _b[0] * _tmp9 - _b[1] * _tmp20 - _b[2] * _tmp17))
_tmp22 = math.acos(_tmp21)
_tmp23 = 1 - _tmp21**2
_tmp24 = _tmp22**2 * alpha**2 / _tmp23
_tmp25 = _tmp12 + _tmp14
_tmp26 = -_tmp7 * (-_tmp13 * _tmp2 + 1) - _tmp8 * (-_tmp2 * _tmp6 + 1)
_tmp27 = _b[0] * _tmp20 + _b[1] * _tmp26 + _b[2] * _tmp25
_tmp28 = math.sqrt(_tmp19**2 * _tmp24 + _tmp24 * _tmp27**2 + epsilon)
_tmp29 = math.cos(_tmp28)
_tmp30 = _tmp28 + epsilon * math.copysign(1, _tmp28)
_tmp31 = _tmp22 * alpha * math.sin(_tmp30) / (math.sqrt(_tmp23) * _tmp30)
_tmp32 = _tmp19 * _tmp31
_tmp33 = _tmp27 * _tmp31
# Output terms
_res = sym.Unit3.from_storage(
[
-_tmp17 * _tmp32 + _tmp20 * _tmp33 + _tmp29 * _tmp9,
_tmp15 * _tmp32 - _tmp20 * _tmp29 + _tmp26 * _tmp33,
-_tmp17 * _tmp29 + _tmp18 * _tmp32 + _tmp25 * _tmp33,
]
)
return _res