Commit 4597884d authored by Olivier Lantsoght's avatar Olivier Lantsoght
Browse files

[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!
Please register or to comment