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.
# 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 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)
if Rres is None:
Rres = np.zeros((4, 4))
Rres[:, 0] = 3.
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:] = 0.
c = cos(angle)
s = sin(angle)
if rot_type == 1:
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!
Please register or to comment