kindred/gears
1
0
Fork 0
Code Issues Pull Requests Actions 1 Packages Projects Releases Wiki Activity

add tranformations

Browse Source
This commit is contained in:
looooo
2024-12-07 23:58:58 +01:00
parent 6bcec43207
commit ea679dd671
1 changed files with 75 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

75
pygears/transformation.py Normal file
View File

@@ -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