From ea679dd671a1f93416af4b17458c62f4d3718a96 Mon Sep 17 00:00:00 2001
From: looooo <sppedflyer@gmail.com>
Date: Sat, 7 Dec 2024 23:58:58 +0100
Subject: [PATCH] add tranformations

---
 pygears/transformation.py | 75 +++++++++++++++++++++++++++++++++++++++
 1 file changed, 75 insertions(+)
 create mode 100644 pygears/transformation.py

diff --git a/pygears/transformation.py b/pygears/transformation.py
new file mode 100644
index 0000000..2cc3131
--- /dev/null
+++ b/pygears/transformation.py
@@ -0,0 +1,75 @@
+import sympy as sym
+import numpy as np
+
+def symbolic_transformation(angle, axis, translation=np.array([0., 0., 0.])):
+    """
+        see http://en.wikipedia.org/wiki/SO%284%29#The_Euler.E2.80.93Rodrigues_formula_for_3D_rotations
+        sympy enabled transformation
+        angle: angle of rotation
+        axis: the axis of the rotation
+        translation: translation of transformation
+    """
+    assert len(axis) == 3
+    a = sym.cos(angle / 2)
+    axis_normalized = axis / sym.sqrt(axis.dot(axis))
+    (b, c, d) = -axis_normalized * sym.sin(angle / 2)
+    mat = sym.Matrix(
+        [
+            [
+                a**2 + b**2 - c**2 - d**2,
+                2 * (b * c - a * d),
+                2 * (b * d + a * c),
+                translation[0],
+            ],
+            [
+                2 * (b * c + a * d),
+                a**2 + c**2 - b**2 - d**2,
+                2 * (c * d - a * b),
+                translation[1],
+            ],
+            [
+                2 * (b * d - a * c),
+                2 * (c * d + a * b),
+                a**2 + d**2 - b**2 - c**2,
+                translation[2],
+            ],
+            [0.0, 0.0, 0.0, 1.0],
+        ]
+    )
+    return sym.simplify(mat)
+
+def numeric_transformation(angle, axis, translation=np.array([0., 0., 0.])):
+    """
+        see http://en.wikipedia.org/wiki/SO%284%29#The_Euler.E2.80.93Rodrigues_formula_for_3D_rotations
+        angle: angle of rotation
+        axis: the axis of the rotation
+        translation: translation of transformation
+    """
+    assert len(axis) == 3
+    a = np.cos(angle / 2)
+    axis_normalized = axis / np.sqrt(axis.dot(axis))
+    (b, c, d) = -axis_normalized * np.sin(angle / 2)
+    mat = np.array(
+        [
+            [
+                a**2 + b**2 - c**2 - d**2,
+                2 * (b * c - a * d),
+                2 * (b * d + a * c),
+                translation[0],
+            ],
+            [
+                2 * (b * c + a * d),
+                a**2 + c**2 - b**2 - d**2,
+                2 * (c * d - a * b),
+                translation[1],
+            ],
+            [
+                2 * (b * d - a * c),
+                2 * (c * d + a * b),
+                a**2 + d**2 - b**2 - c**2,
+                translation[2],
+            ],
+            [0.0, 0.0, 0.0, 1.0],
+        ]
+    )
+    return mat
\ No newline at end of file