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