| from sympy.core.basic import Basic |
| from sympy.core.sympify import sympify |
| from sympy.functions.elementary.trigonometric import (cos, sin) |
| from sympy.matrices.dense import (eye, rot_axis1, rot_axis2, rot_axis3) |
| from sympy.matrices.immutable import ImmutableDenseMatrix as Matrix |
| from sympy.core.cache import cacheit |
| from sympy.core.symbol import Str |
| import sympy.vector |
|
|
|
|
| class Orienter(Basic): |
| """ |
| Super-class for all orienter classes. |
| """ |
|
|
| def rotation_matrix(self): |
| """ |
| The rotation matrix corresponding to this orienter |
| instance. |
| """ |
| return self._parent_orient |
|
|
|
|
| class AxisOrienter(Orienter): |
| """ |
| Class to denote an axis orienter. |
| """ |
|
|
| def __new__(cls, angle, axis): |
| if not isinstance(axis, sympy.vector.Vector): |
| raise TypeError("axis should be a Vector") |
| angle = sympify(angle) |
|
|
| obj = super().__new__(cls, angle, axis) |
| obj._angle = angle |
| obj._axis = axis |
|
|
| return obj |
|
|
| def __init__(self, angle, axis): |
| """ |
| Axis rotation is a rotation about an arbitrary axis by |
| some angle. The angle is supplied as a SymPy expr scalar, and |
| the axis is supplied as a Vector. |
| |
| Parameters |
| ========== |
| |
| angle : Expr |
| The angle by which the new system is to be rotated |
| |
| axis : Vector |
| The axis around which the rotation has to be performed |
| |
| Examples |
| ======== |
| |
| >>> from sympy.vector import CoordSys3D |
| >>> from sympy import symbols |
| >>> q1 = symbols('q1') |
| >>> N = CoordSys3D('N') |
| >>> from sympy.vector import AxisOrienter |
| >>> orienter = AxisOrienter(q1, N.i + 2 * N.j) |
| >>> B = N.orient_new('B', (orienter, )) |
| |
| """ |
| |
| pass |
|
|
| @cacheit |
| def rotation_matrix(self, system): |
| """ |
| The rotation matrix corresponding to this orienter |
| instance. |
| |
| Parameters |
| ========== |
| |
| system : CoordSys3D |
| The coordinate system wrt which the rotation matrix |
| is to be computed |
| """ |
|
|
| axis = sympy.vector.express(self.axis, system).normalize() |
| axis = axis.to_matrix(system) |
| theta = self.angle |
| parent_orient = ((eye(3) - axis * axis.T) * cos(theta) + |
| Matrix([[0, -axis[2], axis[1]], |
| [axis[2], 0, -axis[0]], |
| [-axis[1], axis[0], 0]]) * sin(theta) + |
| axis * axis.T) |
| parent_orient = parent_orient.T |
| return parent_orient |
|
|
| @property |
| def angle(self): |
| return self._angle |
|
|
| @property |
| def axis(self): |
| return self._axis |
|
|
|
|
| class ThreeAngleOrienter(Orienter): |
| """ |
| Super-class for Body and Space orienters. |
| """ |
|
|
| def __new__(cls, angle1, angle2, angle3, rot_order): |
| if isinstance(rot_order, Str): |
| rot_order = rot_order.name |
|
|
| approved_orders = ('123', '231', '312', '132', '213', |
| '321', '121', '131', '212', '232', |
| '313', '323', '') |
| original_rot_order = rot_order |
| rot_order = str(rot_order).upper() |
| if not (len(rot_order) == 3): |
| raise TypeError('rot_order should be a str of length 3') |
| rot_order = [i.replace('X', '1') for i in rot_order] |
| rot_order = [i.replace('Y', '2') for i in rot_order] |
| rot_order = [i.replace('Z', '3') for i in rot_order] |
| rot_order = ''.join(rot_order) |
| if rot_order not in approved_orders: |
| raise TypeError('Invalid rot_type parameter') |
| a1 = int(rot_order[0]) |
| a2 = int(rot_order[1]) |
| a3 = int(rot_order[2]) |
| angle1 = sympify(angle1) |
| angle2 = sympify(angle2) |
| angle3 = sympify(angle3) |
| if cls._in_order: |
| parent_orient = (_rot(a1, angle1) * |
| _rot(a2, angle2) * |
| _rot(a3, angle3)) |
| else: |
| parent_orient = (_rot(a3, angle3) * |
| _rot(a2, angle2) * |
| _rot(a1, angle1)) |
| parent_orient = parent_orient.T |
|
|
| obj = super().__new__( |
| cls, angle1, angle2, angle3, Str(rot_order)) |
| obj._angle1 = angle1 |
| obj._angle2 = angle2 |
| obj._angle3 = angle3 |
| obj._rot_order = original_rot_order |
| obj._parent_orient = parent_orient |
|
|
| return obj |
|
|
| @property |
| def angle1(self): |
| return self._angle1 |
|
|
| @property |
| def angle2(self): |
| return self._angle2 |
|
|
| @property |
| def angle3(self): |
| return self._angle3 |
|
|
| @property |
| def rot_order(self): |
| return self._rot_order |
|
|
|
|
| class BodyOrienter(ThreeAngleOrienter): |
| """ |
| Class to denote a body-orienter. |
| """ |
|
|
| _in_order = True |
|
|
| def __new__(cls, angle1, angle2, angle3, rot_order): |
| obj = ThreeAngleOrienter.__new__(cls, angle1, angle2, angle3, |
| rot_order) |
| return obj |
|
|
| def __init__(self, angle1, angle2, angle3, rot_order): |
| """ |
| Body orientation takes this coordinate system through three |
| successive simple rotations. |
| |
| Body fixed rotations include both Euler Angles and |
| Tait-Bryan Angles, see https://en.wikipedia.org/wiki/Euler_angles. |
| |
| Parameters |
| ========== |
| |
| angle1, angle2, angle3 : Expr |
| Three successive angles to rotate the coordinate system by |
| |
| rotation_order : string |
| String defining the order of axes for rotation |
| |
| Examples |
| ======== |
| |
| >>> from sympy.vector import CoordSys3D, BodyOrienter |
| >>> from sympy import symbols |
| >>> q1, q2, q3 = symbols('q1 q2 q3') |
| >>> N = CoordSys3D('N') |
| |
| A 'Body' fixed rotation is described by three angles and |
| three body-fixed rotation axes. To orient a coordinate system D |
| with respect to N, each sequential rotation is always about |
| the orthogonal unit vectors fixed to D. For example, a '123' |
| rotation will specify rotations about N.i, then D.j, then |
| D.k. (Initially, D.i is same as N.i) |
| Therefore, |
| |
| >>> body_orienter = BodyOrienter(q1, q2, q3, '123') |
| >>> D = N.orient_new('D', (body_orienter, )) |
| |
| is same as |
| |
| >>> from sympy.vector import AxisOrienter |
| >>> axis_orienter1 = AxisOrienter(q1, N.i) |
| >>> D = N.orient_new('D', (axis_orienter1, )) |
| >>> axis_orienter2 = AxisOrienter(q2, D.j) |
| >>> D = D.orient_new('D', (axis_orienter2, )) |
| >>> axis_orienter3 = AxisOrienter(q3, D.k) |
| >>> D = D.orient_new('D', (axis_orienter3, )) |
| |
| Acceptable rotation orders are of length 3, expressed in XYZ or |
| 123, and cannot have a rotation about about an axis twice in a row. |
| |
| >>> body_orienter1 = BodyOrienter(q1, q2, q3, '123') |
| >>> body_orienter2 = BodyOrienter(q1, q2, 0, 'ZXZ') |
| >>> body_orienter3 = BodyOrienter(0, 0, 0, 'XYX') |
| |
| """ |
| |
| pass |
|
|
|
|
| class SpaceOrienter(ThreeAngleOrienter): |
| """ |
| Class to denote a space-orienter. |
| """ |
|
|
| _in_order = False |
|
|
| def __new__(cls, angle1, angle2, angle3, rot_order): |
| obj = ThreeAngleOrienter.__new__(cls, angle1, angle2, angle3, |
| rot_order) |
| return obj |
|
|
| def __init__(self, angle1, angle2, angle3, rot_order): |
| """ |
| Space rotation is similar to Body rotation, but the rotations |
| are applied in the opposite order. |
| |
| Parameters |
| ========== |
| |
| angle1, angle2, angle3 : Expr |
| Three successive angles to rotate the coordinate system by |
| |
| rotation_order : string |
| String defining the order of axes for rotation |
| |
| See Also |
| ======== |
| |
| BodyOrienter : Orienter to orient systems wrt Euler angles. |
| |
| Examples |
| ======== |
| |
| >>> from sympy.vector import CoordSys3D, SpaceOrienter |
| >>> from sympy import symbols |
| >>> q1, q2, q3 = symbols('q1 q2 q3') |
| >>> N = CoordSys3D('N') |
| |
| To orient a coordinate system D with respect to N, each |
| sequential rotation is always about N's orthogonal unit vectors. |
| For example, a '123' rotation will specify rotations about |
| N.i, then N.j, then N.k. |
| Therefore, |
| |
| >>> space_orienter = SpaceOrienter(q1, q2, q3, '312') |
| >>> D = N.orient_new('D', (space_orienter, )) |
| |
| is same as |
| |
| >>> from sympy.vector import AxisOrienter |
| >>> axis_orienter1 = AxisOrienter(q1, N.i) |
| >>> B = N.orient_new('B', (axis_orienter1, )) |
| >>> axis_orienter2 = AxisOrienter(q2, N.j) |
| >>> C = B.orient_new('C', (axis_orienter2, )) |
| >>> axis_orienter3 = AxisOrienter(q3, N.k) |
| >>> D = C.orient_new('C', (axis_orienter3, )) |
| |
| """ |
| |
| pass |
|
|
|
|
| class QuaternionOrienter(Orienter): |
| """ |
| Class to denote a quaternion-orienter. |
| """ |
|
|
| def __new__(cls, q0, q1, q2, q3): |
| q0 = sympify(q0) |
| q1 = sympify(q1) |
| q2 = sympify(q2) |
| q3 = sympify(q3) |
| parent_orient = (Matrix([[q0 ** 2 + q1 ** 2 - q2 ** 2 - |
| q3 ** 2, |
| 2 * (q1 * q2 - q0 * q3), |
| 2 * (q0 * q2 + q1 * q3)], |
| [2 * (q1 * q2 + q0 * q3), |
| q0 ** 2 - q1 ** 2 + |
| q2 ** 2 - q3 ** 2, |
| 2 * (q2 * q3 - q0 * q1)], |
| [2 * (q1 * q3 - q0 * q2), |
| 2 * (q0 * q1 + q2 * q3), |
| q0 ** 2 - q1 ** 2 - |
| q2 ** 2 + q3 ** 2]])) |
| parent_orient = parent_orient.T |
|
|
| obj = super().__new__(cls, q0, q1, q2, q3) |
| obj._q0 = q0 |
| obj._q1 = q1 |
| obj._q2 = q2 |
| obj._q3 = q3 |
| obj._parent_orient = parent_orient |
|
|
| return obj |
|
|
| def __init__(self, angle1, angle2, angle3, rot_order): |
| """ |
| Quaternion orientation orients the new CoordSys3D with |
| Quaternions, defined as a finite rotation about lambda, a unit |
| vector, by some amount theta. |
| |
| This orientation is described by four parameters: |
| |
| q0 = cos(theta/2) |
| |
| q1 = lambda_x sin(theta/2) |
| |
| q2 = lambda_y sin(theta/2) |
| |
| q3 = lambda_z sin(theta/2) |
| |
| Quaternion does not take in a rotation order. |
| |
| Parameters |
| ========== |
| |
| q0, q1, q2, q3 : Expr |
| The quaternions to rotate the coordinate system by |
| |
| Examples |
| ======== |
| |
| >>> from sympy.vector import CoordSys3D |
| >>> from sympy import symbols |
| >>> q0, q1, q2, q3 = symbols('q0 q1 q2 q3') |
| >>> N = CoordSys3D('N') |
| >>> from sympy.vector import QuaternionOrienter |
| >>> q_orienter = QuaternionOrienter(q0, q1, q2, q3) |
| >>> B = N.orient_new('B', (q_orienter, )) |
| |
| """ |
| |
| pass |
|
|
| @property |
| def q0(self): |
| return self._q0 |
|
|
| @property |
| def q1(self): |
| return self._q1 |
|
|
| @property |
| def q2(self): |
| return self._q2 |
|
|
| @property |
| def q3(self): |
| return self._q3 |
|
|
|
|
| def _rot(axis, angle): |
| """DCM for simple axis 1, 2 or 3 rotations. """ |
| if axis == 1: |
| return Matrix(rot_axis1(angle).T) |
| elif axis == 2: |
| return Matrix(rot_axis2(angle).T) |
| elif axis == 3: |
| return Matrix(rot_axis3(angle).T) |
|
|
