def axis_angle_from_matrix(rotation_matrix):
"""
:param rotation_matrix: 4x4 or 3x3 Matrix
:type rotation_matrix: Matrix
:return: 3x1 Matrix, angle
:rtype: (Matrix, Union[float, Symbol])
"""
rm = rotation_matrix
angle = (trace(rm[:3, :3]) - 1) / 2
angle = se.Min(angle, 1)
angle = se.Max(angle, -1)
angle = se.acos(angle)
x = (rm[2, 1] - rm[1, 2])
y = (rm[0, 2] - rm[2, 0])
z = (rm[1, 0] - rm[0, 1])
n = se.sqrt(x * x + y * y + z * z)
m = if_eq_zero(n, 1, n)
axis = se.Matrix([
if_eq_zero(n, 0, x / m),
if_eq_zero(n, 0, y / m),
if_eq_zero(n, 1, z / m)
])
sign = diffable_sign(angle)
axis *= sign
angle = sign * angle
return axis, angle
def test_llvm_float():
import numpy as np
import ctypes
from symengine.lib.symengine_wrapper import LLVMFloat
x, y, z = se.symbols('x, y, z')
l = se.Lambdify([x, y, z], [se.Min(x, y), se.Max(y, z)],
dtype=np.float32,
backend='llvm')
inp = np.array([1, 2, 3], dtype=np.float32)
exp_out = np.array([1, 3], dtype=np.float32)
out = l(inp)
assert type(l) == LLVMFloat
assert out.dtype == np.float32
assert np.allclose(out, exp_out)
def rotation_distance(a_R_b, a_R_c):
"""
:param a_R_b: 4x4 or 3x3 Matrix
:type a_R_b: Matrix
:param a_R_c: 4x4 or 3x3 Matrix
:type a_R_c: Matrix
:return: angle of axis angle representation of b_R_c
:rtype: Union[float, Symbol]
"""
difference = a_R_b.T * a_R_c
angle = (trace(difference[:3, :3]) - 1) / 2
angle = se.Min(angle, 1)
angle = se.Max(angle, -1)
return se.acos(angle)
def axis_angle_from_quaternion(x, y, z, w):
"""
:type x: Union[float, Symbol]
:type y: Union[float, Symbol]
:type z: Union[float, Symbol]
:type w: Union[float, Symbol]
:return: 4x1 Matrix
:rtype: Matrix
"""
l = norm([x, y, z, w])
x, y, z, w = x / l, y / l, z / l, w / l
w2 = se.sqrt(1 - w**2)
angle = (2 * se.acos(se.Min(se.Max(-1, w), 1)))
m = if_eq_zero(w2, 1, w2) # avoid /0
x = if_eq_zero(w2, 0, x / m)
y = if_eq_zero(w2, 0, y / m)
z = if_eq_zero(w2, 1, z / m)
return se.Matrix([x, y, z]), angle