Transformation matrix

meshpy/rotation.py

@@ -39,6 +39,18 @@
 from . import mpy
 
 
+def skew_matrix(vector):
+    """Return the skew matrix for the vector"""
+    skew = np.zeros([3, 3])
+    skew[0, 1] = -vector[2]
+    skew[0, 2] = vector[1]
+    skew[1, 0] = vector[2]
+    skew[1, 2] = -vector[0]
+    skew[2, 0] = -vector[1]
+    skew[2, 1] = vector[0]
+    return skew
+
+
 class Rotation:
     """
     A class that represents a rotation of a coordinate system.
@@ -169,15 +181,7 @@ def get_rotation_matrix(self):
         Return the rotation matrix for this rotation.
         (Krenk (3.50))
         """
-
-        q_skew = np.zeros([3, 3])
-        q_skew[0, 1] = -self.q[3]
-        q_skew[0, 2] = self.q[2]
-        q_skew[1, 0] = self.q[3]
-        q_skew[1, 2] = -self.q[1]
-        q_skew[2, 0] = -self.q[2]
-        q_skew[2, 1] = self.q[1]
-
+        q_skew = skew_matrix(self.q[1:])
         R = (
             (self.q[0] ** 2 - np.dot(self.q[1:], self.q[1:])) * np.eye(3)
             + 2 * self.q[0] * q_skew
@@ -209,6 +213,57 @@ def get_rotation_vector(self):
         else:
             return phi * self.q[1:] / norm
 
+    def get_transformation_matrix(self):
+        """Return the transformation matrix for this rotation"""
+
+        omega = self.get_rotation_vector()
+        omega_norm = np.linalg.norm(omega)
+        omega_skew = skew_matrix(omega)
+
+        # We have to take the inverse of the the rotation angle here, therefore,
+        # we have a branch for small angles where the singularity is not present.
+        if omega_norm**2 > mpy.eps_quaternion:
+            alpha = np.sin(omega_norm) / omega_norm
+            beta = 2.0 * (1.0 - np.cos(omega_norm)) / omega_norm**2
+            transformation_matrix = (
+                np.identity(3)
+                - 0.5 * beta * omega_skew
+                + (1.0 - alpha) / omega_norm**2 * (np.dot(omega_skew, omega_skew))
+            )
+        else:
+            # This is the constant part of the Taylor series expansion. If this
+            # function is used with automatic differentiation, higher order
+            # terms have to be added!
+            transformation_matrix = np.identity(3)
+        return transformation_matrix
+
+    def get_transformation_matrix_inv(self):
+        """Return the inverse of the transformation matrix for this rotation"""
+
+        omega = self.get_rotation_vector()
+        omega_norm = np.linalg.norm(omega)
+        omega_skew = skew_matrix(omega)
+
+        # We have to take the inverse of the the rotation angle here, therefore,
+        # we have a branch for small angles where the singularity is not present.
+        if omega_norm**2 > mpy.eps_quaternion:
+            alpha = np.sin(omega_norm) / omega_norm
+            beta = 2.0 * (1.0 - np.cos(omega_norm)) / omega_norm**2
+            transformation_matrix_inverse = (
+                np.identity(3)
+                + 0.5 * omega_skew
+                + 1.0
+                / omega_norm**2
+                * (1 - alpha / beta)
+                * (np.dot(omega_skew, omega_skew))
+            )
+        else:
+            # This is the constant part of the Taylor series expansion. If this
+            # function is used with automatic differentiation, higher order
+            # terms have to be added!
+            transformation_matrix_inverse = np.identity(3)
+        return transformation_matrix_inverse
+
     def inv(self):
         """
         Return the inverse of this rotation.

tests/testing_mesh_creation_functions.py

@@ -407,8 +407,8 @@ def test_mesh_creation_functions_nurbs_unit(self):
         ]
 
         for t, result_r, result_dr in zip(t_values, results_r, results_dr):
-            self.assertTrue(np.allclose(r(t), result_r, atol=mpy.eps_pos))
-            self.assertTrue(np.allclose(dr(t), result_dr, atol=mpy.eps_pos))
+            self.assertTrue(np.allclose(r(t), result_r, atol=mpy.eps_pos, rtol=0.0))
+            self.assertTrue(np.allclose(dr(t), result_dr, atol=mpy.eps_pos, rtol=0.0))
 
     def test_mesh_creation_functions_node_continuation(self):
         """Test that the node continuation function work as expected."""

tests/testing_rotations.py

@@ -270,6 +270,69 @@ def test_rotation_matrix(self):
                 "test_rotation_matrix: compare t2",
             )
 
+    def test_transformation_matrix(self):
+        """Test that the transformation matrix is computed correctly"""
+
+        rotation_vector_large = [1.0, 2.0, np.pi / 5.0]
+        rotation_large = Rotation.from_rotation_vector(rotation_vector_large)
+        rotation_vector_small = (
+            rotation_vector_large
+            / np.linalg.norm(rotation_vector_large)
+            / 10.0
+            * mpy.eps_quaternion
+        )
+        rotation_small = Rotation.from_rotation_vector(rotation_vector_small)
+
+        # Test transformation matrix
+        transformation_matrix_large_reference = np.array(
+            [
+                [0.44154375784863664, 0.45016106128606925, -0.5440964474342915],
+                [0.05812896596538622, 0.8227612782872282, 0.47165325065541175],
+                [0.7037804689849043, -0.15228520755418617, 0.3646374659356807],
+            ]
+        )
+        self.assertTrue(
+            np.allclose(
+                rotation_large.get_transformation_matrix(),
+                transformation_matrix_large_reference,
+                atol=mpy.eps_quaternion,
+                rtol=0.0,
+            )
+        )
+        self.assertTrue(
+            np.allclose(
+                rotation_small.get_transformation_matrix(),
+                np.identity(3),
+                atol=mpy.eps_quaternion,
+                rtol=0.0,
+            )
+        )
+
+        # Test transformation matrix inverse
+        transformation_matrix_inverse_large_reference = np.array(
+            [
+                [0.5959488405656388, -0.13028167626739834, 1.0577668483049911],
+                [0.4980368544505602, 0.8717652242030102, -0.3844663033900173],
+                [-0.9422331516950085, 0.6155336966099826, 0.5403060272710477],
+            ]
+        )
+        self.assertTrue(
+            np.allclose(
+                rotation_large.get_transformation_matrix_inv(),
+                transformation_matrix_inverse_large_reference,
+                atol=mpy.eps_quaternion,
+                rtol=0.0,
+            )
+        )
+        self.assertTrue(
+            np.allclose(
+                rotation_small.get_transformation_matrix_inv(),
+                np.identity(3),
+                atol=mpy.eps_quaternion,
+                rtol=0.0,
+            )
+        )
+
     def test_smallest_rotation_triad(self):
         """
         Test that the smallest rotation triad is calculated correctly.