diff --git a/.claude/settings.local.json b/.claude/settings.local.json new file mode 100644 index 0000000..f315077 --- /dev/null +++ b/.claude/settings.local.json @@ -0,0 +1,9 @@ +{ + "permissions": { + "allow": [ + "Bash(python:*)" + ], + "deny": [], + "ask": [] + } +} diff --git a/.env b/.env new file mode 100644 index 0000000..5567480 --- /dev/null +++ b/.env @@ -0,0 +1,2 @@ +PYTHON=python3 +VENV=/tmp/pipTest diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..d609a52 --- /dev/null +++ b/.gitignore @@ -0,0 +1,157 @@ +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class + +# Cython and C extensions +*.c +*.so +*.pyd +*.dll + +# Distribution / packaging +.Python +build/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ +lib/ +lib64/ +parts/ +sdist/ +var/ +*.egg-info/ +.installed.cfg +*.egg +MANIFEST + +# PyInstaller +# Usually contains a dist/ folder and build/ artifacts +*.manifest +*.spec + +# PyBuilder +target/ + +# Virtual environments +.env/ +.venv/ +env/ +venv/ +ENV/ +env.bak/ +venv.bak/ + +# pipenv +Pipfile.lock + +# poetry +poetry.lock + +# hatch +*.hatch/ + +# pdm +__pypackages__/ + +# tox and nox +.tox/ +.nox/ + +# pytest +.cache/ +.pytest_cache/ + +# mypy +.mypy_cache/ +.dmypy.json +dmypy.json + +# coverage and test reports +.coverage +.coverage.* +nosetests.xml +coverage.xml +*.cover +.hypothesis/ + +# Pyre type checker +.pyre/ + +# CMake files +CMakeFiles/ +CMakeCache.txt +cmake_install.cmake + + +# Translations +*.mo +*.pot + +# Django stuff +*.log +local_settings.py +db.sqlite3 + +# Flask stuff +instance/ +.webassets-cache + +# Scrapy stuff +.scrapy + +# IPython / Jupyter +.ipynb_checkpoints/ +profile + +# macOS system files +*.DS_Store +.AppleDouble +.LSOverride + +# Thumbnails and metadata +._* + +# Files that might appear in root of a volume +.DocumentRevisions-V100 +.fseventsd +.Spotlight-V100 +.TemporaryItems +.Trashes +.VolumeIcon.icns +.com.apple.timemachine.donotpresent + +# Directories created on remote AFP share +.AppleDB +.AppleDesktop +Network Trash Folder +Temporary Items +.apdisk + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ + +# Sphinx documentation build +docs/_build/ + +# IDEs and editors +# IntelliJ IDEA family +.idea/ +*.iml +*.ipr +*.iws +.idea_modules/ + +# Visual Studio Code +.vscode/ + +# Cookiecutter +output/ +python_boilerplate/ +cookiecutter-pypackage-env/ diff --git a/.pylintrc b/.pylintrc new file mode 100644 index 0000000..f32cab5 --- /dev/null +++ b/.pylintrc @@ -0,0 +1,34 @@ +# https://pylint.readthedocs.io/en/v2.12.2/user_guide/options.html + +[BASIC] +module-naming-style=any +const-naming-style=any +class-naming-style=any +function-naming-style=any +method-naming-style=any +attr-naming-style=any +argument-naming-style=any +variable-naming-style=any +class-attribute-naming-style=any +class-const-naming-style=any +inlinevar-naming-style=any + +# Custom regexes to allow both snake_case and camelCase +module-rgx=(?:(?P[a-z_][a-z0-9_]+)|(?P[a-z]+(?:[A-Z][a-z0-9]*)*))$ +const-rgx=(?:(?P[A-Z_][A-Z0-9_]*)|(?P[A-Z][a-zA-Z0-9]*))$ +class-rgx=(?:(?P[A-Z][a-zA-Z0-9]*)|(?P[a-z_][a-z0-9_]+))$ +function-rgx=(?:(?P[a-z_][a-z0-9_]*)|(?P[a-z]+(?:[A-Z][a-z0-9]*)*))$ +method-rgx=(?:(?P[a-z_][a-z0-9_]*)|(?P[a-z]+(?:[A-Z][a-z0-9]*)*))$ +attr-rgx=(?:(?P[a-z_][a-z0-9_]*)|(?P[a-z]+(?:[A-Z][a-z0-9]*)*))$ +argument-rgx=(?:(?P[a-z_][a-z0-9_]*)|(?P[a-z]+(?:[A-Z][a-z0-9]*)*))$ +variable-rgx=(?:(?P[a-z_][a-z0-9_]*)|(?P[a-z]+(?:[A-Z][a-z0-9]*)*))$ +class-attribute-rgx=(?:(?P[a-z_][a-z0-9_]*)|(?P[a-z]+(?:[A-Z][a-z0-9]*)*))$ +class-const-rgx=(?:(?P[A-Z_][A-Z0-9_]*)|(?P[A-Z][a-zA-Z0-9]*))$ +inlinevar-rgx=(?:(?P[a-z_][a-z0-9_]*)|(?P[a-z]+(?:[A-Z][a-z0-9]*)*))$ + +[design] +max-locals = 24 + +[MESSAGES CONTROL] +disable=W0621 +disable=W0102 diff --git a/examples/sphericalPlotting/plotArcs.py b/examples/sphericalPlotting/plotArcs.py new file mode 100644 index 0000000..991acb2 --- /dev/null +++ b/examples/sphericalPlotting/plotArcs.py @@ -0,0 +1,45 @@ +from typing import List + +from foundationTypes.mathTypes.UnitSphericalArc import UnitSphericalArc + +from pyMathToolsPlotHelpers.plotUnitSpherical import ( + deg45, + deg90, + deg180, + deg22_5, + plot_unit_spherical_multiplot, +) + +from pyMathTools.spherical.constructors import ( + arc_from_two_points, +) +from pyMathTools.spherical.sphericalTransforms import compute_spherical_arc_endpoint + + +def demo() -> None: + """Demonstration of multiplot view showing spherical small circles in three different projections.""" + + arc1 = UnitSphericalArc(orient=0, azimuth=0, polar=0, arc_length=deg90) + arc2 = UnitSphericalArc(orient=deg90, azimuth=0, polar=deg90, arc_length=deg90) + az, po = compute_spherical_arc_endpoint(arc2) + arc3 = arc_from_two_points( + azimuth1=az, + polar1=po, + azimuth2=arc1.azimuth, + polar2=arc1.polar, + ) + + arcs: List[UnitSphericalArc] = [ + arc1, + arc2, + arc3, + ] + + _ = plot_unit_spherical_multiplot( + arcs=arcs, + show_plot=True, + ) + + +if __name__ == "__main__": + demo() diff --git a/examples/sphericalPlotting/plotQuatUnitCircles.py b/examples/sphericalPlotting/plotQuatUnitCircles.py new file mode 100644 index 0000000..9f12860 --- /dev/null +++ b/examples/sphericalPlotting/plotQuatUnitCircles.py @@ -0,0 +1,80 @@ +""" +Example: Visualizing Quaternion-Derived Small Circles on the Unit Sphere + +This script demonstrates how to convert quaternions to spherical small circles +and visualize them using multiple projections. It serves as a validation and +walkthrough for the quaternion-to-spherical conversion and plotting utilities. + +Steps: +------ +1. Import required modules and functions. +2. Convert several canonical quaternions (identity and axis-aligned) to + UnitSphericalSmallCircle objects. +3. Plot these circles using the multiplot function, which shows flat projection, + polar, and 3D views. + +Usage: +------ +Run this script directly to see the resulting plots and validate the conversion. + +Expected Output: +---------------- +- The identity quaternion (`quaternion.one`) should map to the north pole. +- `quaternion.x`, `quaternion.y`, and `quaternion.z` should map to circles + centered on the X, Y, and Z axes, respectively. + +""" + +from typing import List +from numpy import pi + +from foundationTypes.mathTypes.UnitSphericalSmallCircle import UnitSphericalSmallCircle + +from pyMathTools.spatial.Quaternion import Quaternion + +from pyMathToolsPlotHelpers.plotUnitSpherical import ( + plot_unit_spherical_multiplot, +) + + +def main(): + """ + Convert canonical quaternions to spherical small circles and plot them. + + - quaternion.one: Identity quaternion (should map to north pole) + - quaternion.x: X-axis quaternion + - quaternion.y: Y-axis quaternion + - quaternion.z: Z-axis quaternion + + All circles use a radius angle of pi/8 except y and z, which use the default. + """ + + quat4 = Quaternion.from_unit_x_to_vector([1, 0, 0]) + quat5 = Quaternion.from_unit_x_to_vector([0, 1, 0]) + quat6 = Quaternion.from_unit_x_to_vector([0, 0, 1]) + + quat1 = Quaternion.from_components(0, 1, 0, 0) + quat2 = Quaternion.from_components(0, 0, 1, 0) + quat3 = Quaternion.from_components(0, 0, 0, 1) + + circles: List[UnitSphericalSmallCircle] = [ + quat1.to_unitSphericalSmallCircle(radius_angle=pi / 8), + quat2.to_unitSphericalSmallCircle(radius_angle=pi / 8), + quat3.to_unitSphericalSmallCircle(), + quat4.to_unitSphericalSmallCircle(radius_angle=pi / 16), + quat5.to_unitSphericalSmallCircle(radius_angle=pi / 16), + quat6.to_unitSphericalSmallCircle(radius_angle=pi / 16), + ] + + quats: List[Quaternion] = [quat1, quat2, quat3, quat4, quat5, quat6] + + # from_vector_part + _ = plot_unit_spherical_multiplot( + circles=circles, + quaternions=quats, + show_plot=True, + ) + + +if __name__ == "__main__": + main() diff --git a/pyproject.toml b/pyproject.toml index 09e2439..afb03c6 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -22,6 +22,7 @@ license = {text = "MIT"} dependencies = [ "pyFoundationTools @ git+https://github.com/kopecn/py-foundationTools.git@dev", "matplotlib", + "numpy-quaternion", ] [project.optional-dependencies] diff --git a/src/pyMathTools/hints.py b/src/pyMathTools/hints.py new file mode 100644 index 0000000..2305056 --- /dev/null +++ b/src/pyMathTools/hints.py @@ -0,0 +1,31 @@ +""" +Type hinting utilities for complex mathematical operations. + +This module provides reusable type hints for NumPy arrays and scalars, +enabling strict and clear typing in mathematical code throughout the package. +""" + +from typing import Union, TYPE_CHECKING +import numpy as np +from numpy import float64 +from numpy.typing import NDArray +import quaternion + +if TYPE_CHECKING: + from pyMathTools.spatial.quaternion import Quaternion + +# Basic numeric types +FloatOrNDArray = Union[float, NDArray[float64]] +FloatNDArray = NDArray[float64] + +# Quaternion types +QuaternionLike = Union["Quaternion", np.quaternion, NDArray[np.quaternion]] +FloatOrQuaternion = Union["Quaternion", float, np.quaternion] +FloatArray3 = NDArray[float64] # Shape (..., 3) +FloatArray4 = NDArray[float64] # Shape (..., 4) +RotationMatrix = NDArray[float64] # Shape (..., 3, 3) + + +if __name__ == "__main__": + q = quaternion.one + assert isinstance(q, np.quaternion) diff --git a/src/pyMathTools/pyMathTools.py b/src/pyMathTools/pyMathTools.py deleted file mode 100644 index dd0b80e..0000000 --- a/src/pyMathTools/pyMathTools.py +++ /dev/null @@ -1 +0,0 @@ -"""Main module.""" diff --git a/src/pyMathTools/spatial/Quaternion.py b/src/pyMathTools/spatial/Quaternion.py new file mode 100644 index 0000000..966ee3c --- /dev/null +++ b/src/pyMathTools/spatial/Quaternion.py @@ -0,0 +1,782 @@ +""" +Fully-typed OOP wrapper around numpy-quaternions library +""" + +from __future__ import annotations +from dataclasses import dataclass +from typing import TypeVar, Type, Any +import numpy as np +from foundationTypes.mathTypes.QuaternionType import QuaternionType + +# Import all necessary functions from quaternion module +from quaternion import ( + as_float_array, + as_quat_array, + from_float_array, + as_rotation_matrix, + from_rotation_matrix, + as_rotation_vector, + from_rotation_vector, + as_euler_angles, + from_euler_angles, + as_vector_part, + from_vector_part, + rotate_vectors, + isclose as quat_isclose, + allclose as quat_allclose, +) +from quaternion.quaternion_time_series import slerp as quat_slerp + +# Import custom type hints +from pyMathTools.hints import ( + FloatArray3, + FloatArray4, + RotationMatrix, + FloatOrQuaternion, +) +from foundationTypes.mathTypes.UnitSphericalSmallCircle import UnitSphericalSmallCircle + + +T = TypeVar("T", bound="Quaternion") + + +def from_float(x: Any) -> float: + assert isinstance(x, (float, int)) and not isinstance(x, bool) + return float(x) + + +@dataclass +class Quaternion(QuaternionType): + """ + wrapper class for numpy-quaternion for allowing + """ + + __q: np.quaternion + + # MARK: - Constructors + + @staticmethod + def fromNumpyQuaternion(q: np.quaternion) -> "Quaternion": + return Quaternion(q) + + @classmethod + def from_components( + cls: Type[T], w: float = 1, x: float = 0, y: float = 0, z: float = 0 + ) -> T: + """Create a quaternion instance from individual w, x, y, z components. + + Args: + w: The scalar (real) component + x: The x component of the vector part + y: The y component of the vector part + z: The z component of the vector part + + Returns: + A concrete QuaternionType instance of the calling class type + """ + return cls(np.quaternion(w, x, y, z)) + + @classmethod + def from_float_array(cls: Type[T], array: FloatArray4) -> T: + """Create a quaternion from a 4-element float array [w, x, y, z]. + + Args: + array: 4-element array in [w, x, y, z] order + + Returns: + A concrete QuaternionType instance of the calling class type + + Raises: + ValueError: If array doesn't have exactly 4 elements + """ + q = from_float_array(array) + return cls(q) + + @staticmethod + def as_quat_array(array: np.ndarray) -> np.ndarray: + """View a float array as an array of quaternions. + + The input array must have a final dimension whose size is 4. + Each set of 4 components will be interpreted as [w, x, y, z]. + + This is a fast operation (no data copy) when the array is C-contiguous. + + Args: + array: Float array with last dimension of size 4 + Shape can be (..., 4) where ... represents any number of dimensions + Examples: + - [w, x, y, z] -> single quaternion + - [[w1,x1,y1,z1], [w2,x2,y2,z2]] -> array of 2 quaternions + + Returns: + NumPy array of quaternions with shape (...) + + Examples: + >>> # Single quaternion + >>> Quaternion.as_quat_array([1, 0, 0, 0]) + quaternion(1, 0, 0, 0) + + >>> # Array of quaternions + >>> Quaternion.as_quat_array([[1,0,0,0], [0.707,0,0,0.707]]) + array([quaternion(1, 0, 0, 0), quaternion(0.707, 0, 0, 0.707)]) + """ + return as_quat_array(array) + + # MARK: - Properties + + @property + def w(self) -> float: + return self.__q.w + + @property + def x(self) -> float: + return self.__q.x + + @property + def y(self) -> float: + return self.__q.y + + @property + def z(self) -> float: + return self.__q.z + + @property + def q(self) -> np.quaternion: + return self.__q + + # MARK: - Arithmetic Operators + + def __add__(self, other: FloatOrQuaternion) -> Quaternion: + """Add two quaternions or a quaternion and a scalar.""" + if isinstance(other, Quaternion): + return Quaternion(self.__q + other.q) + elif isinstance(other, (float, int)): + return Quaternion(self.__q + other) + elif isinstance(other, np.quaternion): + return Quaternion(self.__q + other) + return NotImplemented + + def __radd__(self, other: FloatOrQuaternion) -> Quaternion: + """Right addition.""" + return self.__add__(other) + + def __sub__(self, other: FloatOrQuaternion) -> Quaternion: + """Subtract two quaternions or a scalar from a quaternion.""" + if isinstance(other, Quaternion): + return Quaternion(self.__q - other.q) + elif isinstance(other, (float, int)): + return Quaternion(self.__q - other) + elif isinstance(other, np.quaternion): + return Quaternion(self.__q - other) + return NotImplemented + + def __rsub__(self, other: FloatOrQuaternion) -> Quaternion: + """Right subtraction.""" + if isinstance(other, (float, int)): + return Quaternion(other - self.__q) + elif isinstance(other, np.quaternion): + return Quaternion(other - self.__q) + return NotImplemented + + def __mul__(self, other: FloatOrQuaternion) -> Quaternion: + """Multiply two quaternions or a quaternion by a scalar.""" + if isinstance(other, Quaternion): + return Quaternion(self.__q * other.q) + elif isinstance(other, (float, int)): + return Quaternion(self.__q * other) + elif isinstance(other, np.quaternion): + return Quaternion(self.__q * other) + return NotImplemented + + def __rmul__(self, other: FloatOrQuaternion) -> Quaternion: + """Right multiplication.""" + return self.__mul__(other) + + def __truediv__(self, other: FloatOrQuaternion) -> Quaternion: + """Divide a quaternion by another quaternion or scalar.""" + if isinstance(other, Quaternion): + return Quaternion(self.__q / other.q) + elif isinstance(other, (float, int)): + return Quaternion(self.__q / other) + elif isinstance(other, np.quaternion): + return Quaternion(self.__q / other) + return NotImplemented + + def __rtruediv__(self, other: FloatOrQuaternion) -> Quaternion: + """Right division.""" + if isinstance(other, (float, int)): + return Quaternion(other / self.__q) + elif isinstance(other, np.quaternion): + return Quaternion(other / self.__q) + return NotImplemented + + def __pow__(self, exponent: float) -> Quaternion: + """Raise quaternion to a power.""" + return Quaternion(self.__q**exponent) + + def __neg__(self) -> Quaternion: + """Negate the quaternion.""" + return Quaternion(-self.__q) + + def __pos__(self) -> Quaternion: + """Unary positive.""" + return Quaternion(+self.__q) + + def __abs__(self) -> float: + """Return the absolute value (norm) of the quaternion.""" + return float(abs(self.__q)) + + # MARK: - Comparison Operators + + def __eq__(self, other: object) -> bool: + """Check equality with another quaternion.""" + if isinstance(other, Quaternion): + return bool(self.__q == other.q) + elif isinstance(other, np.quaternion): + return bool(self.__q == other) + return False + + def __ne__(self, other: object) -> bool: + """Check inequality with another quaternion.""" + return not self.__eq__(other) + + def __repr__(self) -> str: + """Return a detailed string representation.""" + return f"Quaternion(w={self.w}, x={self.x}, y={self.y}, z={self.z})" + + def __str__(self) -> str: + """Return a user-friendly string representation.""" + return f"quaternion({self.w}, {self.x}, {self.y}, {self.z})" + + # MARK: - Unary Operations + + def conjugate(self) -> Quaternion: + """Return the conjugate of this quaternion. + + The conjugate of q = w + xi + yj + zk is q* = w - xi - yj - zk + """ + return Quaternion(self.__q.conjugate()) + + def inverse(self) -> Quaternion: + """Return the inverse of this quaternion. + + The inverse is defined as q^-1 = q* / |q|^2 + """ + return Quaternion(1.0 / self.__q) + + def normalized(self) -> Quaternion: + """Return a normalized (unit) version of this quaternion.""" + return Quaternion(self.__q / abs(self.__q)) + + # MARK: - Utility Properties + + @property + def norm(self) -> float: + """Return the norm (magnitude) of the quaternion.""" + return float(abs(self.__q)) + + @property + def is_unit(self) -> bool: + """Check if this is a unit quaternion (norm approximately 1).""" + return bool(np.isclose(abs(self.__q), 1.0)) + + @property + def angle(self) -> float: + """Return the rotation angle in radians (for unit quaternions).""" + return float(2.0 * np.arccos(np.clip(self.w, -1.0, 1.0))) + + @property + def axis(self) -> FloatArray3: + """Return the rotation axis as a unit vector (for unit quaternions). + + Returns the zero vector for the identity quaternion. + """ + vect = np.array([self.x, self.y, self.z]) + norm = np.linalg.norm(vect) + if norm < 1e-10: + return np.array([0.0, 0.0, 1.0]) # Default axis for identity + return vect / norm + + @property + def vector_spherical(self) -> tuple[float, float]: + """Return the quaternion's pointing direction in spherical coordinates (ISO physics convention). + + The quaternion is applied to a reference direction (+X axis) to get the + pointing direction, which is then converted to spherical coordinates. + + Returns: + tuple[azimuth, polar] where: + - azimuth (φ): angle in xy-plane from +x axis, range (-π, π] + - polar (θ): angle from +z axis (colatitude), range [0, π] + + Notes: + Reference direction is +X axis: [1, 0, 0] + - Identity quaternion (1,0,0,0) points to +X: azimuth=0, polar=π/2 + """ + # Reference direction: positive X-axis + reference_direction = np.array([1.0, 0.0, 0.0]) + + # Apply quaternion rotation to the reference direction + rotated_vector = self.rotate_vector(reference_direction) + + # Normalize to ensure it's a unit vector + x, y, z = rotated_vector + r = np.sqrt(x**2 + y**2 + z**2) + if r > 1e-10: + x, y, z = x / r, y / r, z / r + + # Convert to spherical coordinates (physics convention) + polar = float(np.arccos(np.clip(z, -1, 1))) # colatitude (0 to π) + azimuth = float(np.arctan2(y, x)) # azimuth (-π to π) + + return (azimuth, polar) + + @property + def vector_cartesian(self) -> tuple[float, float, float]: + """Return the quaternion's pointing direction as a cartesian unit vector. + + The quaternion is applied to a reference direction (+X axis) to get the + pointing direction as a unit vector in cartesian coordinates. + + Returns: + tuple[x, y, z] - unit vector components + + Notes: + Reference direction is +X axis: [1, 0, 0] + - Identity quaternion (1,0,0,0) points to +X: (1, 0, 0) + """ + # Reference direction: positive X-axis + reference_direction = np.array([1.0, 0.0, 0.0]) + + # Apply quaternion rotation to the reference direction + rotated_vector = self.rotate_vector(reference_direction) + + # Normalize to ensure it's a unit vector + x, y, z = rotated_vector + r = np.sqrt(x**2 + y**2 + z**2) + if r > 1e-10: + x, y, z = x / r, y / r, z / r + + return (float(x), float(y), float(z)) + + # MARK: - Array Conversion Methods + + def as_float_array(self) -> FloatArray4: + """View the quaternion as a float array [w, x, y, z]. + + This function is fast because no data is copied; the returned + quantity is just a "view" of the original. + """ + return as_float_array(self.__q) + + def to_components(self) -> tuple[float, float, float, float]: + """Return quaternion components as a tuple (w, x, y, z).""" + return (self.w, self.x, self.y, self.z) + + # MARK: - Rotation Matrix Conversions + + def to_rotation_matrix(self) -> RotationMatrix: + """Convert this quaternion to a 3x3 rotation matrix. + + For any quaternion q, this returns a matrix m such that, for every + vector v, we have: + m @ v.vec == q * v * q.conjugate() + + Returns: + 3x3 rotation matrix as a numpy array + + Raises: + ZeroDivisionError: If this quaternion has zero norm + """ + return as_rotation_matrix(self.__q) + + @classmethod + def from_rotation_matrix(cls: Type[T], matrix: RotationMatrix) -> T: + """Create a quaternion from a 3x3 rotation matrix. + + Args: + matrix: 3x3 rotation matrix. Should be orthogonal, but the + algorithm handles non-orthogonal matrices by finding + the closest orthogonal matrix. + + Returns: + Unit quaternion representing the rotation + + Raises: + ValueError: If matrix is not 3x3 + """ + q = from_rotation_matrix(matrix) + return cls(q) + + # MARK: - Rotation Vector (Axis-Angle) Conversions + + def to_rotation_vector(self) -> FloatArray3: + """Convert this quaternion to axis-angle representation. + + Returns a vector whose direction is the rotation axis and whose + magnitude is the rotation angle in radians. + + Returns: + 3-element array representing axis-angle rotation + """ + return as_rotation_vector(self.__q) + + @classmethod + def from_rotation_vector(cls: Type[T], rotation_vector: FloatArray3) -> T: + """Create a quaternion from axis-angle representation. + + Args: + rotation_vector: 3-element vector where the direction is the + rotation axis and the magnitude is the angle + in radians. + + Returns: + Unit quaternion representing the rotation + """ + q = from_rotation_vector(rotation_vector) + return cls(q) + + # MARK: - Euler Angle Conversions + + def to_euler_angles(self) -> FloatArray3: + """Convert this quaternion to Euler angles (alpha, beta, gamma). + + WARNING: Euler angles are problematic and should be avoided when + possible. Use quaternions or rotation matrices instead. + + Assumes the Euler angles correspond to the quaternion R via: + R = exp(alpha*z/2) * exp(beta*y/2) * exp(gamma*z/2) + + Returns: + Array of (alpha, beta, gamma) in radians + """ + return as_euler_angles(self.__q) + + @classmethod + def from_euler_angles( + cls: Type[T], + alpha: float, + beta: float, + gamma: float, + ) -> T: + """Create a quaternion from Euler angles. + + WARNING: Euler angles are problematic and should be avoided when + possible. Use quaternions or rotation matrices instead. + + Assumes the Euler angles correspond to the quaternion R via: + R = exp(alpha*z/2) * exp(beta*y/2) * exp(gamma*z/2) + + Args: + alpha: First Euler angle (radians) + beta: Second Euler angle (radians) + gamma: Third Euler angle (radians) + + Returns: + Quaternion representing the rotation + """ + q = from_euler_angles(alpha, beta, gamma) + return cls(q) + + # MARK: - Vector Part Operations + + @classmethod + def from_vector_part(cls: Type[T], vector: FloatArray3) -> T: + """Create a quaternion from a 3D vector (pure quaternion). + + This creates a quaternion with w=0 and vector part equal to the input. + + Args: + vector: 3-element array representing the vector part + + Returns: + Pure quaternion (w=0) + """ + q = from_vector_part(vector) + return cls(q) + + def to_vector_part(self) -> FloatArray3: + """Extract the vector (imaginary) part as a 3D array. + + Returns: + 3-element array [x, y, z] + """ + return as_vector_part(self.__q) + + def rotate_vector(self, vector: FloatArray3) -> FloatArray3: + """Rotate a 3D vector by this quaternion. + + This computes: q * v * q.conjugate() + where v is treated as a pure quaternion (w=0). + + Args: + vector: 3-element vector to rotate + + Returns: + Rotated 3-element vector + """ + return rotate_vectors(self.__q, vector) + + # MARK: - Interpolation Methods + + def slerp(self, other: Quaternion, t: float) -> Quaternion: + """Spherical linear interpolation between this and another quaternion. + + Uses the numpy-quaternions implementation for robust and efficient SLERP. + + Args: + other: Target quaternion + t: Interpolation parameter in [0, 1], where 0 returns self + and 1 returns other + + Returns: + Interpolated quaternion + """ + # Use the package's slerp with t1=0.0, t2=1.0 for normalized interpolation + result = quat_slerp(self.__q, other.q, 0.0, 1.0, t) + return Quaternion(result) + + # MARK: - Comparison Utilities + + def isclose( + self, + other: Quaternion, + rtol: float = 1e-9, + atol: float = 1e-11, + ) -> bool: + """Check if this quaternion is close to another within a tolerance. + + This method accounts for the double cover property of quaternions: + q and -q represent the same rotation, so both are checked. + + Args: + other: Quaternion to compare with + rtol: Relative tolerance (default: 1e-9) + atol: Absolute tolerance (default: 1e-11) + + Returns: + True if quaternions are close within tolerance (accounting for double cover) + """ + # Check both q and -q due to double cover + return bool( + quat_isclose(self.__q, other.q, rtol=rtol, atol=atol) + or quat_isclose(self.__q, -other.q, rtol=rtol, atol=atol) + ) + + @staticmethod + def allclose( + q1: Quaternion, + q2: Quaternion, + rtol: float = 1e-9, + atol: float = 1e-11, + ) -> bool: + """Check if two quaternions are close within a tolerance. + + This method accounts for the double cover property of quaternions: + q and -q represent the same rotation, so both are checked. + + Args: + q1: First quaternion + q2: Second quaternion + rtol: Relative tolerance (default: 1e-9) + atol: Absolute tolerance (default: 1e-11) + + Returns: + True if quaternions are close within tolerance (accounting for double cover) + """ + # Check both q2 and -q2 due to double cover + return bool( + quat_allclose(q1.q, q2.q, rtol=rtol, atol=atol) + or quat_allclose(q1.q, -q2.q, rtol=rtol, atol=atol) + ) + + # MARK: - Special Constructors + + @classmethod + def identity(cls: Type[T]) -> T: + """Create an identity quaternion (no rotation). + + Returns: + Quaternion(1, 0, 0, 0) + """ + return cls.from_components(w=1.0, x=0.0, y=0.0, z=0.0) + + @classmethod + def from_axis_angle(cls: Type[T], axis: FloatArray3, angle: float) -> T: + """Create a quaternion from a rotation axis and angle. + + Args: + axis: 3D rotation axis (will be normalized) + angle: Rotation angle in radians + + Returns: + Unit quaternion representing the rotation + """ + axis_normalized = axis / np.linalg.norm(axis) + half_angle = angle / 2.0 + sin_half = np.sin(half_angle) + return cls.from_components( + w=np.cos(half_angle), + x=axis_normalized[0] * sin_half, + y=axis_normalized[1] * sin_half, + z=axis_normalized[2] * sin_half, + ) + + @classmethod + def _from_unit_direction_to_vector( + cls: Type[T], + start_direction: FloatArray3, + target_vector: FloatArray3, + perpendicular_axis: FloatArray3, + ) -> T: + """Helper method to create a quaternion rotating from a unit direction to target. + + Args: + start_direction: Starting unit direction vector + target_vector: Target direction vector (will be normalized) + perpendicular_axis: Axis to use for 180° rotation case + + Returns: + Unit quaternion representing the rotation + """ + # Normalize target vector + target = np.array(target_vector, dtype=float) + target_norm = np.linalg.norm(target) + if target_norm < 1e-10: + # Zero vector, return identity + return cls.identity() + target = target / target_norm + + # Compute dot product + dot = np.dot(start_direction, target) + + # Check for special cases + if dot > 0.9999: + # Vectors are parallel (same direction) + return cls.identity() + elif dot < -0.9999: + # Vectors are opposite (180° rotation needed) + # Use the provided perpendicular axis + perp = np.array(perpendicular_axis, dtype=float) + perp = perp / np.linalg.norm(perp) + return cls.from_components(w=0.0, x=perp[0], y=perp[1], z=perp[2]) + + # General case: compute rotation axis and angle + # Rotation axis is perpendicular to both vectors + axis = np.cross(start_direction, target) + axis = axis / np.linalg.norm(axis) + + # Compute rotation angle + angle = np.arccos(np.clip(dot, -1.0, 1.0)) + + # Create quaternion from axis-angle + return cls.from_axis_angle(axis, angle) + + @classmethod + def from_unit_x_to_vector(cls: Type[T], target_vector: FloatArray3) -> T: + """Create a quaternion that rotates the +X axis to point toward target_vector. + + This constructor computes the rotation that transforms the unit X direction + [1, 0, 0] to align with the given target vector. + + Args: + target_vector: Target direction vector (will be normalized) + + Returns: + Unit quaternion representing the rotation from +X to target_vector + + Notes: + - If target_vector points in +X direction, returns identity quaternion + - If target_vector points in -X direction, returns 180° rotation about +Z axis + - For all other directions, uses the shortest rotation path + + Examples: + >>> # Rotate +X to point toward +Y + >>> q = Quaternion.from_unit_x_to_vector([0, 1, 0]) + >>> # Rotate +X to point toward +Z + >>> q = Quaternion.from_unit_x_to_vector([0, 0, 1]) + """ + start = np.array([1.0, 0.0, 0.0]) + perpendicular = np.array([0.0, 0.0, 1.0]) # +Z for 180° case + return cls._from_unit_direction_to_vector(start, target_vector, perpendicular) + + @classmethod + def from_unit_y_to_vector(cls: Type[T], target_vector: FloatArray3) -> T: + """Create a quaternion that rotates the +Y axis to point toward target_vector. + + This constructor computes the rotation that transforms the unit Y direction + [0, 1, 0] to align with the given target vector. + + Args: + target_vector: Target direction vector (will be normalized) + + Returns: + Unit quaternion representing the rotation from +Y to target_vector + + Notes: + - If target_vector points in +Y direction, returns identity quaternion + - If target_vector points in -Y direction, returns 180° rotation about +Z axis + - For all other directions, uses the shortest rotation path + + Examples: + >>> # Rotate +Y to point toward +X + >>> q = Quaternion.from_unit_y_to_vector([1, 0, 0]) + >>> # Rotate +Y to point toward +Z + >>> q = Quaternion.from_unit_y_to_vector([0, 0, 1]) + """ + start = np.array([0.0, 1.0, 0.0]) + perpendicular = np.array([0.0, 0.0, 1.0]) # +Z for 180° case + return cls._from_unit_direction_to_vector(start, target_vector, perpendicular) + + @classmethod + def from_unit_z_to_vector(cls: Type[T], target_vector: FloatArray3) -> T: + """Create a quaternion that rotates the +Z axis to point toward target_vector. + + This constructor computes the rotation that transforms the unit Z direction + [0, 0, 1] to align with the given target vector. + + Args: + target_vector: Target direction vector (will be normalized) + + Returns: + Unit quaternion representing the rotation from +Z to target_vector + + Notes: + - If target_vector points in +Z direction, returns identity quaternion + - If target_vector points in -Z direction, returns 180° rotation about +X axis + - For all other directions, uses the shortest rotation path + + Examples: + >>> # Rotate +Z to point toward +X + >>> q = Quaternion.from_unit_z_to_vector([1, 0, 0]) + >>> # Rotate +Z to point toward +Y + >>> q = Quaternion.from_unit_z_to_vector([0, 1, 0]) + """ + start = np.array([0.0, 0.0, 1.0]) + perpendicular = np.array([1.0, 0.0, 0.0]) # +X for 180° case + return cls._from_unit_direction_to_vector(start, target_vector, perpendicular) + + @classmethod + def from_dict(cls, obj: Any) -> "Quaternion": + """Create a quaternion instance from a dictionary representation. + + Args: + obj: Dictionary containing 'w', 'x', 'y', 'z' keys + + Returns: + A concrete Quaternion instance of the calling class type + """ + w = from_float(obj.get("w")) + x = from_float(obj.get("x")) + y = from_float(obj.get("y")) + z = from_float(obj.get("z")) + # Note: This calls the concrete implementation's from_components + # cls ensures the correct subclass type is returned + return cls.from_components(w, x, y, z) + + def to_unitSphericalSmallCircle( + q: Quaternion, radius_angle: float = np.pi / 4 + ) -> UnitSphericalSmallCircle: + """ """ + a, p = q.vector_spherical + return UnitSphericalSmallCircle(azimuth=a, polar=p, radius_angle=radius_angle) diff --git a/src/pyMathTools/spatial/__init__.py b/src/pyMathTools/spatial/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/pyMathTools/spherical/__init__.py b/src/pyMathTools/spherical/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/pyMathTools/spherical/constructors.py b/src/pyMathTools/spherical/constructors.py new file mode 100644 index 0000000..2a28fe3 --- /dev/null +++ b/src/pyMathTools/spherical/constructors.py @@ -0,0 +1,97 @@ +""" +Constructor functions for UnitSphericalSmallCircle objects. + +This module provides utilities for creating UnitSphericalSmallCircle instances +from various representations, using ISO Physics Convention for spherical coordinates. +""" + +import numpy as np +from numpy import atan2, acos, pi + +from foundationTypes.mathTypes.UnitSphericalArc import UnitSphericalArc + + +def arc_from_two_points( + azimuth1: float, + polar1: float, + azimuth2: float, + polar2: float, + isPositive: bool = True, +) -> UnitSphericalArc: + """ + Create a UnitSphericalArc connecting two points on a unit sphere. + + Uses ISO Physics Convention for spherical coordinates: + - φ (azimuth): angle in xy-plane from +x axis + - θ (polar): angle from +z axis (colatitude), range [0, π] + + The arc follows a great circle path between the two points. The direction of + travel is determined by the isPositive parameter. + + Parameters: + ----------- + azimuth1 : float + Starting point azimuth angle (φ₁) in radians + polar1 : float + Starting point polar angle (θ₁) in radians (colatitude from +z) + azimuth2 : float + Ending point azimuth angle (φ₂) in radians + polar2 : float + Ending point polar angle (θ₂) in radians (colatitude from +z) + isPositive : bool, optional + If True (default), creates arc with positive arc_length + If False, creates arc with negative arc_length (opposite direction) + + Returns: + -------- + UnitSphericalArc + Arc starting at (azimuth1, polar1) that reaches toward (azimuth2, polar2) + + Notes: + ------ + Uses spherical trigonometry formulas: + - arc_length: Great circle distance using spherical law of cosines + - orient: Initial bearing computed relative to north direction + + Examples: + --------- + >>> # Arc from north pole to equator at 0° azimuth + >>> arc = arc_from_two_points(0, 0, 0, np.pi/2, isPositive=True) + """ + # Great circle distance using spherical law of cosines + # In ISO physics convention: polar = θ (colatitude from +z axis) + # γ = arccos(cos(θ1)*cos(θ2) + sin(θ1)*sin(θ2)*cos(φ2 - φ1)) + + cos_theta1 = np.cos(polar1) + sin_theta1 = np.sin(polar1) + cos_theta2 = np.cos(polar2) + sin_theta2 = np.sin(polar2) + + delta_phi = azimuth2 - azimuth1 + cos_delta_phi = np.cos(delta_phi) + + # Arc length (great circle distance) + cos_arc_length = cos_theta1 * cos_theta2 + sin_theta1 * sin_theta2 * cos_delta_phi + arc_length = np.arccos(np.clip(cos_arc_length, -1.0, 1.0)) + + # Initial bearing from point 1 to point 2 (relative to north) + # This gives the orient parameter + # bearing = atan2(sin(Δφ)*sin(θ2), cos(θ1)*sin(θ2) - sin(θ1)*cos(θ2)*cos(Δφ)) + + sin_delta_phi = np.sin(delta_phi) + + y_component = sin_delta_phi * sin_theta2 + x_component = cos_theta1 * sin_theta2 - sin_theta1 * cos_theta2 * cos_delta_phi + + orient = atan2(y_component, x_component) + + # Apply isPositive parameter + if not isPositive: + arc_length = -arc_length + + return UnitSphericalArc( + azimuth=azimuth1, + polar=polar1, + orient=orient, + arc_length=arc_length, + ) diff --git a/src/pyMathTools/spherical/sphericalGenerators.py b/src/pyMathTools/spherical/sphericalGenerators.py new file mode 100644 index 0000000..6310ba3 --- /dev/null +++ b/src/pyMathTools/spherical/sphericalGenerators.py @@ -0,0 +1,266 @@ +""" +Generators for spherical geometry primitives. + +This module provides functions for generating points on spherical circles, arcs, +and converting quaternions to spherical vectors. + +ISO Physics Convention for Spherical Coordinates +------------------------------------------------- +All functions in this module use the ISO physics convention: + +Notation: (r, θ, φ) where r=1 for unit sphere + +θ (theta) - Polar Angle (Colatitude): + - Measured from +z axis downward + - Range: [0, π] + - θ = 0: North pole (+z axis) + - θ = π/2: Equator (xy-plane) + - θ = π: South pole (-z axis) + +φ (phi) - Azimuthal Angle: + - Measured from +x axis in xy-plane, counterclockwise when viewed from above + - Range: [0, 2π) + - φ = 0: +x axis + - φ = π/2: +y axis + - φ = π: -x axis + - φ = 3π/2: -y axis + +Cartesian Conversion: + x = r*sin(θ)*cos(φ) + y = r*sin(θ)*sin(φ) + z = r*cos(θ) + +Inverse Conversion: + θ = arccos(z/r) + φ = arctan2(y, x) + r = sqrt(x² + y² + z²) +""" + +from typing import Tuple +from numpy import ( + clip, + pi, + arctan2, + cos, + sin, + array, + cross, + linspace, + zeros, + sqrt, + arccos, + dot, +) +from numpy.linalg import norm +from foundationTypes.mathTypes.UnitSphericalSmallCircle import UnitSphericalSmallCircle +from foundationTypes.mathTypes.UnitSphericalArc import UnitSphericalArc + +from pyMathTools.hints import FloatNDArray + + +def generate_spherical_small_circle_points( + circle: UnitSphericalSmallCircle, + num_points: int = 120, +) -> Tuple[FloatNDArray, FloatNDArray]: + """ + Generate points on a true small circle on a unit sphere. + + A small circle is the set of all points at a fixed angular distance (radius_angle) + from a center point on the sphere. + + Uses ISO Physics Convention (see module docstring for details). + + Parameters: + ----------- + circle : UnitSphericalSmallCircle + Small circle specification with: + - azimuth (φ): angle in xy-plane from +x axis [0, 2π) + - polar (θ): angle from +z axis (colatitude) [0, π] + - radius_angle: angular radius of the circle + num_points : int + Number of points to generate around the circle + + Returns: + -------- + azimuth_points : NDArray + Azimuth angles (φ) of points on the circle in radians + polar_points : NDArray + Polar angles (θ) of points on the circle in radians + """ + # Use physics convention directly: polar ∈ [0, π], where 0 = north pole, π/2 = equator + # Convert center to Cartesian coordinates (physics convention) + # theta (polar/colatitude) = 0 at north pole, π at south pole + # phi (azimuth) = rotation about Z axis + x_center = sin(circle.polar) * cos(circle.azimuth) + y_center = sin(circle.polar) * sin(circle.azimuth) + z_center = cos(circle.polar) + center = array([x_center, y_center, z_center]) + + # Create orthonormal basis at the center point + # North pole direction + north = array([0, 0, 1]) + + # Tangent vector 1 (east direction) + if abs(z_center) < 0.999: # Not at poles + east = cross(north, center) + east = east / norm(east) + else: # At poles, use arbitrary east direction + east = array([1, 0, 0]) + + # Tangent vector 2 (north direction in tangent plane) + north_tangent = cross(center, east) + north_tangent = north_tangent / norm(north_tangent) + + # Generate points around the circle + theta = linspace(-pi, pi, num_points) + + # Points on the small circle using rotation + # The small circle is at angular distance radius_angle from center + azimuth_points = zeros(num_points) + polar_points = zeros(num_points) + + for i, t in enumerate(theta): + # Local coordinates: move radius_angle away from center in direction t + local_direction = cos(t) * east + sin(t) * north_tangent + + # Rotate center point toward local_direction by radius_angle + # Using Rodrigues' rotation formula + axis = cross(center, local_direction) + axis_norm = norm(axis) + + if axis_norm > 1e-10: + axis = axis / axis_norm + # Rotate center around axis by radius_angle + cos_angle = cos(circle.radius_angle) + sin_angle = sin(circle.radius_angle) + + point = ( + cos_angle * center + + sin_angle * cross(axis, center) + + (1 - cos_angle) * dot(axis, center) * axis + ) + else: + # If no rotation needed (shouldn't happen) + point = center + + # Convert back to spherical coordinates (physics convention) + x, y, z = point + r = sqrt(x**2 + y**2 + z**2) + x, y, z = x / r, y / r, z / r # Normalize to unit sphere + + # Output in physics convention for compatibility with downstream code + polar_points[i] = arccos(clip(z, -1, 1)) # colatitude (0 to π) + azimuth_points[i] = arctan2(y, x) # azimuth + + return azimuth_points, polar_points + + +def generate_spherical_arc_points( + arc: UnitSphericalArc, + num_points: int = 120, +) -> Tuple[FloatNDArray, FloatNDArray]: + """ + Generate points along an arc on a unit sphere. + + The arc starts at the point (azimuth, polar) and extends along a great circle + in the direction determined by the orient parameter for the specified arc_length. + + The orientation follows the right-hand rule: point your right thumb along the radial + vector (from origin through the start point), and your fingers curl in the positive + direction of rotation (positive orient). + + Uses ISO Physics Convention (see module docstring for details). + + Parameters: + ----------- + arc : UnitSphericalArc + Arc specification with: + - azimuth (φ): starting point angle in xy-plane from +x axis [0, 2π) + - polar (θ): starting point angle from +z axis (colatitude) [0, π] + - orient: rotation angle about the radial vector (radians) + Following right-hand rule: thumb along radial, fingers curl in +orient direction + - arc_length: extension along great circle (radians, can be negative) + num_points : int + Number of points to generate along the arc + + Returns: + -------- + azimuth_points : NDArray + Azimuth angles (φ) of points along the arc in radians + polar_points : NDArray + Polar angles (θ) of points along the arc in radians + """ + # Use physics convention directly: polar ∈ [0, π], where 0 = north pole, π/2 = equator + # Convert starting point to Cartesian coordinates (physics convention) + x_start = sin(arc.polar) * cos(arc.azimuth) + y_start = sin(arc.polar) * sin(arc.azimuth) + z_start = cos(arc.polar) + start_point = array([x_start, y_start, z_start]) + + # Create tangent direction based on the orient parameter + # The orient defines the rotation angle about the radial vector (right-hand rule) + # We need to create an orthonormal basis in the tangent plane and rotate by orient + # Convention: orient=0 points towards the north pole in the tangent plane + + north = array([0, 0, 1]) + + # Create an orthonormal basis in the tangent plane + # Following right-hand rule: thumb along radial, fingers curl in positive orient direction + if abs(z_start) < 0.999: # Not at poles + # East direction: perpendicular to both north and radial + east = cross(north, start_point) + east = east / norm(east) + + # North direction in tangent plane: completes right-handed system + # cross(north_tangent, east) = start_point (outward radial) + north_tangent = cross(east, start_point) + north_tangent = north_tangent / norm(north_tangent) + + ref_tangent = north_tangent + second_tangent = east + else: # At poles, use arbitrary reference + ref_tangent = array([1, 0, 0]) + second_tangent = array([0, 1, 0]) + + # Rotate by the orient angle about the radial vector (right-hand rule) + # orient=0 points north, positive orient rotates towards east + initial_direction = cos(arc.orient) * ref_tangent + sin(arc.orient) * second_tangent + initial_direction = initial_direction / norm(initial_direction) + + # The rotation axis for the arc is perpendicular to the initial direction and radial + rotation_axis = cross(start_point, initial_direction) + rotation_axis = rotation_axis / norm(rotation_axis) + + # Generate points along the arc + # Arc length parameter: 0 to arc_length + t = linspace(0, arc.arc_length, num_points) + + azimuth_points = zeros(num_points) + polar_points = zeros(num_points) + + for i, arc_dist in enumerate(t): + # Move along great circle in the direction determined by orient + # Using Rodrigues' rotation formula + # Rotation axis is perpendicular to initial direction (follows from right-hand rule) + axis = rotation_axis + + # Rotate start_point around axis by arc_dist + cos_angle = cos(arc_dist) + sin_angle = sin(arc_dist) + + point = ( + cos_angle * start_point + + sin_angle * cross(axis, start_point) + + (1 - cos_angle) * dot(axis, start_point) * axis + ) + + # Convert back to spherical coordinates (physics convention) + x, y, z = point + r = sqrt(x**2 + y**2 + z**2) + x, y, z = x / r, y / r, z / r # Normalize to unit sphere + + # Output in physics convention for compatibility with downstream code + polar_points[i] = arccos(clip(z, -1, 1)) # colatitude (0 to π) + azimuth_points[i] = arctan2(y, x) # azimuth + + return azimuth_points, polar_points diff --git a/src/pyMathTools/spherical/sphericalTransforms.py b/src/pyMathTools/spherical/sphericalTransforms.py new file mode 100644 index 0000000..b202fbd --- /dev/null +++ b/src/pyMathTools/spherical/sphericalTransforms.py @@ -0,0 +1,210 @@ +""" +Transforms for spherical coordinate systems. + +This module provides functions for converting to and from spherical coordinates, +including projections and other mathematical transformations commonly used in +geometry, mapping, and scientific computing. + +ISO Physics Convention +----------------------- +All functions use the ISO physics convention for spherical coordinates: +- θ (theta/polar): angle from +z axis (colatitude), range [0, π] +- φ (phi/azimuth): angle in xy-plane from +x axis, range [0, 2π) + +See pyMathTools.generators.sphericalGenerators module docstring for complete details. +""" + +from typing import Tuple +from pyMathTools.hints import FloatOrNDArray +from numpy import float64 +from numpy.typing import NDArray +from numpy import ( + clip, + pi, + arctan2, + cos, + sin, + arccos, +) +from numpy.linalg import norm + +from foundationTypes.mathTypes.UnitSphericalArc import UnitSphericalArc + + +def plate_carree_transform( + azimuth: FloatOrNDArray, polar: FloatOrNDArray +) -> tuple[FloatOrNDArray, FloatOrNDArray]: + """ + Apply plate carrée (equirectangular) projection to spherical coordinates. + + This is a simple cylindrical projection that preserves angular measurements, + making it suitable for applications where actual angles in radians are needed. + + Uses ISO Physics Convention (see module docstring for details). + + Parameters: + ----------- + azimuth : float or ndarray + Azimuth angle(s) φ in radians + Angle in xy-plane from +x axis, range [0, 2π) + polar : float or ndarray + Polar angle(s) θ in radians + Angle from +z axis (colatitude), range [0, π] + - θ = 0: north pole (+z) + - θ = π/2: equator + - θ = π: south pole (-z) + + Returns: + -------- + x : float or ndarray + Projected x-coordinate = azimuth (φ) in radians [0, 2π) + y : float or ndarray + Projected y-coordinate = latitude in radians [-π/2, π/2] + - y = π/2: north pole + - y = 0: equator + - y = -π/2: south pole + + Notes: + ------ + Conversion formula: latitude = π/2 - polar + """ + x = azimuth + # Convert physics polar angle (θ, 0 to π) to latitude (-π/2 to π/2) + # latitude = π/2 - θ + y = pi / 2 - polar + return x, y + + +def spherical_to_cartesian(azimuth: float, polar: float) -> tuple[float, float, float]: + """ + Convert spherical coordinates to Cartesian coordinates on the unit sphere. + + Uses ISO physics convention: + - polar (theta): angle from +z axis (colatitude), range [0, π] + - azimuth (phi): angle in xy-plane from +x axis, range (-π, π] or [0, 2π) + + Args: + azimuth: Azimuth angle (φ) in radians - angle in xy-plane from +x axis + polar: Polar angle (θ) in radians - angle from +z axis (colatitude) + + Returns: + Tuple with Cartesian coordinates [x, y, z] on unit sphere + + Notes: + Conversion formulas (ISO physics convention): + - x = sin(θ) * cos(φ) + - y = sin(θ) * sin(φ) + - z = cos(θ) + """ + x = sin(polar) * cos(azimuth) + y = sin(polar) * sin(azimuth) + z = cos(polar) + + return (x, y, z) + + +def cartesian_to_spherical(point: NDArray[float64]) -> tuple: + """ + Convert Cartesian coordinates to spherical coordinates. + + Uses ISO physics convention: + - polar (theta): angle from +z axis (colatitude), range [0, π] + - azimuth (phi): angle in xy-plane from +x axis, range (-π, π] + + Args: + point: Cartesian coordinates [x, y, z] + + Returns: + Tuple of (azimuth, polar) in radians + """ + point = point / norm(point) + x, y, z = point + + # Azimuth: angle in xy-plane from +x axis + azimuth = arctan2(y, x) + + # Polar: angle from +z axis (colatitude) + polar = arccos(clip(z, -1.0, 1.0)) + + return azimuth, polar + + +def compute_spherical_arc_endpoint( + arc: UnitSphericalArc, +) -> Tuple[float, float]: + """ + Compute the endpoint of a UnitSphericalArc on a unit sphere. + + Parameters + ---------- + arc : UnitSphericalArc + The spherical arc containing: + - azimuth: Starting azimuth angle in radians (0 to 2π) + - polar: Starting polar angle in radians (colatitude/zenith angle) + measured from vertical +Z axis (0 at north pole, + π/2 at equator, π at south pole) + - arc_length: Length of the arc in radians (angular distance to travel) + - orient: Orientation/bearing of the arc in radians (-π to π) + This is the direction to travel from the start point + + Returns + ------- + Tuple[float, float] + (azimuth_end, polar_end) - endpoint in spherical coordinates + polar_end uses the same colatitude convention + + Notes + ----- + This uses spherical trigonometry to compute great circle navigation. + The polar angle uses colatitude convention (angle from +Z axis): + - polar = 0 at north pole (+z axis) + - polar = π/2 at equator + - polar = π at south pole (-z axis) + + Examples + -------- + >>> arc = UnitSphericalArc( + ... arc_length=deg2rad(45), + ... azimuth=0.0, + ... orient=0.0, + ... polar=deg2rad(90) # Starting at equator + ... ) + >>> azimuth_end, polar_end = compute_spherical_arc_endpoint(arc) + """ + # polar is already in colatitude convention (angle from +Z axis) + theta_start = arc.polar + + # Using spherical trigonometry formulas for great circle navigation: + # cos(theta_end) = cos(theta_start)*cos(arc_length) + + # sin(theta_start)*sin(arc_length)*cos(orient) + cos_theta_end = cos(theta_start) * cos(arc.arc_length) + sin(theta_start) * sin( + arc.arc_length + ) * cos(arc.orient) + theta_end = arccos(clip(cos_theta_end, -1.0, 1.0)) + + # Compute the change in azimuth using: + # sin(Δazimuth) = sin(arc_length)*sin(orient) / sin(theta_end) + # cos(Δazimuth) = (cos(arc_length) - cos(theta_start)*cos(theta_end)) / + # (sin(theta_start)*sin(theta_end)) + + if abs(sin(theta_start)) < 1e-10: + # Special case: starting point at pole + # When starting from a pole, the ending azimuth is simply the bearing direction + azimuth_end = arc.orient + elif abs(sin(theta_end)) < 1e-10: + # Special case: endpoint at pole + azimuth_end = arc.azimuth + else: + sin_delta_az = sin(arc.arc_length) * sin(arc.orient) / sin(theta_end) + cos_delta_az = (cos(arc.arc_length) - cos(theta_start) * cos(theta_end)) / ( + sin(theta_start) * sin(theta_end) + ) + delta_azimuth = arctan2(sin_delta_az, cos_delta_az) + azimuth_end = arc.azimuth + delta_azimuth + + # Normalize azimuth to [0, 2π) + azimuth_end = azimuth_end % (2 * pi) + + polar_end = theta_end + + return azimuth_end, polar_end diff --git a/src/pyMathToolsPlotHelpers/plotUnitSpherical.py b/src/pyMathToolsPlotHelpers/plotUnitSpherical.py new file mode 100644 index 0000000..8ac8fad --- /dev/null +++ b/src/pyMathToolsPlotHelpers/plotUnitSpherical.py @@ -0,0 +1,873 @@ +from typing import List, Tuple, Optional +import numpy as np +import matplotlib.pyplot as plt +from matplotlib.figure import Figure +from matplotlib.axes import Axes + +from foundationTypes.mathTypes.UnitSphericalSmallCircle import UnitSphericalSmallCircle +from foundationTypes.mathTypes.UnitSphericalArc import UnitSphericalArc + +from pyMathTools.spatial.Quaternion import Quaternion +from pyMathTools.spherical.sphericalTransforms import plate_carree_transform +from pyMathTools.spherical.sphericalGenerators import ( + generate_spherical_small_circle_points, + generate_spherical_arc_points, +) + +deg45 = np.deg2rad(45) +deg90 = np.deg2rad(90) +deg180 = np.deg2rad(180) +deg22_5 = np.pi / 8 + + +def plot_unit_spherical_advanced( + circles: List[UnitSphericalSmallCircle] | None = None, + arcs: List[UnitSphericalArc] | None = None, + quaternions: List[Quaternion] | None = None, + num_points: int = 120, + figsize: Tuple[int, int] = (10, 6), + title: str = "Small Circles on Sphere (Flattened)", + colors: Optional[List[str]] = None, + labels: Optional[List[str]] = None, + show_centers: bool = True, + show_plot: bool = False, + ax: Optional[Axes] = None, + arc_colors: Optional[List[str]] = ["red", "green", "blue"], + circle_colors: Optional[List[str]] = ["red", "green", "blue"], + quaternion_colors: Optional[List[str]] = ["red", "green", "blue"], +) -> Tuple[Figure, Axes]: + """ + Advanced version with more customization options. + + Parameters: + ----------- + circles : List[UnitSphericalSmallCircle] or array-like + List of UnitSphericalSmallCircle objects or array of [azimuth, polar, radius] lists + arcs : List[UnitSphericalArc], optional + List of UnitSphericalArc objects. + Each arc starts at (azimuth, polar) and extends along a great circle + for the specified arc_length (positive or negative) + quaternions : List[Quaternion], optional + List of Quaternion objects. + Each quaternion is visualized as a unit vector from origin to its pointing direction + num_points : int, optional + Number of points to generate around each circle + figsize : tuple, optional + Figure size (ignored if ax is provided) + title : str, optional + Plot title + colors : list, optional + List of colors for each circle (deprecated, use circle_colors instead) + labels : list, optional + List of labels for each circle + show_centers : bool, optional + Whether to show center points (default: True) + show_plot : bool, optional + Whether to show the plot (default: False, ignored if ax is provided) + ax : matplotlib.axes.Axes, optional + Axes to plot on. If None, creates new figure and axes + arc_colors : List[str], optional + List of colors for arcs. Defaults to ["red", "green", "blue"]. + If insufficient colors, uses matplotlib's color cycle. + circle_colors : List[str], optional + List of colors for circles. Defaults to ["red", "green", "blue"]. + If insufficient colors, uses matplotlib's color cycle. + quaternion_colors : List[str], optional + List of colors for quaternions. Defaults to ["red", "green", "blue"]. + If insufficient colors, uses matplotlib's color cycle. + + Returns: + -------- + fig, ax : matplotlib figure and axes objects + """ + + # Helper function to get color with fallback to matplotlib's color cycle + def get_color(color_list: Optional[List[str]], idx: int) -> str: + """Get color from list or fallback to matplotlib's color cycle.""" + if color_list is None or len(color_list) == 0: + return f"C{idx}" + if idx < len(color_list): + return color_list[idx] + return f"C{idx}" + + # Track whether we created the axes or it was provided + created_ax = ax is None + + if ax is None: + fig, ax = plt.subplots(figsize=figsize) + else: + fig = ax.get_figure() + + if circles is not None: + for idx, circle in enumerate(circles): + + # Generate true spherical small circle points + azimuth_points, polar_points = generate_spherical_small_circle_points( + circle, num_points + ) + + # Apply Plate Carrée projection + x_points, y_points = plate_carree_transform(azimuth_points, polar_points) + + # Determine color and label (prioritize new color parameters over deprecated colors) + if colors is not None and idx < len(colors): + color = colors[idx] + else: + color = get_color(circle_colors, idx) + label = labels[idx] if labels is not None and idx < len(labels) else None + + # Plot the circle + ax.scatter( + np.degrees(x_points), + np.degrees(y_points), + s=10, + alpha=0.6, + c=color, + label=label, + ) + + # Plot center point if requested + if show_centers: + x_center, y_center = plate_carree_transform( + circle.azimuth, circle.polar + ) + ax.scatter( + np.degrees(x_center), + np.degrees(y_center), + s=100, + marker="x", + c=color, + linewidths=2, + ) + + # Generate points for each arc + if arcs is not None: + for idx, arc in enumerate(arcs): + + # Generate spherical arc points + azimuth_points, polar_points = generate_spherical_arc_points( + arc, num_points + ) + + # Apply Plate Carrée projection + x_points, y_points = plate_carree_transform(azimuth_points, polar_points) + + # Determine color and label + color = get_color(arc_colors, idx) + label = labels[idx] if labels is not None and idx < len(labels) else None + + # Plot the arc as a line + ax.plot( + np.degrees(x_points), + np.degrees(y_points), + linewidth=2, + alpha=0.8, + c=color, + label=label, + marker="o", + markersize=3, + ) + + # Plot quaternions as vectors + if quaternions is not None: + for idx, quat in enumerate(quaternions): + # Convert quaternion to spherical coordinates + azimuth, polar = quat.vector_spherical + + # Apply Plate Carrée projection to the endpoint + x_end, y_end = plate_carree_transform(azimuth, polar) + + # Origin is at (0, 0) in this projection + x_origin, y_origin = plate_carree_transform(0, np.pi / 2) + + # Determine color and label + color = get_color(quaternion_colors, idx) + label = labels[idx] if labels is not None and idx < len(labels) else None + + if label is None: + label = f"Quat {idx + 1}: w:{quat.w:.1f}, x:{quat.x:.1f}, y:{quat.y:.1f}, z:{quat.z:.1f}" + + # Plot vector as an arrow from origin to the point + ax.annotate( + "", + xy=(np.degrees(x_end), np.degrees(y_end)), + xytext=(np.degrees(x_origin), np.degrees(y_origin)), + arrowprops=dict( + arrowstyle="->", + lw=2, + color=color, + alpha=0.8, + ), + ) + + # Plot endpoint + ax.scatter( + np.degrees(x_end), + np.degrees(y_end), + s=100, + marker="*", + c=color, + label=label, + zorder=10, + ) + + # Set up axes + ax.set_xlabel("Azimuth (deg)", fontsize=12) + ax.set_ylabel("Latitude (deg)", fontsize=12) + ax.set_xlim(-180, 180) + + # Add grid + ax.grid(True, alpha=0.3) + + # Set ticks + ax.set_xticks([-180, -90, 0, 90, 180]) + ax.set_xticklabels(["-180°", "-90°", "0°", "90°", "180°"]) + + ax.set_title(title, fontsize=14) + + if labels is not None: + ax.legend() + + # Only call tight_layout and show if we created the axes + if created_ax: + plt.tight_layout() + if show_plot: + plt.show() + + return fig, ax + + +def plot_unit_spherical_polar( + circles: List[UnitSphericalSmallCircle] | None = None, + arcs: List[UnitSphericalArc] | None = None, + quaternions: List[Quaternion] | None = None, + num_points: int = 120, + figsize: Tuple[int, int] = (8, 8), + title: str = "Small Circles on Sphere (Polar View)", + labels: Optional[List[str]] = None, + ax: Optional[Axes] = None, + show_plot: bool = False, + arc_colors: Optional[List[str]] = ["red", "green", "blue"], + circle_colors: Optional[List[str]] = ["red", "green", "blue"], + quaternion_colors: Optional[List[str]] = ["red", "green", "blue"], +) -> Tuple[Figure, Axes]: + """ + Plot small circles and arcs on a sphere using polar projection. + Azimuth is shown as the angle, polar angle is mapped to radius. + + Parameters: + ----------- + circles : List[UnitSphericalSmallCircle] or array-like + List of UnitSphericalSmallCircle objects or array of [azimuth, polar, radius] lists + arcs : List[UnitSphericalArc], optional + List of UnitSphericalArc objects. + Each arc starts at (azimuth, polar) and extends along a great circle + for the specified arc_length (positive or negative) + num_points : int, optional + Number of points to generate around each circle (default: 120) + figsize : tuple, optional + Figure size (default: (8, 8)) + title : str, optional + Plot title + labels : list, optional + List of labels for each element + ax : matplotlib.axes.Axes, optional + Axes to plot on. If None, creates new figure and axes + show_plot : bool, optional + Whether to show the plot (default: False, ignored if ax is provided) + arc_colors : List[str], optional + List of colors for arcs. Defaults to ["red", "green", "blue"]. + If insufficient colors, uses matplotlib's color cycle. + circle_colors : List[str], optional + List of colors for circles. Defaults to ["red", "green", "blue"]. + If insufficient colors, uses matplotlib's color cycle. + quaternion_colors : List[str], optional + List of colors for quaternions. Defaults to ["red", "green", "blue"]. + If insufficient colors, uses matplotlib's color cycle. + + Returns: + -------- + fig, ax : matplotlib figure and axes objects + """ + + # Helper function to get color with fallback to matplotlib's color cycle + def get_color(color_list: Optional[List[str]], idx: int) -> str: + """Get color from list or fallback to matplotlib's color cycle.""" + if color_list is None or len(color_list) == 0: + return f"C{idx}" + if idx < len(color_list): + return color_list[idx] + return f"C{idx}" + + # Track whether we created the axes or it was provided + created_ax = ax is None + + if ax is None: + fig, ax = plt.subplots(figsize=figsize, subplot_kw={"projection": "polar"}) + else: + fig = ax.get_figure() + + if circles is not None: + for idx, circle in enumerate(circles): + + # Generate true spherical small circle points + azimuth_points, polar_points = generate_spherical_small_circle_points( + circle, num_points + ) + + # Map polar angle to radius (0 to 1) for polar plot + # polar ranges from -pi/2 to pi/2, map to 0 to 1 + radius_points = (polar_points + np.pi / 2) / np.pi + + # Handle azimuth wraparound by detecting large jumps in the ORIGINAL order + # Insert NaN values at discontinuities to break the line + azimuth_plot = [azimuth_points[0]] + radius_plot = [radius_points[0]] + + for i in range(1, len(azimuth_points)): + # If there's a jump greater than π, we've crossed the -π to π discontinuity + if abs(azimuth_points[i] - azimuth_points[i - 1]) > np.pi: + # Insert NaN to break the line + azimuth_plot.append(np.nan) + radius_plot.append(np.nan) + + azimuth_plot.append(azimuth_points[i]) + radius_plot.append(radius_points[i]) + + # Close the circle by connecting back to the first point + # Check if we need to handle wraparound at the end + if abs(azimuth_points[0] - azimuth_points[-1]) > np.pi: + # Large jump at the end - don't connect + pass + else: + # Normal case - connect back to start + azimuth_plot.append(azimuth_points[0]) + radius_plot.append(radius_points[0]) + + azimuth_plot = np.array(azimuth_plot) + radius_plot = np.array(radius_plot) + + # Convert radius to degrees for legend + radius_deg = np.degrees(circle.radius_angle) + azimuth_deg = np.degrees(circle.azimuth) + polar_deg = np.degrees(circle.polar) + label = f"Circle {idx + 1}: A:{azimuth_deg:.1f}°, P:{polar_deg:.1f}°, R:{radius_deg:.1f}°" + + # Get color for this circle + color = get_color(circle_colors, idx) + + # Plot in polar coordinates using plot instead of scatter for continuous lines + ax.plot( + azimuth_plot, + radius_plot, + linewidth=2, + alpha=0.7, + label=label, + color=color, + ) + + # Generate points for each arc + if arcs is not None: + for idx, arc in enumerate(arcs): + + # Generate spherical arc points + azimuth_points, polar_points = generate_spherical_arc_points( + arc, num_points + ) + + # Map polar angle to radius (0 to 1) for polar plot + radius_points = (polar_points + np.pi / 2) / np.pi + + # Handle azimuth wraparound + azimuth_plot = [azimuth_points[0]] + radius_plot = [radius_points[0]] + + for i in range(1, len(azimuth_points)): + # If there's a jump greater than π, we've crossed the -π to π discontinuity + if abs(azimuth_points[i] - azimuth_points[i - 1]) > np.pi: + # Insert NaN to break the line + azimuth_plot.append(np.nan) + radius_plot.append(np.nan) + + azimuth_plot.append(azimuth_points[i]) + radius_plot.append(radius_points[i]) + + azimuth_plot = np.array(azimuth_plot) + radius_plot = np.array(radius_plot) + + # Convert to degrees for legend + arc_length_deg = np.degrees(arc.arc_length) + azimuth_deg = np.degrees(arc.azimuth) + polar_deg = np.degrees(arc.polar) + orient_deg = np.degrees(arc.orient) + label = f"Arc {idx + 1}: A:{azimuth_deg:.1f}°, P:{polar_deg:.1f}°, L:{arc_length_deg:.1f}°, O:{orient_deg:.1f}°" + + # Get color for this arc + color = get_color(arc_colors, idx) + + # Plot the arc + ax.plot( + azimuth_plot, + radius_plot, + linewidth=2, + alpha=0.8, + label=label, + marker="o", + markersize=3, + color=color, + ) + + # Plot quaternions as vectors + if quaternions is not None: + for idx, quat in enumerate(quaternions): + # Convert quaternion to spherical coordinates + azimuth, polar = quat.vector_spherical + + # Map polar angle to radius (0 to 1) for polar plot + radius = (polar + np.pi / 2) / np.pi + + # Get color for this quaternion + color = get_color(quaternion_colors, idx) + + # Plot vector as an arrow from origin + ax.annotate( + "", + xy=(azimuth, radius), + xytext=(0, 0), + arrowprops=dict(arrowstyle="->", lw=2, color=color, alpha=0.8), + ) + + # Plot endpoint + azimuth_deg = np.degrees(azimuth) + polar_deg = np.degrees(polar) + label = f"Quat {idx + 1}: w:{quat.w:.1f}, x:{quat.x:.1f}, y:{quat.y:.1f}, z:{quat.z:.1f}" + + ax.scatter( + azimuth, radius, s=150, marker="*", label=label, c=color, zorder=10 + ) + + ax.set_title(title, fontsize=14, pad=20) + + if labels is not None: + ax.legend(loc="upper right", bbox_to_anchor=(1.3, 1.1)) + + ax.grid(True, alpha=0.3) + + # Only call tight_layout and show if we created the axes + if created_ax: + plt.tight_layout() + if show_plot: + plt.show() + + return fig, ax + + +def plot_unit_spherical_3d( + circles: List[UnitSphericalSmallCircle] | None = None, + arcs: List[UnitSphericalArc] | None = None, + quaternions: List[Quaternion] | None = None, + num_points: int = 120, + figsize: Tuple[int, int] = (10, 10), + title: str = "Small Circles on Unit Sphere (3D)", + show_sphere: bool = True, + show_legend: bool = True, + show_plot: bool = False, + ax: Optional[Axes] = None, + arc_colors: Optional[List[str]] = ["red", "green", "blue"], + circle_colors: Optional[List[str]] = ["red", "green", "blue"], + quaternion_colors: Optional[List[str]] = ["red", "green", "blue"], +) -> Tuple[Figure, Axes]: + """ + Plot small circles and arcs on a 3D unit sphere. + + Parameters: + ----------- + circles : List[UnitSphericalSmallCircle] or array-like + List of UnitSphericalSmallCircle objects or array of [azimuth, polar, radius] lists + arcs : List[UnitSphericalArc], optional + List of UnitSphericalArc objects. + Each arc starts at (azimuth, polar) and extends along a great circle + for the specified arc_length (positive or negative) + num_points : int, optional + Number of points to generate around each circle (default: 120) + figsize : tuple, optional + Figure size (default: (10, 10), ignored if ax is provided) + title : str, optional + Plot title + show_sphere : bool, optional + Whether to show the sphere surface (default: True) + show_legend : bool, optional + Whether to show the legend (default: True) + show_plot : bool, optional + Whether to show the plot (default: False, ignored if ax is provided) + ax : matplotlib.axes.Axes, optional + 3D axes to plot on. If None, creates new figure and axes + arc_colors : List[str], optional + List of colors for arcs. Defaults to ["red", "green", "blue"]. + If insufficient colors, uses matplotlib's color cycle. + circle_colors : List[str], optional + List of colors for circles. Defaults to ["red", "green", "blue"]. + If insufficient colors, uses matplotlib's color cycle. + quaternion_colors : List[str], optional + List of colors for quaternions. Defaults to ["red", "green", "blue"]. + If insufficient colors, uses matplotlib's color cycle. + + Returns: + -------- + fig, ax : matplotlib figure and axes objects + """ + + # Helper function to get color with fallback to matplotlib's color cycle + def get_color(color_list: Optional[List[str]], idx: int) -> str: + """Get color from list or fallback to matplotlib's color cycle.""" + if color_list is None or len(color_list) == 0: + return f"C{idx}" + if idx < len(color_list): + return color_list[idx] + return f"C{idx}" + + # Track whether we created the axes or it was provided + created_ax = ax is None + + if ax is None: + fig = plt.figure(figsize=figsize) + ax = fig.add_subplot(111, projection="3d") + else: + fig = ax.get_figure() + + # Optionally plot the sphere surface (physics convention) + if show_sphere: + phi = np.linspace(0, 2 * np.pi, 30) # azimuth + theta = np.linspace(0, np.pi, 20) # polar angle from north to south + x_sphere = np.outer(np.sin(theta), np.cos(phi)) + y_sphere = np.outer(np.sin(theta), np.sin(phi)) + z_sphere = np.outer(np.cos(theta), np.ones(np.size(phi))) + ax.plot_surface(x_sphere, y_sphere, z_sphere, alpha=0.1, color="lightblue") + + # Plot each circle + if circles is not None: + for idx, circle in enumerate(circles): + + # Generate true spherical small circle points + azimuth_points, polar_points = generate_spherical_small_circle_points( + circle, num_points + ) + + # Convert to Cartesian coordinates (unit sphere, r=1, physics convention) + # theta (polar) from 0 (north pole) to pi (south pole) + # phi (azimuth) rotation about Z axis + x = np.sin(polar_points) * np.cos(azimuth_points) + y = np.sin(polar_points) * np.sin(azimuth_points) + z = np.cos(polar_points) + + # Convert radius to degrees for legend + radius_deg = np.degrees(circle.radius_angle) + azimuth_deg = np.degrees(circle.azimuth) + polar_deg = np.degrees(circle.polar) + label = f"Circle {idx + 1}: A:{azimuth_deg:.1f}°, P:{polar_deg:.1f}°, R:{radius_deg:.1f}°" + + # Get color for this circle + color = get_color(circle_colors, idx) + + # Plot the circle + ax.scatter(x, y, z, s=2, alpha=0.8, label=label, c=color) + + # Plot each arc + if arcs is not None: + for idx, arc in enumerate(arcs): + + # Generate spherical arc points + azimuth_points, polar_points = generate_spherical_arc_points( + arc, num_points + ) + + # Convert to Cartesian coordinates (unit sphere, r=1, physics convention) + x = np.sin(polar_points) * np.cos(azimuth_points) + y = np.sin(polar_points) * np.sin(azimuth_points) + z = np.cos(polar_points) + + # Convert to degrees for legend + arc_length_deg = np.degrees(arc.arc_length) + azimuth_deg = np.degrees(arc.azimuth) + polar_deg = np.degrees(arc.polar) + orient_deg = np.degrees(arc.orient) + label = f"Arc {idx + 1}: A:{azimuth_deg:.1f}°, P:{polar_deg:.1f}°, L:{arc_length_deg:.1f}°, O:{orient_deg:.1f}°" + + # Get color for this arc + color = get_color(arc_colors, idx) + + # Plot the arc as a line + ax.plot( + x, + y, + z, + linewidth=1.5, + alpha=0.8, + label=label, + marker="o", + markersize=1, + color=color, + ) + + # Plot quaternions as vectors + if quaternions is not None: + for idx, quat in enumerate(quaternions): + # Convert quaternion to Cartesian coordinates + x, y, z = quat.vector_cartesian + + # Get color for this quaternion + color = get_color(quaternion_colors, idx) + + # Plot vector from origin to the point + ax.quiver( + 0, + 0, + 0, + x, + y, + z, + length=1.0, + arrow_length_ratio=0.05, + color=color, + linewidth=1.5, + alpha=0.9, + ) + + # Plot endpoint + label = f"Quat {idx + 1}: w:{quat.w:.1f}, x:{quat.x:.1f}, y:{quat.y:.1f}, z:{quat.z:.1f}" + + ax.scatter(x, y, z, s=50, marker="*", label=label, c=color, zorder=10) + + ax.set_xlabel("X") + ax.set_ylabel("Y") + ax.set_zlabel("Z") + ax.set_title(title, fontsize=14) + + # Add legend if requested, positioned outside the plot area + if show_legend: + ax.legend(loc="upper left", bbox_to_anchor=(1.05, 1.0)) + + # Set equal aspect ratio + ax.set_box_aspect([1, 1, 1]) + + # Set initial view angle: physics convention with Z up, Y right, X down-left + # elev=20 looks from slightly above, azim=45 gives the proper orientation + ax.view_init(elev=20, azim=45) + + # Only call tight_layout and show if we created the axes + if created_ax: + plt.tight_layout() + if show_plot: + plt.show() + + return fig, ax + + +def plot_unit_spherical_multiplot( + circles: List[UnitSphericalSmallCircle] | None = None, + arcs: List[UnitSphericalArc] | None = None, + quaternions: List[Quaternion] | None = None, + num_points: int = 120, + figsize: Tuple[int, int] = (24, 6), + title: str = "Small Circles on Unit Sphere - Multiple Views", + show_plot: bool = False, + arc_colors: Optional[List[str]] = ["red", "green", "blue"], + circle_colors: Optional[List[str]] = ["red", "green", "blue"], + quaternion_colors: Optional[List[str]] = ["red", "green", "blue"], +) -> Figure: + """ + Create a multiplot showing Plate Carrée, Top View, and 3D views of spherical small circles. + + Parameters: + ----------- + circles : List[UnitSphericalSmallCircle] or array-like + List of UnitSphericalSmallCircle objects or array of [azimuth, polar, radius] lists + arcs : List[UnitSphericalArc], optional + List of UnitSphericalArc objects + quaternions : List[Quaternion], optional + List of Quaternion objects + num_points : int, optional + Number of points to generate around each circle (default: 120) + figsize : tuple, optional + Figure size (default: (24, 6)) + title : str, optional + Overall title + show_plot : bool, optional + Whether to show the plot (default: False) + arc_colors : List[str], optional + List of colors for arcs. Defaults to ["red", "green", "blue"]. + If insufficient colors, uses matplotlib's color cycle. + circle_colors : List[str], optional + List of colors for circles. Defaults to ["red", "green", "blue"]. + If insufficient colors, uses matplotlib's color cycle. + quaternion_colors : List[str], optional + List of colors for quaternions. Defaults to ["red", "green", "blue"]. + If insufficient colors, uses matplotlib's color cycle. + + Returns: + -------- + fig : matplotlib figure object + """ + # Create figure with overall title + fig = plt.figure(figsize=figsize) + fig.suptitle(title, fontsize=16) + + # Create subplots + ax1 = fig.add_subplot(131) # Plate Carrée projection + ax2 = fig.add_subplot(132, projection="polar") # Top view + ax3 = fig.add_subplot(133, projection="3d") # 3D view + + # 1. Plate Carrée projection using advanced plot function + plot_unit_spherical_advanced( + circles=circles, + arcs=arcs, + quaternions=quaternions, + num_points=num_points, + title="Plate Carrée Projection", + show_centers=False, + ax=ax1, + arc_colors=arc_colors, + circle_colors=circle_colors, + quaternion_colors=quaternion_colors, + ) + # Customize for multiplot + ax1.set_xlabel("Azimuth (deg)", fontsize=10) + ax1.set_ylabel("Latitude (deg)", fontsize=10) + ax1.set_title("Plate Carrée Projection", fontsize=12) + + # 2. Top view (looking down z-axis) + plot_unit_spherical_polar( + circles=circles, + arcs=arcs, + quaternions=quaternions, + num_points=num_points, + title="Top View (Down Z-Axis)", + ax=ax2, + arc_colors=arc_colors, + circle_colors=circle_colors, + quaternion_colors=quaternion_colors, + ) + # Customize for multiplot + ax2.set_xlabel("Azimuth (deg)", fontsize=10) + ax2.set_ylabel("Latitude (deg)", fontsize=10) + ax2.set_title("Top View (Down Z-Axis)", fontsize=12) + + # 3. 3D view + plot_unit_spherical_3d( + circles=circles, + arcs=arcs, + quaternions=quaternions, + num_points=num_points, + title="3D View", + show_sphere=True, + show_legend=True, + ax=ax3, + arc_colors=arc_colors, + circle_colors=circle_colors, + quaternion_colors=quaternion_colors, + ) + # Customize for multiplot + ax3.set_xlabel("X", fontsize=10) + ax3.set_ylabel("Y", fontsize=10) + ax3.set_zlabel("Z", fontsize=10) + ax3.set_title("3D View", fontsize=12) + # Position legend outside plot area with smaller font + ax3.legend(loc="upper left", bbox_to_anchor=(1.05, 1.0), fontsize=8) + + plt.tight_layout() + + if show_plot: + plt.show() + + return fig + + +def demo() -> None: + """Demonstration of multiplot view showing spherical small circles in three different projections.""" + + # Define 4 circles using UnitSphericalSmallCircle dataclass + circles: List[UnitSphericalSmallCircle] = [ + UnitSphericalSmallCircle( + azimuth=0, + polar=0, + radius_angle=deg45, + ), + UnitSphericalSmallCircle( + azimuth=0, + polar=np.pi / 2, + radius_angle=deg45, + ), + UnitSphericalSmallCircle( + azimuth=0, + polar=np.pi, + radius_angle=deg45, + ), + UnitSphericalSmallCircle( + azimuth=0, + polar=-np.pi / 2, + radius_angle=deg45, + ), + UnitSphericalSmallCircle( + azimuth=-np.pi / 2, + polar=np.pi / 2, + radius_angle=deg45, + ), + UnitSphericalSmallCircle( + azimuth=np.pi / 2, + polar=np.pi / 2, + radius_angle=deg45, + ), + UnitSphericalSmallCircle( + azimuth=-deg45, + polar=deg45, + radius_angle=deg45, + ), + UnitSphericalSmallCircle( + azimuth=np.deg2rad(135), + polar=deg45, + radius_angle=deg45, + ), + ] + + arcs: List[UnitSphericalArc] = [ + UnitSphericalArc(orient=0, azimuth=0, polar=0, arc_length=deg22_5), + UnitSphericalArc(orient=deg45, azimuth=0, polar=0, arc_length=deg22_5), + UnitSphericalArc(orient=deg90, azimuth=0, polar=0, arc_length=deg22_5), + UnitSphericalArc(orient=0, azimuth=0, polar=deg45, arc_length=deg22_5), + UnitSphericalArc(orient=0, azimuth=deg45, polar=deg45, arc_length=deg22_5), + UnitSphericalArc(orient=0, azimuth=deg90, polar=deg45, arc_length=deg22_5), + UnitSphericalArc(orient=deg45, azimuth=0, polar=deg45, arc_length=deg22_5), + UnitSphericalArc(orient=deg45, azimuth=deg45, polar=deg45, arc_length=deg22_5), + UnitSphericalArc(orient=deg45, azimuth=deg90, polar=deg45, arc_length=deg22_5), + UnitSphericalArc(orient=deg90, azimuth=0, polar=deg45, arc_length=deg22_5), + UnitSphericalArc(orient=deg90, azimuth=deg45, polar=deg45, arc_length=deg22_5), + UnitSphericalArc(orient=deg90, azimuth=deg90, polar=deg45, arc_length=deg22_5), + UnitSphericalArc(orient=0, azimuth=0, polar=deg45, arc_length=-deg22_5), + UnitSphericalArc(orient=0, azimuth=deg45, polar=deg45, arc_length=-deg22_5), + UnitSphericalArc(orient=0, azimuth=deg90, polar=deg45, arc_length=-deg22_5), + ] + + quats: List[Quaternion] = [ + Quaternion.from_components(w=1, x=0, y=0, z=0), + Quaternion.from_components(w=0, x=1, y=0, z=0), + Quaternion.from_components(w=0, x=0, y=1, z=0), + Quaternion.from_components(w=0, x=0, y=0, z=1), + Quaternion.from_components(w=0, x=0.707, y=0, z=0.707), + Quaternion.from_components(w=0, x=0.707, y=0.707, z=0), + Quaternion.from_components(w=0, x=0, y=0.707, z=0.707), + Quaternion.from_components(w=0.707, x=0.707, y=0, z=0), + Quaternion.from_components(w=0.707, x=0, y=0.707, z=0), + Quaternion.from_components(w=0.707, x=0, y=0, z=0.707), + Quaternion.from_components(w=0, x=0.577, y=-0.577, z=0.577), + ] + + _ = plot_unit_spherical_multiplot( + # circles=circles, + # arcs=arcs, + quaternions=quats, + show_plot=True, + ) + + +if __name__ == "__main__": + demo() diff --git a/src/pyMathToolsPlotHelpers/plotUnitSphericalSmallCircles.py b/src/pyMathToolsPlotHelpers/plotUnitSphericalSmallCircles.py deleted file mode 100644 index 7213cb5..0000000 --- a/src/pyMathToolsPlotHelpers/plotUnitSphericalSmallCircles.py +++ /dev/null @@ -1,605 +0,0 @@ -from typing import List, Union -import numpy as np -import matplotlib.pyplot as plt -from mpl_toolkits.mplot3d import Axes3D -from foundationTypes.mathTypes.UnitSphericalSmallCircle import UnitSphericalSmallCircle - - -def generate_spherical_small_circle_points(azimuth_center, polar_center, radius_angle, num_points=120): - """ - Generate points on a true small circle on a unit sphere. - - A small circle is the set of all points at a fixed angular distance (radius_angle) - from a center point on the sphere. - - Parameters: - ----------- - azimuth_center : float - Azimuth angle of circle center (radians, 0 to 2π) - polar_center : float - Polar angle of circle center (radians, -π/2 to π/2) - radius_angle : float - Angular radius of the circle (radians) - num_points : int - Number of points to generate around the circle - - Returns: - -------- - azimuth_points : ndarray - Azimuth angles of points on the circle - polar_points : ndarray - Polar angles of points on the circle - """ - # Convert center to Cartesian coordinates - x_center = np.cos(polar_center) * np.cos(azimuth_center) - y_center = np.cos(polar_center) * np.sin(azimuth_center) - z_center = np.sin(polar_center) - center = np.array([x_center, y_center, z_center]) - - # Create orthonormal basis at the center point - # North pole direction - north = np.array([0, 0, 1]) - - # Tangent vector 1 (east direction) - if np.abs(z_center) < 0.999: # Not at poles - east = np.cross(north, center) - east = east / np.linalg.norm(east) - else: # At poles, use arbitrary east direction - east = np.array([1, 0, 0]) - - # Tangent vector 2 (north direction in tangent plane) - north_tangent = np.cross(center, east) - north_tangent = north_tangent / np.linalg.norm(north_tangent) - - # Generate points around the circle - theta = np.linspace(0, 2 * np.pi, num_points) - - # Points on the small circle using rotation - # The small circle is at angular distance radius_angle from center - azimuth_points = np.zeros(num_points) - polar_points = np.zeros(num_points) - - for i, t in enumerate(theta): - # Local coordinates: move radius_angle away from center in direction t - local_direction = np.cos(t) * east + np.sin(t) * north_tangent - - # Rotate center point toward local_direction by radius_angle - # Using Rodrigues' rotation formula - axis = np.cross(center, local_direction) - axis_norm = np.linalg.norm(axis) - - if axis_norm > 1e-10: - axis = axis / axis_norm - # Rotate center around axis by radius_angle - cos_angle = np.cos(radius_angle) - sin_angle = np.sin(radius_angle) - - point = (cos_angle * center + - sin_angle * np.cross(axis, center) + - (1 - cos_angle) * np.dot(axis, center) * axis) - else: - # If no rotation needed (shouldn't happen) - point = center - - # Convert back to spherical coordinates - x, y, z = point - r = np.sqrt(x**2 + y**2 + z**2) - x, y, z = x/r, y/r, z/r # Normalize to unit sphere - - polar_points[i] = np.arcsin(np.clip(z, -1, 1)) - azimuth_points[i] = np.arctan2(y, x) - if azimuth_points[i] < 0: - azimuth_points[i] += 2 * np.pi - - return azimuth_points, polar_points - - -def mercator_projection(azimuth, polar): - """ - Apply Mercator projection to spherical coordinates. - - Parameters: - ----------- - azimuth : float or ndarray - Azimuth angle(s) in radians (0 to 2π) - polar : float or ndarray - Polar angle(s) in radians (-π/2 to π/2) - - Returns: - -------- - x : float or ndarray - Mercator x coordinate (longitude) - y : float or ndarray - Mercator y coordinate (latitude projection) - """ - x = azimuth - # Mercator projection: y = ln(tan(π/4 + lat/2)) - # Clamp polar to avoid infinities at poles - polar_clamped = np.clip(polar, -np.pi/2 + 0.001, np.pi/2 - 0.001) - y = np.log(np.tan(np.pi/4 + polar_clamped/2)) - return x, y - - -def plot_spherical_small_circles( - circles: Union[List[UnitSphericalSmallCircle], List[List[float]]], - num_points=120, - figsize=(10, 6), - title="Small Circles on Sphere (Mercator Projection)", -): - """ - Plot small circles on a sphere using Mercator projection. - - Parameters: - ----------- - circles : List[UnitSphericalSmallCircle] or array-like - List of UnitSphericalSmallCircle objects or array of [azimuth, polar, radius] lists. - Each circle represents a small circle on the unit sphere - num_points : int, optional - Number of points to generate around each circle (default: 120) - figsize : tuple, optional - Figure size (default: (10, 6)) - title : str, optional - Plot title - - Returns: - -------- - fig, ax : matplotlib figure and axes objects - """ - fig, ax = plt.subplots(figsize=figsize) - - # Convert UnitSphericalSmallCircle objects to arrays if needed - if circles and isinstance(circles[0], UnitSphericalSmallCircle): - circles = [[c.azimuth, c.polar, c.radius_angle] for c in circles] - - # Convert to numpy array - circles = np.array(circles) - if circles.ndim == 1: - circles = circles.reshape(1, -1) - - # Generate points for each circle - for idx, circle in enumerate(circles): - azimuth_center, polar_center, radius_angle = circle - - # Generate true spherical small circle points - azimuth_points, polar_points = generate_spherical_small_circle_points( - azimuth_center, polar_center, radius_angle, num_points - ) - - # Apply Mercator projection - x_points, y_points = mercator_projection(azimuth_points, polar_points) - - # Convert radius to degrees for legend - radius_deg = np.degrees(radius_angle) - label = f"Circle {idx + 1}: {radius_deg:.1f}°" - - # Plot the circle - ax.scatter(x_points, y_points, s=10, alpha=0.6, label=label) - - # Set up axes - ax.set_xlabel("Longitude (radians)", fontsize=12) - ax.set_ylabel("Mercator Y", fontsize=12) - ax.set_xlim(0, 2 * np.pi) - - # Add grid - ax.grid(True, alpha=0.3) - - # Set x-axis ticks to show multiples of pi - ax.set_xticks([0, np.pi / 2, np.pi, 3 * np.pi / 2, 2 * np.pi]) - ax.set_xticklabels([r"$0$", r"$\pi/2$", r"$\pi$", r"$3\pi/2$", r"$2\pi$"]) - - ax.set_title(title, fontsize=14) - - # Add legend - ax.legend() - - plt.tight_layout() - - return fig, ax - - -def plot_spherical_small_circles_advanced( - circles: Union[List[UnitSphericalSmallCircle], List[List[float]]], - num_points=120, - figsize=(10, 6), - title="Small Circles on Sphere (Flattened)", - colors=None, - labels=None, - show_centers=True, -): - """ - Advanced version with more customization options. - - Parameters: - ----------- - circles : List[UnitSphericalSmallCircle] or array-like - List of UnitSphericalSmallCircle objects or array of [azimuth, polar, radius] lists - num_points : int, optional - Number of points to generate around each circle - figsize : tuple, optional - Figure size - title : str, optional - Plot title - colors : list, optional - List of colors for each circle - labels : list, optional - List of labels for each circle - show_centers : bool, optional - Whether to show center points (default: True) - - Returns: - -------- - fig, ax : matplotlib figure and axes objects - """ - fig, ax = plt.subplots(figsize=figsize) - - # Convert UnitSphericalSmallCircle objects to arrays if needed - if circles and isinstance(circles[0], UnitSphericalSmallCircle): - circles = [[c.azimuth, c.polar, c.radius_angle] for c in circles] - - circles = np.array(circles) - if circles.ndim == 1: - circles = circles.reshape(1, -1) - - for idx, circle in enumerate(circles): - azimuth_center, polar_center, radius_angle = circle - - # Generate true spherical small circle points - azimuth_points, polar_points = generate_spherical_small_circle_points( - azimuth_center, polar_center, radius_angle, num_points - ) - - # Apply Mercator projection - x_points, y_points = mercator_projection(azimuth_points, polar_points) - - # Determine color and label - color = colors[idx] if colors is not None and idx < len(colors) else None - label = labels[idx] if labels is not None and idx < len(labels) else None - - # Plot the circle - ax.scatter(x_points, y_points, s=10, alpha=0.6, c=color, label=label) - - # Plot center point if requested - if show_centers: - x_center, y_center = mercator_projection(azimuth_center, polar_center) - ax.scatter( - x_center, - y_center, - s=100, - marker="x", - c=color if color else "red", - linewidths=2, - ) - - # Set up axes - ax.set_xlabel("Longitude (radians)", fontsize=12) - ax.set_ylabel("Mercator Y", fontsize=12) - ax.set_xlim(0, 2 * np.pi) - - # Add grid - ax.grid(True, alpha=0.3) - - # Set ticks - ax.set_xticks([0, np.pi / 2, np.pi, 3 * np.pi / 2, 2 * np.pi]) - ax.set_xticklabels([r"$0$", r"$\pi/2$", r"$\pi$", r"$3\pi/2$", r"$2\pi$"]) - - ax.set_title(title, fontsize=14) - - if labels is not None: - ax.legend() - - plt.tight_layout() - - return fig, ax - - -def plot_spherical_small_circles_polar( - circles: Union[List[UnitSphericalSmallCircle], List[List[float]]], - num_points=120, - figsize=(8, 8), - title="Small Circles on Sphere (Polar View)", -): - """ - Plot small circles on a sphere using polar projection. - Azimuth is shown as the angle, polar angle is mapped to radius. - - Parameters: - ----------- - circles : List[UnitSphericalSmallCircle] or array-like - List of UnitSphericalSmallCircle objects or array of [azimuth, polar, radius] lists - num_points : int, optional - Number of points to generate around each circle (default: 120) - figsize : tuple, optional - Figure size (default: (8, 8)) - title : str, optional - Plot title - - Returns: - -------- - fig, ax : matplotlib figure and axes objects - """ - fig, ax = plt.subplots(figsize=figsize, subplot_kw={'projection': 'polar'}) - - # Convert UnitSphericalSmallCircle objects to arrays if needed - if circles and isinstance(circles[0], UnitSphericalSmallCircle): - circles = [[c.azimuth, c.polar, c.radius_angle] for c in circles] - - circles = np.array(circles) - if circles.ndim == 1: - circles = circles.reshape(1, -1) - - for idx, circle in enumerate(circles): - azimuth_center, polar_center, radius_angle = circle - - # Generate angles around the circle - theta = np.linspace(0, 2 * np.pi, num_points) - - # Generate circle points in spherical space - azimuth_points = azimuth_center + radius_angle * np.cos(theta) - polar_points = polar_center + radius_angle * np.sin(theta) - - # Handle wrapping for azimuth (0 to 2*pi) - azimuth_points = np.mod(azimuth_points, 2 * np.pi) - - # Clip polar angle to valid range (-pi/2 to pi/2) - polar_points = np.clip(polar_points, -np.pi / 2, np.pi / 2) - - # Map polar angle to radius (0 to 1) - # polar ranges from -pi/2 to pi/2, map to 0 to 1 - radius_points = (polar_points + np.pi / 2) / np.pi - - # Convert radius to degrees for legend - radius_deg = np.degrees(radius_angle) - label = f"Circle {idx + 1}: {radius_deg:.1f}°" - - # Plot in polar coordinates - ax.scatter(azimuth_points, radius_points, s=10, alpha=0.6, label=label) - - ax.set_title(title, fontsize=14, pad=20) - ax.legend(loc='upper right', bbox_to_anchor=(1.3, 1.1)) - ax.grid(True, alpha=0.3) - - plt.tight_layout() - - return fig, ax - - -def plot_spherical_small_circles_3d( - circles: Union[List[UnitSphericalSmallCircle], List[List[float]]], - num_points=120, - figsize=(10, 10), - title="Small Circles on Unit Sphere (3D)", - show_sphere=True, -): - """ - Plot small circles on a 3D unit sphere. - - Parameters: - ----------- - circles : List[UnitSphericalSmallCircle] or array-like - List of UnitSphericalSmallCircle objects or array of [azimuth, polar, radius] lists - num_points : int, optional - Number of points to generate around each circle (default: 120) - figsize : tuple, optional - Figure size (default: (10, 10)) - title : str, optional - Plot title - show_sphere : bool, optional - Whether to show the sphere surface (default: True) - - Returns: - -------- - fig, ax : matplotlib figure and axes objects - """ - fig = plt.figure(figsize=figsize) - ax = fig.add_subplot(111, projection='3d') - - # Convert UnitSphericalSmallCircle objects to arrays if needed - if circles and isinstance(circles[0], UnitSphericalSmallCircle): - circles = [[c.azimuth, c.polar, c.radius_angle] for c in circles] - - circles = np.array(circles) - if circles.ndim == 1: - circles = circles.reshape(1, -1) - - # Optionally plot the sphere surface - if show_sphere: - u = np.linspace(0, 2 * np.pi, 30) - v = np.linspace(-np.pi / 2, np.pi / 2, 20) - x_sphere = np.outer(np.cos(v), np.cos(u)) - y_sphere = np.outer(np.cos(v), np.sin(u)) - z_sphere = np.outer(np.sin(v), np.ones(np.size(u))) - ax.plot_surface(x_sphere, y_sphere, z_sphere, alpha=0.1, color='lightblue') - - # Plot each circle - for idx, circle in enumerate(circles): - azimuth_center, polar_center, radius_angle = circle - - # Generate points around the small circle on the sphere - theta = np.linspace(0, 2 * np.pi, num_points) - - # Generate circle points in spherical coordinates - azimuth_points = azimuth_center + radius_angle * np.cos(theta) - polar_points = polar_center + radius_angle * np.sin(theta) - - # Handle wrapping and clipping - azimuth_points = np.mod(azimuth_points, 2 * np.pi) - polar_points = np.clip(polar_points, -np.pi / 2, np.pi / 2) - - # Convert to Cartesian coordinates (unit sphere, r=1) - x = np.cos(polar_points) * np.cos(azimuth_points) - y = np.cos(polar_points) * np.sin(azimuth_points) - z = np.sin(polar_points) - - # Convert radius to degrees for legend - radius_deg = np.degrees(radius_angle) - label = f"Circle {idx + 1}: {radius_deg:.1f}°" - - # Plot the circle - ax.scatter(x, y, z, s=10, alpha=0.8, label=label) - - ax.set_xlabel('X') - ax.set_ylabel('Y') - ax.set_zlabel('Z') - ax.set_title(title, fontsize=14) - ax.legend() - - # Set equal aspect ratio - ax.set_box_aspect([1, 1, 1]) - - plt.tight_layout() - - return fig, ax - - -def plot_spherical_small_circles_multiplot( - circles: Union[List[UnitSphericalSmallCircle], List[List[float]]], - num_points=120, - figsize=(18, 6), - title="Small Circles on Unit Sphere - Multiple Views", -): - """ - Create a multiplot showing flattened, polar, and 3D views of spherical small circles. - - Parameters: - ----------- - circles : List[UnitSphericalSmallCircle] or array-like - List of UnitSphericalSmallCircle objects or array of [azimuth, polar, radius] lists - num_points : int, optional - Number of points to generate around each circle (default: 120) - figsize : tuple, optional - Figure size (default: (18, 6)) - title : str, optional - Overall title - - Returns: - -------- - fig : matplotlib figure object - """ - fig = plt.figure(figsize=figsize) - fig.suptitle(title, fontsize=16) - - # Convert UnitSphericalSmallCircle objects to arrays if needed - circles_array = circles - if circles and isinstance(circles[0], UnitSphericalSmallCircle): - circles_array = [[c.azimuth, c.polar, c.radius_angle] for c in circles] - - circles_array = np.array(circles_array) - if circles_array.ndim == 1: - circles_array = circles_array.reshape(1, -1) - - # 1. Flattened view - ax1 = fig.add_subplot(131) - for idx, circle in enumerate(circles_array): - azimuth_center, polar_center, radius_angle = circle - theta = np.linspace(0, 2 * np.pi, num_points) - azimuth_points = azimuth_center + radius_angle * np.cos(theta) - polar_points = polar_center + radius_angle * np.sin(theta) - azimuth_points = np.mod(azimuth_points, 2 * np.pi) - polar_points = np.clip(polar_points, -np.pi / 2, np.pi / 2) - radius_deg = np.degrees(radius_angle) - label = f"Circle {idx + 1}: {radius_deg:.1f}°" - ax1.scatter(azimuth_points, polar_points, s=10, alpha=0.6, label=label) - - ax1.set_xlabel("Azimuthal Angle (radians)", fontsize=10) - ax1.set_ylabel("Polar Angle (radians)", fontsize=10) - ax1.set_xlim(0, 2 * np.pi) - ax1.set_ylim(-np.pi / 2, np.pi / 2) - ax1.grid(True, alpha=0.3) - ax1.set_title("Flattened View", fontsize=12) - ax1.legend(fontsize=8) - - # 2. Polar view - ax2 = fig.add_subplot(132, projection='polar') - for idx, circle in enumerate(circles_array): - azimuth_center, polar_center, radius_angle = circle - theta = np.linspace(0, 2 * np.pi, num_points) - azimuth_points = azimuth_center + radius_angle * np.cos(theta) - polar_points = polar_center + radius_angle * np.sin(theta) - azimuth_points = np.mod(azimuth_points, 2 * np.pi) - polar_points = np.clip(polar_points, -np.pi / 2, np.pi / 2) - radius_points = (polar_points + np.pi / 2) / np.pi - radius_deg = np.degrees(radius_angle) - label = f"Circle {idx + 1}: {radius_deg:.1f}°" - ax2.scatter(azimuth_points, radius_points, s=10, alpha=0.6, label=label) - - ax2.set_title("Polar View", fontsize=12, pad=20) - ax2.grid(True, alpha=0.3) - ax2.legend(fontsize=8, loc='upper right', bbox_to_anchor=(1.2, 1.1)) - - # 3. 3D view - ax3 = fig.add_subplot(133, projection='3d') - - # Plot sphere surface - u = np.linspace(0, 2 * np.pi, 30) - v = np.linspace(-np.pi / 2, np.pi / 2, 20) - x_sphere = np.outer(np.cos(v), np.cos(u)) - y_sphere = np.outer(np.cos(v), np.sin(u)) - z_sphere = np.outer(np.sin(v), np.ones(np.size(u))) - ax3.plot_surface(x_sphere, y_sphere, z_sphere, alpha=0.1, color='lightblue') - - for idx, circle in enumerate(circles_array): - azimuth_center, polar_center, radius_angle = circle - theta = np.linspace(0, 2 * np.pi, num_points) - azimuth_points = azimuth_center + radius_angle * np.cos(theta) - polar_points = polar_center + radius_angle * np.sin(theta) - azimuth_points = np.mod(azimuth_points, 2 * np.pi) - polar_points = np.clip(polar_points, -np.pi / 2, np.pi / 2) - x = np.cos(polar_points) * np.cos(azimuth_points) - y = np.cos(polar_points) * np.sin(azimuth_points) - z = np.sin(polar_points) - radius_deg = np.degrees(radius_angle) - label = f"Circle {idx + 1}: {radius_deg:.1f}°" - ax3.scatter(x, y, z, s=10, alpha=0.8, label=label) - - ax3.set_xlabel('X', fontsize=10) - ax3.set_ylabel('Y', fontsize=10) - ax3.set_zlabel('Z', fontsize=10) - ax3.set_title("3D View", fontsize=12) - ax3.legend(fontsize=8) - ax3.set_box_aspect([1, 1, 1]) - - plt.tight_layout() - - return fig - - -def demo(): - """Demonstration of multiplot view showing spherical small circles in three different projections.""" - - # Define 4 circles using UnitSphericalSmallCircle dataclass - circles = [ - UnitSphericalSmallCircle( - azimuth=0, - polar=0, - radius_angle=0.6, # Circle at origin (0°, equator) with large radius - ), - UnitSphericalSmallCircle( - azimuth=np.pi, # 180° - polar=np.pi / 4, # 45° north - radius_angle=0.2, - ), - UnitSphericalSmallCircle( - azimuth=3 * np.pi / 2, # 270° - polar=-np.pi / 6, # -30° south - radius_angle=0.25, - ), - UnitSphericalSmallCircle( - azimuth=0, # 0° - polar=-np.pi / 2, # -90° south pole - radius_angle=np.pi / 2, - ), - ] - - # Create the multiplot showing all three views - fig = plot_spherical_small_circles_multiplot( - circles, - num_points=240, - title="Unit Spherical Small Circles - Flattened, Polar, and 3D Views" - ) - - # Show the plot - plt.show() - - -if __name__ == "__main__": - demo() diff --git a/tests/test_quaternion.py b/tests/test_quaternion.py new file mode 100644 index 0000000..384379d --- /dev/null +++ b/tests/test_quaternion.py @@ -0,0 +1,846 @@ +""" +Comprehensive unit tests for the Quaternion class using unittest. + +Tests cover: +- Construction methods +- Properties and attributes +- Arithmetic operations +- Comparison operations +- Unary operations +- Rotation conversions +- Vector operations +- Interpolation (SLERP) +""" + +import unittest +import numpy as np +from numpy.testing import assert_allclose, assert_array_almost_equal + +from pyMathTools.spatial.Quaternion import Quaternion + + +class TestQuaternionConstruction(unittest.TestCase): + """Test various construction methods for Quaternion.""" + + def test_from_components_identity(self): + """Test creating identity quaternion from components.""" + q = Quaternion.from_components(1, 0, 0, 0) + self.assertEqual(q.w, 1.0) + self.assertEqual(q.x, 0.0) + self.assertEqual(q.y, 0.0) + self.assertEqual(q.z, 0.0) + + def test_from_components_general(self): + """Test creating general quaternion from components.""" + q = Quaternion.from_components(0.7071, 0.0, 0.0, 0.7071) + assert_allclose(q.w, 0.7071, rtol=1e-4) + assert_allclose(q.x, 0.0, atol=1e-10) + assert_allclose(q.y, 0.0, atol=1e-10) + assert_allclose(q.z, 0.7071, rtol=1e-4) + + def test_from_components_default(self): + """Test default values in from_components.""" + q = Quaternion.from_components() + self.assertEqual(q.w, 1.0) + self.assertEqual(q.x, 0.0) + self.assertEqual(q.y, 0.0) + self.assertEqual(q.z, 0.0) + + def test_identity(self): + """Test identity quaternion constructor.""" + q = Quaternion.identity() + self.assertEqual(q.w, 1.0) + self.assertEqual(q.x, 0.0) + self.assertEqual(q.y, 0.0) + self.assertEqual(q.z, 0.0) + self.assertTrue(q.is_unit) + + def test_from_axis_angle_z_90(self): + """Test construction from axis-angle (90° around Z).""" + axis = np.array([0, 0, 1]) + angle = np.pi / 2 + q = Quaternion.from_axis_angle(axis, angle) + + assert_allclose(q.w, np.cos(np.pi / 4), rtol=1e-10) + assert_allclose(q.x, 0.0, atol=1e-10) + assert_allclose(q.y, 0.0, atol=1e-10) + assert_allclose(q.z, np.sin(np.pi / 4), rtol=1e-10) + + def test_from_axis_angle_x_180(self): + """Test construction from axis-angle (180° around X).""" + axis = np.array([1, 0, 0]) + angle = np.pi + q = Quaternion.from_axis_angle(axis, angle) + + assert_allclose(q.w, 0.0, atol=1e-10) + assert_allclose(q.x, 1.0, rtol=1e-10) + assert_allclose(q.y, 0.0, atol=1e-10) + assert_allclose(q.z, 0.0, atol=1e-10) + + def test_from_axis_angle_unnormalized(self): + """Test that axis gets normalized automatically.""" + axis = np.array([2, 0, 0]) # Not normalized + angle = np.pi / 2 + q = Quaternion.from_axis_angle(axis, angle) + + # Should be same as normalized axis + self.assertTrue(q.is_unit) + + def test_from_vector_part(self): + """Test creating pure quaternion from vector.""" + vec = np.array([1.0, 2.0, 3.0]) + q = Quaternion.from_vector_part(vec) + + self.assertEqual(q.w, 0.0) + self.assertEqual(q.x, 1.0) + self.assertEqual(q.y, 2.0) + self.assertEqual(q.z, 3.0) + + +class TestQuaternionProperties(unittest.TestCase): + """Test quaternion properties and attributes.""" + + def test_norm_identity(self): + """Test norm of identity quaternion.""" + q = Quaternion.identity() + assert_allclose(q.norm, 1.0) + + def test_norm_general(self): + """Test norm of general quaternion.""" + q = Quaternion.from_components(1, 2, 3, 4) + expected_norm = np.sqrt(1 + 4 + 9 + 16) + assert_allclose(q.norm, expected_norm) + + def test_is_unit_true(self): + """Test is_unit for unit quaternion.""" + q = Quaternion.from_axis_angle(np.array([0, 0, 1]), np.pi / 2) + self.assertTrue(q.is_unit) + + def test_is_unit_false(self): + """Test is_unit for non-unit quaternion.""" + q = Quaternion.from_components(1, 1, 1, 1) + self.assertFalse(q.is_unit) + + def test_angle_identity(self): + """Test angle of identity quaternion.""" + q = Quaternion.identity() + assert_allclose(q.angle, 0.0, atol=1e-10) + + def test_angle_90_degrees(self): + """Test angle extraction for 90° rotation.""" + q = Quaternion.from_axis_angle(np.array([1, 0, 0]), np.pi / 2) + assert_allclose(q.angle, np.pi / 2, rtol=1e-10) + + def test_angle_180_degrees(self): + """Test angle extraction for 180° rotation.""" + q = Quaternion.from_axis_angle(np.array([0, 1, 0]), np.pi) + assert_allclose(q.angle, np.pi, rtol=1e-10) + + def test_axis_z(self): + """Test axis extraction for Z-axis rotation.""" + axis = np.array([0, 0, 1]) + q = Quaternion.from_axis_angle(axis, np.pi / 2) + assert_array_almost_equal(q.axis, axis, decimal=10) + + def test_axis_general(self): + """Test axis extraction for general rotation.""" + axis = np.array([1, 1, 1]) / np.sqrt(3) + q = Quaternion.from_axis_angle(axis, np.pi / 4) + assert_array_almost_equal(q.axis, axis, decimal=10) + + def test_axis_identity(self): + """Test axis for identity quaternion (defaults to Z).""" + q = Quaternion.identity() + # Identity should return default axis + self.assertEqual(q.axis.shape, (3,)) + + def test_to_components(self): + """Test components tuple extraction.""" + q = Quaternion.from_components(1, 2, 3, 4) + w, x, y, z = q.to_components() + self.assertEqual(w, 1.0) + self.assertEqual(x, 2.0) + self.assertEqual(y, 3.0) + self.assertEqual(z, 4.0) + + +class TestQuaternionArithmetic(unittest.TestCase): + """Test arithmetic operations on quaternions.""" + + def test_add_quaternions(self): + """Test quaternion addition.""" + q1 = Quaternion.from_components(1, 2, 3, 4) + q2 = Quaternion.from_components(5, 6, 7, 8) + q3 = q1 + q2 + + self.assertEqual(q3.w, 6.0) + self.assertEqual(q3.x, 8.0) + self.assertEqual(q3.y, 10.0) + self.assertEqual(q3.z, 12.0) + + def test_add_scalar(self): + """Test adding scalar to quaternion.""" + q1 = Quaternion.from_components(1, 2, 3, 4) + q2 = q1 + 5 + + self.assertEqual(q2.w, 6.0) + self.assertEqual(q2.x, 2.0) + self.assertEqual(q2.y, 3.0) + self.assertEqual(q2.z, 4.0) + + def test_radd_scalar(self): + """Test right addition with scalar.""" + q1 = Quaternion.from_components(1, 2, 3, 4) + q2 = 5 + q1 + + self.assertEqual(q2.w, 6.0) + self.assertEqual(q2.x, 2.0) + self.assertEqual(q2.y, 3.0) + self.assertEqual(q2.z, 4.0) + + def test_subtract_quaternions(self): + """Test quaternion subtraction.""" + q1 = Quaternion.from_components(5, 6, 7, 8) + q2 = Quaternion.from_components(1, 2, 3, 4) + q3 = q1 - q2 + + self.assertEqual(q3.w, 4.0) + self.assertEqual(q3.x, 4.0) + self.assertEqual(q3.y, 4.0) + self.assertEqual(q3.z, 4.0) + + def test_multiply_quaternions(self): + """Test quaternion multiplication (Hamilton product).""" + # 90° rotation around Z followed by 90° rotation around X + q1 = Quaternion.from_axis_angle(np.array([0, 0, 1]), np.pi / 2) + q2 = Quaternion.from_axis_angle(np.array([1, 0, 0]), np.pi / 2) + q3 = q1 * q2 + + # Result should be a valid rotation + self.assertTrue(q3.is_unit) + + def test_multiply_scalar(self): + """Test multiplying quaternion by scalar.""" + q1 = Quaternion.from_components(1, 2, 3, 4) + q2 = q1 * 2 + + self.assertEqual(q2.w, 2.0) + self.assertEqual(q2.x, 4.0) + self.assertEqual(q2.y, 6.0) + self.assertEqual(q2.z, 8.0) + + def test_divide_quaternions(self): + """Test quaternion division.""" + q1 = Quaternion.from_axis_angle(np.array([0, 0, 1]), np.pi / 2) + q2 = Quaternion.from_axis_angle(np.array([0, 0, 1]), np.pi / 4) + q3 = q1 / q2 + + # Should give approximately pi/4 rotation around Z + self.assertTrue(q3.is_unit) + assert_allclose(q3.angle, np.pi / 4, rtol=1e-10) + + def test_divide_scalar(self): + """Test dividing quaternion by scalar.""" + q1 = Quaternion.from_components(2, 4, 6, 8) + q2 = q1 / 2 + + self.assertEqual(q2.w, 1.0) + self.assertEqual(q2.x, 2.0) + self.assertEqual(q2.y, 3.0) + self.assertEqual(q2.z, 4.0) + + def test_power(self): + """Test quaternion power operation.""" + q1 = Quaternion.from_axis_angle(np.array([0, 0, 1]), np.pi / 2) + q2 = q1 ** 2 + + # (90° rotation)^2 should be 180° rotation + assert_allclose(q2.angle, np.pi, rtol=1e-10) + + def test_negate(self): + """Test quaternion negation.""" + q1 = Quaternion.from_components(1, 2, 3, 4) + q2 = -q1 + + self.assertEqual(q2.w, -1.0) + self.assertEqual(q2.x, -2.0) + self.assertEqual(q2.y, -3.0) + self.assertEqual(q2.z, -4.0) + + def test_positive(self): + """Test unary positive.""" + q1 = Quaternion.from_components(1, 2, 3, 4) + q2 = +q1 + + self.assertEqual(q2.w, 1.0) + self.assertEqual(q2.x, 2.0) + self.assertEqual(q2.y, 3.0) + self.assertEqual(q2.z, 4.0) + + def test_abs(self): + """Test absolute value (norm).""" + q = Quaternion.from_components(1, 2, 3, 4) + expected_norm = np.sqrt(30) + assert_allclose(abs(q), expected_norm) + + +class TestQuaternionComparison(unittest.TestCase): + """Test comparison operations.""" + + def test_equality_same(self): + """Test equality for identical quaternions.""" + q1 = Quaternion.from_components(1, 2, 3, 4) + q2 = Quaternion.from_components(1, 2, 3, 4) + self.assertEqual(q1, q2) + + def test_equality_different(self): + """Test equality for different quaternions.""" + q1 = Quaternion.from_components(1, 2, 3, 4) + q2 = Quaternion.from_components(1, 2, 3, 5) + self.assertNotEqual(q1, q2) + + def test_inequality(self): + """Test inequality operator.""" + q1 = Quaternion.from_components(1, 2, 3, 4) + q2 = Quaternion.from_components(1, 2, 3, 5) + self.assertNotEqual(q1, q2) + + def test_isclose_true(self): + """Test isclose for nearly equal quaternions.""" + q1 = Quaternion.from_components(1.0, 2.0, 3.0, 4.0) + q2 = Quaternion.from_components(1.0 + 1e-10, 2.0, 3.0, 4.0) + self.assertTrue(q1.isclose(q2)) + + def test_isclose_false(self): + """Test isclose for different quaternions.""" + q1 = Quaternion.from_components(1.0, 2.0, 3.0, 4.0) + q2 = Quaternion.from_components(1.1, 2.0, 3.0, 4.0) + self.assertFalse(q1.isclose(q2)) + + def test_allclose_true(self): + """Test static allclose method.""" + q1 = Quaternion.from_components(1.0, 2.0, 3.0, 4.0) + q2 = Quaternion.from_components(1.0 + 1e-10, 2.0, 3.0, 4.0) + self.assertTrue(Quaternion.allclose(q1, q2)) + + def test_repr(self): + """Test __repr__ output.""" + q = Quaternion.from_components(1, 2, 3, 4) + repr_str = repr(q) + self.assertIn("Quaternion", repr_str) + self.assertIn("w=1", repr_str) + self.assertIn("x=2", repr_str) + + def test_str(self): + """Test __str__ output.""" + q = Quaternion.from_components(1, 2, 3, 4) + str_repr = str(q) + self.assertIn("quaternion", str_repr) + + +class TestQuaternionUnaryOperations(unittest.TestCase): + """Test unary operations on quaternions.""" + + def test_conjugate(self): + """Test quaternion conjugate.""" + q = Quaternion.from_components(1, 2, 3, 4) + q_conj = q.conjugate() + + self.assertEqual(q_conj.w, 1.0) + self.assertEqual(q_conj.x, -2.0) + self.assertEqual(q_conj.y, -3.0) + self.assertEqual(q_conj.z, -4.0) + + def test_conjugate_identity(self): + """Test conjugate of identity.""" + q = Quaternion.identity() + q_conj = q.conjugate() + self.assertEqual(q, q_conj) + + def test_inverse_unit(self): + """Test inverse of unit quaternion.""" + q = Quaternion.from_axis_angle(np.array([0, 0, 1]), np.pi / 2) + q_inv = q.inverse() + + # For unit quaternions, inverse equals conjugate + self.assertTrue(q_inv.isclose(q.conjugate())) + + def test_inverse_times_quaternion(self): + """Test that q * q^-1 = identity.""" + q = Quaternion.from_components(1, 2, 3, 4) + q_inv = q.inverse() + result = q * q_inv + + # Should be close to identity + identity = Quaternion.identity() + self.assertTrue(result.isclose(identity, atol=1e-10)) + + def test_normalized_general(self): + """Test normalization of general quaternion.""" + q = Quaternion.from_components(1, 2, 3, 4) + q_norm = q.normalized() + + self.assertTrue(q_norm.is_unit) + assert_allclose(q_norm.norm, 1.0) + + def test_normalized_already_unit(self): + """Test normalizing already unit quaternion.""" + q = Quaternion.identity() + q_norm = q.normalized() + + self.assertTrue(q.isclose(q_norm)) + + +class TestRotationMatrixConversions(unittest.TestCase): + """Test conversions to/from rotation matrices.""" + + def test_to_rotation_matrix_identity(self): + """Test identity quaternion to rotation matrix.""" + q = Quaternion.identity() + matrix = q.to_rotation_matrix() + + expected = np.eye(3) + assert_array_almost_equal(matrix, expected, decimal=10) + + def test_to_rotation_matrix_90z(self): + """Test 90° Z rotation to matrix.""" + q = Quaternion.from_axis_angle(np.array([0, 0, 1]), np.pi / 2) + matrix = q.to_rotation_matrix() + + # 90° Z rotation matrix + expected = np.array([ + [0, -1, 0], + [1, 0, 0], + [0, 0, 1] + ]) + assert_array_almost_equal(matrix, expected, decimal=10) + + def test_to_rotation_matrix_180x(self): + """Test 180° X rotation to matrix.""" + q = Quaternion.from_axis_angle(np.array([1, 0, 0]), np.pi) + matrix = q.to_rotation_matrix() + + # 180° X rotation matrix + expected = np.array([ + [1, 0, 0], + [0, -1, 0], + [0, 0, -1] + ]) + assert_array_almost_equal(matrix, expected, decimal=10) + + def test_from_rotation_matrix_identity(self): + """Test creating quaternion from identity matrix.""" + matrix = np.eye(3) + q = Quaternion.from_rotation_matrix(matrix) + + self.assertTrue(q.isclose(Quaternion.identity())) + + def test_from_rotation_matrix_90z(self): + """Test creating quaternion from 90° Z rotation matrix.""" + matrix = np.array([ + [0, -1, 0], + [1, 0, 0], + [0, 0, 1] + ]) + q = Quaternion.from_rotation_matrix(matrix) + + expected = Quaternion.from_axis_angle(np.array([0, 0, 1]), np.pi / 2) + self.assertTrue(q.isclose(expected)) + + def test_roundtrip_matrix_conversion(self): + """Test quaternion -> matrix -> quaternion roundtrip.""" + q_original = Quaternion.from_axis_angle(np.array([1, 1, 1]) / np.sqrt(3), np.pi / 3) + matrix = q_original.to_rotation_matrix() + q_recovered = Quaternion.from_rotation_matrix(matrix) + + # Should be close (up to sign ambiguity) + self.assertTrue(q_recovered.isclose(q_original) or q_recovered.isclose(-q_original)) + + +class TestRotationVectorConversions(unittest.TestCase): + """Test conversions to/from rotation vectors (axis-angle).""" + + def test_to_rotation_vector_identity(self): + """Test identity quaternion to rotation vector.""" + q = Quaternion.identity() + vec = q.to_rotation_vector() + + assert_array_almost_equal(vec, np.zeros(3), decimal=10) + + def test_to_rotation_vector_90z(self): + """Test 90° Z rotation to rotation vector.""" + q = Quaternion.from_axis_angle(np.array([0, 0, 1]), np.pi / 2) + vec = q.to_rotation_vector() + + expected = np.array([0, 0, np.pi / 2]) + assert_array_almost_equal(vec, expected, decimal=10) + + def test_from_rotation_vector_identity(self): + """Test creating quaternion from zero rotation vector.""" + vec = np.zeros(3) + q = Quaternion.from_rotation_vector(vec) + + self.assertTrue(q.isclose(Quaternion.identity())) + + def test_from_rotation_vector_general(self): + """Test creating quaternion from rotation vector.""" + vec = np.array([0, np.pi / 2, 0]) # 90° around Y + q = Quaternion.from_rotation_vector(vec) + + expected = Quaternion.from_axis_angle(np.array([0, 1, 0]), np.pi / 2) + self.assertTrue(q.isclose(expected)) + + def test_roundtrip_rotation_vector(self): + """Test quaternion -> rotation vector -> quaternion roundtrip.""" + q_original = Quaternion.from_axis_angle(np.array([1, 0, 0]), np.pi / 4) + vec = q_original.to_rotation_vector() + q_recovered = Quaternion.from_rotation_vector(vec) + + self.assertTrue(q_recovered.isclose(q_original)) + + +class TestEulerAngleConversions(unittest.TestCase): + """Test conversions to/from Euler angles.""" + + def test_to_euler_angles_identity(self): + """Test identity quaternion to Euler angles.""" + q = Quaternion.identity() + euler = q.to_euler_angles() + + # Identity should give zero or small angles + self.assertEqual(euler.shape, (3,)) + + def test_from_euler_angles_zeros(self): + """Test creating quaternion from zero Euler angles.""" + q = Quaternion.from_euler_angles(0, 0, 0) + + self.assertTrue(q.isclose(Quaternion.identity())) + + def test_roundtrip_euler_angles(self): + """Test quaternion -> Euler -> quaternion roundtrip.""" + q_original = Quaternion.from_axis_angle(np.array([0, 0, 1]), np.pi / 2) + euler = q_original.to_euler_angles() + q_recovered = Quaternion.from_euler_angles(euler[0], euler[1], euler[2]) + + # Should be close (Euler angles have ambiguities) + self.assertTrue(q_recovered.isclose(q_original) or q_recovered.isclose(-q_original)) + + +class TestVectorOperations(unittest.TestCase): + """Test vector-related operations.""" + + def test_to_vector_part(self): + """Test extracting vector part.""" + q = Quaternion.from_components(1, 2, 3, 4) + vec = q.to_vector_part() + + assert_array_almost_equal(vec, np.array([2, 3, 4])) + + def test_rotate_vector_identity(self): + """Test rotating vector with identity quaternion.""" + q = Quaternion.identity() + vec = np.array([1, 2, 3]) + rotated = q.rotate_vector(vec) + + assert_array_almost_equal(rotated, vec, decimal=10) + + def test_rotate_vector_90z(self): + """Test rotating vector 90° around Z.""" + q = Quaternion.from_axis_angle(np.array([0, 0, 1]), np.pi / 2) + vec = np.array([1, 0, 0]) + rotated = q.rotate_vector(vec) + + expected = np.array([0, 1, 0]) + assert_array_almost_equal(rotated, expected, decimal=10) + + def test_rotate_vector_180x(self): + """Test rotating vector 180° around X.""" + q = Quaternion.from_axis_angle(np.array([1, 0, 0]), np.pi) + vec = np.array([0, 1, 0]) + rotated = q.rotate_vector(vec) + + expected = np.array([0, -1, 0]) + assert_array_almost_equal(rotated, expected, decimal=10) + + def test_rotate_vector_general(self): + """Test general vector rotation.""" + q = Quaternion.from_axis_angle(np.array([1, 1, 1]) / np.sqrt(3), 2 * np.pi / 3) + vec = np.array([1, 0, 0]) + rotated = q.rotate_vector(vec) + + # 120° rotation around (1,1,1) should permute coordinates + expected = np.array([0, 1, 0]) + assert_array_almost_equal(rotated, expected, decimal=8) + + +class TestSLERP(unittest.TestCase): + """Test spherical linear interpolation.""" + + def test_slerp_endpoints(self): + """Test SLERP at endpoints.""" + q1 = Quaternion.identity() + q2 = Quaternion.from_axis_angle(np.array([0, 0, 1]), np.pi / 2) + + # At t=0, should get q1 + q_start = q1.slerp(q2, 0.0) + self.assertTrue(q_start.isclose(q1)) + + # At t=1, should get q2 + q_end = q1.slerp(q2, 1.0) + self.assertTrue(q_end.isclose(q2)) + + def test_slerp_midpoint(self): + """Test SLERP at midpoint.""" + q1 = Quaternion.identity() + q2 = Quaternion.from_axis_angle(np.array([0, 0, 1]), np.pi / 2) + + q_mid = q1.slerp(q2, 0.5) + + # At t=0.5, should be 45° rotation + expected = Quaternion.from_axis_angle(np.array([0, 0, 1]), np.pi / 4) + self.assertTrue(q_mid.isclose(expected)) + + def test_slerp_unit_preservation(self): + """Test that SLERP preserves unit norm.""" + q1 = Quaternion.from_axis_angle(np.array([1, 0, 0]), np.pi / 4) + q2 = Quaternion.from_axis_angle(np.array([0, 1, 0]), np.pi / 3) + + for t in np.linspace(0, 1, 10): + q_interp = q1.slerp(q2, t) + self.assertTrue(q_interp.is_unit) + + def test_slerp_smooth_interpolation(self): + """Test that SLERP produces smooth interpolation.""" + q1 = Quaternion.identity() + q2 = Quaternion.from_axis_angle(np.array([0, 0, 1]), np.pi) + + angles = [] + for t in np.linspace(0, 1, 11): + q_interp = q1.slerp(q2, t) + angles.append(q_interp.angle) + + # Angles should increase monotonically + for i in range(len(angles) - 1): + self.assertLessEqual(angles[i], angles[i + 1]) + + def test_slerp_opposite_quaternions(self): + """Test SLERP between opposite quaternions.""" + q1 = Quaternion.from_axis_angle(np.array([0, 0, 1]), 0) + q2 = Quaternion.from_axis_angle(np.array([0, 0, 1]), np.pi) + + q_mid = q1.slerp(q2, 0.5) + + # Should interpolate correctly + assert_allclose(q_mid.angle, np.pi / 2, rtol=1e-10) + + +class TestArrayConversions(unittest.TestCase): + """Test array conversion methods.""" + + def test_as_float_array(self): + """Test conversion to float array.""" + q = Quaternion.from_components(1, 2, 3, 4) + arr = q.as_float_array() + + expected = np.array([1, 2, 3, 4]) + assert_array_almost_equal(arr, expected) + + def test_as_float_array_shape(self): + """Test shape of float array.""" + q = Quaternion.from_components(1, 2, 3, 4) + arr = q.as_float_array() + + self.assertEqual(arr.shape, (4,)) + + +class TestEdgeCases(unittest.TestCase): + """Test edge cases and special scenarios.""" + + def test_very_small_rotation(self): + """Test very small rotation angle.""" + q = Quaternion.from_axis_angle(np.array([1, 0, 0]), 1e-10) + + self.assertTrue(q.isclose(Quaternion.identity())) + + def test_very_large_angle(self): + """Test angle larger than 2π.""" + # Should wrap around + q = Quaternion.from_axis_angle(np.array([0, 0, 1]), 3 * np.pi) + + # 3π should be equivalent to π rotation + expected = Quaternion.from_axis_angle(np.array([0, 0, 1]), np.pi) + self.assertTrue(q.isclose(expected) or q.isclose(-expected)) + + def test_zero_norm_handling(self): + """Test that operations handle near-zero quaternions appropriately.""" + # This would be an invalid quaternion, but we test defensive handling + # Note: actual zero quaternion would cause division by zero in some operations + q = Quaternion.from_components(1e-15, 1e-15, 1e-15, 1e-15) + + # Should still be able to normalize + q_norm = q.normalized() + self.assertTrue(q_norm.is_unit) + + +class TestQuaternionSerialization(unittest.TestCase): + """Test serialization/deserialization methods and DataModelHelper inheritance.""" + + def test_to_dict_identity(self): + """Test to_dict for identity quaternion.""" + q = Quaternion.identity() + d = q.to_dict() + + self.assertIsInstance(d, dict) + self.assertIn("w", d) + self.assertIn("x", d) + self.assertIn("y", d) + self.assertIn("z", d) + self.assertEqual(d["w"], 1.0) + self.assertEqual(d["x"], 0.0) + self.assertEqual(d["y"], 0.0) + self.assertEqual(d["z"], 0.0) + + def test_to_dict_general(self): + """Test to_dict for general quaternion.""" + q = Quaternion.from_components(1.0, 2.0, 3.0, 4.0) + d = q.to_dict() + + self.assertEqual(d["w"], 1.0) + self.assertEqual(d["x"], 2.0) + self.assertEqual(d["y"], 3.0) + self.assertEqual(d["z"], 4.0) + + def test_to_dict_normalized(self): + """Test to_dict preserves values for normalized quaternion.""" + q = Quaternion.from_axis_angle(np.array([1, 0, 0]), np.pi / 2) + d = q.to_dict() + + # Check that all components are present and are floats + self.assertIsInstance(d["w"], float) + self.assertIsInstance(d["x"], float) + self.assertIsInstance(d["y"], float) + self.assertIsInstance(d["z"], float) + + # Verify components match + self.assertAlmostEqual(d["w"], q.w) + self.assertAlmostEqual(d["x"], q.x) + self.assertAlmostEqual(d["y"], q.y) + self.assertAlmostEqual(d["z"], q.z) + + def test_from_dict_identity(self): + """Test from_dict for identity quaternion.""" + d = {"w": 1.0, "x": 0.0, "y": 0.0, "z": 0.0} + q = Quaternion.from_dict(d) + + self.assertIsInstance(q, Quaternion) + self.assertEqual(q.w, 1.0) + self.assertEqual(q.x, 0.0) + self.assertEqual(q.y, 0.0) + self.assertEqual(q.z, 0.0) + + def test_from_dict_general(self): + """Test from_dict for general quaternion.""" + d = {"w": 1.0, "x": 2.0, "y": 3.0, "z": 4.0} + q = Quaternion.from_dict(d) + + self.assertEqual(q.w, 1.0) + self.assertEqual(q.x, 2.0) + self.assertEqual(q.y, 3.0) + self.assertEqual(q.z, 4.0) + + def test_from_dict_with_integers(self): + """Test from_dict accepts integer values.""" + d = {"w": 1, "x": 2, "y": 3, "z": 4} + q = Quaternion.from_dict(d) + + self.assertEqual(q.w, 1.0) + self.assertEqual(q.x, 2.0) + self.assertEqual(q.y, 3.0) + self.assertEqual(q.z, 4.0) + + def test_roundtrip_serialization_identity(self): + """Test roundtrip: quaternion -> dict -> quaternion for identity.""" + q_original = Quaternion.identity() + d = q_original.to_dict() + q_recovered = Quaternion.from_dict(d) + + self.assertTrue(q_original.isclose(q_recovered)) + + def test_roundtrip_serialization_general(self): + """Test roundtrip: quaternion -> dict -> quaternion for general quaternion.""" + q_original = Quaternion.from_components(1.0, 2.0, 3.0, 4.0) + d = q_original.to_dict() + q_recovered = Quaternion.from_dict(d) + + self.assertTrue(q_original.isclose(q_recovered)) + + def test_roundtrip_serialization_rotation(self): + """Test roundtrip for rotation quaternion.""" + q_original = Quaternion.from_axis_angle(np.array([1, 1, 1]) / np.sqrt(3), np.pi / 3) + d = q_original.to_dict() + q_recovered = Quaternion.from_dict(d) + + self.assertTrue(q_original.isclose(q_recovered)) + + def test_roundtrip_with_negatives(self): + """Test roundtrip with negative components.""" + q_original = Quaternion.from_components(-0.5, -0.5, 0.5, 0.5) + d = q_original.to_dict() + q_recovered = Quaternion.from_dict(d) + + self.assertEqual(q_recovered.w, -0.5) + self.assertEqual(q_recovered.x, -0.5) + self.assertEqual(q_recovered.y, 0.5) + self.assertEqual(q_recovered.z, 0.5) + + def test_to_dict_returns_new_dict(self): + """Test that to_dict returns a new dictionary each time.""" + q = Quaternion.from_components(1.0, 2.0, 3.0, 4.0) + d1 = q.to_dict() + d2 = q.to_dict() + + self.assertIsNot(d1, d2) + self.assertEqual(d1, d2) + + def test_dict_modification_doesnt_affect_quaternion(self): + """Test that modifying the dict doesn't affect the quaternion.""" + q = Quaternion.from_components(1.0, 2.0, 3.0, 4.0) + d = q.to_dict() + + # Modify the dictionary + d["w"] = 999.0 + d["x"] = 999.0 + + # Original quaternion should be unchanged + self.assertEqual(q.w, 1.0) + self.assertEqual(q.x, 2.0) + + def test_inheritance_from_quaternion_type(self): + """Test that Quaternion properly inherits from QuaternionType.""" + from foundationTypes.mathTypes.QuaternionType import QuaternionType + + q = Quaternion.from_components(1.0, 2.0, 3.0, 4.0) + self.assertIsInstance(q, QuaternionType) + + def test_inheritance_from_data_model_helper(self): + """Test that Quaternion inherits from DataModelHelper.""" + from foundationTypes.dataModelHelper import DataModelHelper + + q = Quaternion.from_components(1.0, 2.0, 3.0, 4.0) + self.assertIsInstance(q, DataModelHelper) + + def test_has_serialization_methods(self): + """Test that Quaternion has the required serialization methods.""" + q = Quaternion.identity() + + # Check methods exist + self.assertTrue(hasattr(q, "to_dict")) + self.assertTrue(hasattr(Quaternion, "from_dict")) + + # Check they're callable + self.assertTrue(callable(q.to_dict)) + self.assertTrue(callable(Quaternion.from_dict)) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/testpyMathTools.py b/tests/testpyMathTools.py deleted file mode 100644 index 5bb39ca..0000000 --- a/tests/testpyMathTools.py +++ /dev/null @@ -1,11 +0,0 @@ -import pytest - -# from pyMathTools import pyMathTools - - -def test_content(): - """Sample pytest test function.""" - - -if __name__ == "__main__": - test_content()