28 lines
1.0 KiB
Python
28 lines
1.0 KiB
Python
from LinearAlgebra.Direction import Direction
|
|
from LinearAlgebra.Quaternion import Quaternion
|
|
|
|
class SwingTwist:
|
|
"""A rotation using swing and twist angle components"""
|
|
def __init__(self, swing: Direction, twist: float):
|
|
if swing.vertical > 90 or swing.vertical < -90:
|
|
swing.horizontal += 180
|
|
swing.vertical = 180 - swing.vertical
|
|
twist += 180
|
|
|
|
## Swing component of the rotation
|
|
self.swing = swing
|
|
## The twist component of the rotation
|
|
self.twist = twist
|
|
|
|
@staticmethod
|
|
def Degrees(horizontal: float, vertical: float, twist: float):
|
|
direction = Direction(horizontal, vertical)
|
|
swing_twist = SwingTwist(direction, twist)
|
|
return swing_twist
|
|
|
|
def ToQuaternion(self) -> Quaternion:
|
|
"""Convert the SwingTwist rotation to a Quaternion"""
|
|
q = Quaternion.Euler(-self.swing.vertical,
|
|
self.swing.horizontal,
|
|
self.twist)
|
|
return q |