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