diff --git a/doc/changelog.d/1806.added.md b/doc/changelog.d/1806.added.md new file mode 100644 index 0000000000..c1bd214626 --- /dev/null +++ b/doc/changelog.d/1806.added.md @@ -0,0 +1 @@ +matrix helper methods \ No newline at end of file diff --git a/src/ansys/geometry/core/math/matrix.py b/src/ansys/geometry/core/math/matrix.py index f0fdcf41f3..fb9577316b 100644 --- a/src/ansys/geometry/core/math/matrix.py +++ b/src/ansys/geometry/core/math/matrix.py @@ -30,6 +30,7 @@ from ansys.geometry.core.typing import Real, RealSequence if TYPE_CHECKING: + from ansys.geometry.core.math.frame import Frame from ansys.geometry.core.math.vector import Vector3D # For type hints DEFAULT_MATRIX33 = np.identity(3) @@ -308,3 +309,61 @@ def create_rotation( ] ) return matrix + + @classmethod + def create_matrix_from_rotation_about_axis(cls, axis: "Vector3D", angle: float) -> "Matrix44": + """ + Create a matrix representing a rotation about a given axis. + + Parameters + ---------- + axis : Vector3D + The axis of rotation. + angle : float + The angle of rotation in radians. + + Returns + ------- + Matrix44 + A 4x4 matrix representing the rotation. + """ + axis_dir = axis.normalize() + x, y, z = axis_dir[0], axis_dir[1], axis_dir[2] + + k = np.array([[0, -z, y], [z, 0, -x], [-y, x, 0]]) + + identity = np.eye(3) + cos_theta = np.cos(angle) + sin_theta = np.sin(angle) + + # Rodrigues' rotation formula + rotation_3x3 = identity + sin_theta * k + (1 - cos_theta) * (k @ k) + + # Convert to a 4x4 homogeneous matrix + rotation_matrix = np.eye(4) + rotation_matrix[:3, :3] = rotation_3x3 + + return cls(rotation_matrix) + + @classmethod + def create_matrix_from_mapping(cls, frame: "Frame") -> "Matrix44": + """ + Create a matrix representing the specified mapping. + + Parameters + ---------- + frame : Frame + The frame containing the origin and direction vectors. + + Returns + ------- + Matrix44 + A 4x4 matrix representing the translation and rotation defined by the frame. + """ + from ansys.geometry.core.math.vector import Vector3D + + translation_matrix = Matrix44.create_translation( + Vector3D([frame.origin[0], frame.origin[1], frame.origin[2]]) + ) + rotation_matrix = Matrix44.create_rotation(frame.direction_x, frame.direction_y) + return translation_matrix * rotation_matrix diff --git a/src/ansys/geometry/core/math/vector.py b/src/ansys/geometry/core/math/vector.py index b986196175..3e850b535f 100644 --- a/src/ansys/geometry/core/math/vector.py +++ b/src/ansys/geometry/core/math/vector.py @@ -32,6 +32,7 @@ from ansys.geometry.core.math.point import Point2D, Point3D from ansys.geometry.core.misc.accuracy import Accuracy from ansys.geometry.core.misc.checks import check_ndarray_is_float_int +from ansys.geometry.core.misc.measurements import Angle from ansys.geometry.core.misc.units import UNITS from ansys.geometry.core.typing import Real, RealSequence @@ -163,6 +164,22 @@ def transform(self, matrix: "Matrix44") -> "Vector3D": result_vector = Vector3D(result_4x1[0:3]) return result_vector + @check_input_types + def rotate_vector(self, vector: "Vector3D", angle: Real | Quantity | Angle) -> "Vector3D": + """Rotate a vector around a given axis by a specified angle.""" + if self.is_zero: + raise Exception("Invalid vector operation: rotation axis cannot be zero.") + + # Convert angle to Angle object and get its value in radians + angle = angle if isinstance(angle, Angle) else Angle(angle) + angle_m = angle.value.m_as(UNITS.radian) + + axis = self.normalize() + parallel = axis * (vector.dot(axis)) + perpendicular1 = vector - parallel + perpendicular2 = axis.cross(perpendicular1) + return parallel + perpendicular1 * np.cos(angle_m) + perpendicular2 * np.sin(angle_m) + @check_input_types def get_angle_between(self, v: "Vector3D") -> Quantity: """Get the angle between this 3D vector and another 3D vector. diff --git a/tests/test_math.py b/tests/test_math.py index 779845e003..630cbfc5f5 100644 --- a/tests/test_math.py +++ b/tests/test_math.py @@ -24,6 +24,7 @@ from beartype.roar import BeartypeCallHintParamViolation import numpy as np +from pint import Quantity import pytest from ansys.geometry.core.math import ( @@ -627,6 +628,24 @@ def test_vector2d_errors(): v1.get_angle_between(v2) +def test_rotate_vector(): + """Test the rotate_vector method.""" + # Define the vectors and angle + axis = Vector3D([0.0, 0.0, 1.0]) + vector = Vector3D([1.0, 0.0, 0.0]) + + angle = Quantity(np.pi / 2) # 90 degrees + + # Expected result after rotating vector around axis by 90 degrees + expected_vector = Vector3D([0.0, 1.0, 0.0]) + + # Call the method under test + result_vector = axis.rotate_vector(vector, angle) + + # Assert that the result matches the expected vector + assert np.allclose(result_vector, expected_vector) + + def test_matrix(): """Simple test to create a ``Matrix``.""" # Create two matrix objects @@ -827,6 +846,82 @@ def test_create_rotation_matrix(): assert np.array_equal(expected_matrix, rotation_matrix) +def test_create_matrix_from_rotation_about_axis_x(): + """Test the create_matrix_from_rotation_about_axis method for rotation about the x-axis.""" + axis = Vector3D([1.0, 0.0, 0.0]) + angle = np.pi / 2 # 90 degrees + expected_matrix = Matrix44([[1, 0, 0, 0], [0, 0, -1, 0], [0, 1, 0, 0], [0, 0, 0, 1]]) + + result_matrix = Matrix44.create_matrix_from_rotation_about_axis(axis, angle) + + print(result_matrix) + assert np.allclose(result_matrix, expected_matrix) + + +def test_create_matrix_from_rotation_about_axis_y(): + """Test the create_matrix_from_rotation_about_axis method for rotation about the y-axis.""" + axis = Vector3D([0.0, 1.0, 0.0]) + angle = np.pi / 2 # 90 degrees + expected_matrix = Matrix44([[0, 0, 1, 0], [0, 1, 0, 0], [-1, 0, 0, 0], [0, 0, 0, 1]]) + + result_matrix = Matrix44.create_matrix_from_rotation_about_axis(axis, angle) + assert np.allclose(result_matrix, expected_matrix) + + +def test_create_matrix_from_rotation_about_axis_z(): + """Test the create_matrix_from_rotation_about_axis method for rotation about the z-axis.""" + axis = Vector3D([0.0, 0.0, 1.0]) + angle = np.pi / 2 # 90 degrees + expected_matrix = Matrix44([[0, -1, 0, 0], [1, 0, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) + + result_matrix = Matrix44.create_matrix_from_rotation_about_axis(axis, angle) + assert np.allclose(result_matrix, expected_matrix) + + +def test_create_matrix_from_rotation_about_arbitrary_axis(): + """Test the create_matrix_from_rotation_about_axis method for + rotation about an arbitrary axis. + """ + axis = Vector3D([1.0, 1.0, 1.0]).normalize() + angle = np.pi / 3 # 60 degrees + # Expected matrix calculated using external tools or libraries + expected_matrix = Matrix44( + [ + [0.66666667, -0.33333333, 0.66666667, 0], + [0.66666667, 0.66666667, -0.33333333, 0], + [-0.33333333, 0.66666667, 0.66666667, 0], + [0, 0, 0, 1], + ] + ) + + result_matrix = Matrix44.create_matrix_from_rotation_about_axis(axis, angle) + assert np.allclose(result_matrix, expected_matrix) + + +def test_create_matrix_from_mapping(): + """Test the create_matrix_from_mapping method.""" + # Define the frame with origin and direction vectors + origin = Vector3D([1.0, 2.0, 3.0]) + direction_x = Vector3D([1.0, 0.0, 0.0]) + direction_y = Vector3D([0.0, 1.0, 0.0]) + frame = Frame(origin, direction_x, direction_y) + + # Create the expected translation matrix + expected_translation_matrix = Matrix44.create_translation(origin) + + # Create the expected rotation matrix + expected_rotation_matrix = Matrix44.create_rotation(direction_x, direction_y) + + # Create the expected result by multiplying the translation and rotation matrices + expected_matrix = expected_translation_matrix * expected_rotation_matrix + + # Call the method under test + result_matrix = Matrix44.create_matrix_from_mapping(frame) + + # Assert that the result matches the expected matrix + assert np.allclose(result_matrix, expected_matrix) + + def test_frame(): """``Frame`` construction and equivalency.""" origin = Point3D([42, 99, 13])