Commit 4597884d by Olivier Lantsoght

### [MBsysPy][Algebra] Faster rotation matrix

parent 3cc01b61
 ... ... @@ -10,6 +10,7 @@ Mainly calls corresponding numpy function but ignoring first element of array. # Author : Robotran Team # (c) Universite catholique de Louvain, 2020 import numpy as np from math import cos, sin def norm(v, size_4=True): ... ... @@ -378,7 +379,10 @@ def __get_index_0_matrix__(M, size_4): def rotation_matrix(rot_type, angle, Rres=None): """Compute the rotation matrix for a specified angle around a specific axis. The first index is 1. """Compute the rotation matrix for a specified angle around a specific axis. The rotation matrices uses the MBsysC convention wich means that the first index is 1. Parameters ---------- ... ... @@ -389,8 +393,8 @@ def rotation_matrix(rot_type, angle, Rres=None): 3: rotation along z-axis. angle : float Rotation angle expressed in radian. Mres : list or numpy.ndarray, optional Vector containing the result if not None. Mres : numpy.ndarray, optional Matrix containing the result if not None. default: None Raises ------ ... ... @@ -399,52 +403,39 @@ def rotation_matrix(rot_type, angle, Rres=None): Returns ------- Rot : numpy.ndarray Rres : numpy.ndarray Rotation matrix. """ Rot = np.eye(4) Rot[0, :] = np.array([3, 3, 3, 3]) Rot[1:, 0] = np.array([3, 3, 3]) if rot_type not in [1, 2, 3]: raise ValueError('{:} is not a valid rotation type'.format(rot_type)) if Rres is None: Rres = np.zeros((4, 4)) Rres[:, 0] = 3. else: Rres[1:, 1:] = 0. c = cos(angle) s = sin(angle) if rot_type == 1: if Rres is not None: Rres[1:, 1:] = np.eye(3) Rres[2, 2] = np.cos(angle) Rres[3, 2] = -np.sin(angle) Rres[2, 3] = np.sin(angle) Rres[3, 3] = np.cos(angle) else: Rot[2, 2] = np.cos(angle) Rot[3, 2] = -np.sin(angle) Rot[2, 3] = np.sin(angle) Rot[3, 3] = np.cos(angle) return Rot Rres[1, 1] = 1. Rres[2, 2] = c Rres[3, 2] = -s Rres[2, 3] = s Rres[3, 3] = c elif rot_type == 2: if Rres is not None: Rres[1:, 1:] = np.eye(3) Rres[1, 1] = np.cos(angle) Rres[3, 1] = np.sin(angle) Rres[1, 3] = -np.sin(angle) Rres[3, 3] = np.cos(angle) else: Rot[1, 1] = np.cos(angle) Rot[3, 1] = np.sin(angle) Rot[1, 3] = -np.sin(angle) Rot[3, 3] = np.cos(angle) return Rot Rres[2, 2] = 1. Rres[1, 1] = c Rres[3, 1] = s Rres[1, 3] = -s Rres[3, 3] = c elif rot_type == 3: if Rres is not None: Rres[1:, 1:] = np.eye(3) Rres[1, 1] = np.cos(angle) Rres[2, 1] = -np.sin(angle) Rres[1, 2] = np.sin(angle) Rres[2, 2] = np.cos(angle) else: Rot[1, 1] = np.cos(angle) Rot[2, 1] = -np.sin(angle) Rot[1, 2] = np.sin(angle) Rot[2, 2] = np.cos(angle) return Rot else: raise ValueError(str(rot_type) + ' is not a valid rotation type') Rres[3, 3] = 1. Rres[1, 1] = c Rres[2, 1] = -s Rres[1, 2] = s Rres[2, 2] = c return Rres
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!