diff --git a/doc/changelog.d/6151.miscellaneous.md b/doc/changelog.d/6151.miscellaneous.md new file mode 100644 index 00000000000..9f74db2f29e --- /dev/null +++ b/doc/changelog.d/6151.miscellaneous.md @@ -0,0 +1 @@ +Refactored quaternion implementation \ No newline at end of file diff --git a/doc/source/API/generic.rst b/doc/source/API/generic.rst index a8496cc0378..1a7a7d6ea87 100644 --- a/doc/source/API/generic.rst +++ b/doc/source/API/generic.rst @@ -41,3 +41,35 @@ The following methods allows to read and parse files. read_configuration_file write_configuration_file compute_fft + + +Quaternion +~~~~~~~~~~ + +PyAEDT contains an implementation of fundamental quaternion operations. + +Quaternions are only used to represent rotations in 3D space. They are not used to represent translations or other transformations. +Only methods related to rotations are implemented. + +.. currentmodule:: ansys.aedt.core.generic.quaternion + +.. autosummary:: + :toctree: _autosummary + :nosignatures: + + Quaternion + + +Math utils +~~~~~~~~~~ + +MathUtils is a class that provides mathematical utility methods like numerical comparisons and checks. + +.. currentmodule:: ansys.aedt.core.generic.math_utils + +.. autosummary:: + :toctree: _autosummary + :nosignatures: + + MathUtils + diff --git a/src/ansys/aedt/core/generic/math_utils.py b/src/ansys/aedt/core/generic/math_utils.py new file mode 100644 index 00000000000..c301bbd6091 --- /dev/null +++ b/src/ansys/aedt/core/generic/math_utils.py @@ -0,0 +1,162 @@ +# -*- coding: utf-8 -*- +# +# Copyright (C) 2021 - 2025 ANSYS, Inc. and/or its affiliates. +# SPDX-License-Identifier: MIT +# +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. + +import math +from sys import float_info + +from ansys.aedt.core.generic.general_methods import pyaedt_function_handler + + +class MathUtils: + """MathUtils is a utility class that provides methods for numerical comparisons and checks.""" + + EPSILON = float_info.epsilon * 10.0 + + @staticmethod + @pyaedt_function_handler() + def is_zero(x: float, eps: float = EPSILON) -> bool: + """Check if a number is close to zero within a small epsilon tolerance. + + Parameters: + x: float + Number to check. + eps : float + Tolerance for the comparison. Default is ``EPSILON``. + + Returns: + bool + ``True`` if the number is numerically zero, ``False`` otherwise. + + """ + return abs(x) < eps + + @staticmethod + @pyaedt_function_handler() + def is_close(a: float, b: float, relative_tolerance: float = 1e-9, absolute_tolerance: float = 0.0) -> bool: + """Whether two numbers are close to each other given relative and absolute tolerances. + + Parameters + ---------- + a : float, int + First number to compare. + b : float, int + Second number to compare. + relative_tolerance : float + Relative tolerance. The default value is ``1e-9``. + absolute_tolerance : float + Absolute tolerance. The default value is ``0.0``. + + Returns + ------- + bool + ``True`` if the two numbers are closed, ``False`` otherwise. + """ + return abs(a - b) <= max(relative_tolerance * max(abs(a), abs(b)), absolute_tolerance) + + @staticmethod + @pyaedt_function_handler() + def is_equal(a: float, b: float, eps: float = EPSILON) -> bool: + """ + Return True if numbers a and b are equal within a small epsilon tolerance. + + Parameters: + a: float + First number. + b: float + Second number. + eps : float + Tolerance for the comparison. Default is ``EPSILON``. + + Returns: + bool + ``True`` if the absolute difference between a and b is less than epsilon, ``False`` otherwise. + """ + return abs(a - b) < eps + + @staticmethod + @pyaedt_function_handler() + def atan2(y: float, x: float) -> float: + """Implementation of atan2 that does not suffer from the following issues: + math.atan2(0.0, 0.0) = 0.0 + math.atan2(-0.0, 0.0) = -0.0 + math.atan2(0.0, -0.0) = 3.141592653589793 + math.atan2(-0.0, -0.0) = -3.141592653589793 + and returns always 0.0. + + Parameters + ---------- + y : float + Y-axis value for atan2. + + x : float + X-axis value for atan2. + + Returns + ------- + float + + """ + if abs(y) < MathUtils.EPSILON: + y = 0.0 + if abs(x) < MathUtils.EPSILON: + x = 0.0 + return math.atan2(y, x) + + @staticmethod + @pyaedt_function_handler() + def is_scalar_number(x): + """Check if a value is a scalar number (int or float). + + Parameters + ---------- + x : object + Value to check. + + Returns + ------- + bool + ``True`` if x is a scalar number, ``False`` otherwise. + """ + return isinstance(x, (int, float)) + + @staticmethod + @pyaedt_function_handler() + def fix_negative_zero(value): + """Fix the negative zero. + It supports lists (and nested lists). + + Parameters + ---------- + value : float, List + Value to be fixed. + + Returns + ------- + float, List + Fixed value. + + """ + if isinstance(value, list): + return [MathUtils.fix_negative_zero(item) for item in value] + return 0.0 if value == 0.0 and math.copysign(1.0, value) == -1.0 else value diff --git a/src/ansys/aedt/core/generic/quaternion.py b/src/ansys/aedt/core/generic/quaternion.py new file mode 100644 index 00000000000..845941014d8 --- /dev/null +++ b/src/ansys/aedt/core/generic/quaternion.py @@ -0,0 +1,932 @@ +# -*- coding: utf-8 -*- +# +# Copyright (C) 2021 - 2025 ANSYS, Inc. and/or its affiliates. +# SPDX-License-Identifier: MIT +# +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. + +import math + +from ansys.aedt.core.generic.general_methods import pyaedt_function_handler +from ansys.aedt.core.generic.math_utils import MathUtils +from ansys.aedt.core.modeler.geometry_operators import GeometryOperators + + +class Quaternion: + """ + Implements fundamental quaternion operations. + + Quaternions are created using ``Quaternion(a, b, c, d)``. + + Quaternions are only used to represent rotations in 3D space. + They are not used to represent translations or other transformations. + Only methods related to rotations are implemented. + + The quaternion is defined as: + .. math:: + q = a + bi + cj + dk + where ``a`` is the scalar part and ``b``, ``c``, and ``d`` are the vector parts. + + This updated class offers enhanced functionality compared to the previous implementation, + supporting both intrinsic and extrinsic rotations. Note that AEDT coordinate systems use intrinsic rotation. + + + References + ---------- + + [1] https://en.wikipedia.org/wiki/Quaternion + [2] https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles + [3] https://www.euclideanspace.com/maths/geometry/rotations/conversions/ + + """ + + def __init__(self, a=0, b=0, c=0, d=0): + """Initialize the quaternion. + Quaternions are created using ``Quaternion(a, b, c, d)``, representing the form q = a + bi + cj + dk. + + Parameters + ---------- + + a, b, c, d : float + The quaternion coefficients. + """ + try: + a, b, c, d = float(a), float(b), float(c), float(d) + except ValueError: + raise TypeError("Quaternion coefficients must be convertible to float.") + self._args = (a, b, c, d) + # normalize each element in self._args so that if it's "close enough to zero", it's set explicitly to 0.0. + self._args = tuple(0.0 if MathUtils.is_zero(x) else x for x in self._args) + + @property + def a(self): + return self._args[0] + + @property + def b(self): + return self._args[1] + + @property + def c(self): + return self._args[2] + + @property + def d(self): + return self._args[3] + + @classmethod + @pyaedt_function_handler() + def _to_quaternion(cls, obj): + if isinstance(obj, cls): + return obj + elif isinstance(obj, (tuple, list)) and len(obj) == 4: + return cls(*obj) + else: + raise TypeError(f"Cannot convert {obj} to {cls.__name__}.") + + @staticmethod + @pyaedt_function_handler() + def _is_valid_rotation_sequence(sequence): + """ + Validates that the input string is a valid 3-character rotation sequence + using only the axes 'x', 'y', or 'z', case-insensitively. + + Parameters + ---------- + sequence : str + The rotation sequence string to validate. + + Returns + ------- + bool + ``True`` if valid, ``False`` otherwise. + """ + if not isinstance(sequence, str): + return False + if len(sequence) != 3: + return False + return all(char.lower() in {"x", "y", "z"} for char in sequence) + + @classmethod + @pyaedt_function_handler() + def from_euler(cls, angles, sequence, extrinsic=False): + """Creates a normalized rotation quaternion from the Euler angles using the specified rotation sequence. + + Parameters + ---------- + + angles : list, tuple of 3 floats + The Euler angles in radians. + + sequence : str + A three-character string indicating the rotation axis sequence (e.g., "xyz" or "ZYX"). + It is case-insensitive and must contain only the characters 'x', 'y', or 'z'. + + extrinsic : bool, optional + If ``True``, the rotation is treated as extrinsic. + If ``False`` (default), it is treated as intrinsic. + + Returns + ------- + + Quaternion + A unit quaternion representing the rotation defined by the Euler angles in the given sequence. + + Examples + -------- + + >>> from ansys.aedt.core.generic.quaternion import Quaternion + >>> from math import pi + >>> q = Quaternion.from_euler([pi/2, 0, 0], 'xyz') + >>> q + Quaternion(0.7071067811865476, 0.7071067811865476, 0, 0) + + >>> q = Quaternion.from_euler([0, pi/2, pi], 'zyz', extrinsic=True) + >>> q + Quaternion(0, -0.7071067811865476, 0, 0.7071067811865476) + + >>> q = Quaternion.from_euler([0, pi/2, pi], 'zyz') + >>> q + Quaternion(0, 0.7071067811865476, 0, 0.7071067811865476) + """ + + if len(angles) != 3: + raise ValueError("Three rotation angles are required.") + + if not Quaternion._is_valid_rotation_sequence(sequence): + raise ValueError("sequence must be a 3-character string, using only the axes 'x', 'y', or 'z'.") + + i, j, k = sequence.lower() + + # converting the sequence into indexes + ei = [1 if n == i else 0 for n in "xyz"] + ej = [1 if n == j else 0 for n in "xyz"] + ek = [1 if n == k else 0 for n in "xyz"] + + # evaluate the quaternions + qi = cls.from_axis_angle(ei, angles[0]) + qj = cls.from_axis_angle(ej, angles[1]) + qk = cls.from_axis_angle(ek, angles[2]) + + if extrinsic: + return qk * qj * qi + else: + return qi * qj * qk + + @pyaedt_function_handler() + def to_euler(self, sequence, extrinsic=False): + """ + Converts the quaternion to Euler angles using the specified rotation sequence. + + The conversion follows the method described in [1]. In degenerate (gimbal lock) cases, + the third angle is set to zero for stability. + + Parameters + ---------- + sequence : str + A three-character string indicating the rotation axis sequence (e.g., "xyz" or "ZYX"). + It is case-insensitive and must contain only the characters 'x', 'y', or 'z'. + + extrinsic : bool, optional + If ``True``, the rotation is treated as extrinsic. + If ``False`` (default), it is treated as intrinsic. + + Note + ---- + Tait–Bryan angles (Heading, Pitch, Bank) correspond to an intrinsic "ZYX" sequence. + + Returns + ------- + tuple of float + A tuple of three Euler angles representing the same rotation as the quaternion. + Angle in radians. + + Examples + -------- + + >>> from ansys.aedt.core.generic.quaternion import Quaternion + >>> q = Quaternion(0.9069661433330367, -0.17345092325178477, -0.3823030778615049, -0.03422789400943274) + >>> q.to_euler("zxz") + (-2.0344439357957027, 0.8664730673456006, 1.9590019609437583) + >>> q.to_euler("zyz") + (2.677945044588987, 0.8664730673456006, -2.7533870194409316) + + References + ---------- + + [1] https://doi.org/10.1371/journal.pone.0276302 + [2] https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles + """ + # fmt: off + if not Quaternion._is_valid_rotation_sequence(sequence): + raise ValueError("sequence must be a 3-character string, using only the axes 'x', 'y', or 'z'.") + + if MathUtils.is_zero(self.norm()): + raise ValueError('A quaternion with norm 0 cannot be converted.') + + angles = [0, 0, 0] + + i, j, k = sequence.lower() + + # converting the sequence into indexes + i = 'xyz'.index(i) + 1 + j = 'xyz'.index(j) + 1 + k = 'xyz'.index(k) + 1 + + if not extrinsic: + i, k = k, i + + # check if the rotation sequence is symmetric + symmetric = i == k + if symmetric: + k = 6 - i - j + + # determine the parity of the permutation + sign = (i - j) * (j - k) * (k - i) // 2 + + # permutate elements + elements = [self.a, self.b, self.c, self.d] + a = elements[0] + b = elements[i] + c = elements[j] + d = elements[k] * sign + + if not symmetric: + a, b, c, d = a-c, b+d, c+a, d-b + + angles[1] = 2 * MathUtils.atan2(math.sqrt(c*c + d*d), math.sqrt(a*a + b*b)) + if not symmetric: + angles[1] -= math.pi / 2 + + # Check for singularities in some numerical cases + case = 0 + if MathUtils.is_zero(c) and MathUtils.is_zero(d): + case = 1 + if MathUtils.is_zero(a) and MathUtils.is_zero(b): + case = 2 + + if case == 0: + angles[0] = MathUtils.atan2(b, a) + MathUtils.atan2(d, c) + angles[2] = MathUtils.atan2(b, a) - MathUtils.atan2(d, c) + else: + # here we consider any degenerate case + if extrinsic: + angles[0] = 0.0 + else: + angles[2] = 0.0 + + if case == 1: + if extrinsic: + angles[2] = 2 * MathUtils.atan2(b, a) + else: + angles[0] = 2 * MathUtils.atan2(b, a) + else: + if extrinsic: + angles[2] = -2 * MathUtils.atan2(d, c) + else: + angles[0] = 2 * MathUtils.atan2(d, c) + + # for Tait-Bryan angles + if not symmetric: + angles[0] *= sign + + if extrinsic: + return tuple(angles[::-1]) + else: + return tuple(angles) + # fmt: on + + @classmethod + @pyaedt_function_handler() + def from_axis_angle(cls, axis, angle): + """Creates a normalized rotation quaternion from a given axis and rotation angle. + + + Parameters + ---------- + + axis : List or tuple of float + A 3D vector representing the axis of rotation. + angle : float + The rotation angle in radians. + + Returns + ------- + + Quaternion + A unit quaternion representing the rotation around the specified axis by the given angle. + + Examples + -------- + + >>> from ansys.aedt.core.generic.quaternion import Quaternion + >>> from math import pi, sqrt + >>> Quaternion.from_axis_angle((sqrt(3)/3, sqrt(3)/3, sqrt(3)/3), 2*pi/3) + Quaternion(0.5, 0.5, 0.5, 0.5) + """ + if len(axis) != 3: + raise ValueError("axis must be a list or tuple containing 3 floats.") + if MathUtils.is_zero(GeometryOperators.v_norm(axis)): + raise ValueError("axis must be a non-zero vector.") + + (x, y, z) = GeometryOperators.normalize_vector(axis) + s = math.sin(angle * 0.5) + a = math.cos(angle * 0.5) + b = x * s + c = y * s + d = z * s + return cls(a, b, c, d) + + @pyaedt_function_handler() + def to_axis_angle(self): + """Convert a quaternion to the axis angle rotation formulation. + + Returns + ------- + tuple + ((ux, uy, uz), theta) containing the rotation axes expressed as X, Y, Z components of + the unit vector ``u`` and the rotation angle theta expressed in radians. + + Examples + -------- + + >>> from ansys.aedt.core.generic.quaternion import Quaternion + >>> q = Quaternion(1, 1, 1, 1) + >>> (axis, angle) = q.to_axis_angle() + >>> axis + (0.5773502691896257, 0.5773502691896257, 0.5773502691896257) + >>> angle + 2.0943951023931953 + + """ + # fmt: off + q = self + if MathUtils.is_zero(q.norm()): + raise ValueError('A quaternion with norm 0 cannot be converted.') + + if q.a < 0: + q = q * -1 + + q = q.normalize() + theta = 2 * math.acos(q.a) + n = math.sqrt(1 - q.a*q.a) # q.a < 1 in a normalized quaternion + if MathUtils.is_zero(n): + # Handle the case where the quaternion is a multiple of (1, 0, 0, 0) + # which corresponds to no rotation + return (1.0, 0.0, 0.0), 0.0 + x = q.b / n + y = q.c / n + z = q.d / n + u = (x, y, z) + return u, theta + # fmt: on + + @classmethod + @pyaedt_function_handler() + def from_rotation_matrix(cls, rotation_matrix): + """Converts a 3x3 rotation matrix to a quaternion. + It uses the method described in [1]. + + Parameters + ---------- + + rotation_matrix: List or tuple + Rotation matrix defined as a list of lists or a tuple of tuples. + The matrix should be 3x3 and orthogonal. + The matrix is assumed to be in the form: + ((m00, m01, m02), (m10, m11, m12), (m20, m21, m22)) + + Returns + ------- + + Quaternion + The quaternion defined by the rotation matrix. + + References + ---------- + [1] https://doi.org/10.1145/325334.325242 (pp. 245-254) + + Examples + -------- + >>> from ansys.aedt.core.generic.quaternion import Quaternion + >>> x = (0.7053456158585982, 0.07053456158585963, 0.7053456158585982) + >>> y = (0.19470872568244832, 0.937486456989565, -0.2884573713814046) + >>> z = (-0.681598176590997, 0.34079908829549865, 0.6475182677614472) + >>> rotation_matrix = Quaternion.axis_to_rotation_matrix(x, y, z) + >>> q = Quaternion.from_rotation_matrix(rotation_matrix) + >>> q + Quaternion(0.9069661433330367, -0.17345092325178477, -0.3823030778615049, -0.03422789400943274) + + """ + # fmt: off + wrong_type = False + if not isinstance(rotation_matrix, (list, tuple)) or len(rotation_matrix) != 3: + wrong_type = True + for row in rotation_matrix: + if not isinstance(row, (list, tuple)) or len(row) != 3: + wrong_type = True + if wrong_type: + raise ValueError("rotation_matrix must be a 3x3 matrix defined as a list of lists or a tuple of tuples.") + + if not GeometryOperators.is_orthogonal_matrix(rotation_matrix): + raise ValueError("The rotation matrix must be orthogonal.") + + m00, m01, m02 = rotation_matrix[0] + m10, m11, m12 = rotation_matrix[1] + m20, m21, m22 = rotation_matrix[2] + + trace = m00 + m11 + m22 + + if trace > 0: + S = math.sqrt(trace + 1.0) * 2 # S=4*w + w = 0.25 * S + x = (m21 - m12) / S + y = (m02 - m20) / S + z = (m10 - m01) / S + elif (m00 > m11) and (m00 > m22): + S = math.sqrt(1.0 + m00 - m11 - m22) * 2 # S=4*x + w = (m21 - m12) / S + x = 0.25 * S + y = (m01 + m10) / S + z = (m02 + m20) / S + elif m11 > m22: + S = math.sqrt(1.0 + m11 - m00 - m22) * 2 # S=4*y + w = (m02 - m20) / S + x = (m01 + m10) / S + y = 0.25 * S + z = (m12 + m21) / S + else: + S = math.sqrt(1.0 + m22 - m00 - m11) * 2 # S=4*z + w = (m10 - m01) / S + x = (m02 + m20) / S + y = (m12 + m21) / S + z = 0.25 * S + + return cls(w, x, y, z) + # fmt: on + + @pyaedt_function_handler() + def to_rotation_matrix(self): + """Returns the rotation matrix corresponding to the quaternion. + + + Returns + ------- + + tuple + A 3×3 rotation matrix equivalent to the quaternion's rotation. + The matrix is provided in the form: + ((m00, m01, m02), (m10, m11, m12), (m20, m21, m22)) + + Examples + -------- + >>> from ansys.aedt.core.generic.quaternion import Quaternion + >>> q = Quaternion(0.9069661433330367, -0.17345092325178477, -0.3823030778615049, -0.03422789400943274) + >>> rotation_matrix = q.to_rotation_matrix() + >>> rotation_matrix + ((0.7053456158585982, 0.07053456158585963, 0.7053456158585982), + (0.19470872568244832, 0.937486456989565, -0.2884573713814046), + (-0.681598176590997, 0.34079908829549865, 0.6475182677614472)) + + """ + # fmt: off + q = self + if MathUtils.is_zero(q.norm()): + raise ValueError('A quaternion with norm 0 cannot be converted.') + + s = q.norm() ** -2 + + m00 = 1 - 2*s*(q.c**2 + q.d**2) + m01 = 2*s*(q.b*q.c - q.d*q.a) + m02 = 2*s*(q.b*q.d + q.c*q.a) + + m10 = 2*s*(q.b*q.c + q.d*q.a) + m11 = 1 - 2*s*(q.b**2 + q.d**2) + m12 = 2*s*(q.c*q.d - q.b*q.a) + + m20 = 2*s*(q.b*q.d - q.c*q.a) + m21 = 2*s*(q.c*q.d + q.b*q.a) + m22 = 1 - 2*s*(q.b**2 + q.c**2) + + return (m00, m01, m02), (m10, m11, m12), (m20, m21, m22) + # fmt: on + + @staticmethod + @pyaedt_function_handler() + def rotation_matrix_to_axis(rotation_matrix): + """Convert a rotation matrix to the corresponding axis of rotation. + Parameters + ---------- + rotation_matrix : tuple of tuples or list of lists + A 3x3 rotation matrix defined as a tuple of tuples or a list of lists. + The matrix should be orthogonal. + Returns + ------- + tuple + The X, Y, and Z axes of the rotated frame. + Examples + -------- + >>> from ansys.aedt.core.generic.quaternion import Quaternion + >>> rotation_matrix = ((0.7071067811865476, 0.0, 0.7071067811865476), + ... (0.0, 1.0, 0.0), + ... (-0.7071067811865476, 0.0, 0.7071067811865476)) + >>> x, y, z = Quaternion.rotation_matrix_to_axis(rotation_matrix) + >>> x + (0.7071067811865476, 0.0, -0.7071067811865476) + >>> y + (0.0, 1.0, 0.0) + >>> z + (-0.7071067811865476, 0.0, 0.7071067811865476) + """ + + if not GeometryOperators.is_orthogonal_matrix(rotation_matrix): + raise ValueError("The rotation matrix must be orthogonal.") + + m00, m01, m02 = rotation_matrix[0] + m10, m11, m12 = rotation_matrix[1] + m20, m21, m22 = rotation_matrix[2] + + x = tuple(GeometryOperators.normalize_vector((m00, m10, m20))) + y = tuple(GeometryOperators.normalize_vector((m01, m11, m21))) + z = tuple(GeometryOperators.normalize_vector((m02, m12, m22))) + + return x, y, z + + @staticmethod + @pyaedt_function_handler() + def axis_to_rotation_matrix(x_axis, y_axis, z_axis): + """Construct a rotation matrix from three orthonormal axes. + + Parameters + ---------- + x_axis : tuple of float + The X axis of the rotated frame. + y_axis : tuple of float + The Y axis of the rotated frame. + z_axis : tuple of float + The Z axis of the rotated frame. + + Returns + ------- + tuple of tuples + A 3x3 rotation matrix where each column is one of the given axes. + + Raises + ------ + ValueError + If the axes do not form an orthonormal basis. + """ + if not GeometryOperators.is_orthonormal_triplet(x_axis, y_axis, z_axis): + raise ValueError("The provided axes must form an orthonormal basis.") + + return ( + (x_axis[0], y_axis[0], z_axis[0]), + (x_axis[1], y_axis[1], z_axis[1]), + (x_axis[2], y_axis[2], z_axis[2]), + ) + + @pyaedt_function_handler() + def rotate_vector(self, v): + """Evaluate the rotation of a vector, defined by a quaternion. + + Evaluated as: + ``"q = q0 + q' = q0 + q1i + q2j + q3k"``, + ``"w = qvq* = (q0^2 - |q'|^2)v + 2(q' • v)q' + 2q0(q' x v)"``. + + Parameters + ---------- + v : tuple or List + ``(x, y, z)`` coordinates for the vector to be rotated. + + Returns + ------- + tuple + ``(w1, w2, w3)`` coordinates for the rotated vector ``w``. + + Examples + -------- + + >>> from ansys.aedt.core.generic.quaternion import Quaternion + >>> q = Quaternion(0.9238795325112867, 0.0, -0.3826834323650898, 0.0) + >>> v = (1, 0, 0) + >>> q.rotate_vector(v) + (0.7071067811865475, 0.0, 0.7071067811865476) + + """ + q = self + q = q.normalize() + q2 = q * Quaternion(0, v[0], v[1], v[2]) * q.conjugate() + return q2.b, q2.c, q2.d + + @pyaedt_function_handler() + def inverse_rotate_vector(self, v): + """Evaluate the inverse rotation of a vector that is defined by a quaternion. + It can also be the rotation of the coordinate frame with respect to the vector. + + q = q0 + q' = q0 + iq1 + jq2 + kq3 + q* = q0 - q' = q0 - iq1 - jq2 - kq3 + w = q*vq + + Parameters + ---------- + v : tuple or List + ``(x, y, z)`` coordinates for the vector to be rotated. + + Returns + ------- + tuple + ``(w1, w2, w3)`` coordinates for the rotated vector ``w``. + + Examples + -------- + + >>> from ansys.aedt.core.generic.quaternion import Quaternion + >>> q = Quaternion(0.9238795325112867, 0.0, -0.3826834323650898, 0.0) + >>> v = (1, 0, 0) + >>> q.rotate_vector(v) + (0.7071067811865475, 0.0, 0.7071067811865476) + + """ + q = self + q = q.normalize() + q2 = q.conjugate() * Quaternion(0, v[0], v[1], v[2]) * q + return q2.b, q2.c, q2.d + + def __add__(self, other): + return self.add(other) + + def __radd__(self, other): + return self.add(other) + + def __sub__(self, other): + return self + (-other) + + def __mul__(self, other): + return self._q_prod(self, other) + + def __rmul__(self, other): + return self._q_prod(other, self) + + def __neg__(self): + return Quaternion(-self.a, -self.b, -self.c, -self.d) + + def __truediv__(self, other): + return self._q_div(self, other) + + def __rtruediv__(self, other): + return self._q_div(other, self) + + def __eq__(self, other): + # fmt: off + if not isinstance(other, Quaternion): + return False + return ( + MathUtils.is_equal(self.a, other.a) and + MathUtils.is_equal(self.b, other.b) and + MathUtils.is_equal(self.c, other.c) and + MathUtils.is_equal(self.d, other.d) + ) + # fmt: on + + def __repr__(self): + return f"{type(self).__name__}({self.a}, {self.b}, {self.c}, {self.d})" + + @pyaedt_function_handler() + def add(self, other): + """Adds another quaternion or compatible value to this quaternion. + + Parameters + ---------- + other : Quaternion, List, tuple, float, or int + The value to be added. + It can be another Quaternion or a sequence that can be interpreted as one. + It can also be a scalar value (float or int). + + Returns + ------- + Quaternion + A new quaternion representing the sum of this quaternion and the provided value. + + Examples + -------- + + >>> from ansys.aedt.core.generic.quaternion import Quaternion + >>> q1 = Quaternion(1, 2, 3, 4) + >>> q2 = Quaternion(5, 6, 7, 8) + >>> q1.add(q2) + Quaternion(6, 8, 10, 12) + >>> q1 + 7 + Quaternion(8, 2, 3, 4) + + """ + q1 = self + + # Check what to do with other + if MathUtils.is_scalar_number(other): + return Quaternion(q1.a + other, q1.b, q1.c, q1.d) + + q2 = self._to_quaternion(other) + return Quaternion(q1.a + q2.a, q1.b + q2.b, q1.c + q2.c, q1.d + q2.d) + + @pyaedt_function_handler() + def mul(self, other): + """Performs quaternion multiplication with another quaternion or compatible value. + + Parameters + ---------- + other : Quaternion, List, tuple, float, or int + The value to multiply with this quaternion. + It can be another Quaternion or a sequence that can be interpreted as one. + It can also be a scalar value (float or int). + + Returns + ------- + Quaternion + A new quaternion representing the product of this quaternion and the given value. + + Examples + -------- + + >>> from ansys.aedt.core.generic.quaternion import Quaternion + >>> q1 = Quaternion(1, 2, 3, 4) + >>> q2 = Quaternion(5, 6, 7, 8) + >>> q1.mul(q2) + Quaternion(-60, 12, 30, 24) + >>> q1.mul(2) + Quaternion(2, 4, 6, 8) + """ + return self._q_prod(self, other) + + @staticmethod + @pyaedt_function_handler() + def _q_prod(q1, q2): + """Performs quaternion multiplication with another quaternion or compatible value. + This internal method has the purpose to deal with cases where one of the two factors is a scalar""" + # fmt: off + # Check what is q1 and q2 + if MathUtils.is_scalar_number(q1) and MathUtils.is_scalar_number(q2): + return q1*q2 + elif MathUtils.is_scalar_number(q1): + nn = q1 + qq = Quaternion._to_quaternion(q2) + return Quaternion(qq.a*nn, qq.b*nn, qq.c*nn, qq.d*nn) + elif MathUtils.is_scalar_number(q2): + nn = q2 + qq = Quaternion._to_quaternion(q1) + return Quaternion(qq.a*nn, qq.b*nn, qq.c*nn, qq.d*nn) + else: + return Quaternion.hamilton_prod(Quaternion._to_quaternion(q1), Quaternion._to_quaternion(q2)) + # fmt: on + + @staticmethod + @pyaedt_function_handler() + def hamilton_prod(q1, q2): + """Evaluate the Hamilton product of two quaternions, ``q1`` and ``q2``, defined as: + q1 = p0 + p' = p0 + ip1 + jp2 + kp3 + q2 = q0 + q' = q0 + iq1 + jq2 + kq3 + m = q1*q2 = p0q0 - p' • q' + p0q' + q0p' + p' x q' + + Parameters + ---------- + + q1 : Quaternion, List, tuple + The value to multiply with this quaternion. + It can be another Quaternion or a sequence that can be interpreted as one. + q2 : Quaternion, List, tuple + The value to multiply with this quaternion. + It can be another Quaternion or a sequence that can be interpreted as one. + + q1 and q2 must be quaternions or compatible values. Multiplicaiton between quaternions and scalar values is + handled by the method ``mul`` + + Returns + ------- + + Quaternion + The quaternion result of the multiplication between q1 and q2 + + Examples + -------- + + >>> from ansys.aedt.core.generic.quaternion import Quaternion + >>> q1 = Quaternion(1, 2, 3, 4) + >>> q2 = Quaternion(5, 6, 7, 8) + >>> Quaternion.hamilton_prod(q1,q2) + Quaternion(-60, 12, 30, 24) + """ + # fmt: off + q1 = Quaternion._to_quaternion(q1) + q2 = Quaternion._to_quaternion(q2) + + return Quaternion(q1.a*q2.a - q1.b*q2.b - q1.c*q2.c - q1.d*q2.d, + q1.a*q2.b + q1.b*q2.a + q1.c*q2.d - q1.d*q2.c, + q1.a*q2.c - q1.b*q2.d + q1.c*q2.a + q1.d*q2.b, + q1.a*q2.d + q1.b*q2.c - q1.c*q2.b + q1.d*q2.a, + ) + # fmt: on + + @pyaedt_function_handler() + def conjugate(self): + """Returns the conjugate of the quaternion.""" + q = self + return Quaternion(q.a, -q.b, -q.c, -q.d) + + @pyaedt_function_handler() + def norm(self): + """Returns the norm of the quaternion.""" + # fmt: off + q = self + return math.sqrt(q.a**2 + q.b**2 + q.c**2 + q.d**2) + # fmt: on + + @pyaedt_function_handler() + def normalize(self): + """Returns the normalized form of the quaternion.""" + # fmt: off + q = self + if MathUtils.is_zero(q.norm()): + raise ValueError("Cannot normalize a quaternion with zero norm.") + return q * (1/q.norm()) + # fmt: on + + @pyaedt_function_handler() + def inverse(self): + """Returns the inverse of the quaternion.""" + # fmt: off + q = self + if MathUtils.is_zero(q.norm()): + raise ValueError("Cannot compute inverse for a quaternion with zero norm.") + return (1/q.norm()**2) * q.conjugate() + # fmt: on + + @pyaedt_function_handler() + def div(self, other): + """Performs quaternion division with another quaternion or compatible value. + + Parameters + ---------- + other : Quaternion, List, tuple, float, or int + The value to divide with this quaternion. + It can be another Quaternion or a sequence that can be interpreted as one. + It can also be a scalar value (float or int). + + Returns + ------- + Quaternion + A new quaternion representing the division of this quaternion and the given value. + + Examples + -------- + + >>> from ansys.aedt.core.generic.quaternion import Quaternion + >>> q1 = Quaternion(1, 2, 3, 4) + >>> q2 = Quaternion(1, -1, 1, 2) + >>> q1.div(q2) + Quaternion(10/7, 1/7, 10/7, -3/7) + >>> q1.div(2) + Quaternion(0.5, 1, 1.5, 2) + """ + return self._q_div(self, other) + + @staticmethod + @pyaedt_function_handler() + def _q_div(q1, q2): + """Performs quaternion division with another quaternion or compatible value. + This internal method has the purpose to deal with cases where one of the two factors is a scalar""" + # fmt: off + # Check what is q1 and q2 + if MathUtils.is_scalar_number(q1) and MathUtils.is_scalar_number(q2): + return q1*q2**-1 + elif MathUtils.is_scalar_number(q1): + nn = q1 + qq = Quaternion._to_quaternion(q2).inverse() + return Quaternion(qq.a*nn, qq.b*nn, qq.c*nn, qq.d*nn) + elif MathUtils.is_scalar_number(q2): + nn = q2**-1 + qq = Quaternion._to_quaternion(q1) + return Quaternion(qq.a*nn, qq.b*nn, qq.c*nn, qq.d*nn) + else: + return Quaternion.hamilton_prod(Quaternion._to_quaternion(q1), Quaternion._to_quaternion(q2).inverse()) + # fmt: on + + @pyaedt_function_handler() + def coefficients(self): + """Returns the coefficients of the quaternion as a tuple.""" + return self.a, self.b, self.c, self.d diff --git a/src/ansys/aedt/core/modeler/cad/modeler.py b/src/ansys/aedt/core/modeler/cad/modeler.py index 138ee247a69..84ddcc26c70 100644 --- a/src/ansys/aedt/core/modeler/cad/modeler.py +++ b/src/ansys/aedt/core/modeler/cad/modeler.py @@ -36,6 +36,7 @@ from ansys.aedt.core.generic.general_methods import pyaedt_function_handler from ansys.aedt.core.generic.general_methods import settings from ansys.aedt.core.generic.numbers import _units_assignment +from ansys.aedt.core.generic.quaternion import Quaternion from ansys.aedt.core.modeler.cad.elements_3d import EdgePrimitive from ansys.aedt.core.modeler.cad.elements_3d import FacePrimitive from ansys.aedt.core.modeler.cad.elements_3d import VertexPrimitive @@ -663,7 +664,7 @@ def update(self): class CoordinateSystem(BaseCoordinateSystem, object): - """Manages coordinate system data and execution. + """Manages the coordinate system data and execution. Parameters ---------- @@ -833,12 +834,13 @@ def change_cs_mode(self, mode_type=0): ``True`` when successful, ``False`` when failed. """ - legacy_update = self.auto_update + previous_auto_update = self.auto_update self.auto_update = False if mode_type == 0: # "Axis/Position" if self.props and (self.props["Mode"] == "Euler Angle ZXZ" or self.props["Mode"] == "Euler Angle ZYZ"): self.props["Mode"] = "Axis/Position" - x, y, z = GeometryOperators.quaternion_to_axis(self.quaternion) + m = self.quaternion.to_rotation_matrix() + x, y, _ = Quaternion.rotation_matrix_to_axis(m) xaxis = x ypoint = y self.props["XAxisXvec"] = xaxis[0] @@ -855,7 +857,7 @@ def change_cs_mode(self, mode_type=0): elif mode_type == 1: # "Euler Angle ZXZ" if self.props and self.props["Mode"] == "Euler Angle ZYZ": self.props["Mode"] = "Euler Angle ZXZ" - a, b, g = GeometryOperators.quaternion_to_euler_zxz(self.quaternion) + a, b, g = self.quaternion.to_euler("zxz") phi = GeometryOperators.rad2deg(a) theta = GeometryOperators.rad2deg(b) psi = GeometryOperators.rad2deg(g) @@ -866,7 +868,7 @@ def change_cs_mode(self, mode_type=0): self.update() elif self.props and self.props["Mode"] == "Axis/Position": self.props["Mode"] = "Euler Angle ZXZ" - a, b, g = GeometryOperators.quaternion_to_euler_zxz(self.quaternion) + a, b, g = self.quaternion.to_euler("zxz") phi = GeometryOperators.rad2deg(a) theta = GeometryOperators.rad2deg(b) psi = GeometryOperators.rad2deg(g) @@ -884,7 +886,7 @@ def change_cs_mode(self, mode_type=0): elif mode_type == 2: # "Euler Angle ZYZ" if self.props and self.props["Mode"] == "Euler Angle ZXZ": self.props["Mode"] = "Euler Angle ZYZ" - a, b, g = GeometryOperators.quaternion_to_euler_zyz(self.quaternion) + a, b, g = self.quaternion.to_euler("zyz") phi = GeometryOperators.rad2deg(a) theta = GeometryOperators.rad2deg(b) psi = GeometryOperators.rad2deg(g) @@ -895,7 +897,7 @@ def change_cs_mode(self, mode_type=0): self.update() elif self.props and self.props["Mode"] == "Axis/Position": self.props["Mode"] = "Euler Angle ZYZ" - a, b, g = GeometryOperators.quaternion_to_euler_zyz(self.quaternion) + a, b, g = self.quaternion.to_euler("zyz") phi = GeometryOperators.rad2deg(a) theta = GeometryOperators.rad2deg(b) psi = GeometryOperators.rad2deg(g) @@ -911,8 +913,8 @@ def change_cs_mode(self, mode_type=0): self.mode = "zyz" self.update() else: # pragma: no cover - raise ValueError('mode_type=0 for "Axis/Position", =1 for "Euler Angle ZXZ", =2 for "Euler Angle ZYZ"') - self.auto_update = legacy_update + raise ValueError('Set mode_type=0 for "Axis/Position", =1 for "Euler Angle ZXZ", =2 for "Euler Angle ZYZ"') + self.auto_update = previous_auto_update return True @pyaedt_function_handler() @@ -1068,8 +1070,8 @@ def create( elif mode == "axisrotation": th = GeometryOperators.deg2rad(theta) - q = GeometryOperators.axis_angle_to_quaternion(u, th) - a, b, c = GeometryOperators.quaternion_to_euler_zyz(q) + q = Quaternion.from_axis_angle(u, th) + a, b, c = q.to_euler("zyz") phi = GeometryOperators.rad2deg(a) theta = GeometryOperators.rad2deg(b) psi = GeometryOperators.rad2deg(c) @@ -1087,17 +1089,77 @@ def create( self._ref_cs = reference_cs return self.update() + @staticmethod + @pyaedt_function_handler() + def pointing_to_axis(x_pointing, y_pointing): + """Retrieve the axes from the HFSS X axis and Y pointing axis as per + the definition of the AEDT interface coordinate system. + + Parameters + ---------- + x_pointing : List or tuple + ``(x, y, z)`` coordinates for the X axis. + + y_pointing : List or tuple + ``(x, y, z)`` coordinates for the Y pointing axis. + + Returns + ------- + tuple + ``(Xx, Xy, Xz), (Yx, Yy, Yz), (Zx, Zy, Zz)`` of the three axes (normalized). + """ + zpt = GeometryOperators.v_cross(x_pointing, y_pointing) + ypt = GeometryOperators.v_cross(zpt, x_pointing) + + xp = tuple(GeometryOperators.normalize_vector(x_pointing)) + zp = tuple(GeometryOperators.normalize_vector(zpt)) + yp = tuple(GeometryOperators.normalize_vector(ypt)) + + return xp, yp, zp + + def _get_numeric_value(self, value=None, init=False, destroy=False): + """Get numeric value from a variable. + Parameters + ---------- + value : str, int, float + Value to be converted. + init : bool, optional + If ``True``, initializes the temp variable in the variable manager. + If ``True``, all other parameters are ignored. + The default is ``False``. + destroy : bool, optional + If ``True``, destroys the temp variable in the variable manager. + If ``True``, all other parameters are ignored. + The default is ``False``. + Returns + ------- + float + Numeric value. + + """ + if init: + self._modeler._app.variable_manager["temp_var"] = 0 + return True + elif destroy: + del self._modeler._app.variable_manager["temp_var"] + return True + elif value is not None: + self._modeler._app.variable_manager["temp_var"] = value + return self._modeler._app.variable_manager["temp_var"].numeric_value + else: + raise ValueError("Either set value or init or destroy must be True.") # pragma: no cover + @property def quaternion(self): """Quaternion computed based on specific axis mode. Returns ------- - list + Quaternion """ if self._quaternion: return self._quaternion - self._modeler._app.variable_manager["temp_var"] = 0 + self._get_numeric_value(init=True) if self.mode == "axis" or self.mode == "view": x1 = self.props["XAxisXvec"] x2 = self.props["XAxisYvec"] @@ -1105,40 +1167,25 @@ def quaternion(self): y1 = self.props["YAxisXvec"] y2 = self.props["YAxisYvec"] y3 = self.props["YAxisZvec"] - self._modeler._app.variable_manager["temp_var"] = x1 - x_pointing_num = [self._modeler._app.variable_manager["temp_var"].numeric_value] - self._modeler._app.variable_manager["temp_var"] = x2 - x_pointing_num.append(self._modeler._app.variable_manager["temp_var"].numeric_value) - self._modeler._app.variable_manager["temp_var"] = x3 - x_pointing_num.append(self._modeler._app.variable_manager["temp_var"].numeric_value) - self._modeler._app.variable_manager["temp_var"] = y1 - y_pointing_num = [self._modeler._app.variable_manager["temp_var"].numeric_value] - self._modeler._app.variable_manager["temp_var"] = y2 - y_pointing_num.append(self._modeler._app.variable_manager["temp_var"].numeric_value) - self._modeler._app.variable_manager["temp_var"] = y3 - y_pointing_num.append(self._modeler._app.variable_manager["temp_var"].numeric_value) - x, y, z = GeometryOperators.pointing_to_axis(x_pointing_num, y_pointing_num) - a, b, g = GeometryOperators.axis_to_euler_zyz(x, y, z) - self._quaternion = GeometryOperators.euler_zyz_to_quaternion(a, b, g) - del self._modeler._app.variable_manager["temp_var"] + x_axis = (x1, x2, x3) + x_pointing_num = [self._get_numeric_value(i) for i in x_axis] + y_axis = (y1, y2, y3) + y_pointing_num = [self._get_numeric_value(i) for i in y_axis] + x, y, z = CoordinateSystem.pointing_to_axis(x_pointing_num, y_pointing_num) + m = Quaternion.axis_to_rotation_matrix(x, y, z) + self._quaternion = Quaternion.from_rotation_matrix(m) elif self.mode == "zxz": - self._modeler._app.variable_manager["temp_var"] = self.props["Phi"] - a = GeometryOperators.deg2rad(self._modeler._app.variable_manager["temp_var"].numeric_value) - self._modeler._app.variable_manager["temp_var"] = self.props["Theta"] - b = GeometryOperators.deg2rad(self._modeler._app.variable_manager["temp_var"].numeric_value) - self._modeler._app.variable_manager["temp_var"] = self.props["Psi"] - g = GeometryOperators.deg2rad(self._modeler._app.variable_manager["temp_var"].numeric_value) - self._quaternion = GeometryOperators.euler_zxz_to_quaternion(a, b, g) - del self._modeler._app.variable_manager["temp_var"] + a = self._get_numeric_value(self.props["Phi"]) + b = self._get_numeric_value(self.props["Theta"]) + g = self._get_numeric_value(self.props["Psi"]) + self._quaternion = Quaternion.from_euler((a, b, g), "zxz") elif self.mode == "zyz" or self.mode == "axisrotation": - self._modeler._app.variable_manager["temp_var"] = self.props["Phi"] - a = GeometryOperators.deg2rad(self._modeler._app.variable_manager["temp_var"].numeric_value) - self._modeler._app.variable_manager["temp_var"] = self.props["Theta"] - b = GeometryOperators.deg2rad(self._modeler._app.variable_manager["temp_var"].numeric_value) - self._modeler._app.variable_manager["temp_var"] = self.props["Psi"] - g = GeometryOperators.deg2rad(self._modeler._app.variable_manager["temp_var"].numeric_value) - self._quaternion = GeometryOperators.euler_zyz_to_quaternion(a, b, g) - del self._modeler._app.variable_manager["temp_var"] + a = self._get_numeric_value(self.props["Phi"]) + b = self._get_numeric_value(self.props["Theta"]) + g = self._get_numeric_value(self.props["Psi"]) + self._quaternion = Quaternion.from_euler((a, b, g), "zyz") + + self._get_numeric_value(destroy=True) return self._quaternion @property @@ -1149,19 +1196,19 @@ def origin(self): ------- list """ - self._modeler._app.variable_manager["temp_var"] = self.props["OriginX"] - x = self._modeler._app.variable_manager["temp_var"].numeric_value - self._modeler._app.variable_manager["temp_var"] = self.props["OriginY"] - y = self._modeler._app.variable_manager["temp_var"].numeric_value - self._modeler._app.variable_manager["temp_var"] = self.props["OriginZ"] - z = self._modeler._app.variable_manager["temp_var"].numeric_value - del self._modeler._app.variable_manager["temp_var"] + self._get_numeric_value(init=True) + x = self._get_numeric_value(self.props["OriginX"]) + y = self._get_numeric_value(self.props["OriginY"]) + z = self._get_numeric_value(self.props["OriginZ"]) + + self._get_numeric_value(destroy=True) + return [x, y, z] @origin.setter def origin(self, origin): """Set the coordinate system origin in model units.""" - legacy_update = self.auto_update + previous_auto_update = self.auto_update self.auto_update = False origin_x = self._modeler._app.value_with_units(origin[0], self.model_units) origin_y = self._modeler._app.value_with_units(origin[1], self.model_units) @@ -1170,7 +1217,7 @@ def origin(self, origin): self.props["OriginY"] = origin_y self.props["OriginZ"] = origin_z self.update() - self.auto_update = legacy_update + self.auto_update = previous_auto_update @property def _orientation(self): diff --git a/src/ansys/aedt/core/modeler/cad/primitives.py b/src/ansys/aedt/core/modeler/cad/primitives.py index 0478fa71b1f..f0f093b238f 100644 --- a/src/ansys/aedt/core/modeler/cad/primitives.py +++ b/src/ansys/aedt/core/modeler/cad/primitives.py @@ -44,6 +44,7 @@ from ansys.aedt.core.generic.numbers import _units_assignment from ansys.aedt.core.generic.numbers import decompose_variable_value from ansys.aedt.core.generic.numbers import is_number +from ansys.aedt.core.generic.quaternion import Quaternion from ansys.aedt.core.modeler.cad.components_3d import UserDefinedComponent from ansys.aedt.core.modeler.cad.elements_3d import EdgePrimitive from ansys.aedt.core.modeler.cad.elements_3d import FacePrimitive @@ -1589,11 +1590,11 @@ def create_coordinate_system( view : str, int optional View for the coordinate system if ``mode="view"``. Options are ``"iso"``, ``None``, ``"XY"``, ``"XZ"``, and ``"XY"``. The - default is ``"iso"``. the ``"rotate"`` option is obsolete. You can + default is ``"iso"``. The ``"rotate"`` option is obsolete. You can also use the ``ansys.aedt.core.generic.constants.VIEW`` enumerator. .. note:: - For backward compatibility, ``mode="view"`` and ``view="rotate"`` are the same as + For backward compatibility, ``mode="view", view="rotate"`` are the same as ``mode="axis"``. Because the "rotate" option in the "view" mode is obsolete, use ``mode="axis"`` instead. @@ -1864,7 +1865,7 @@ def get_total_transformation(p, cs): p1 = p else: p1 = get_total_transformation(p, refcs) - p2 = GeometryOperators.q_rotation_inv(GeometryOperators.v_sub(p1, o), q) + p2 = q.inverse_rotate_vector(GeometryOperators.v_sub(p1, o)) return p2 p = get_total_transformation(point, ref_cs_name) @@ -1933,10 +1934,9 @@ def invert_cs(self, coordinate_system, to_global=False): Returns ------- - list - Origin coordinates. - list - Quaternion. + tuple + List of the ``[x, y, z]`` coordinates of the origin and the quaternion defining the + coordinate system. """ if isinstance(coordinate_system, BaseCoordinateSystem): cs = coordinate_system @@ -1950,12 +1950,12 @@ def invert_cs(self, coordinate_system, to_global=False): if to_global: o, q = self.reference_cs_to_global(coordinate_system) - o = GeometryOperators.v_prod(-1, GeometryOperators.q_rotation(o, q)) - q = [q[0], -q[1], -q[2], -q[3]] + o = GeometryOperators.v_prod(-1, q.rotate_vector(o)) + q = q.conjugate() else: q = cs.quaternion - q = [q[0], -q[1], -q[2], -q[3]] - o = GeometryOperators.v_prod(-1, GeometryOperators.q_rotation(cs.origin, q)) + q = q.conjugate() + o = GeometryOperators.v_prod(-1, q.rotate_vector(cs.origin)) return o, q @pyaedt_function_handler() @@ -1969,10 +1969,9 @@ def reference_cs_to_global(self, coordinate_system): Returns ------- - list - Origin coordinates. - list - Quaternion. + tuple + List of the ``[x, y, z]`` coordinates of the origin and the quaternion defining the + coordinate system in the global coordinates. """ cs_names = [i.name for i in self.coordinate_systems] if isinstance(coordinate_system, BaseCoordinateSystem): @@ -1989,9 +1988,9 @@ def reference_cs_to_global(self, coordinate_system): while ref_cs_name != "Global": ref_cs = self.coordinate_systems[cs_names.index(ref_cs_name)] quaternion_ref = ref_cs.quaternion - quaternion = GeometryOperators.q_prod(quaternion_ref, quaternion) + quaternion = quaternion_ref * quaternion origin_ref = ref_cs.origin - origin = GeometryOperators.v_sum(origin_ref, GeometryOperators.q_rotation(origin, quaternion_ref)) + origin = GeometryOperators.v_sum(origin_ref, quaternion_ref.rotate_vector(origin)) ref_cs_name = ref_cs.ref_cs return origin, quaternion @@ -2027,7 +2026,8 @@ def duplicate_coordinate_system_to_global(self, coordinate_system): raise AttributeError("coordinate_system must either be a string or a CoordinateSystem object.") if isinstance(cs, CoordinateSystem): o, q = self.reference_cs_to_global(coordinate_system) - x, y, _ = GeometryOperators.quaternion_to_axis(q) + mm = q.to_rotation_matrix() + x, y, _ = Quaternion.rotation_matrix_to_axis(mm) reference_cs = "Global" name = cs.name + "_RefToGlobal" if name in cs_names: diff --git a/src/ansys/aedt/core/modeler/cad/primitives_3d.py b/src/ansys/aedt/core/modeler/cad/primitives_3d.py index 9b17a1399f3..581d428245a 100644 --- a/src/ansys/aedt/core/modeler/cad/primitives_3d.py +++ b/src/ansys/aedt/core/modeler/cad/primitives_3d.py @@ -1425,7 +1425,7 @@ def _create_reference_cs_from_3dcomp(self, assignment, password): for name in dirs: os.rmdir(os.path.join(root, name)) os.rmdir(temp_folder) - phi, theta, psi = GeometryOperators.quaternion_to_euler_zxz(q) + phi, theta, psi = q.to_euler('zxz') cs_name = assignment.name + "_" + wcs + "_ref" if cs_name not in [i.name for i in self.coordinate_systems]: self.create_coordinate_system( diff --git a/src/ansys/aedt/core/modeler/geometry_operators.py b/src/ansys/aedt/core/modeler/geometry_operators.py index c6a46272f14..eb86ba74390 100644 --- a/src/ansys/aedt/core/modeler/geometry_operators.py +++ b/src/ansys/aedt/core/modeler/geometry_operators.py @@ -25,12 +25,14 @@ import math import re import sys +import warnings from ansys.aedt.core.generic.constants import AXIS from ansys.aedt.core.generic.constants import PLANE from ansys.aedt.core.generic.constants import SWEEPDRAFT from ansys.aedt.core.generic.constants import scale_units from ansys.aedt.core.generic.general_methods import pyaedt_function_handler +from ansys.aedt.core.generic.math_utils import MathUtils class GeometryOperators(object): @@ -464,11 +466,9 @@ def v_norm(a): Evaluated norm in the same unit as the coordinates for the input vector. """ - t = 0 - for i in a: - t += i**2 - m = t**0.5 - return m + n = math.sqrt(sum(x * x for x in a)) + + return n @staticmethod @pyaedt_function_handler() @@ -800,7 +800,7 @@ def v_angle(a, b): @staticmethod @pyaedt_function_handler() - def pointing_to_axis(x_pointing, y_pointing): + def pointing_to_axis(*args, **kwargs): """Retrieve the axes from the HFSS X axis and Y pointing axis as per the definition of the AEDT interface coordinate system. @@ -817,21 +817,21 @@ def pointing_to_axis(x_pointing, y_pointing): tuple ``[Xx, Xy, Xz], [Yx, Yy, Yz], [Zx, Zy, Zz]`` of the three axes (normalized). """ - zpt = GeometryOperators.v_cross(x_pointing, y_pointing) - ypt = GeometryOperators.v_cross(zpt, x_pointing) - - xp = GeometryOperators.normalize_vector(x_pointing) - zp = GeometryOperators.normalize_vector(zpt) - yp = GeometryOperators.normalize_vector(ypt) + warnings.warn( + "GeometryOperators.pointing_to_axis is deprecated and has been moved to CoordinateSystem.pointing_to_axis.", + DeprecationWarning, + stacklevel=2, + ) + from ansys.aedt.core.modeler.cad.modeler import CoordinateSystem # import here to avoid circular imports - return xp, yp, zp + return CoordinateSystem.pointing_to_axis(*args, **kwargs) @staticmethod @pyaedt_function_handler() def axis_to_euler_zxz(x, y, z): """Retrieve Euler angles of a frame following the rotation sequence ZXZ. - Provides assumption for the gimbal lock problem. + Provides an assumption for the gimbal lock problem. Parameters ---------- @@ -848,6 +848,16 @@ def axis_to_euler_zxz(x, y, z): (phi, theta, psi) containing the Euler angles in radians. """ + warnings.warn( + "GeometryOperators.axis_to_euler_zxz is deprecated. Consider using the Quaternion class." + ">>> from ansys.aedt.core.generic.quaternion import Quaternion" + ">>> m = Quaternion.axis_to_rotation_matrix(x, y, z)" + ">>> q = Quaternion.from_rotation_matrix(m)" + ">>> phi, theta, psi = q.to_euler('zxz')", + DeprecationWarning, + stacklevel=2, + ) + tol = 1e-16 x1 = x[0] x2 = x[1] @@ -857,17 +867,17 @@ def axis_to_euler_zxz(x, y, z): z2 = z[1] z3 = z[2] if GeometryOperators.v_norm(GeometryOperators.v_sub(z, [0, 0, 1])) < tol: - phi = GeometryOperators.atan2(x2, x1) + phi = MathUtils.atan2(x2, x1) theta = 0.0 psi = 0.0 elif GeometryOperators.v_norm(GeometryOperators.v_sub(z, [0, 0, -1])) < tol: - phi = GeometryOperators.atan2(x2, x1) + phi = MathUtils.atan2(x2, x1) theta = math.pi psi = 0.0 else: - phi = GeometryOperators.atan2(z1, -z2) + phi = MathUtils.atan2(z1, -z2) theta = math.acos(z3) - psi = GeometryOperators.atan2(x3, y3) + psi = MathUtils.atan2(x3, y3) return phi, theta, psi @staticmethod @@ -892,6 +902,15 @@ def axis_to_euler_zyz(x, y, z): (phi, theta, psi) containing the Euler angles in radians. """ + warnings.warn( + "GeometryOperators.axis_to_euler_zxz is deprecated. Consider using the Quaternion class." + ">>> from ansys.aedt.core.generic.quaternion import Quaternion" + ">>> m = Quaternion.axis_to_rotation_matrix(x, y, z)" + ">>> q = Quaternion.from_rotation_matrix(m)" + ">>> phi, theta, psi = q.to_euler('zyz')", + DeprecationWarning, + stacklevel=2, + ) tol = 1e-16 x1 = x[0] x2 = x[1] @@ -901,233 +920,19 @@ def axis_to_euler_zyz(x, y, z): z2 = z[1] z3 = z[2] if GeometryOperators.v_norm(GeometryOperators.v_sub(z, [0, 0, 1])) < tol: - phi = GeometryOperators.atan2(-x1, x2) + phi = MathUtils.atan2(-x1, x2) theta = 0.0 psi = math.pi / 2 elif GeometryOperators.v_norm(GeometryOperators.v_sub(z, [0, 0, -1])) < tol: - phi = GeometryOperators.atan2(-x1, x2) + phi = MathUtils.atan2(-x1, x2) theta = math.pi psi = math.pi / 2 else: - phi = GeometryOperators.atan2(z2, z1) + phi = MathUtils.atan2(z2, z1) theta = math.acos(z3) - psi = GeometryOperators.atan2(y3, -x3) + psi = MathUtils.atan2(y3, -x3) return phi, theta, psi - @staticmethod - @pyaedt_function_handler() - def quaternion_to_axis(q): - """Convert a quaternion to a rotated frame defined by X, Y, and Z axes. - - Parameters - ---------- - q : List - List of ``[q1, q2, q3, q4]`` coordinates for the quaternion. - - Returns - ------- - tuple - [Xx, Xy, Xz], [Yx, Yy, Yz], [Zx, Zy, Zz] of the three axes (normalized). - - """ - q1 = q[0] - q2 = q[1] - q3 = q[2] - q4 = q[3] - - m11 = q1 * q1 + q2 * q2 - q3 * q3 - q4 * q4 - m12 = 2.0 * (q2 * q3 - q1 * q4) - m13 = 2.0 * (q2 * q4 + q1 * q3) - - m21 = 2.0 * (q2 * q3 + q1 * q4) - m22 = q1 * q1 - q2 * q2 + q3 * q3 - q4 * q4 - m23 = 2.0 * (q3 * q4 - q1 * q2) - - m31 = 2.0 * (q2 * q4 - q1 * q3) - m32 = 2.0 * (q3 * q4 + q1 * q2) - m33 = q1 * q1 - q2 * q2 - q3 * q3 + q4 * q4 - - x = GeometryOperators.normalize_vector([m11, m21, m31]) - y = GeometryOperators.normalize_vector([m12, m22, m32]) - z = GeometryOperators.normalize_vector([m13, m23, m33]) - - return x, y, z - - @staticmethod - @pyaedt_function_handler() - def quaternion_to_axis_angle(q): - """Convert a quaternion to the axis angle rotation formulation. - - Parameters - ---------- - q : List - List of ``[q1, q2, q3, q4]`` coordinates for the quaternion. - - Returns - ------- - tuple - ([ux, uy, uz], theta) containing the rotation axes expressed as X, Y, Z components of - the unit vector ``u`` and the rotation angle theta expressed in radians. - - """ - q1 = q[0] - q2 = q[1] - q3 = q[2] - q4 = q[3] - n = (q2 * q2 + q3 * q3 + q4 * q4) ** 0.5 - u = [q2 / n, q3 / n, q4 / n] - theta = 2.0 * GeometryOperators.atan2(n, q1) - return u, theta - - @staticmethod - @pyaedt_function_handler() - def axis_angle_to_quaternion(u, theta): - """Convert the axis angle rotation formulation to a quaternion. - - Parameters - ---------- - u : List - List of ``[ux, uy, uz]`` coordinates for the rotation axis. - - theta : float - Angle of rotation in radians. - - Returns - ------- - List - List of ``[q1, q2, q3, q4]`` coordinates for the quaternion. - - """ - un = GeometryOperators.normalize_vector(u) - s = math.sin(theta * 0.5) - q1 = math.cos(theta * 0.5) - q2 = un[0] * s - q3 = un[1] * s - q4 = un[2] * s - return [q1, q2, q3, q4] - - @staticmethod - @pyaedt_function_handler() - def quaternion_to_euler_zxz(q): - """Convert a quaternion to Euler angles following rotation sequence ZXZ. - - Parameters - ---------- - q : List - List of ``[q1, q2, q3, q4]`` coordinates for the quaternion. - - Returns - ------- - tuple - (phi, theta, psi) containing the Euler angles in radians. - - """ - q1 = q[0] - q2 = q[1] - q3 = q[2] - q4 = q[3] - m13 = 2.0 * (q2 * q4 + q1 * q3) - m23 = 2.0 * (q3 * q4 - q1 * q2) - m33 = q1 * q1 - q2 * q2 - q3 * q3 + q4 * q4 - m31 = 2.0 * (q2 * q4 - q1 * q3) - m32 = 2.0 * (q3 * q4 + q1 * q2) - phi = GeometryOperators.atan2(m13, -m23) - theta = GeometryOperators.atan2((1.0 - m33 * m33) ** 0.5, m33) - psi = GeometryOperators.atan2(m31, m32) - return phi, theta, psi - - @staticmethod - @pyaedt_function_handler() - def euler_zxz_to_quaternion(phi, theta, psi): - """Convert the Euler angles following rotation sequence ZXZ to a quaternion. - - Parameters - ---------- - phi : float - Euler angle psi in radians. - theta : float - Euler angle theta in radians. - psi : float - Euler angle phi in radians. - - Returns - ------- - List - List of ``[q1, q2, q3, q4]`` coordinates for the quaternion. - - """ - t1 = phi - t2 = theta - t3 = psi - c = math.cos(t2 * 0.5) - s = math.sin(t2 * 0.5) - q1 = c * math.cos((t1 + t3) * 0.5) - q2 = s * math.cos((t1 - t3) * 0.5) - q3 = s * math.sin((t1 - t3) * 0.5) - q4 = c * math.sin((t1 + t3) * 0.5) - return [q1, q2, q3, q4] - - @staticmethod - @pyaedt_function_handler() - def quaternion_to_euler_zyz(q): - """Convert a quaternion to Euler angles following rotation sequence ZYZ. - - Parameters - ---------- - q : List - List of ``[q1, q2, q3, q4]`` coordinates for the quaternion. - - Returns - ------- - tuple - (phi, theta, psi) containing the Euler angles in radians. - - """ - q1 = q[0] - q2 = q[1] - q3 = q[2] - q4 = q[3] - m13 = 2.0 * (q2 * q4 + q1 * q3) - m23 = 2.0 * (q3 * q4 - q1 * q2) - m33 = q1 * q1 - q2 * q2 - q3 * q3 + q4 * q4 - m31 = 2.0 * (q2 * q4 - q1 * q3) - m32 = 2.0 * (q3 * q4 + q1 * q2) - phi = GeometryOperators.atan2(m23, m13) - theta = GeometryOperators.atan2((1.0 - m33 * m33) ** 0.5, m33) - psi = GeometryOperators.atan2(m32, -m31) - return phi, theta, psi - - @staticmethod - @pyaedt_function_handler() - def euler_zyz_to_quaternion(phi, theta, psi): - """Convert the Euler angles following rotation sequence ZYZ to a quaternion. - - Parameters - ---------- - phi : float - Euler angle psi in radians. - theta : float - Euler angle theta in radians. - psi : float - Euler angle phi in radians. - - Returns - ------- - List - List of ``[q1, q2, q3, q4]`` coordinates for the quaternion. - - """ - t1 = phi - t2 = theta - t3 = psi - c = math.cos(t2 * 0.5) - s = math.sin(t2 * 0.5) - q1 = c * math.cos((t1 + t3) * 0.5) - q2 = -s * math.sin((t1 - t3) * 0.5) - q3 = s * math.cos((t1 - t3) * 0.5) - q4 = c * math.sin((t1 + t3) * 0.5) - return [q1, q2, q3, q4] - @staticmethod @pyaedt_function_handler() def deg2rad(angle): @@ -1168,134 +973,129 @@ def rad2deg(angle): @staticmethod @pyaedt_function_handler() - def atan2(y, x): - """Implementation of atan2 that does not suffer from the following issues: - math.atan2(0.0, 0.0) = 0.0 - math.atan2(-0.0, 0.0) = -0.0 - math.atan2(0.0, -0.0) = 3.141592653589793 - math.atan2(-0.0, -0.0) = -3.141592653589793 - and returns always 0.0. - + def is_orthonormal_triplet(x, y, z, tol=None): + """Check if three vectors are orthonormal. Parameters ---------- - y : float - Y-axis value for atan2. - - x : float - X-axis value for atan2. + x : List or tuple + List of ``(x1, x2, x3)`` coordinates for the first vector. + y : List or tuple + List of ``(y1, y2, y3)`` coordinates for the second vector. + z : List or tuple + List of ``(z1, z2, z3)`` coordinates for the third vector. + tol : float, optional + Linear tolerance. The default value is ``None``. + If not specified, the value is set to ``MathUtils.EPSILON``. Returns ------- - float - + bool + ``True`` if the three vectors are orthonormal, ``False`` otherwise. """ - eps = 7.0 / 3.0 - 4.0 / 3.0 - 1.0 - if abs(y) < eps: - y = 0.0 - if abs(x) < eps: - x = 0.0 - return math.atan2(y, x) + + if tol is None: + tol = MathUtils.EPSILON + + # Check unit length + if not ( + GeometryOperators.is_unit_vector(x, tol) + and GeometryOperators.is_unit_vector(y, tol) + and GeometryOperators.is_unit_vector(z, tol) + ): + return False + + # Check orthogonality + if not ( + abs(GeometryOperators.v_dot(x, y)) < tol + and abs(GeometryOperators.v_dot(y, z)) < tol + and abs(GeometryOperators.v_dot(z, x)) < tol + ): + return False + + return True @staticmethod @pyaedt_function_handler() - def q_prod(p, q): - """Evaluate the product of two quaternions, ``p`` and ``q``, defined as: - p = p0 + p' = p0 + ip1 + jp2 + kp3. - q = q0 + q' = q0 + iq1 + jq2 + kq3. - r = pq = p0q0 - p' • q' + p0q' + q0p' + p' x q'. + def is_unit_vector(v, tol=None): + """Check if a vector is a unit vector. Parameters ---------- - p : List - List of ``[p1, p2, p3, p4]`` coordinates for quaternion ``p``. - - q : List - List of ``[p1, p2, p3, p4]`` coordinates for quaternion ``q``. + v : List or tuple + List of ``(x1, x2, x3)`` coordinates for the vector. + tol : float, optional + Linear tolerance. + The default value is ``None``. + If not specified, the value is set to ``MathUtils.EPSILON``. Returns ------- - List - List of [r1, r2, r3, r4] coordinates for the result quaternion. - + bool + ``True`` if the vector is a unit vector, ``False`` otherwise. """ - p0 = p[0] - pv = p[1:4] - q0 = q[0] - qv = q[1:4] - - r0 = p0 * q0 - GeometryOperators.v_dot(pv, qv) - t1 = GeometryOperators.v_prod(p0, qv) - t2 = GeometryOperators.v_prod(q0, pv) - t3 = GeometryOperators.v_cross(pv, qv) - rv = GeometryOperators.v_sum(t1, GeometryOperators.v_sum(t2, t3)) - - return [r0, rv[0], rv[1], rv[2]] + if tol is None: + tol = MathUtils.EPSILON + norm = GeometryOperators.v_norm(v) + return abs(norm - 1.0) < tol @staticmethod @pyaedt_function_handler() - def q_rotation(v, q): - """Evaluate the rotation of a vector, defined by a quaternion. + def is_orthogonal_matrix(matrix, tol=None): + """ + Check if a given 3x3 matrix is orthogonal. - Evaluated as: - ``"q = q0 + q' = q0 + iq1 + jq2 + kq3"``, - ``"w = qvq* = (q0^2 - |q'|^2)v + 2(q' • v)q' + 2q0(q' x v)"``. + An orthogonal matrix is a square matrix whose rows and columns are orthonormal vectors. + This method verifies if the transpose of the matrix multiplied by the matrix itself + results in an identity matrix within a specified tolerance. Parameters ---------- - v : List - List of ``[v1, v2, v3]`` coordinates for the vector. - q : List - List of ``[q1, q2, q3, q4]`` coordinates for the quaternion. + matrix : List[List[float]] + A 3x3 matrix represented as a list of lists. + tol : float, optional + Tolerance for numerical comparison. + The default value is ``None``. + If not specified, the value is set to ``MathUtils.EPSILON``. Returns ------- - List - List of ``[w1, w2, w3]`` coordinates for the result vector ``w``. - """ - q0 = q[0] - qv = q[1:4] - - c1 = q0 * q0 - (qv[0] * qv[0] + qv[1] * qv[1] + qv[2] * qv[2]) - t1 = GeometryOperators.v_prod(c1, v) - - c2 = 2.0 * GeometryOperators.v_dot(qv, v) - t2 = GeometryOperators.v_prod(c2, qv) - - t3 = GeometryOperators.v_cross(qv, v) - t4 = GeometryOperators.v_prod(2.0 * q0, t3) - - w = GeometryOperators.v_sum(t1, GeometryOperators.v_sum(t2, t4)) + bool + True if the matrix is orthogonal, False otherwise. - return w + Examples + -------- + Check if a matrix is orthogonal: - @staticmethod - @pyaedt_function_handler() - def q_rotation_inv(v, q): - """Evaluate the inverse rotation of a vector that is defined by a quaternion. + >>> matrix = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] + >>> is_orthogonal_matrix(matrix) + True - It can also be the rotation of the coordinate frame with respect to the vector. + >>> matrix = [[1, 0, 0], [0, 0, 1], [0, 1, 0]] + >>> is_orthogonal_matrix(matrix) + True - q = q0 + q' = q0 + iq1 + jq2 + kq3 - q* = q0 - q' = q0 - iq1 - jq2 - kq3 - w = q*vq + >>> matrix = [[1, 2, 3], [4, 5, 6], [7, 8, 9]] + >>> is_orthogonal_matrix(matrix) + False + """ - Parameters - ---------- - v : List - List of ``[v1, v2, v3]`` coordinates for the vector. + if tol is None: + tol = MathUtils.EPSILON - q : List - List of ``[q1, q2, q3, q4]`` coordinates for the quaternion. + # Transpose of the matrix + transpose = [[matrix[j][i] for j in range(3)] for i in range(3)] - Returns - ------- - List - List of ``[w1, w2, w3]`` coordinates for the vector. + # Multiply transpose × matrix + product = [[sum(transpose[i][k] * matrix[k][j] for k in range(3)) for j in range(3)] for i in range(3)] - """ - q1 = [q[0], -q[1], -q[2], -q[3]] - return GeometryOperators.q_rotation(v, q1) + # Check if the result is close to the identity matrix + for i in range(3): + for j in range(3): + expected = 1.0 if i == j else 0.0 + if not MathUtils.is_equal(product[i][j], expected, tol): + return False + return True @staticmethod @pyaedt_function_handler() @@ -1402,6 +1202,32 @@ def is_small(s): n = GeometryOperators.get_numeric(s) return True if math.fabs(n) < 2.0 * abs(sys.float_info.epsilon) else False + @staticmethod + @pyaedt_function_handler() + def is_vector_equal(v1, v2, tolerance=None): + """Return ``True`` if two vectors are equal. + + Parameters + ---------- + v1 : List + List of ``[x, y, z]`` coordinates for the first vector. + v2 : List + List of ``[x, y, z]`` coordinates for the second vector. + tolerance : float, optional + Linear tolerance. The default value is ``None``. + If not specified, the value is set to ``MathUtils.EPSILON``. + + Returns + ------- + bool + ``True`` if the two vectors are equal, ``False`` otherwise. + """ + if len(v1) != len(v2): + return False + if tolerance is None: + tolerance = MathUtils.EPSILON + return GeometryOperators.v_norm(GeometryOperators.v_sub(v1, v2)) < tolerance + @staticmethod @pyaedt_function_handler() def numeric_cs(cs_in): diff --git a/tests/system/general/test_01_GeometryOperators.py b/tests/system/general/test_01_GeometryOperators.py index 7bce03e42cb..200b37e92fc 100644 --- a/tests/system/general/test_01_GeometryOperators.py +++ b/tests/system/general/test_01_GeometryOperators.py @@ -37,12 +37,7 @@ def is_vector_equal(v, r): - t = 0 - for a, b in zip(v, r): - d = a - b - t += d * d - n = math.sqrt(t) - return n < 1e-12 + return go.is_vector_equal(v, r, tolerance=1e-12) @pytest.fixture(scope="module", autouse=True) @@ -246,98 +241,6 @@ def test_v_angle(self): v2 = [0, 1, 0] assert abs(go.v_angle(v1, v2) - math.pi / 2) < tol - def test_pointing_to_axis(self): - x, y, z = go.pointing_to_axis([1, 0.1, 1], [0.5, 1, 0]) - assert is_vector_equal(x, [0.7053456158585983, 0.07053456158585983, 0.7053456158585983]) - assert is_vector_equal(y, [0.19470872568244801, 0.9374864569895649, -0.28845737138140465]) - assert is_vector_equal(z, [-0.681598176590997, 0.3407990882954985, 0.6475182677614472]) - - def test_axis_to_euler_zxz(self): - x, y, z = go.pointing_to_axis([1, 0.1, 1], [0.5, 1, 0]) - phi, theta, psi = go.axis_to_euler_zxz(x, y, z) - assert abs(phi - (-2.0344439357957027)) < tol - assert abs(theta - 0.8664730673456006) < tol - assert abs(psi - 1.9590019609437583) < tol - x, y, z = go.pointing_to_axis([-0.2, -0.3, 0], [-0.2, 0.3, 0]) - phi, theta, psi = go.axis_to_euler_zxz(x, y, z) - assert abs(phi - (-2.158798930342464)) < tol - assert abs(theta - 3.141592653589793) < tol - assert abs(psi - 0) < tol - x, y, z = go.pointing_to_axis([-0.2, -0.5, 0], [-0.1, -0.4, 0]) - phi, theta, psi = go.axis_to_euler_zxz(x, y, z) - assert abs(phi - (-1.9513027039072295)) < tol - assert abs(theta - 0) < tol - assert abs(psi - 0) < tol - - def test_axis_to_euler_zyz(self): - x, y, z = go.pointing_to_axis([1, 0.1, 1], [0.5, 1, 0]) - phi, theta, psi = go.axis_to_euler_zyz(x, y, z) - assert abs(phi - 2.677945044588987) < tol - assert abs(theta - 0.8664730673456006) < tol - assert abs(psi - (-2.7533870194409316)) < tol - x, y, z = go.pointing_to_axis([-0.2, -0.3, 0], [-0.2, 0.3, 0]) - phi, theta, psi = go.axis_to_euler_zyz(x, y, z) - assert abs(phi - 2.553590050042222) < tol - assert abs(theta - 3.141592653589793) < tol - assert abs(psi - 1.5707963267948966) < tol - x, y, z = go.pointing_to_axis([-0.2, -0.5, 0], [-0.1, -0.4, 0]) - phi, theta, psi = go.axis_to_euler_zyz(x, y, z) - assert abs(phi - 2.7610862764774597) < tol - assert abs(theta - 0) < tol - assert abs(psi - 1.5707963267948966) < tol - - def test_quaternion_to_axis(self): - q = [0.9069661433330367, -0.17345092325178477, -0.3823030778615049, -0.03422789400943274] - x, y, z = go.quaternion_to_axis(q) - assert is_vector_equal(x, [0.7053456158585982, 0.07053456158585963, 0.7053456158585982]) - assert is_vector_equal(y, [0.19470872568244832, 0.937486456989565, -0.2884573713814046]) - assert is_vector_equal(z, [-0.681598176590997, 0.34079908829549865, 0.6475182677614472]) - - def test_quaternion_to_axis_angle(self): - q = [0.9069661433330367, -0.17345092325178477, -0.3823030778615049, -0.03422789400943274] - u, th = go.quaternion_to_axis_angle(q) - assert is_vector_equal(u, [-0.41179835953227295, -0.9076445218972716, -0.0812621249808417]) - assert abs(th - 0.8695437956599169) < tol - - def test_axis_angle_to_quaternion(self): - u = [-0.41179835953227295, -0.9076445218972716, -0.0812621249808417] - th = 0.8695437956599169 - q = go.axis_angle_to_quaternion(u, th) - assert is_vector_equal(q, [0.9069661433330367, -0.17345092325178477, -0.3823030778615049, -0.03422789400943274]) - assert abs(q[0] ** 2 + q[1] ** 2 + q[2] ** 2 + q[3] ** 2 - 1.0) < tol - - def test_quaternion_to_euler_zxz(self): - q = [0.9069661433330367, -0.17345092325178477, -0.3823030778615049, -0.03422789400943274] - phi, theta, psi = go.quaternion_to_euler_zxz(q) - assert abs(phi - (-2.0344439357957027)) < tol - assert abs(theta - 0.8664730673456006) < tol - assert abs(psi - 1.9590019609437583) < tol - - def test_euler_zxz_to_quaternion(self): - phi = -2.0344439357957027 - theta = 0.8664730673456006 - psi = 1.9590019609437583 - q = go.euler_zxz_to_quaternion(phi, theta, psi) - assert is_vector_equal( - q, [0.9069661433330367, -0.17345092325178468, -0.38230307786150497, -0.03422789400943264] - ) - assert abs(q[0] ** 2 + q[1] ** 2 + q[2] ** 2 + q[3] ** 2 - 1.0) < tol - - def test_quaternion_to_euler_zyz(self): - q = [0.9069661433330367, -0.17345092325178477, -0.3823030778615049, -0.03422789400943274] - phi, theta, psi = go.quaternion_to_euler_zyz(q) - assert abs(phi - 2.677945044588987) < tol - assert abs(theta - 0.8664730673456006) < tol - assert abs(psi - (-2.7533870194409316)) < tol - - def test_euler_zyz_to_quaternion(self): - phi = 2.677945044588987 - theta = 0.8664730673456006 - psi = -2.7533870194409316 - q = go.euler_zyz_to_quaternion(phi, theta, psi) - assert is_vector_equal(q, [0.9069661433330367, -0.17345092325178477, -0.3823030778615049, -0.03422789400943274]) - assert abs(q[0] ** 2 + q[1] ** 2 + q[2] ** 2 + q[3] ** 2 - 1.0) < tol - def test_deg2rad(self): assert abs(go.deg2rad(180.0) - math.pi) < tol assert abs(go.deg2rad(180) - math.pi) < tol @@ -345,30 +248,6 @@ def test_deg2rad(self): def test_rad2deg(self): assert abs(go.rad2deg(math.pi) - 180.0) < tol - def test_atan2(self): - assert go.atan2(0.0, 0.0) == 0.0 - assert go.atan2(-0.0, 0.0) == 0.0 - assert go.atan2(0.0, -0.0) == 0.0 - assert go.atan2(-0.0, -0.0) == 0.0 - assert go.atan2(1, 2) == math.atan2(1, 2) - - def test_q_prod(self): - q1 = [0.9069661433330367, -0.17345092325178477, -0.3823030778615049, -0.03422789400943274] - q2 = [0.9238795325112867, 0.0, -0.3826834323650898, 0.0] - q = go.q_prod(q1, q2) - assert is_vector_equal(q, [0.6916264024663118, -0.1733462058496682, -0.7002829056219277, 0.03475434394060616]) - assert abs(q[0] ** 2 + q[1] ** 2 + q[2] ** 2 + q[3] ** 2 - 1.0) < tol - - def test_q_rotation(self): - q2 = [0.9238795325112867, 0.0, -0.3826834323650898, 0.0] - v = go.q_rotation([1, 0, 0], q2) - assert is_vector_equal(v, [0.7071067811865475, 0.0, 0.7071067811865476]) - - def test_q_rotation_inv(self): - q2 = [0.9238795325112867, 0.0, -0.3826834323650898, 0.0] - v = go.q_rotation_inv([1, 0, 0], q2) - assert is_vector_equal(v, [0.7071067811865475, 0.0, -0.7071067811865476]) - def test_get_polygon_centroid(self): p1 = [1, 1, 1] p2 = [1, -1, 1] @@ -625,3 +504,27 @@ def test_wg(self): assert len(wg_calc.get_waveguide_dimensions("WR-75", "in")) == 3 for f in range(1, 200): assert isinstance(wg_calc.find_waveguide(f), str) + + def test_is_vector_equal(self): + # Test with identical vectors + v1 = [1.0, 2.0, 3.0] + v2 = [1.0, 2.0, 3.0] + assert go.is_vector_equal(v1, v2) + + # Test with vectors differing within tolerance + tol = 1e-10 + v3 = [1.0, 2.0 + 1e-11, 3.0] + assert go.is_vector_equal(v1, v3, tolerance=tol) + + # Test with vectors differing beyond tolerance + v4 = [1.0, 2.1, 3.0] + assert not go.is_vector_equal(v1, v4) + + # Test with zero vectors + v5 = [0.0, 0.0, 0.0] + v6 = [0.0, 0.0, 0.0] + assert go.is_vector_equal(v5, v6) + + # Test with vectors of different lengths (should return False) + v7 = [1.0, 2.0] + assert not go.is_vector_equal(v1, v7) diff --git a/tests/system/general/test_02_3D_modeler.py b/tests/system/general/test_02_3D_modeler.py index 75b22ce2c32..2b6584cec33 100644 --- a/tests/system/general/test_02_3D_modeler.py +++ b/tests/system/general/test_02_3D_modeler.py @@ -23,13 +23,16 @@ # SOFTWARE. import secrets -from sys import float_info +from ansys.aedt.core.generic.math_utils import MathUtils from ansys.aedt.core.generic.numbers import decompose_variable_value +from ansys.aedt.core.generic.quaternion import Quaternion from ansys.aedt.core.modeler.cad.elements_3d import FacePrimitive from ansys.aedt.core.modeler.cad.modeler import FaceCoordinateSystem from ansys.aedt.core.modeler.cad.object_3d import Object3d +from ansys.aedt.core.modeler.cad.primitives import CoordinateSystem as cs from ansys.aedt.core.modeler.cad.primitives import PolylineSegment +from ansys.aedt.core.modeler.geometry_operators import GeometryOperators as go import pytest from tests.system.general.conftest import config @@ -40,7 +43,7 @@ else: test_project_name = "Coax_HFSS_t02" -small_number = float_info.epsilon * 10 +small_number = MathUtils.EPSILON secure_random = secrets.SystemRandom() @@ -429,7 +432,7 @@ def test_39_create_coaxial(self): def test_40_create_coordinate_system(self): cs = self.aedtapp.modeler.create_coordinate_system() - self.aedtapp.modeler.coordinate_systems + _ = self.aedtapp.modeler.coordinate_systems cs1 = self.aedtapp.modeler.create_coordinate_system() assert cs assert cs.update() @@ -450,6 +453,7 @@ def test_40_create_coordinate_system(self): cs6 = self.aedtapp.modeler.create_coordinate_system(reference_cs=cs4.name) assert cs4.delete() assert len(self.aedtapp.modeler.coordinate_systems) == 1 + assert self.aedtapp.modeler.coordinate_systems[0].name == cs5.name assert cs5.delete() def test_40a_create_face_coordinate_system(self): @@ -754,7 +758,7 @@ def test_48_coordinate_systems_parametric(self): cs3 = self.aedtapp.modeler.create_coordinate_system(name="CSP3", mode="zxz", phi="var3", theta="var4", psi=0) cs4 = self.aedtapp.modeler.create_coordinate_system(name="CSP4", mode="zxz", phi=43, theta="126deg", psi=0) tol = 1e-9 - assert sum([abs(x1 - x2) for (x1, x2) in zip(cs3.quaternion, cs4.quaternion)]) < tol + assert cs3.quaternion == cs4.quaternion def test_49_sweep_along_path(self): self.aedtapp.modeler.set_working_coordinate_system("Global") @@ -912,10 +916,12 @@ def test_56_global_to_cs(self): p1 = self.aedtapp.modeler.global_to_cs([0, 0, 0], "CS_Test1") p2 = self.aedtapp.modeler.global_to_cs([0, 0, 0], "CS_Test2") p3 = self.aedtapp.modeler.global_to_cs([0, 0, 0], cs2) - assert all(abs(p1[i] - s) < small_number for i, s in enumerate([-2.5455844122716, 1.1313708498985, 1.0])) - assert all(abs(p2[i] - s) < small_number for i, s in enumerate([2.2260086876588, -1.8068578500310, 9.0])) - assert p2 == p3 - assert self.aedtapp.modeler.global_to_cs([0, 0, 0], "Global") == [0, 0, 0] + assert go.is_vector_equal(p1, [-2.5455844122716, 1.1313708498985, 1.0], tolerance=1e-12) + assert go.is_vector_equal(p2, [2.2260086876588, -1.8068578500310, 9.0], tolerance=1e-12) + assert go.is_vector_equal(p2, p3, tolerance=1e-12) + origin = [0, 0, 0] + p4 = self.aedtapp.modeler.global_to_cs([0, 0, 0], "Global") + assert go.is_vector_equal(p4, origin, tolerance=1e-12) def test_57_duplicate_coordinate_system_to_global(self): self.aedtapp.modeler.create_coordinate_system( @@ -934,10 +940,8 @@ def test_57_duplicate_coordinate_system_to_global(self): assert self.aedtapp.modeler.duplicate_coordinate_system_to_global("CS_Test4") assert self.aedtapp.modeler.duplicate_coordinate_system_to_global(cs4) o, q = self.aedtapp.modeler.reference_cs_to_global("CS_Test4") - assert all(abs(o[i] - s) < 10 * small_number for i, s in enumerate([1.82842712474619, 2.20832611206852, 9.0])) - assert all( - abs(q[i] - s) < 10 * small_number for i, s in enumerate([-0.0, -0.09853761796664, 0.99513332666807, 0.0]) - ) + assert go.is_vector_equal(o, [1.82842712474619, 2.20832611206852, 9.0], tolerance=1e-12) + assert q == Quaternion(-0.0, -0.09853761796664, 0.99513332666807, 0.0) assert self.aedtapp.modeler.reference_cs_to_global(cs4) box = self.aedtapp.modeler.create_box([0, 0, 0], [2, 2, 2]) face = box.faces[0] @@ -1056,13 +1060,11 @@ def test_58_invert_cs(self): y_pointing=[-0.55470019622523, 0.83205029433784, 0], ) o, q = self.aedtapp.modeler.invert_cs("CS_Test6", to_global=False) - res = o + q - sol = [3.716491314709036, -4.160251471689218, 8.0, 0.9570920264890529, -0.0, -0.0, -0.28978414868843005] - assert all(abs(res[i] - sol[i]) < 10 * small_number for i in range(3)) + assert go.is_vector_equal(o, [3.716491314709036, -4.160251471689218, 8.0], tolerance=1e-12) + assert q == Quaternion(0.9570920264890529, -0.0, -0.0, -0.28978414868843005) o, q = self.aedtapp.modeler.invert_cs("CS_Test6", to_global=True) - res = o + q - sol = [2.2260086876588385, -1.8068578500310104, 9.0, 0, 0.09853761796664223, -0.9951333266680702, 0] - assert all(abs(res[i] - sol[i]) < 10 * small_number for i in range(3)) + assert go.is_vector_equal(o, [2.2260086876588385, -1.8068578500310104, 9.0], tolerance=1e-12) + assert q == Quaternion(0, 0.09853761796664223, -0.9951333266680702, 0) assert self.aedtapp.modeler.invert_cs(cs6, to_global=True) def test_59a_region_property(self): @@ -1224,3 +1226,9 @@ def test_clean_objects_name(self): def test_64_id(self): box_0 = self.aedtapp.modeler.create_box([0, 0, 0], [10, 10, 10], name="Object_Part_ids") assert self.aedtapp.modeler[box_0.faces[0].id].name == box_0.name + + def test_pointing_to_axis(self): + x, y, z = cs.pointing_to_axis([1, 0.1, 1], [0.5, 1, 0]) + assert go.is_vector_equal(x, [0.7053456158585983, 0.07053456158585983, 0.7053456158585983]) + assert go.is_vector_equal(y, [0.19470872568244801, 0.9374864569895649, -0.28845737138140465]) + assert go.is_vector_equal(z, [-0.681598176590997, 0.3407990882954985, 0.6475182677614472]) diff --git a/tests/unit/test_math_utils.py b/tests/unit/test_math_utils.py new file mode 100644 index 00000000000..d48eb6162d4 --- /dev/null +++ b/tests/unit/test_math_utils.py @@ -0,0 +1,69 @@ +# -*- coding: utf-8 -*- +# +# Copyright (C) 2021 - 2025 ANSYS, Inc. and/or its affiliates. +# SPDX-License-Identifier: MIT +# +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. + +import math + +from ansys.aedt.core.generic.math_utils import MathUtils +import pytest + + +@pytest.fixture(scope="module", autouse=True) +def desktop(): + """Override the desktop fixture to DO NOT open the Desktop when running this test class""" + return + + +class TestMathUtils: + def test_is_zero(self): + assert MathUtils.is_zero(0.0) is True + assert MathUtils.is_zero(1e-15) is True + assert MathUtils.is_zero(1e-5) is False + + def test_is_close(self): + assert MathUtils.is_close(1.0, 1.0 + 1e-10) is True + assert MathUtils.is_close(1.0, 1.1) is False + assert MathUtils.is_close(1.0, 1.0, relative_tolerance=0.0, absolute_tolerance=0.1) is True + + def test_is_equal(self): + assert MathUtils.is_equal(1.0, 1.0 + 1e-15) is True + assert MathUtils.is_equal(1.0, 1.1) is False + + def test_atan2(self): + assert MathUtils.atan2(0.0, 0.0) == 0.0 + assert MathUtils.atan2(-0.0, 0.0) == 0.0 + assert MathUtils.atan2(0.0, -0.0) == 0.0 + assert MathUtils.atan2(-0.0, -0.0) == 0.0 + assert MathUtils.atan2(1, 2) == math.atan2(1, 2) + + def test_is_scalar_number(self): + assert MathUtils.is_scalar_number(1) is True + assert MathUtils.is_scalar_number(1.0) is True + assert MathUtils.is_scalar_number("string") is False + assert MathUtils.is_scalar_number([1, 2, 3]) is False + + def test_fix_negative_zero(self): + assert MathUtils.fix_negative_zero(-0.0) == 0.0 + assert MathUtils.fix_negative_zero(0.0) == 0.0 + assert MathUtils.fix_negative_zero([-0.0, 0.0, -1.0]) == [0.0, 0.0, -1.0] + assert MathUtils.fix_negative_zero([[-0.0], [0.0, -1.0]]) == [[0.0], [0.0, -1.0]] diff --git a/tests/unit/test_quaternion.py b/tests/unit/test_quaternion.py new file mode 100644 index 00000000000..b1e1d56cea1 --- /dev/null +++ b/tests/unit/test_quaternion.py @@ -0,0 +1,1105 @@ +# -*- coding: utf-8 -*- +# +# Copyright (C) 2021 - 2025 ANSYS, Inc. and/or its affiliates. +# SPDX-License-Identifier: MIT +# +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. + +import math + +from ansys.aedt.core.generic.math_utils import MathUtils +from ansys.aedt.core.generic.quaternion import Quaternion +from ansys.aedt.core.modeler.cad.primitives import CoordinateSystem as cs +from ansys.aedt.core.modeler.geometry_operators import GeometryOperators +import pytest + +tol = MathUtils.EPSILON +is_vector_equal = GeometryOperators.is_vector_equal + + +@pytest.fixture(scope="module", autouse=True) +def desktop(): + """Override the desktop fixture to DO NOT open the Desktop when running this test class""" + return + + +class TestQuaternion: + def test_initialization(self): + q = Quaternion(1, 2, 3, 4) + assert q.a == 1 + assert q.b == 2 + assert q.c == 3 + assert q.d == 4 + + def test_initialization_default(self): + q_default = Quaternion() + assert q_default.a == 0.0 + assert q_default.b == 0.0 + assert q_default.c == 0.0 + assert q_default.d == 0.0 + + def test_initialization_invalid(self): + with pytest.raises(TypeError): + Quaternion("a", 2.0, 3.0, 4.0) + + def test_to_quaternion_with_quaternion(self): + # Test with an existing Quaternion object + q = Quaternion(1, 2, 3, 4) + result = Quaternion._to_quaternion(q) + assert result.a == 1 + assert result.b == 2 + assert result.c == 3 + assert result.d == 4 + + def test_to_quaternion_with_tuple(self): + # Test with a tuple + q_tuple = (5, 6, 7, 8) + result = Quaternion._to_quaternion(q_tuple) + assert result.a == 5 + assert result.b == 6 + assert result.c == 7 + assert result.d == 8 + + def test_to_quaternion_with_list(self): + # Test with a list + q_list = [9, 10, 11, 12] + result = Quaternion._to_quaternion(q_list) + assert result.a == 9 + assert result.b == 10 + assert result.c == 11 + assert result.d == 12 + + def test_to_quaternion_with_invalid_type(self): + # Test with an invalid type + with pytest.raises(TypeError, match="Cannot convert .* to Quaternion."): + Quaternion._to_quaternion("invalid_input") + + def test_to_quaternion_with_invalid_length(self): + # Test with a tuple of invalid length + with pytest.raises(TypeError, match="Cannot convert .* to Quaternion."): + Quaternion._to_quaternion((1, 2, 3)) + + def test_valid_sequences(self): + # Test valid rotation sequences + assert Quaternion._is_valid_rotation_sequence("xyz") is True + assert Quaternion._is_valid_rotation_sequence("ZYX") is True + assert Quaternion._is_valid_rotation_sequence("xYz") is True + + def test_invalid_sequences_length(self): + # Test invalid sequences with incorrect length + assert Quaternion._is_valid_rotation_sequence("xy") is False + assert Quaternion._is_valid_rotation_sequence("xyzz") is False + + def test_invalid_sequences_characters(self): + # Test invalid sequences with unsupported characters + assert Quaternion._is_valid_rotation_sequence("abc") is False + assert Quaternion._is_valid_rotation_sequence("123") is False + assert Quaternion._is_valid_rotation_sequence("x1z") is False + + def test_invalid_type(self): + # Test invalid input types + assert Quaternion._is_valid_rotation_sequence(123) is False + assert Quaternion._is_valid_rotation_sequence(None) is False + assert Quaternion._is_valid_rotation_sequence(["x", "y", "z"]) is False + + def test_add_two_quaternions(self): + # Test addition of two quaternions + q1 = Quaternion(1, 2, 3, 4) + q2 = Quaternion(5, 6, 7, 8) + result = q1.add(q2) + assert result.a == 6 + assert result.b == 8 + assert result.c == 10 + assert result.d == 12 + + def test_add_quaternion_and_scalar(self): + # Test addition of a quaternion and a scalar + q = Quaternion(1, 2, 3, 4) + scalar = 5 + result = q.add(scalar) + assert result.a == 6 + assert result.b == 2 + assert result.c == 3 + assert result.d == 4 + + def test_add_scalar_and_quaternion(self): + # Test addition of a scalar and a quaternion + scalar = 3 + q = Quaternion(1, 2, 3, 4) + result = q + scalar + assert result.a == 4 + assert result.b == 2 + assert result.c == 3 + assert result.d == 4 + + def test_add_quaternion_and_list(self): + # Test addition of a quaternion and a list + q = Quaternion(1, 2, 3, 4) + other = [5, 6, 7, 8] + result = q.add(other) + assert result.a == 6 + assert result.b == 8 + assert result.c == 10 + assert result.d == 12 + + def test_add_quaternion_and_tuple(self): + # Test addition of a quaternion and a tuple + q = Quaternion(1, 2, 3, 4) + other = (5, 6, 7, 8) + result = q.add(other) + assert result.a == 6 + assert result.b == 8 + assert result.c == 10 + assert result.d == 12 + + def test_addition(self): + q1 = Quaternion(1, 2, 3, 4) + q2 = Quaternion(5, 6, 7, 8) + q3 = q1 + q2 + assert q3.a == 6 + assert q3.b == 8 + assert q3.c == 10 + assert q3.d == 12 + q4 = q1 + 5 + assert q4.a == 6 + assert q4.b == 2 + assert q4.c == 3 + assert q4.d == 4 + q5 = 5 + q1 + assert q5.a == 6 + assert q5.b == 2 + assert q5.c == 3 + assert q5.d == 4 + + def test_subtract_two_quaternions(self): + # Test subtraction of two quaternions + q1 = Quaternion(1, 2, 3, 4) + q2 = Quaternion(5, 6, 7, 8) + result = q1 - q2 + assert result.a == -4 + assert result.b == -4 + assert result.c == -4 + assert result.d == -4 + + def test_negative(self): + # Test negation of a quaternion + q = Quaternion(1, 2, 3, 4) + result = -q + assert result.a == -1 + assert result.b == -2 + assert result.c == -3 + assert result.d == -4 + + def test_hamilton_prod_general(self): + # Test Hamilton product of two general quaternions + q1 = Quaternion(1, 2, 3, 4) + q2 = Quaternion(5, 6, 7, 8) + result = Quaternion.hamilton_prod(q1, q2) + assert result.a == -60 + assert result.b == 12 + assert result.c == 30 + assert result.d == 24 + + def test_hamilton_prod_with_identity(self): + # Test Hamilton product with the identity quaternion + q1 = Quaternion(1, 2, 3, 4) + identity = Quaternion(1, 0, 0, 0) + result = Quaternion.hamilton_prod(q1, identity) + assert result.a == q1.a + assert result.b == q1.b + assert result.c == q1.c + assert result.d == q1.d + + def test_hamilton_prod_with_zero(self): + # Test Hamilton product with a zero quaternion + q1 = Quaternion(1, 2, 3, 4) + zero = Quaternion(0, 0, 0, 0) + result = Quaternion.hamilton_prod(q1, zero) + assert result.a == 0 + assert result.b == 0 + assert result.c == 0 + assert result.d == 0 + + def test_hamilton_prod_commutativity(self): + # Test non-commutativity of Hamilton product + q1 = Quaternion(1, 2, 3, 4) + q2 = Quaternion(5, 6, 7, 8) + result1 = Quaternion.hamilton_prod(q1, q2) + result2 = Quaternion.hamilton_prod(q2, q1) + assert result1 != result2 + + def test_q_prod_two_quaternions(self): + # Test multiplication of two quaternions + q1 = Quaternion(1, 2, 3, 4) + q2 = Quaternion(5, 6, 7, 8) + result = Quaternion._q_prod(q1, q2) + assert result.a == -60 + assert result.b == 12 + assert result.c == 30 + assert result.d == 24 + + def test_q_prod_quaternion_and_scalar(self): + # Test multiplication of a quaternion and a scalar + q = Quaternion(1, 2, 3, 4) + scalar = 2 + result = Quaternion._q_prod(q, scalar) + assert result.a == 2 + assert result.b == 4 + assert result.c == 6 + assert result.d == 8 + + def test_q_prod_scalar_and_quaternion(self): + # Test multiplication of a scalar and a quaternion + scalar = 3 + q = Quaternion(1, 2, 3, 4) + result = Quaternion._q_prod(scalar, q) + assert result.a == 3 + assert result.b == 6 + assert result.c == 9 + assert result.d == 12 + + def test_q_prod_two_scalars(self): + # Test multiplication of two scalars + scalar1 = 3 + scalar2 = 4 + result = Quaternion._q_prod(scalar1, scalar2) + assert result == 12 + + def test_multiplication(self): + q1 = Quaternion(1, 2, 3, 4) + q2 = Quaternion(5, 6, 7, 8) + q3 = q1 * q2 + assert q3.a == -60 + assert q3.b == 12 + assert q3.c == 30 + assert q3.d == 24 + q4 = q1 * 5 + assert q4.a == 5 + assert q4.b == 10 + assert q4.c == 15 + assert q4.d == 20 + q5 = 5 * q1 + assert q5.a == 5 + assert q5.b == 10 + assert q5.c == 15 + assert q5.d == 20 + + def test_mul(self): + q1 = Quaternion(1, 2, 3, 4) + q2 = Quaternion(5, 6, 7, 8) + q3 = q1.mul(q2) + assert q3.a == -60 + assert q3.b == 12 + assert q3.c == 30 + assert q3.d == 24 + q4 = q1.mul(5) + assert q4.a == 5 + assert q4.b == 10 + assert q4.c == 15 + assert q4.d == 20 + + def test_conjugate(self): + # Test conjugate of a general quaternion + q = Quaternion(1, 2, 3, 4) + qc = q.conjugate() + assert qc.a == 1 + assert qc.b == -2 + assert qc.c == -3 + assert qc.d == -4 + + def test_conjugate_unit_quaternion(self): + # Test conjugate of a unit quaternion + q = Quaternion(1, 0, 0, 0) + qc = q.conjugate() + assert qc.a == 1 + assert qc.b == 0 + assert qc.c == 0 + assert qc.d == 0 + + def test_conjugate_zero_quaternion(self): + # Test conjugate of a zero quaternion + q = Quaternion(0, 0, 0, 0) + qc = q.conjugate() + assert qc.a == 0 + assert qc.b == 0 + assert qc.c == 0 + assert qc.d == 0 + + def test_norm(self): + # Test norm of a general quaternion + q = Quaternion(1, 2, 3, 4) + expected_norm = math.sqrt(1**2 + 2**2 + 3**2 + 4**2) + assert MathUtils.is_close(q.norm(), expected_norm) + + def test_norm_unit_quaternion(self): + # Test norm of a unit quaternion + q = Quaternion(1 / math.sqrt(2), 0, 1 / math.sqrt(2), 0) + assert MathUtils.is_close(q.norm(), 1.0) + + def test_norm_zero_quaternion(self): + # Test norm of a zero quaternion + q = Quaternion(0, 0, 0, 0) + assert MathUtils.is_close(q.norm(), 0.0) + + def test_normalize(self): + # Test normalization of a non-zero quaternion + q = Quaternion(1, 2, 3, 4) + qn = q.normalize() + norm = math.sqrt(1**2 + 2**2 + 3**2 + 4**2) + assert MathUtils.is_close(qn.a, 1 / norm) + assert MathUtils.is_close(qn.b, 2 / norm) + assert MathUtils.is_close(qn.c, 3 / norm) + assert MathUtils.is_close(qn.d, 4 / norm) + assert MathUtils.is_close(qn.norm(), 1.0) + + def test_normalize_unit_quaternion(self): + # Test normalization of a unit quaternion (should remain unchanged) + q = Quaternion(1 / math.sqrt(2), 0, 1 / math.sqrt(2), 0) + qn = q.normalize() + assert MathUtils.is_close(qn.a, q.a) + assert MathUtils.is_close(qn.b, q.b) + assert MathUtils.is_close(qn.c, q.c) + assert MathUtils.is_close(qn.d, q.d) + assert MathUtils.is_close(qn.norm(), 1.0) + + def test_normalize_zero_norm(self): + # Test normalization of a zero-norm quaternion + q = Quaternion(0, 0, 0, 0) + with pytest.raises(ValueError, match="Cannot normalize a quaternion with zero norm."): + q.normalize() + + def test_inverse(self): + # Test inverse of a non-zero quaternion + q = Quaternion(1, 2, 3, 4) + qi = q.inverse() + q_identity = q * qi + assert MathUtils.is_close(qi.a, 1.0 / 30) + assert MathUtils.is_close(qi.b, -1.0 / 15) + assert MathUtils.is_close(qi.c, -1.0 / 10) + assert MathUtils.is_close(qi.d, -1.0 / 7.5) + assert MathUtils.is_close(q_identity.a, 1.0) + assert MathUtils.is_close(q_identity.b, 0.0) + assert MathUtils.is_close(q_identity.c, 0.0) + assert MathUtils.is_close(q_identity.d, 0.0) + + def test_inverse_unit_quaternion(self): + # Test inverse of a unit quaternion + q = Quaternion(1, 0, 0, 0) + qi = q.inverse() + assert qi.a == 1 + assert qi.b == 0 + assert qi.c == 0 + assert qi.d == 0 + + def test_inverse_zero_norm(self): + # Test inverse of a zero-norm quaternion + q = Quaternion(0, 0, 0, 0) + with pytest.raises(ValueError, match="Cannot compute inverse for a quaternion with zero norm"): + q.inverse() + + def test_division(self): + q1 = Quaternion(1, 2, 3, 4) + q2 = Quaternion(1, -1, 1, 2) + q3 = q1 / q2 + assert MathUtils.is_close(q3.a, 10 / 7) + assert MathUtils.is_close(q3.b, 1 / 7) + assert MathUtils.is_close(q3.c, 10 / 7) + assert MathUtils.is_close(q3.d, -3 / 7) + q4 = q1 / 5 + assert MathUtils.is_close(q4.a, 0.2) + assert MathUtils.is_close(q4.b, 0.4) + assert MathUtils.is_close(q4.c, 0.6) + assert MathUtils.is_close(q4.d, 0.8) + q5 = 5 / q1 + assert MathUtils.is_close(q5.a, 1 / 6) + assert MathUtils.is_close(q5.b, -1 / 3) + assert MathUtils.is_close(q5.c, -0.5) + assert MathUtils.is_close(q5.d, -2 / 3) + + def test_div(self): + q1 = Quaternion(1, 2, 3, 4) + q2 = Quaternion(1, -1, 1, 2) + q3 = q1.div(q2) + assert MathUtils.is_close(q3.a, 10 / 7) + assert MathUtils.is_close(q3.b, 1 / 7) + assert MathUtils.is_close(q3.c, 10 / 7) + assert MathUtils.is_close(q3.d, -3 / 7) + q4 = q1.div(5) + assert MathUtils.is_close(q4.a, 0.2) + assert MathUtils.is_close(q4.b, 0.4) + assert MathUtils.is_close(q4.c, 0.6) + assert MathUtils.is_close(q4.d, 0.8) + + def test_division_by_scalar(self): + q = Quaternion(1, 2, 3, 4) + result = Quaternion._q_div(q, 2) + assert result.a == 0.5 + assert result.b == 1.0 + assert result.c == 1.5 + assert result.d == 2.0 + + def test_scalar_division_by_quaternion(self): + q = Quaternion(1, 2, 3, 4) + result = Quaternion._q_div(2, q) + assert pytest.approx(result.a) == 1 / 15 + assert pytest.approx(result.b) == -2 / 15 + assert pytest.approx(result.c) == -1 / 5 + assert pytest.approx(result.d) == -4 / 15 + + def test_quaternion_division(self): + q1 = Quaternion(1, 2, 3, 4) + q2 = Quaternion(1, -1, 1, 2) + result = Quaternion._q_div(q1, q2) + assert pytest.approx(result.a) == 10 / 7 + assert pytest.approx(result.b) == 1 / 7 + assert pytest.approx(result.c) == 10 / 7 + assert pytest.approx(result.d) == -3 / 7 + + def test_division_by_zero_scalar(self): + q = Quaternion(1, 2, 3, 4) + with pytest.raises(ZeroDivisionError): + Quaternion._q_div(q, 0) + + def test_division_by_zero_quaternion(self): + q1 = Quaternion(1, 2, 3, 4) + q2 = Quaternion(0, 0, 0, 0) + with pytest.raises(ValueError, match="Cannot compute inverse for a quaternion with zero norm"): + Quaternion._q_div(q1, q2) + + def test_division_two_scalar(self): + result = Quaternion._q_div(10, 7) + assert pytest.approx(result) == 10 / 7 + + def test_eq_method(self): + # Test equality of two identical quaternions + q1 = Quaternion(1.0, 2.0, 3.0, 4.0) + q2 = Quaternion(1.0, 2.0, 3.0, 4.0) + assert q1 == q2 + + # Test inequality of two different quaternions + q3 = Quaternion(1.0, 2.0, 3.0, 5.0) + assert q1 != q3 + + # Test equality with floating-point precision + q4 = Quaternion(1.0 + 1e-15, 2.0, 3.0, 4.0) + assert q1 == q4 + + # Test inequality with a non-Quaternion object + assert q1 != (1.0, 2.0, 3.0, 4.0) + + def test_repr_method(self): + # Test string representation of a quaternion + q = Quaternion(1.0, 2.0, 3.0, 4.0) + expected_repr = "Quaternion(1.0, 2.0, 3.0, 4.0)" + assert repr(q) == expected_repr + + def test_coefficients(self): + # Test with default quaternion + q = Quaternion() + assert q.coefficients() == (0.0, 0.0, 0.0, 0.0) + + # Test with positive coefficients + q = Quaternion(1, 2, 3, 4) + assert q.coefficients() == (1, 2, 3, 4) + + # Test with negative coefficients + q = Quaternion(-1, -2, -3, -4) + assert q.coefficients() == (-1, -2, -3, -4) + + # Test with mixed coefficients + q = Quaternion(1.5, -2.5, 3.5, -4.5) + assert q.coefficients() == (1.5, -2.5, 3.5, -4.5) + + def test_from_euler_valid_xyz(self): + # Test valid Euler angles with 'xyz' sequence + angles = [math.pi / 2, 0, 0] + q = Quaternion.from_euler(angles, "xyz") + assert MathUtils.is_close(q.a, 0.7071067811865476) + assert MathUtils.is_close(q.b, 0.7071067811865476) + assert MathUtils.is_close(q.c, 0.0) + assert MathUtils.is_close(q.d, 0.0) + + def test_from_euler_valid_zyz_extrinsic(self): + # Test valid Euler angles with 'zyz' sequence and extrinsic rotation + angles = [0, math.pi / 2, math.pi] + q = Quaternion.from_euler(angles, "zyz", extrinsic=True) + assert MathUtils.is_close(q.a, 0.0) + assert MathUtils.is_close(q.b, -0.7071067811865476) + assert MathUtils.is_close(q.c, 0.0) + assert MathUtils.is_close(q.d, 0.7071067811865476) + + def test_from_euler_invalid_angles_length(self): + # Test invalid angles length + angles = [math.pi / 2, 0] + with pytest.raises(ValueError, match="Three rotation angles are required."): + Quaternion.from_euler(angles, "xyz") + + def test_from_euler_invalid_sequence(self): + # Test invalid rotation sequence + angles = [math.pi / 2, 0, 0] + with pytest.raises( + ValueError, match="sequence must be a 3-character string, using only the axes 'x', 'y', or 'z'." + ): + Quaternion.from_euler(angles, "abc") + + def test_from_euler_case_insensitivity(self): + # Test case insensitivity of the sequence + angles = [math.pi / 2, 0, 0] + q = Quaternion.from_euler(angles, "XYZ") + assert MathUtils.is_close(q.a, 0.7071067811865476) + assert MathUtils.is_close(q.b, 0.7071067811865476) + assert MathUtils.is_close(q.c, 0.0) + assert MathUtils.is_close(q.d, 0.0) + + def test_to_euler_valid_xyz(self): + # Test valid quaternion to Euler angles with 'xyz' sequence + q = Quaternion(0.7071067811865476, 0.7071067811865476, 0, 0) + angles = q.to_euler("xyz") + assert MathUtils.is_close(angles[0], math.pi / 2) + assert MathUtils.is_close(angles[1], 0) + assert MathUtils.is_close(angles[2], 0) + + def test_to_euler_valid_zyz_extrinsic(self): + # Test valid quaternion to Euler angles with 'zyz' sequence and extrinsic rotation + q = Quaternion(0, -0.7071067811865476, 0, 0.7071067811865476) + angles = q.to_euler("zyz", extrinsic=True) + assert MathUtils.is_close(angles[0], 0) + assert MathUtils.is_close(angles[1], math.pi / 2) + assert MathUtils.is_close(angles[2], math.pi) + + def test_to_euler_invalid_sequence(self): + # Test invalid rotation sequence + q = Quaternion(1, 0, 0, 0) + with pytest.raises( + ValueError, match="sequence must be a 3-character string, using only the axes 'x', 'y', or 'z'." + ): + q.to_euler("abc") + + def test_to_euler_zero_norm(self): + # Test quaternion with zero norm + q = Quaternion(0, 0, 0, 0) + with pytest.raises(ValueError, match="A quaternion with norm 0 cannot be converted."): + q.to_euler("xyz") + + def test_to_euler_case_insensitivity(self): + # Test case insensitivity of the sequence + q = Quaternion(0.7071067811865476, 0.7071067811865476, 0, 0) + angles = q.to_euler("XYZ") + assert MathUtils.is_close(angles[0], math.pi / 2) + assert MathUtils.is_close(angles[1], 0) + assert MathUtils.is_close(angles[2], 0) + + def test_to_euler_case_degenarate1_extrinsic(self): + # Test case insensitivity of the sequence + q = Quaternion(0.7071067811865476, 0.7071067811865476, 0, 0) + angles = q.to_euler("XYZ", extrinsic=True) + assert MathUtils.is_close(angles[0], math.pi / 2) + assert MathUtils.is_close(angles[1], 0) + assert MathUtils.is_close(angles[2], 0) + + def test_to_euler_case_degenarate2_extrinsic(self): + # Test case insensitivity of the sequence + q = Quaternion(0, 0, 1, 1) + angles = q.to_euler("XYZ", extrinsic=True) + assert MathUtils.is_close(angles[0], math.pi / 2) + assert MathUtils.is_close(angles[1], 0) + assert MathUtils.is_close(angles[2], math.pi) + + def test_from_axis_angle_valid(self): + # Test valid axis and angle + axis = (1, 0, 0) + angle = math.pi / 2 + q = Quaternion.from_axis_angle(axis, angle) + assert MathUtils.is_close(q.a, math.sqrt(2) / 2) + assert MathUtils.is_close(q.b, math.sqrt(2) / 2) + assert MathUtils.is_close(q.c, 0.0) + assert MathUtils.is_close(q.d, 0.0) + + def test_from_axis_angle_normalized_axis(self): + # Test with a non-normalized axis + axis = (2, 0, 0) + angle = math.pi / 2 + q = Quaternion.from_axis_angle(axis, angle) + assert MathUtils.is_close(q.a, math.sqrt(2) / 2) + assert MathUtils.is_close(q.b, math.sqrt(2) / 2) + assert MathUtils.is_close(q.c, 0.0) + assert MathUtils.is_close(q.d, 0.0) + + def test_from_axis_angle_zero_angle(self): + # Test with zero angle + axis = (0, 1, 0) + angle = 0 + q = Quaternion.from_axis_angle(axis, angle) + assert MathUtils.is_close(q.a, 1.0) + assert MathUtils.is_close(q.b, 0.0) + assert MathUtils.is_close(q.c, 0.0) + assert MathUtils.is_close(q.d, 0.0) + + def test_from_axis_angle_invalid_axis_length(self): + # Test with an invalid axis length + axis = (1, 0) + angle = math.pi / 2 + with pytest.raises(ValueError, match="axis must be a list or tuple containing 3 floats."): + Quaternion.from_axis_angle(axis, angle) + + def test_from_axis_angle_zero_axis(self): + # Test with a zero vector as axis + axis = (0, 0, 0) + angle = math.pi / 2 + with pytest.raises(ValueError, match="axis must be a non-zero vector."): + Quaternion.from_axis_angle(axis, angle) + + def test_to_axis_angle_valid(self): + # Test valid quaternion to axis and angle + q = Quaternion(math.sqrt(2) / 2, math.sqrt(2) / 2, 0, 0) + axis, angle = q.to_axis_angle() + assert MathUtils.is_close(axis[0], 1.0) + assert MathUtils.is_close(axis[1], 0.0) + assert MathUtils.is_close(axis[2], 0.0) + assert MathUtils.is_close(angle, math.pi / 2) + + def test_to_axis_angle_zero_rotation(self): + # Test quaternion representing zero rotation + q = Quaternion(1, 0, 0, 0) + axis, angle = q.to_axis_angle() + assert MathUtils.is_close(axis[0], 1.0) + assert MathUtils.is_close(axis[1], 0.0) + assert MathUtils.is_close(axis[2], 0.0) + assert MathUtils.is_close(angle, 0.0) + + def test_to_axis_angle_invalid_zero_norm(self): + # Test quaternion with zero norm + q = Quaternion(0, 0, 0, 0) + with pytest.raises(ValueError, match="A quaternion with norm 0 cannot be converted."): + q.to_axis_angle() + + def test_to_axis_angle_negative_w(self): + # Test quaternion with negative scalar part + q = Quaternion(-math.sqrt(2) / 2, math.sqrt(2) / 2, 0, 0) + axis, angle = q.to_axis_angle() + assert MathUtils.is_close(axis[0], -1.0) + assert MathUtils.is_close(axis[1], 0.0) + assert MathUtils.is_close(axis[2], 0.0) + assert MathUtils.is_close(angle, math.pi / 2) + + def test_to_axis_angle_normalized(self): + # Test quaternion normalization before conversion + q = Quaternion(2, 2, 0, 0) + axis, angle = q.to_axis_angle() + assert MathUtils.is_close(axis[0], 1.0) + assert MathUtils.is_close(axis[1], 0.0) + assert MathUtils.is_close(axis[2], 0.0) + assert MathUtils.is_close(angle, math.pi / 2) + + def test_from_rotation_matrix_valid(self): + # Test valid rotation matrix + rotation_matrix = ( + (0.7071067811865476, -0.7071067811865475, 0.0), + (0.7071067811865475, 0.7071067811865476, 0.0), + (0.0, 0.0, 1.0), + ) + q = Quaternion.from_rotation_matrix(rotation_matrix) + assert MathUtils.is_close(q.a, 0.9238795325112867) + assert MathUtils.is_close(q.b, 0.0) + assert MathUtils.is_close(q.c, 0.0) + assert MathUtils.is_close(q.d, 0.3826834323650898) + + def test_from_rotation_matrix_invalid_non_orthogonal(self): + # Test invalid non-orthogonal matrix + # fmt: off + rotation_matrix = ( + (1, 0, 0), + (0, 1, 0), + (0, 0, 2), # Not orthogonal + ) + # fmt: on + with pytest.raises(ValueError, match="The rotation matrix must be orthogonal."): + Quaternion.from_rotation_matrix(rotation_matrix) + + def test_from_rotation_matrix_invalid_size(self): + # Test invalid matrix size + # fmt: off + rotation_matrix = ( + (1, 0), + (0, 1), + ) + # fmt: on + with pytest.raises( + ValueError, match="rotation_matrix must be a 3x3 matrix defined as a list of lists or a tuple of tuples." + ): + Quaternion.from_rotation_matrix(rotation_matrix) + + def test_from_rotation_matrix_identity(self): + # Test identity matrix + # fmt: off + rotation_matrix = ( + (1, 0, 0), + (0, 1, 0), + (0, 0, 1), + ) + # fmt: on + q = Quaternion.from_rotation_matrix(rotation_matrix) + assert MathUtils.is_close(q.a, 1.0) + assert MathUtils.is_close(q.b, 0.0) + assert MathUtils.is_close(q.c, 0.0) + assert MathUtils.is_close(q.d, 0.0) + + def test_from_rotation_matrix_180_degree_rotation(self): + # Test 180-degree rotation about the Z-axis + # fmt: off + rotation_matrix = ( + (-1, 0, 0), + (0, -1, 0), + (0, 0, 1), + ) + # fmt: on + q = Quaternion.from_rotation_matrix(rotation_matrix) + assert MathUtils.is_close(q.a, 0.0) + assert MathUtils.is_close(q.b, 0.0) + assert MathUtils.is_close(q.c, 0.0) + assert MathUtils.is_close(q.d, 1.0) + + def test_to_rotation_matrix_valid(self): + # Test valid quaternion to rotation matrix + q = Quaternion(0.9238795325112867, 0.0, 0.0, 0.3826834323650898) + rotation_matrix = q.to_rotation_matrix() + expected_matrix = ( + (0.7071067811865476, -0.7071067811865475, 0.0), + (0.7071067811865475, 0.7071067811865476, 0.0), + (0.0, 0.0, 1.0), + ) + for i in range(3): + for j in range(3): + assert MathUtils.is_close(rotation_matrix[i][j], expected_matrix[i][j]) + + def test_to_rotation_matrix_zero_norm(self): + # Test quaternion with zero norm + q = Quaternion(0, 0, 0, 0) + with pytest.raises(ValueError, match="A quaternion with norm 0 cannot be converted."): + q.to_rotation_matrix() + + def test_to_rotation_matrix_normalized(self): + # Test quaternion normalization before conversion + q = Quaternion(2, 0, 0, 2) + rotation_matrix = q.to_rotation_matrix() + # fmt: off + expected_matrix = ( + (0.0, -1.0, 0.0), + (1.0, 0.0, 0.0), + (0.0, 0.0, 1.0), + ) + # fmt: on + for i in range(3): + GeometryOperators.is_vector_equal(rotation_matrix[i], expected_matrix[i]) + + def test_to_rotation_matrix_identity(self): + # Test identity quaternion + q = Quaternion(1, 0, 0, 0) + rotation_matrix = q.to_rotation_matrix() + # fmt: off + expected_matrix = ( + (1.0, 0.0, 0.0), + (0.0, 1.0, 0.0), + (0.0, 0.0, 1.0), + ) + # fmt: on + for i in range(3): + GeometryOperators.is_vector_equal(rotation_matrix[i], expected_matrix[i]) + + def test_to_rotation_matrix_negative_w(self): + # Test quaternion with negative scalar part + q = Quaternion(-0.9238795325112867, 0.0, 0.0, -0.3826834323650898) + rotation_matrix = q.to_rotation_matrix() + expected_matrix = ( + (0.7071067811865476, 0.7071067811865475, 0.0), + (-0.7071067811865475, 0.7071067811865476, 0.0), + (0.0, 0.0, 1.0), + ) + for i in range(3): + GeometryOperators.is_vector_equal(rotation_matrix[i], expected_matrix[i]) + + def test_rotation_matrix_to_axis_valid(self): + # Test valid rotation matrix to axes + rotation_matrix = ( + (0.7071067811865476, 0.0, 0.7071067811865476), + (0.0, 1.0, 0.0), + (-0.7071067811865476, 0.0, 0.7071067811865476), + ) + x, y, z = Quaternion.rotation_matrix_to_axis(rotation_matrix) + assert MathUtils.is_close(x[0], 0.7071067811865476) + assert MathUtils.is_close(x[1], 0.0) + assert MathUtils.is_close(x[2], -0.7071067811865476) + assert MathUtils.is_close(y[0], 0.0) + assert MathUtils.is_close(y[1], 1.0) + assert MathUtils.is_close(y[2], 0.0) + assert MathUtils.is_close(z[0], 0.7071067811865476) + assert MathUtils.is_close(z[1], 0.0) + assert MathUtils.is_close(z[2], 0.7071067811865476) + + def test_rotation_matrix_to_axis_invalid(self): + # Test invalid non-orthogonal matrix + # fmt: off + rotation_matrix = ( + (1, 0, 0), + (0, 1, 0), + (0, 0, 2), # Not orthogonal + ) + # fmt: on + with pytest.raises(ValueError, match="The rotation matrix must be orthogonal."): + Quaternion.rotation_matrix_to_axis(rotation_matrix) + + def test_axis_to_rotation_matrix_valid(self): + # Test valid axes to rotation matrix + x_axis = (1, 0, 0) + y_axis = (0, 1, 0) + z_axis = (0, 0, 1) + rotation_matrix = Quaternion.axis_to_rotation_matrix(x_axis, y_axis, z_axis) + # fmt: off + expected_matrix = ( + (1.0, 0.0, 0.0), + (0.0, 1.0, 0.0), + (0.0, 0.0, 1.0), + ) + # fmt: on + for i in range(3): + for j in range(3): + assert MathUtils.is_close(rotation_matrix[i][j], expected_matrix[i][j]) + + def test_axis_to_rotation_matrix_invalid(self): + # Test invalid axes (not orthonormal) + x_axis = (1, 0, 0) + y_axis = (1, 1, 0) # Not orthogonal to x_axis + z_axis = (0, 0, 1) + with pytest.raises(ValueError, match="The provided axes must form an orthonormal basis."): + Quaternion.axis_to_rotation_matrix(x_axis, y_axis, z_axis) + + def test_rotate_vector_valid(self): + # Test rotating a vector using a valid quaternion + q = Quaternion(0.9238795325112867, 0.0, -0.3826834323650898, 0.0) # 45-degree rotation about Y-axis + v = (1, 0, 0) # Vector along X-axis + rotated_v = q.rotate_vector(v) + assert MathUtils.is_close(rotated_v[0], 0.7071067811865475) + assert MathUtils.is_close(rotated_v[1], 0.0) + assert MathUtils.is_close(rotated_v[2], 0.7071067811865476) + + def test_rotate_vector_zero_quaternion(self): + # Test rotating a vector with a zero quaternion + q = Quaternion(0, 0, 0, 0) + v = (1, 0, 0) + with pytest.raises(ValueError, match="Cannot normalize a quaternion with zero norm."): + q.rotate_vector(v) + + def test_rotate_vector_identity_quaternion(self): + # Test rotating a vector with the identity quaternion + q = Quaternion(1, 0, 0, 0) # Identity quaternion + v = (1, 2, 3) # Arbitrary vector + rotated_v = q.rotate_vector(v) + assert MathUtils.is_close(rotated_v[0], 1.0) + assert MathUtils.is_close(rotated_v[1], 2.0) + assert MathUtils.is_close(rotated_v[2], 3.0) + + def test_rotate_vector_180_degree_rotation(self): + # Test 180-degree rotation about Z-axis + q = Quaternion(0, 0, 0, 1) # 180-degree rotation about Z-axis + v = (1, 0, 0) # Vector along X-axis + rotated_v = q.rotate_vector(v) + assert MathUtils.is_close(rotated_v[0], -1.0) + assert MathUtils.is_close(rotated_v[1], 0.0) + assert MathUtils.is_close(rotated_v[2], 0.0) + + def test_rotate_vector_normalized_quaternion(self): + # Test rotating a vector with a non-normalized quaternion + q = Quaternion(2, 0, 0, 2) # Non-normalized quaternion + v = (0, 1, 0) # Vector along Y-axis + rotated_v = q.rotate_vector(v) + assert MathUtils.is_close(rotated_v[0], -1.0) + assert MathUtils.is_close(rotated_v[1], 0.0) + assert MathUtils.is_close(rotated_v[2], 0.0) + + def test_inverse_rotate_vector_valid(self): + # Test inverse rotation of a vector using a valid quaternion + q = Quaternion(0.9238795325112867, 0.0, -0.3826834323650898, 0.0) # 45-degree rotation about Y-axis + v = (0.7071067811865475, 0.0, 0.7071067811865476) # Rotated vector + original_v = q.inverse_rotate_vector(v) + assert MathUtils.is_close(original_v[0], 1.0) + assert MathUtils.is_close(original_v[1], 0.0) + assert MathUtils.is_close(original_v[2], 0.0) + + def test_inverse_rotate_vector_zero_quaternion(self): + # Test inverse rotation with a zero quaternion + q = Quaternion(0, 0, 0, 0) + v = (1, 0, 0) + with pytest.raises(ValueError, match="Cannot normalize a quaternion with zero norm."): + q.inverse_rotate_vector(v) + + def test_inverse_rotate_vector_identity_quaternion(self): + # Test inverse rotation with the identity quaternion + q = Quaternion(1, 0, 0, 0) # Identity quaternion + v = (1, 2, 3) # Arbitrary vector + original_v = q.inverse_rotate_vector(v) + assert MathUtils.is_close(original_v[0], 1.0) + assert MathUtils.is_close(original_v[1], 2.0) + assert MathUtils.is_close(original_v[2], 3.0) + + def test_inverse_rotate_vector_180_degree_rotation(self): + # Test 180-degree inverse rotation about Z-axis + q = Quaternion(0, 0, 0, 1) # 180-degree rotation about Z-axis + v = (-1.0, 0.0, 0.0) # Rotated vector + original_v = q.inverse_rotate_vector(v) + assert MathUtils.is_close(original_v[0], 1.0) + assert MathUtils.is_close(original_v[1], 0.0) + assert MathUtils.is_close(original_v[2], 0.0) + + def test_inverse_rotate_vector_normalized_quaternion(self): + # Test inverse rotation with a non-normalized quaternion + q = Quaternion(2, 0, 0, 2) # Non-normalized quaternion + v = (-1, 0, 0) # Vector along Y-axis + original_v = q.inverse_rotate_vector(v) + assert MathUtils.is_close(original_v[0], 0.0) + assert MathUtils.is_close(original_v[1], 1.0) + assert MathUtils.is_close(original_v[2], 0.0) + + # Here we have the old tests from GeometryOperator + + def test_axis_to_euler_zxz(self): + x, y, z = cs.pointing_to_axis([1, 0.1, 1], [0.5, 1, 0]) + m = Quaternion.axis_to_rotation_matrix(x, y, z) + q = Quaternion.from_rotation_matrix(m) + phi, theta, psi = q.to_euler("zxz") + assert abs(phi - (-2.0344439357957027)) < tol + assert abs(theta - 0.8664730673456006) < tol + assert abs(psi - 1.9590019609437583) < tol + x, y, z = cs.pointing_to_axis([-0.2, -0.3, 0], [-0.2, 0.3, 0]) + m = Quaternion.axis_to_rotation_matrix(x, y, z) + q = Quaternion.from_rotation_matrix(m) + phi, theta, psi = q.to_euler("zxz") + assert abs(phi - (4.124386376837122)) < tol + assert abs(theta - 3.141592653589793) < tol + assert abs(psi - 0) < tol + x, y, z = cs.pointing_to_axis([-0.2, -0.5, 0], [-0.1, -0.4, 0]) + m = Quaternion.axis_to_rotation_matrix(x, y, z) + q = Quaternion.from_rotation_matrix(m) + phi, theta, psi = q.to_euler("zxz") + assert abs(phi - (-1.9513027039072615)) < tol + assert abs(theta - 0) < tol + assert abs(psi - 0) < tol + + def test_axis_to_euler_zyz(self): + x, y, z = cs.pointing_to_axis([1, 0.1, 1], [0.5, 1, 0]) + m = Quaternion.axis_to_rotation_matrix(x, y, z) + q = Quaternion.from_rotation_matrix(m) + phi, theta, psi = q.to_euler("zyz") + assert abs(phi - 2.677945044588987) < tol + assert abs(theta - 0.8664730673456006) < tol + assert abs(psi - (-2.7533870194409316)) < tol + x, y, z = cs.pointing_to_axis([-0.2, -0.3, 0], [-0.2, 0.3, 0]) + m = Quaternion.axis_to_rotation_matrix(x, y, z) + q = Quaternion.from_rotation_matrix(m) + phi, theta, psi = q.to_euler("zyz") + assert abs(phi - 0.982793723247329) < tol + assert abs(theta - 3.141592653589793) < tol + assert abs(psi - 0) < tol + x, y, z = cs.pointing_to_axis([-0.2, -0.5, 0], [-0.1, -0.4, 0]) + m = Quaternion.axis_to_rotation_matrix(x, y, z) + q = Quaternion.from_rotation_matrix(m) + phi, theta, psi = q.to_euler("zyz") + assert abs(phi - (-1.9513027039072615)) < tol + assert abs(theta - 0) < tol + assert abs(psi - 0) < tol + + def test_quaternion_to_axis(self): + q = [0.9069661433330367, -0.17345092325178477, -0.3823030778615049, -0.03422789400943274] + m = Quaternion(*q).to_rotation_matrix() + x, y, z = Quaternion.rotation_matrix_to_axis(m) + assert is_vector_equal(x, [0.7053456158585982, 0.07053456158585963, 0.7053456158585982]) + assert is_vector_equal(y, [0.19470872568244832, 0.937486456989565, -0.2884573713814046]) + assert is_vector_equal(z, [-0.681598176590997, 0.34079908829549865, 0.6475182677614472]) + + def test_to_axis_normalized(self): + # Test quaternion normalization before conversion + q = Quaternion(2, 2, 2, 2) + m = q.to_rotation_matrix() + x, y, z = Quaternion.rotation_matrix_to_axis(m) + assert MathUtils.is_close(x[0], 0.0) + assert MathUtils.is_close(x[1], 1.0) + assert MathUtils.is_close(x[2], 0.0) + assert MathUtils.is_close(y[0], 0.0) + assert MathUtils.is_close(y[1], 0.0) + assert MathUtils.is_close(y[2], 1.0) + assert MathUtils.is_close(z[0], 1.0) + assert MathUtils.is_close(z[1], 0.0) + assert MathUtils.is_close(z[2], 0.0) + + def test_quaternion_to_axis_angle(self): + q = [0.9069661433330367, -0.17345092325178477, -0.3823030778615049, -0.03422789400943274] + u, th = Quaternion(*q).to_axis_angle() + assert is_vector_equal(u, [-0.41179835953227295, -0.9076445218972716, -0.0812621249808417]) + assert abs(th - 0.8695437956599169) < tol + + def test_axis_angle_to_quaternion(self): + u = [-0.41179835953227295, -0.9076445218972716, -0.0812621249808417] + th = 0.8695437956599169 + q = Quaternion.from_axis_angle(u, th) + assert is_vector_equal( + q.coefficients(), [0.9069661433330367, -0.17345092325178477, -0.3823030778615049, -0.03422789400943274] + ) + assert abs(q.a**2 + q.b**2 + q.c**2 + q.d**2 - 1.0) < tol + + def test_quaternion_to_euler_zxz(self): + q = [0.9069661433330367, -0.17345092325178477, -0.3823030778615049, -0.03422789400943274] + phi, theta, psi = Quaternion(*q).to_euler("zxz") + assert abs(phi - (-2.0344439357957027)) < tol + assert abs(theta - 0.8664730673456006) < tol + assert abs(psi - 1.9590019609437583) < tol + + def test_euler_zxz_to_quaternion(self): + phi = -2.0344439357957027 + theta = 0.8664730673456006 + psi = 1.9590019609437583 + q = Quaternion.from_euler((phi, theta, psi), "zxz") + assert is_vector_equal( + q.coefficients(), [0.9069661433330367, -0.17345092325178468, -0.38230307786150497, -0.03422789400943264] + ) + assert abs(q.a**2 + q.b**2 + q.c**2 + q.d**2 - 1.0) < tol + + def test_quaternion_to_euler_zyz(self): + q = [0.9069661433330367, -0.17345092325178477, -0.3823030778615049, -0.03422789400943274] + phi, theta, psi = Quaternion(*q).to_euler("zyz") + assert abs(phi - 2.677945044588987) < tol + assert abs(theta - 0.8664730673456006) < tol + assert abs(psi - (-2.7533870194409316)) < tol + + def test_euler_zyz_to_quaternion(self): + phi = 2.677945044588987 + theta = 0.8664730673456006 + psi = -2.7533870194409316 + q = Quaternion.from_euler((phi, theta, psi), "zyz") + assert is_vector_equal( + q.coefficients(), [0.9069661433330367, -0.17345092325178477, -0.3823030778615049, -0.03422789400943274] + ) + assert abs(q.a**2 + q.b**2 + q.c**2 + q.d**2 - 1.0) < tol + + def test_bug_6037(self): + # Test for bug #6037 + q = Quaternion(0.9801685681070907, 0.0, 0.0, 0.19816553205564127) + angles = q.to_euler("zxz") + assert MathUtils.is_close(angles[0], 0.3989719626660737) + assert MathUtils.is_close(angles[1], 0) + assert MathUtils.is_close(angles[2], 0)