89 lines
3.1 KiB
Python
89 lines
3.1 KiB
Python
from .Direction import *
|
|
from .Quaternion import *
|
|
|
|
class SwingTwist:
|
|
"""A rotation using swing and twist angle components"""
|
|
def __init__(self, swing: Direction, twist: Angle):
|
|
## 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):
|
|
horizontal_angle = Angle.Degrees(horizontal)
|
|
vertical_angle = Angle.Degrees(vertical)
|
|
twist_angle = Angle.Degrees(twist)
|
|
|
|
deg90 = Angle.Degrees(90)
|
|
deg180 = Angle.Degrees(180)
|
|
if vertical_angle > deg90 or vertical_angle < -deg90:
|
|
horizontal_angle += deg180
|
|
vertical_angle = deg180 - vertical_angle
|
|
twist_angle += deg180
|
|
|
|
direction = Direction(horizontal_angle, vertical_angle)
|
|
swing_twist = SwingTwist(direction, twist_angle)
|
|
return swing_twist
|
|
@staticmethod
|
|
def Radians(horizontal: float, vertical: float, twist: float):
|
|
horizontal_angle = Angle.Radians(horizontal)
|
|
vertical_angle = Angle.Radians(vertical)
|
|
twist_angle = Angle.Radians(twist)
|
|
|
|
deg90 = Angle.Radians(math.pi / 2)
|
|
deg180 = Angle.Radians(math.pi)
|
|
if vertical_angle > deg90 or vertical_angle < -deg90:
|
|
horizontal_angle += deg180
|
|
vertical_angle = deg180 - vertical_angle
|
|
twist_angle += deg180
|
|
|
|
direction = Direction(horizontal_angle, vertical_angle)
|
|
swing_twist = SwingTwist(direction, twist_angle)
|
|
return swing_twist
|
|
|
|
def ToQuaternion(self) -> Quaternion:
|
|
"""Convert the SwingTwist rotation to a Quaternion"""
|
|
q = Quaternion.FromAngles(
|
|
-self.swing.vertical,
|
|
self.swing.horizontal,
|
|
self.twist
|
|
)
|
|
return q
|
|
@staticmethod
|
|
def FromQuaternion(q: Quaternion):
|
|
angles = Quaternion.ToAngles(q)
|
|
# direction = Direction(angles[0], angles[1])
|
|
# r: SwingTwist = SwingTwist(direction, angles[2])
|
|
r = SwingTwist.Degrees(
|
|
angles[0].InDegrees(),
|
|
angles[1].InDegrees(),
|
|
angles[2].InDegrees()
|
|
)
|
|
return r
|
|
|
|
def FromAngleAxis(angle: Angle, axis: Direction):
|
|
vectorAxis: Vector3 = axis.ToVector3();
|
|
q: Quaternion = Quaternion.FromAngleAxis(angle, vectorAxis);
|
|
return SwingTwist.FromQuaternion(q)
|
|
|
|
|
|
def __eq__(self, other) -> bool:
|
|
"""! Check if this orientation is equal to the given orientation
|
|
@param other The orientation to check against
|
|
@return true if it is identical to the given orientation
|
|
@note This uses float comparison to check equality which may have strange
|
|
effects. Equality on floats should be avoided.
|
|
"""
|
|
return (
|
|
self.swing == other.swing and
|
|
self.twist == other.twist
|
|
)
|
|
|
|
@staticmethod
|
|
def Angle(r1, r2) -> Angle:
|
|
q1: Quaternion = r1.ToQuaternion()
|
|
q2: Quaternion = r2.ToQuaternion()
|
|
angle: float = Quaternion.Angle(q1, q2)
|
|
return angle
|
|
|