Geometry Tutorial

This is an introductory walkthrough of the symbolic geometry package in symforce. Symforce natively contains the following geometry objects: complex numbers, quaternions, dual quaternions, matrices, 2D and 3D rotations, and 2D and 3D poses (translation + rotation).

In this tutorial we will demonstrate a few of the ways these geometry objects can be constructed and composed. Here we pay special attention to 3D rotations, poses, and vectors, as they are typically the most commonly used geometric objects.

# Setup
import symforce

symforce.set_symbolic_api("sympy")
symforce.set_log_level("warning")

import symforce.symbolic as sf
from symforce import ops
from symforce.notebook_util import display

Rotations

Rotations can be defined from and converted to a number of different representations as shown below, but always use a quaternion as the underlying representation. We use the notation world_R_body to represent a rotation that rotates a point written in the body frame into the world frame.

Note that in some cases a small epsilon can be provided to prevent numerical errors (e.g. division by zero) when converting to/from rotation representations. Furthermore, converting between certain representations can require additional symbolic expressions to guard against degenerate cases. For example, a rotation constructed from a rotation matrix results in more complexity than when constructed using an axis-angle representation as shown below.

# Identity definition
display(sf.Rot3())

<Rot3 <Q xyzw=[0, 0, 0, 1]>>

# Symbolic definition
display(sf.Rot3.symbolic("R"))

<Rot3 <Q xyzw=[R_x, R_y, R_z, R_w]>>

# From a quaternion of the form qw + qv0 + qv1 + qv2
qv = sf.V3.symbolic("qv")
qw = sf.Symbol("qw")
quat = sf.Quaternion(xyz=qv, w=qw)
display(quat)
# we can use this quaternion to initialize a sf.Rot3:
R_from_quat = sf.Rot3(quat)
display(R_from_quat)

<Q xyzw=[qv0, qv1, qv2, qw]>

<Rot3 <Q xyzw=[qv0, qv1, qv2, qw]>>

# To/From rotation matrix

# Rotate about x-axis
theta = sf.Symbol("theta")
R_mat = sf.Matrix(
    [
        [1, 0, 0],
        [0, sf.cos(theta), -sf.sin(theta)],
        [0, sf.sin(theta), sf.cos(theta)],
    ]
)
R = sf.Rot3.from_rotation_matrix(R_mat)

display(R_mat)
display(R)  # Note the additional expressions required to avoid numerical errors
display(R.to_rotation_matrix())

$\displaystyle \left[\begin{matrix}1 & 0 & 0\\0 & \cos{\left(\theta \right)} & - \sin{\left(\theta \right)}\\0 & \sin{\left(\theta \right)} & \cos{\left(\theta \right)}\end{matrix}\right]$

<Rot3 <Q xyzw=[(1 - Max(0, sign(Max(1, 2*cos(theta) + 1, cos(theta)) - 1.1)))*(sqrt(Max(0, 2 - 2*cos(theta)))/2 + Max(0, sign(Max(1, 2*cos(theta) + 1, cos(theta)) - 1.1))/2) + 2*sin(theta)*Min(1 - Max(0, sign(-2*cos(theta) + Max(1, 2*cos(theta) + 1, cos(theta)) - 1.1)), 1 - Max(0, 1 - Max(0, sign(Max(1, 2*cos(theta) + 1, cos(theta)) - 1.1)), Min(1 - Max(0, sign(-cos(theta) + Max(1, 2*cos(theta) + 1, cos(theta)) - 0.1)), Max(1 - Max(0, 1 - Max(0, sign(Max(1, 2*cos(theta) + 1, cos(theta)) - 1.1))), 1 - Max(0, 1 - Max(0, sign(Max(1, 2*cos(theta) + 1, cos(theta)) - 1.1)), Min(1 - Max(0, 1 - Max(0, sign(Max(1, 2*cos(theta) + 1, cos(theta)) - 1.1))), 1 - Max(0, sign(-cos(theta) + Max(1, 2*cos(theta) + 1, cos(theta)) - 0.1))))))))/(2*sqrt(Max(0, 2*cos(theta) + 2)) - 2*Min(1 - Max(0, sign(-2*cos(theta) + Max(1, 2*cos(theta) + 1, cos(theta)) - 1.1)), 1 - Max(0, 1 - Max(0, sign(Max(1, 2*cos(theta) + 1, cos(theta)) - 1.1)), Min(1 - Max(0, sign(-cos(theta) + Max(1, 2*cos(theta) + 1, cos(theta)) - 0.1)), Max(1 - Max(0, 1 - Max(0, sign(Max(1, 2*cos(theta) + 1, cos(theta)) - 1.1))), 1 - Max(0, 1 - Max(0, sign(Max(1, 2*cos(theta) + 1, cos(theta)) - 1.1)), Min(1 - Max(0, 1 - Max(0, sign(Max(1, 2*cos(theta) + 1, cos(theta)) - 1.1))), 1 - Max(0, sign(-cos(theta) + Max(1, 2*cos(theta) + 1, cos(theta)) - 0.1)))))))) + 2), (1/2 - Min(1 - Max(0, 1 - Max(0, sign(Max(1, 2*cos(theta) + 1, cos(theta)) - 1.1))), 1 - Max(0, sign(-cos(theta) + Max(1, 2*cos(theta) + 1, cos(theta)) - 0.1)))/2)*Min(1 - Max(0, 1 - Max(0, sign(Max(1, 2*cos(theta) + 1, cos(theta)) - 1.1))), 1 - Max(0, sign(-cos(theta) + Max(1, 2*cos(theta) + 1, cos(theta)) - 0.1))), (1/2 - Min(1 - Max(0, sign(-cos(theta) + Max(1, 2*cos(theta) + 1, cos(theta)) - 0.1)), 1 - Max(0, 1 - Max(0, sign(Max(1, 2*cos(theta) + 1, cos(theta)) - 1.1)), Min(1 - Max(0, 1 - Max(0, sign(Max(1, 2*cos(theta) + 1, cos(theta)) - 1.1))), 1 - Max(0, sign(-cos(theta) + Max(1, 2*cos(theta) + 1, cos(theta)) - 0.1)))))/2)*Min(1 - Max(0, sign(-cos(theta) + Max(1, 2*cos(theta) + 1, cos(theta)) - 0.1)), 1 - Max(0, 1 - Max(0, sign(Max(1, 2*cos(theta) + 1, cos(theta)) - 1.1)), Min(1 - Max(0, 1 - Max(0, sign(Max(1, 2*cos(theta) + 1, cos(theta)) - 1.1))), 1 - Max(0, sign(-cos(theta) + Max(1, 2*cos(theta) + 1, cos(theta)) - 0.1))))), 2*(1 - Max(0, sign(Max(1, 2*cos(theta) + 1, cos(theta)) - 1.1)))*sin(theta)/(2*sqrt(Max(0, 2 - 2*cos(theta))) + 2*Max(0, sign(Max(1, 2*cos(theta) + 1, cos(theta)) - 1.1))) + (sqrt(Max(0, 2*cos(theta) + 2))/2 - Min(1 - Max(0, sign(-2*cos(theta) + Max(1, 2*cos(theta) + 1, cos(theta)) - 1.1)), 1 - Max(0, 1 - Max(0, sign(Max(1, 2*cos(theta) + 1, cos(theta)) - 1.1)), Min(1 - Max(0, sign(-cos(theta) + Max(1, 2*cos(theta) + 1, cos(theta)) - 0.1)), Max(1 - Max(0, 1 - Max(0, sign(Max(1, 2*cos(theta) + 1, cos(theta)) - 1.1))), 1 - Max(0, 1 - Max(0, sign(Max(1, 2*cos(theta) + 1, cos(theta)) - 1.1)), Min(1 - Max(0, 1 - Max(0, sign(Max(1, 2*cos(theta) + 1, cos(theta)) - 1.1))), 1 - Max(0, sign(-cos(theta) + Max(1, 2*cos(theta) + 1, cos(theta)) - 0.1))))))))/2 + 1/2)*Min(1 - Max(0, sign(-2*cos(theta) + Max(1, 2*cos(theta) + 1, cos(theta)) - 1.1)), 1 - Max(0, 1 - Max(0, sign(Max(1, 2*cos(theta) + 1, cos(theta)) - 1.1)), Min(1 - Max(0, sign(-cos(theta) + Max(1, 2*cos(theta) + 1, cos(theta)) - 0.1)), Max(1 - Max(0, 1 - Max(0, sign(Max(1, 2*cos(theta) + 1, cos(theta)) - 1.1))), 1 - Max(0, 1 - Max(0, sign(Max(1, 2*cos(theta) + 1, cos(theta)) - 1.1)), Min(1 - Max(0, 1 - Max(0, sign(Max(1, 2*cos(theta) + 1, cos(theta)) - 1.1))), 1 - Max(0, sign(-cos(theta) + Max(1, 2*cos(theta) + 1, cos(theta)) - 0.1))))))))]>>

$\displaystyle \left[\begin{matrix}- 2 \left(\frac{1}{2} - \frac{\min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)}{2}\right)^{2} \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)^{2} - 2 \left(\frac{1}{2} - \frac{\min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)}{2}\right)^{2} \min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)^{2} + 1 & \left(\frac{1}{2} - \frac{\min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)}{2}\right) \left(2 \cdot \left(1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right) \left(\frac{\sqrt{\max\left(0, 2 - 2 \cos{\left(\theta \right)}\right)}}{2} + \frac{\max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)}{2}\right) + \frac{4 \sin{\left(\theta \right)} \min\left(1 - \max\left(0, \operatorname{sign}{\left(- 2 \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), \max\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)\right)\right)\right)}{2 \sqrt{\max\left(0, 2 \cos{\left(\theta \right)} + 2\right)} - 2 \min\left(1 - \max\left(0, \operatorname{sign}{\left(- 2 \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), \max\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)\right)\right)\right) + 2}\right) \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right) - 2 \cdot \left(\frac{1}{2} - \frac{\min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)}{2}\right) \left(\frac{2 \cdot \left(1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right) \sin{\left(\theta \right)}}{2 \sqrt{\max\left(0, 2 - 2 \cos{\left(\theta \right)}\right)} + 2 \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)} + \left(\frac{\sqrt{\max\left(0, 2 \cos{\left(\theta \right)} + 2\right)}}{2} - \frac{\min\left(1 - \max\left(0, \operatorname{sign}{\left(- 2 \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), \max\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)\right)\right)\right)}{2} + \frac{1}{2}\right) \min\left(1 - \max\left(0, \operatorname{sign}{\left(- 2 \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), \max\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)\right)\right)\right)\right) \min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right) & 2 \cdot \left(\frac{1}{2} - \frac{\min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)}{2}\right) \left(\frac{2 \cdot \left(1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right) \sin{\left(\theta \right)}}{2 \sqrt{\max\left(0, 2 - 2 \cos{\left(\theta \right)}\right)} + 2 \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)} + \left(\frac{\sqrt{\max\left(0, 2 \cos{\left(\theta \right)} + 2\right)}}{2} - \frac{\min\left(1 - \max\left(0, \operatorname{sign}{\left(- 2 \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), \max\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)\right)\right)\right)}{2} + \frac{1}{2}\right) \min\left(1 - \max\left(0, \operatorname{sign}{\left(- 2 \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), \max\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)\right)\right)\right)\right) \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right) + \left(\frac{1}{2} - \frac{\min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)}{2}\right) \left(2 \cdot \left(1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right) \left(\frac{\sqrt{\max\left(0, 2 - 2 \cos{\left(\theta \right)}\right)}}{2} + \frac{\max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)}{2}\right) + \frac{4 \sin{\left(\theta \right)} \min\left(1 - \max\left(0, \operatorname{sign}{\left(- 2 \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), \max\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)\right)\right)\right)}{2 \sqrt{\max\left(0, 2 \cos{\left(\theta \right)} + 2\right)} - 2 \min\left(1 - \max\left(0, \operatorname{sign}{\left(- 2 \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), \max\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)\right)\right)\right) + 2}\right) \min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)\\\left(\frac{1}{2} - \frac{\min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)}{2}\right) \left(2 \cdot \left(1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right) \left(\frac{\sqrt{\max\left(0, 2 - 2 \cos{\left(\theta \right)}\right)}}{2} + \frac{\max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)}{2}\right) + \frac{4 \sin{\left(\theta \right)} \min\left(1 - \max\left(0, \operatorname{sign}{\left(- 2 \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), \max\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)\right)\right)\right)}{2 \sqrt{\max\left(0, 2 \cos{\left(\theta \right)} + 2\right)} - 2 \min\left(1 - \max\left(0, \operatorname{sign}{\left(- 2 \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), \max\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)\right)\right)\right) + 2}\right) \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right) + 2 \cdot \left(\frac{1}{2} - \frac{\min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)}{2}\right) \left(\frac{2 \cdot \left(1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right) \sin{\left(\theta \right)}}{2 \sqrt{\max\left(0, 2 - 2 \cos{\left(\theta \right)}\right)} + 2 \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)} + \left(\frac{\sqrt{\max\left(0, 2 \cos{\left(\theta \right)} + 2\right)}}{2} - \frac{\min\left(1 - \max\left(0, \operatorname{sign}{\left(- 2 \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), \max\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)\right)\right)\right)}{2} + \frac{1}{2}\right) \min\left(1 - \max\left(0, \operatorname{sign}{\left(- 2 \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), \max\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)\right)\right)\right)\right) \min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right) & - 2 \left(\frac{1}{2} - \frac{\min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)}{2}\right)^{2} \min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)^{2} - 2 \left(\left(1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right) \left(\frac{\sqrt{\max\left(0, 2 - 2 \cos{\left(\theta \right)}\right)}}{2} + \frac{\max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)}{2}\right) + \frac{2 \sin{\left(\theta \right)} \min\left(1 - \max\left(0, \operatorname{sign}{\left(- 2 \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), \max\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)\right)\right)\right)}{2 \sqrt{\max\left(0, 2 \cos{\left(\theta \right)} + 2\right)} - 2 \min\left(1 - \max\left(0, \operatorname{sign}{\left(- 2 \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), \max\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)\right)\right)\right) + 2}\right)^{2} + 1 & 2 \cdot \left(\frac{1}{2} - \frac{\min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)}{2}\right) \left(\frac{1}{2} - \frac{\min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)}{2}\right) \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right) \min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right) - \left(2 \cdot \left(1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right) \left(\frac{\sqrt{\max\left(0, 2 - 2 \cos{\left(\theta \right)}\right)}}{2} + \frac{\max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)}{2}\right) + \frac{4 \sin{\left(\theta \right)} \min\left(1 - \max\left(0, \operatorname{sign}{\left(- 2 \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), \max\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)\right)\right)\right)}{2 \sqrt{\max\left(0, 2 \cos{\left(\theta \right)} + 2\right)} - 2 \min\left(1 - \max\left(0, \operatorname{sign}{\left(- 2 \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), \max\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)\right)\right)\right) + 2}\right) \left(\frac{2 \cdot \left(1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right) \sin{\left(\theta \right)}}{2 \sqrt{\max\left(0, 2 - 2 \cos{\left(\theta \right)}\right)} + 2 \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)} + \left(\frac{\sqrt{\max\left(0, 2 \cos{\left(\theta \right)} + 2\right)}}{2} - \frac{\min\left(1 - \max\left(0, \operatorname{sign}{\left(- 2 \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), \max\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)\right)\right)\right)}{2} + \frac{1}{2}\right) \min\left(1 - \max\left(0, \operatorname{sign}{\left(- 2 \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), \max\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)\right)\right)\right)\right)\\- 2 \cdot \left(\frac{1}{2} - \frac{\min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)}{2}\right) \left(\frac{2 \cdot \left(1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right) \sin{\left(\theta \right)}}{2 \sqrt{\max\left(0, 2 - 2 \cos{\left(\theta \right)}\right)} + 2 \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)} + \left(\frac{\sqrt{\max\left(0, 2 \cos{\left(\theta \right)} + 2\right)}}{2} - \frac{\min\left(1 - \max\left(0, \operatorname{sign}{\left(- 2 \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), \max\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)\right)\right)\right)}{2} + \frac{1}{2}\right) \min\left(1 - \max\left(0, \operatorname{sign}{\left(- 2 \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), \max\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)\right)\right)\right)\right) \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right) + \left(\frac{1}{2} - \frac{\min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)}{2}\right) \left(2 \cdot \left(1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right) \left(\frac{\sqrt{\max\left(0, 2 - 2 \cos{\left(\theta \right)}\right)}}{2} + \frac{\max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)}{2}\right) + \frac{4 \sin{\left(\theta \right)} \min\left(1 - \max\left(0, \operatorname{sign}{\left(- 2 \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), \max\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)\right)\right)\right)}{2 \sqrt{\max\left(0, 2 \cos{\left(\theta \right)} + 2\right)} - 2 \min\left(1 - \max\left(0, \operatorname{sign}{\left(- 2 \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), \max\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)\right)\right)\right) + 2}\right) \min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right) & 2 \cdot \left(\frac{1}{2} - \frac{\min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)}{2}\right) \left(\frac{1}{2} - \frac{\min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)}{2}\right) \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right) \min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right) + \left(2 \cdot \left(1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right) \left(\frac{\sqrt{\max\left(0, 2 - 2 \cos{\left(\theta \right)}\right)}}{2} + \frac{\max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)}{2}\right) + \frac{4 \sin{\left(\theta \right)} \min\left(1 - \max\left(0, \operatorname{sign}{\left(- 2 \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), \max\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)\right)\right)\right)}{2 \sqrt{\max\left(0, 2 \cos{\left(\theta \right)} + 2\right)} - 2 \min\left(1 - \max\left(0, \operatorname{sign}{\left(- 2 \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), \max\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)\right)\right)\right) + 2}\right) \left(\frac{2 \cdot \left(1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right) \sin{\left(\theta \right)}}{2 \sqrt{\max\left(0, 2 - 2 \cos{\left(\theta \right)}\right)} + 2 \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)} + \left(\frac{\sqrt{\max\left(0, 2 \cos{\left(\theta \right)} + 2\right)}}{2} - \frac{\min\left(1 - \max\left(0, \operatorname{sign}{\left(- 2 \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), \max\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)\right)\right)\right)}{2} + \frac{1}{2}\right) \min\left(1 - \max\left(0, \operatorname{sign}{\left(- 2 \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), \max\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)\right)\right)\right)\right) & - 2 \left(\frac{1}{2} - \frac{\min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)}{2}\right)^{2} \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)^{2} - 2 \left(\left(1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right) \left(\frac{\sqrt{\max\left(0, 2 - 2 \cos{\left(\theta \right)}\right)}}{2} + \frac{\max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)}{2}\right) + \frac{2 \sin{\left(\theta \right)} \min\left(1 - \max\left(0, \operatorname{sign}{\left(- 2 \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), \max\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)\right)\right)\right)}{2 \sqrt{\max\left(0, 2 \cos{\left(\theta \right)} + 2\right)} - 2 \min\left(1 - \max\left(0, \operatorname{sign}{\left(- 2 \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right), \max\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right), \min\left(1 - \max\left(0, 1 - \max\left(0, \operatorname{sign}{\left(\max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 1.1 \right)}\right)\right), 1 - \max\left(0, \operatorname{sign}{\left(- \cos{\left(\theta \right)} + \max\left(1, 2 \cos{\left(\theta \right)} + 1, \cos{\left(\theta \right)}\right) - 0.1 \right)}\right)\right)\right)\right)\right)\right)\right) + 2}\right)^{2} + 1\end{matrix}\right]$

# To/From Euler angles
R = sf.Rot3.from_yaw_pitch_roll(0, 0, theta)  # Yaw rotation only
ypr = R.to_yaw_pitch_roll()

display(R)
display(ops.StorageOps.simplify(list(ypr)))  # Simplify YPR expression

<Rot3 <Q xyzw=[sin(theta/2), 0, 0, cos(theta/2)]>>

$\displaystyle \left[ 0, \ 0, \ \operatorname{atan}_{2}{\left(\sin{\left(\theta \right)},\cos{\left(\theta \right)} \right)}\right]$

# From axis-angle representation

# Rotate about x-axis
R = sf.Rot3.from_angle_axis(angle=theta, axis=sf.Vector3(1, 0, 0))

display(R)

<Rot3 <Q xyzw=[sin(theta/2), 0, 0, cos(theta/2)]>>

Now that we can construct rotations, we can use them to rotate vectors as one would expect.

# Rotation defining orientation of body frame wrt world frame
world_R_body = sf.Rot3.symbolic("R")

# Point written in body frame
body_t_point = sf.Vector3.symbolic("p")

# Point written in world frame
world_t_point = world_R_body * body_t_point

display(world_t_point)

$\displaystyle \left[\begin{matrix}p_{0} \left(- 2 R_{y}^{2} - 2 R_{z}^{2} + 1\right) + p_{1} \left(- 2 R_{w} R_{z} + 2 R_{x} R_{y}\right) + p_{2} \cdot \left(2 R_{w} R_{y} + 2 R_{x} R_{z}\right)\\p_{0} \cdot \left(2 R_{w} R_{z} + 2 R_{x} R_{y}\right) + p_{1} \left(- 2 R_{x}^{2} - 2 R_{z}^{2} + 1\right) + p_{2} \left(- 2 R_{w} R_{x} + 2 R_{y} R_{z}\right)\\p_{0} \left(- 2 R_{w} R_{y} + 2 R_{x} R_{z}\right) + p_{1} \cdot \left(2 R_{w} R_{x} + 2 R_{y} R_{z}\right) + p_{2} \left(- 2 R_{x}^{2} - 2 R_{y}^{2} + 1\right)\end{matrix}\right]$

Chaining rotations and inverting rotations works as one would expect as well:

body_R_cam = sf.Rot3.symbolic("R_cam")
world_R_cam = world_R_body * body_R_cam

# Rotation inverse = negate vector part of quaternion
cam_R_body = body_R_cam.inverse()
display(body_R_cam)
display(cam_R_body)

<Rot3 <Q xyzw=[R_cam_x, R_cam_y, R_cam_z, R_cam_w]>>

<Rot3 <Q xyzw=[-R_cam_x, -R_cam_y, -R_cam_z, R_cam_w]>>

We can also easily substitute numerical values into symbolic expressions using geo objects themselves. This makes it very convenient to substitute numeric values into large symbolic expressions constructed using many different geo objects.

world_R_body_numeric = sf.Rot3.from_yaw_pitch_roll(0.1, -2.3, 0.7)
display(world_t_point.subs(world_R_body, world_R_body_numeric))

$\displaystyle \left[\begin{matrix}- 0.662947416398295 p_{0} - 0.554353314451006 p_{1} - 0.503182994394693 p_{2}\\- 0.0665166116342196 p_{0} + 0.713061539471145 p_{1} - 0.697938952419008 p_{2}\\0.74570521217672 p_{0} - 0.429226797490819 p_{1} - 0.509596009450867 p_{2}\end{matrix}\right]$

Poses

Poses are defined as a rotation plus a translation, and are constructed as such. We use the notation world_T_body to represent a pose that transforms from the body frame to the world frame.

# Symbolic construction
world_T_body = sf.Pose3.symbolic("T")
display(world_T_body)

<Pose3 R=<Rot3 <Q xyzw=[T.R_x, T.R_y, T.R_z, T.R_w]>>, t=(T.t0, T.t1, T.t2)>

# Construction from a rotation and translation

# Orientation of body frame wrt world frame
world_R_body = sf.Rot3.symbolic("R")

# Position of body frame wrt world frame written in the world frame
world_t_body = sf.Vector3.symbolic("t")

world_T_body = sf.Pose3(R=world_R_body, t=world_t_body)
display(world_T_body)

<Pose3 R=<Rot3 <Q xyzw=[R_x, R_y, R_z, R_w]>>, t=(t0, t1, t2)>

Similar to rotations, we can compose poses with poses, compose poses with 3D points, and invert poses as one would expect.

# Compose pose with a pose
body_T_cam = sf.Pose3.symbolic("T_cam")
world_T_cam = world_T_body * body_T_cam

# Compose pose with a point
body_t_point = sf.Vector3.symbolic("p")  # Position in body frame
# Equivalent to: world_R_body * body_t_point + world_t_body
world_t_point = world_T_body * body_t_point
display(world_t_point)

$\displaystyle \left[\begin{matrix}p_{0} \left(- 2 R_{y}^{2} - 2 R_{z}^{2} + 1\right) + p_{1} \left(- 2 R_{w} R_{z} + 2 R_{x} R_{y}\right) + p_{2} \cdot \left(2 R_{w} R_{y} + 2 R_{x} R_{z}\right) + t_{0}\\p_{0} \cdot \left(2 R_{w} R_{z} + 2 R_{x} R_{y}\right) + p_{1} \left(- 2 R_{x}^{2} - 2 R_{z}^{2} + 1\right) + p_{2} \left(- 2 R_{w} R_{x} + 2 R_{y} R_{z}\right) + t_{1}\\p_{0} \left(- 2 R_{w} R_{y} + 2 R_{x} R_{z}\right) + p_{1} \cdot \left(2 R_{w} R_{x} + 2 R_{y} R_{z}\right) + p_{2} \left(- 2 R_{x}^{2} - 2 R_{y}^{2} + 1\right) + t_{2}\end{matrix}\right]$

# Invert a pose
body_T_world = world_T_body.inverse()
display(world_T_body)
display(body_T_world)

<Pose3 R=<Rot3 <Q xyzw=[R_x, R_y, R_z, R_w]>>, t=(t0, t1, t2)>

<Pose3 R=<Rot3 <Q xyzw=[-R_x, -R_y, -R_z, R_w]>>, t=(-t0*(-2*R_y**2 - 2*R_z**2 + 1) - t1*(2*R_w*R_z + 2*R_x*R_y) - t2*(-2*R_w*R_y + 2*R_x*R_z), -t0*(-2*R_w*R_z + 2*R_x*R_y) - t1*(-2*R_x**2 - 2*R_z**2 + 1) - t2*(2*R_w*R_x + 2*R_y*R_z), -t0*(2*R_w*R_y + 2*R_x*R_z) - t1*(-2*R_w*R_x + 2*R_y*R_z) - t2*(-2*R_x**2 - 2*R_y**2 + 1))>

Vectors and Matrices

Vectors and matrices are all represented using subclasses of sf.Matrix class, and can be constructed in several different ways as shown below.

# Matrix construction. The statements below all create the same 2x3 matrix object

# Construction from 2D list
m1 = sf.Matrix([[1, 2, 3], [4, 5, 6]])

# Construction using specified size + data
m2 = sf.Matrix(2, 3, [1, 2, 3, 4, 5, 6])

# sf.MatrixNM creates a matrix with shape NxM (defined by default for 6x6
# matrices and smaller)
m3 = sf.Matrix23(1, 2, 3, 4, 5, 6)
m4 = sf.Matrix23([1, 2, 3, 4, 5, 6])

# Construction using aliases
m5 = sf.M([[1, 2, 3], [4, 5, 6]])
m6 = sf.M(2, 3, [1, 2, 3, 4, 5, 6])
m7 = sf.M23(1, 2, 3, 4, 5, 6)
m8 = sf.M23([1, 2, 3, 4, 5, 6])

# Construction from block matrices of appropriate dimensions
m9 = sf.Matrix23.block_matrix([[sf.M13([1, 2, 3])], [sf.M13([3, 4, 5])]])

# Vector constructors. The statements below all create the same 3x1 vector object

# Construction from 2D list
v1 = sf.Matrix([[1], [2], [3]])

# Construction from 1D list. We assume a 1D list represents a column vector.
v2 = sf.Matrix([1, 2, 3])

# Construction using aliases (defined by default for 9x1 vectors and smaller)
v3 = sf.Matrix31(1, 2, 3)
v4 = sf.M31(1, 2, 3)
v5 = sf.Vector3(1, 2, 3)
v6 = sf.V3(1, 2, 3)

We can also use a few typical matrix constructors:

# Matrix of zeros
z1 = sf.Matrix23.zero()
z2 = sf.Matrix.zeros(2, 3)

# Matrix of ones
o1 = sf.Matrix23.one()
o2 = sf.Matrix.ones(2, 3)

Note that the Matrix class itself does not contain group or lie group methods, to prevent confusion between the identity matrix and inverse matrix, and the group operations under addition. The group operations are implemented separately for matrices under addition, and are accessed through ops.GroupOps and ops.LieGroupOps.

zero_matrix = sf.Matrix33.zero()
identity_matrix = sf.Matrix33.eye()

# We could also write:
zero_matrix = ops.GroupOps.identity(sf.Matrix33)

display(zero_matrix)
display(identity_matrix)

$\displaystyle \left[\begin{matrix}0 & 0 & 0\\0 & 0 & 0\\0 & 0 & 0\end{matrix}\right]$

$\displaystyle \left[\begin{matrix}1 & 0 & 0\\0 & 1 & 0\\0 & 0 & 1\end{matrix}\right]$

And, of course, matrix math works as one would expect:

# Matrix multiplication
m23 = sf.M23.symbolic("lhs")
m31 = sf.V3.symbolic("rhs")
display(m23 * m31)

$\displaystyle \left[\begin{matrix}lhs_{0 0} rhs_{0} + lhs_{0 1} rhs_{1} + lhs_{0 2} rhs_{2}\\lhs_{1 0} rhs_{0} + lhs_{1 1} rhs_{1} + lhs_{1 2} rhs_{2}\end{matrix}\right]$

# Vector operations
norm = m31.norm()
squared_norm = m31.squared_norm()
unit_vec = m31.normalized()
display(unit_vec)

$\displaystyle \left[\begin{matrix}\frac{rhs_{0}}{\sqrt{rhs_{0}^{2} + rhs_{1}^{2} + rhs_{2}^{2}}}\\\frac{rhs_{1}}{\sqrt{rhs_{0}^{2} + rhs_{1}^{2} + rhs_{2}^{2}}}\\\frac{rhs_{2}}{\sqrt{rhs_{0}^{2} + rhs_{1}^{2} + rhs_{2}^{2}}}\end{matrix}\right]$

m33 = 5 * sf.Matrix33.eye()  # Element-wise multiplication with scalar
display(m33.inv())  # Matrix inverse

$\displaystyle \left[\begin{matrix}\frac{1}{5} & 0 & 0\\0 & \frac{1}{5} & 0\\0 & 0 & \frac{1}{5}\end{matrix}\right]$

One of the most powerful operations we can use matrices for is to compute jacobians with respect to other geo objects. By default we compute jacobians with respect to the tangent space of the given object.

R0 = sf.Rot3.symbolic("R0")
R1 = sf.Rot3.symbolic("R1")
residual = sf.M(R0.local_coordinates(R1))
display(residual)

$\displaystyle \left[\begin{matrix}\frac{2 \cdot \left(2 \min\left(0, \operatorname{sign}{\left(R_{0 w} R_{1 w} + R_{0 x} R_{1 x} + R_{0 y} R_{1 y} + R_{0 z} R_{1 z} \right)}\right) + 1\right) \left(R_{0 w} R_{1 x} - R_{0 x} R_{1 w} - R_{0 y} R_{1 z} + R_{0 z} R_{1 y}\right) \operatorname{acos}{\left(\min\left(1.0, \left|{R_{0 w} R_{1 w} + R_{0 x} R_{1 x} + R_{0 y} R_{1 y} + R_{0 z} R_{1 z}}\right|\right) \right)}}{\sqrt{1 - \min\left(1.0, \left|{R_{0 w} R_{1 w} + R_{0 x} R_{1 x} + R_{0 y} R_{1 y} + R_{0 z} R_{1 z}}\right|\right)^{2}}}\\\frac{2 \cdot \left(2 \min\left(0, \operatorname{sign}{\left(R_{0 w} R_{1 w} + R_{0 x} R_{1 x} + R_{0 y} R_{1 y} + R_{0 z} R_{1 z} \right)}\right) + 1\right) \left(R_{0 w} R_{1 y} + R_{0 x} R_{1 z} - R_{0 y} R_{1 w} - R_{0 z} R_{1 x}\right) \operatorname{acos}{\left(\min\left(1.0, \left|{R_{0 w} R_{1 w} + R_{0 x} R_{1 x} + R_{0 y} R_{1 y} + R_{0 z} R_{1 z}}\right|\right) \right)}}{\sqrt{1 - \min\left(1.0, \left|{R_{0 w} R_{1 w} + R_{0 x} R_{1 x} + R_{0 y} R_{1 y} + R_{0 z} R_{1 z}}\right|\right)^{2}}}\\\frac{2 \cdot \left(2 \min\left(0, \operatorname{sign}{\left(R_{0 w} R_{1 w} + R_{0 x} R_{1 x} + R_{0 y} R_{1 y} + R_{0 z} R_{1 z} \right)}\right) + 1\right) \left(R_{0 w} R_{1 z} - R_{0 x} R_{1 y} + R_{0 y} R_{1 x} - R_{0 z} R_{1 w}\right) \operatorname{acos}{\left(\min\left(1.0, \left|{R_{0 w} R_{1 w} + R_{0 x} R_{1 x} + R_{0 y} R_{1 y} + R_{0 z} R_{1 z}}\right|\right) \right)}}{\sqrt{1 - \min\left(1.0, \left|{R_{0 w} R_{1 w} + R_{0 x} R_{1 x} + R_{0 y} R_{1 y} + R_{0 z} R_{1 z}}\right|\right)^{2}}}\end{matrix}\right]$

jacobian = residual.jacobian(R1)
# The jacobian is quite a complex symbolic expression, so we don't display it for
# convenience.
# The shape is equal to (dimension of residual) x (dimension of tangent space)
display(jacobian.shape)

$\displaystyle \left( 3, \ 3\right)$

General properties of geo objects

Storage operations

All geometric types implement the “Storage” interface. This means that they can:

1. Be serialized into a list of scalar expressions (.to_storage())

2. Be reconstructed from a list of scalar expressions (.from_storage())

3. Use common symbolic operations (symbolic construction, substitution, simplification, etc.)

# Serialization to scalar list
rot = sf.Rot3()
elements = rot.to_storage()
assert len(elements) == rot.storage_dim()
display(elements)

$\displaystyle \left[ 0, \ 0, \ 0, \ 1\right]$

# Construction from scalar list
rot2 = sf.Rot3.from_storage(elements)
assert rot == rot2

# Symbolic operations
rot_sym = sf.Rot3.symbolic("rot_sym")
rot_num = rot_sym.subs(rot_sym, rot)

display(rot_sym)
display(rot_num)
display(rot_num.simplify())  # Simplify internal symbolic expressions
display(rot_num.evalf())  # Numerical evaluation

<Rot3 <Q xyzw=[rot_sym_x, rot_sym_y, rot_sym_z, rot_sym_w]>>

<Rot3 <Q xyzw=[0, 0, 0, 1]>>

<Rot3 <Q xyzw=[0, 0, 0, 1]>>

<Rot3 <Q xyzw=[0, 0, 0, 1.00000000000000]>>

Group operations

All geometric types also implement the “Group” interface, meaning that geometric objects:

1. Can be composed with objects of the same type to produce an object of the same type (.compose())

2. Have an identity element (.identity())

3. Can be inverted (.inverse())

4. Can be created to represent the relation between two other objects of the same type (.between())

# Construct two random rotations
R1 = sf.Rot3.random()
R2 = sf.Rot3.random()

# Composition
display(R1.compose(R2))  # For rotations this is the same as R1 * R2

<Rot3 <Q xyzw=[-0.463545518898991, -0.594641005190260, -0.435186004693471, 0.492078010252356]>>

# Identity
R_identity = sf.Rot3.identity()
display(R1)
display(R_identity * R1)

<Rot3 <Q xyzw=[-0.394948806517800, -0.0487438353062433, -0.0571188089568191, 0.915629248338626]>>

<Rot3 <Q xyzw=[-0.394948806517800, -0.0487438353062433, -0.0571188089568191, 0.915629248338626]>>

# Inverse
R1_inv = R1.inverse()
display(R_identity)
display(R1_inv * R1)

<Rot3 <Q xyzw=[0, 0, 0, 1]>>

<Rot3 <Q xyzw=[0, 0, 0, 1.00000000000000]>>

# Between
R_delta = R1.between(R2)
display(R1 * R_delta)
display(R2)

<Rot3 <Q xyzw=[-0.217337661144407, -0.375085901893963, -0.582619893381795, 0.687480157593263]>>

<Rot3 <Q xyzw=[-0.217337661144407, -0.375085901893963, -0.582619893381795, 0.687480157593263]>>

Lie Group operations

Rotations, poses, and matrices all implement the “LieGroup” interface, meaning that they each have a tangent space. There are many great references on Lie groups out there already, so instead of introducing them here, we recommend checking out Frank Dellaert’s, Ethan Eade’s, or JL Blanco’s tutorials. In SymForce, objects which are a Lie Group can:

1. Be used to compute the tangent space vector about the identity element (.to_tangent())

2. Be constructed from a tangent space vector about the identity element (.from_tangent())

3. Be perturbed by a tangent space vector about the given element (.retract())

4. Be used to compute the tangent space perturbation needed to obtain another given element (.local_coordinates())

5. Be used to compute a jacobian describing the relation between the underlying data of the object (e.g. a quaternion for a rotation) and the tangent space vector about the given element (.storage_D_tangent())

# To/From tangent space vector about identity element
R1 = sf.Rot3.random()
tangent_vec = R1.to_tangent()
R1_recovered = sf.Rot3.from_tangent(tangent_vec)

assert len(tangent_vec) == R1.tangent_dim()
display(R1)
display(R1_recovered)

<Rot3 <Q xyzw=[0.681330596223185, -0.100931790495282, 0.0801011619747841, 0.720545068797133]>>

<Rot3 <Q xyzw=[0.681330596223185, -0.100931790495282, 0.0801011619747841, 0.720545068797133]>>

# Tangent space perturbations

# Perturb R1 by the given vector in the tangent space around R1
R2 = R1.retract([0.1, 2.3, -0.5])

# Compute the tangent vector pointing from R1 to R2, in the tangent space
# around R1
recovered_tangent_vec = R1.local_coordinates(R2)

display(recovered_tangent_vec)

$\displaystyle \left[ 0.1, \ 2.3, \ -0.5\right]$

# Jacobian of storage w.r.t tangent space perturbation

# We chain storage_D_tangent together with jacobians of larger symbolic
# expressions taken with respect to the symbolic elements of the object (e.g. a
# quaternion for rotations) to compute the jacobian wrt the tanget space about
# the element.
# I.e. residual_D_tangent = residual_D_storage * storage_D_tangent

jacobian = R1.storage_D_tangent()
assert jacobian.shape == (R1.storage_dim(), R1.tangent_dim())

For more details on Storage/Group/LieGroup operations, see the Ops tutorial.

Poses and SE(n)

Poses in robotics are often represented using the Special Euclidean group, SE(2) and SE(3) for 2D and 3D respectively. Additionally, the manifold is often defined so that the from_tangent function is the matrix exponential of a linear combination of generators.

This has the advantage of simpler theoretical treatment in some cases, but has disadvantages in terms of the number of operations required to implement commonly used functions like retract. For these reasons, instead of using the SE(n) from_tangent and to_tangent functions, ours decouple the perturbations to rotation and translation. This means our Pose2 and Pose3 objects represent neither SE(n) nor SO(n) x R^n; they compose like objects on SE(n) but have tangent space operations analogous to SO(n) x R^n.

For most users, this should all happen under the hood. The most common observable difference is that retract(g, v) != compose(g, from_tangent(v)), so simplifications done by hand that rely on this fact must be treated with care. We do also provide a Pose3_SE3 class for users who need this functionality in symbolic expressions - this class has no runtime equivalent and is unsupported by much of SymForce, including the Optimizer.