You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
351 lines
11 KiB
Python
351 lines
11 KiB
Python
7 months ago
|
from math import asin, atan2, pi, sqrt
|
||
|
|
import numpy as np
|
||
|
|
|
||
|
|
class Matrix_3x3():
|
||
|
|
|
||
|
|
def __init__(self, matrix = None) -> None:
|
||
|
|
'''
|
||
|
|
Constructor examples:
|
||
|
|
a = Matrix_3x3( ) # create identity matrix
|
||
|
|
b = Matrix_3x3( [[1,1,1],[2,2,2],[3,3,3]] ) # manually initialize matrix
|
||
|
|
c = Matrix_3x3( [1,1,1,2,2,2,3,3,3] ) # manually initialize matrix
|
||
|
|
d = Matrix_3x3( b ) # copy constructor
|
||
|
|
'''
|
||
|
|
if matrix is None:
|
||
|
|
self.m = np.identity(3)
|
||
|
|
elif type(matrix) == Matrix_3x3:
|
||
|
|
self.m = np.copy(matrix.m)
|
||
|
|
else:
|
||
|
|
self.m = np.asarray(matrix)
|
||
|
|
self.m.shape = (3,3) #reshape if needed, throw error if impossible
|
||
|
|
|
||
|
|
|
||
|
|
self.rotation_shortcuts={(1,0,0):self.rotate_x_rad, (-1, 0, 0):self._rotate_x_neg_rad,
|
||
|
|
(0,1,0):self.rotate_y_rad, ( 0,-1, 0):self._rotate_y_neg_rad,
|
||
|
|
(0,0,1):self.rotate_z_rad, ( 0, 0,-1):self._rotate_z_neg_rad}
|
||
|
|
|
||
|
|
@classmethod
|
||
|
|
def from_rotation_deg(cls, euler_vec):
|
||
|
|
'''
|
||
|
|
Create rotation matrix from Euler angles, in degrees.
|
||
|
|
Rotation order: RotZ*RotY*RotX
|
||
|
|
|
||
|
|
Parameters
|
||
|
|
----------
|
||
|
|
euler_vec : array_like, length 3
|
||
|
|
vector with Euler angles (x,y,z) aka (roll, pitch, yaw)
|
||
|
|
|
||
|
|
Example
|
||
|
|
----------
|
||
|
|
Matrix_3x3.from_rotation_deg((roll,pitch,yaw)) # Creates: RotZ(yaw)*RotY(pitch)*RotX(roll)
|
||
|
|
'''
|
||
|
|
mat = cls().rotate_z_deg(euler_vec[2], True).rotate_y_deg(euler_vec[1], True).rotate_x_deg(euler_vec[0], True)
|
||
|
|
return mat
|
||
|
|
|
||
|
|
def get_roll_deg(self):
|
||
|
|
''' Get angle around the x-axis in degrees, Rotation order: RotZ*RotY*RotX=Rot '''
|
||
|
|
if self.m[2,1] == 0 and self.m[2,2] == 0:
|
||
|
|
return 180
|
||
|
|
return atan2(self.m[2,1], self.m[2,2]) * 180 / pi
|
||
|
|
|
||
|
|
def get_pitch_deg(self):
|
||
|
|
''' Get angle around the y-axis in degrees, Rotation order: RotZ*RotY*RotX=Rot '''
|
||
|
|
return atan2(-self.m[2,0], sqrt(self.m[2,1]*self.m[2,1] + self.m[2,2]*self.m[2,2])) * 180 / pi
|
||
|
|
|
||
|
|
def get_yaw_deg(self):
|
||
|
|
''' Get angle around the z-axis in degrees, Rotation order: RotZ*RotY*RotX=Rot '''
|
||
|
|
if self.m[1,0] == 0 and self.m[0,0] == 0:
|
||
|
|
return atan2(self.m[0,1], self.m[1,1]) * 180 / pi
|
||
|
|
return atan2(self.m[1,0], self.m[0,0]) * 180 / pi
|
||
|
|
|
||
|
|
def get_inclination_deg(self):
|
||
|
|
''' Get inclination of z-axis in relation to reference z-axis '''
|
||
|
|
return 90 - (asin(self.m[2,2]) * 180 / pi)
|
||
|
|
|
||
|
|
|
||
|
|
def rotate_deg(self, rotation_vec, rotation_deg, in_place=False):
|
||
|
|
'''
|
||
|
|
Rotates the current rotation matrix
|
||
|
|
|
||
|
|
Parameters
|
||
|
|
----------
|
||
|
|
rotation_vec : array_like, length 3
|
||
|
|
rotation vector
|
||
|
|
rotation_rad : float
|
||
|
|
rotation in degrees
|
||
|
|
in_place: bool, optional
|
||
|
|
* True: the internal matrix is changed in-place (default)
|
||
|
|
* False: a new matrix is returned and the current one is not changed
|
||
|
|
|
||
|
|
Returns
|
||
|
|
-------
|
||
|
|
result : Matrix_3x3
|
||
|
|
self is returned if in_place is True
|
||
|
|
'''
|
||
|
|
return self.rotate_rad(rotation_vec, rotation_deg * (pi/180) , in_place)
|
||
|
|
|
||
|
|
|
||
|
|
def rotate_rad(self, rotation_vec, rotation_rad, in_place=False):
|
||
|
|
'''
|
||
|
|
Rotates the current rotation matrix
|
||
|
|
|
||
|
|
Parameters
|
||
|
|
----------
|
||
|
|
rotation_vec : array_like, length 3
|
||
|
|
rotation vector
|
||
|
|
rotation_rad : float
|
||
|
|
rotation in radians
|
||
|
|
in_place: bool, optional
|
||
|
|
* True: the internal matrix is changed in-place (default)
|
||
|
|
* False: a new matrix is returned and the current one is not changed
|
||
|
|
|
||
|
|
Returns
|
||
|
|
-------
|
||
|
|
result : Matrix_3x3
|
||
|
|
self is returned if in_place is True
|
||
|
|
'''
|
||
|
|
|
||
|
|
if rotation_rad == 0: return
|
||
|
|
|
||
|
|
shortcut = self.rotation_shortcuts.get(tuple(a for a in rotation_vec))
|
||
|
|
if shortcut:
|
||
|
|
return shortcut(rotation_rad, in_place)
|
||
|
|
|
||
|
|
c = np.math.cos(rotation_rad)
|
||
|
|
c1 = 1 - c
|
||
|
|
s = np.math.sin(rotation_rad)
|
||
|
|
x = rotation_vec[0]
|
||
|
|
y = rotation_vec[1]
|
||
|
|
z = rotation_vec[2]
|
||
|
|
xxc1 = x * x * c1
|
||
|
|
yyc1 = y * y * c1
|
||
|
|
zzc1 = z * z * c1
|
||
|
|
xyc1 = x * y * c1
|
||
|
|
xzc1 = x * z * c1
|
||
|
|
yzc1 = y * z * c1
|
||
|
|
xs = x * s
|
||
|
|
ys = y * s
|
||
|
|
zs = z * s
|
||
|
|
|
||
|
|
mat = np.array([
|
||
|
|
[xxc1 + c, xyc1 - zs, xzc1 + ys],
|
||
|
|
[xyc1 + zs, yyc1 + c, yzc1 - xs],
|
||
|
|
[xzc1 - ys, yzc1 + xs, zzc1 + c]])
|
||
|
|
|
||
|
|
return self.multiply(mat, in_place)
|
||
|
|
|
||
|
|
|
||
|
|
def _rotate_x_neg_rad(self, rotation_rad, in_place=False):
|
||
|
|
self.rotate_x_rad(-rotation_rad, in_place)
|
||
|
|
|
||
|
|
def _rotate_y_neg_rad(self, rotation_rad, in_place=False):
|
||
|
|
self.rotate_y_rad(-rotation_rad, in_place)
|
||
|
|
|
||
|
|
def _rotate_z_neg_rad(self, rotation_rad, in_place=False):
|
||
|
|
self.rotate_z_rad(-rotation_rad, in_place)
|
||
|
|
|
||
|
|
def rotate_x_rad(self, rotation_rad, in_place=False):
|
||
|
|
'''
|
||
|
|
Rotates the current rotation matrix around the x-axis
|
||
|
|
|
||
|
|
Parameters
|
||
|
|
----------
|
||
|
|
rotation_rad : float
|
||
|
|
rotation in radians
|
||
|
|
in_place: bool, optional
|
||
|
|
* True: the internal matrix is changed in-place (default)
|
||
|
|
* False: a new matrix is returned and the current one is not changed
|
||
|
|
|
||
|
|
Returns
|
||
|
|
-------
|
||
|
|
result : Matrix_3x3
|
||
|
|
self is returned if in_place is True
|
||
|
|
'''
|
||
|
|
if rotation_rad == 0:
|
||
|
|
return self if in_place else Matrix_3x3(self)
|
||
|
|
|
||
|
|
c = np.math.cos(rotation_rad)
|
||
|
|
s = np.math.sin(rotation_rad)
|
||
|
|
|
||
|
|
mat = np.array([
|
||
|
|
[1, 0, 0],
|
||
|
|
[0, c,-s],
|
||
|
|
[0, s, c]])
|
||
|
|
|
||
|
|
return self.multiply(mat, in_place)
|
||
|
|
|
||
|
|
def rotate_y_rad(self, rotation_rad, in_place=False):
|
||
|
|
'''
|
||
|
|
Rotates the current rotation matrix around the y-axis
|
||
|
|
|
||
|
|
Parameters
|
||
|
|
----------
|
||
|
|
rotation_rad : float
|
||
|
|
rotation in radians
|
||
|
|
in_place: bool, optional
|
||
|
|
* True: the internal matrix is changed in-place (default)
|
||
|
|
* False: a new matrix is returned and the current one is not changed
|
||
|
|
|
||
|
|
Returns
|
||
|
|
-------
|
||
|
|
result : Matrix_3x3
|
||
|
|
self is returned if in_place is True
|
||
|
|
'''
|
||
|
|
if rotation_rad == 0:
|
||
|
|
return self if in_place else Matrix_3x3(self)
|
||
|
|
|
||
|
|
c = np.math.cos(rotation_rad)
|
||
|
|
s = np.math.sin(rotation_rad)
|
||
|
|
|
||
|
|
mat = np.array([
|
||
|
|
[ c, 0, s],
|
||
|
|
[ 0, 1, 0],
|
||
|
|
[-s, 0, c]])
|
||
|
|
|
||
|
|
return self.multiply(mat, in_place)
|
||
|
|
|
||
|
|
def rotate_z_rad(self, rotation_rad, in_place=False):
|
||
|
|
'''
|
||
|
|
Rotates the current rotation matrix around the z-axis
|
||
|
|
|
||
|
|
Parameters
|
||
|
|
----------
|
||
|
|
rotation_rad : float
|
||
|
|
rotation in radians
|
||
|
|
in_place: bool, optional
|
||
|
|
* True: the internal matrix is changed in-place (default)
|
||
|
|
* False: a new matrix is returned and the current one is not changed
|
||
|
|
|
||
|
|
Returns
|
||
|
|
-------
|
||
|
|
result : Matrix_3x3
|
||
|
|
self is returned if in_place is True
|
||
|
|
'''
|
||
|
|
if rotation_rad == 0:
|
||
|
|
return self if in_place else Matrix_3x3(self)
|
||
|
|
|
||
|
|
c = np.math.cos(rotation_rad)
|
||
|
|
s = np.math.sin(rotation_rad)
|
||
|
|
|
||
|
|
mat = np.array([
|
||
|
|
[ c,-s, 0],
|
||
|
|
[ s, c, 0],
|
||
|
|
[ 0, 0, 1]])
|
||
|
|
|
||
|
|
return self.multiply(mat, in_place)
|
||
|
|
|
||
|
|
def rotate_x_deg(self, rotation_deg, in_place=False):
|
||
|
|
'''
|
||
|
|
Rotates the current rotation matrix around the x-axis
|
||
|
|
|
||
|
|
Parameters
|
||
|
|
----------
|
||
|
|
rotation_rad : float
|
||
|
|
rotation in degrees
|
||
|
|
in_place: bool, optional
|
||
|
|
* True: the internal matrix is changed in-place (default)
|
||
|
|
* False: a new matrix is returned and the current one is not changed
|
||
|
|
|
||
|
|
Returns
|
||
|
|
-------
|
||
|
|
result : Matrix_3x3
|
||
|
|
self is returned if in_place is True
|
||
|
|
'''
|
||
|
|
return self.rotate_x_rad(rotation_deg * (pi/180), in_place)
|
||
|
|
|
||
|
|
def rotate_y_deg(self, rotation_deg, in_place=False):
|
||
|
|
'''
|
||
|
|
Rotates the current rotation matrix around the y-axis
|
||
|
|
|
||
|
|
Parameters
|
||
|
|
----------
|
||
|
|
rotation_rad : float
|
||
|
|
rotation in degrees
|
||
|
|
in_place: bool, optional
|
||
|
|
* True: the internal matrix is changed in-place (default)
|
||
|
|
* False: a new matrix is returned and the current one is not changed
|
||
|
|
|
||
|
|
Returns
|
||
|
|
-------
|
||
|
|
result : Matrix_3x3
|
||
|
|
self is returned if in_place is True
|
||
|
|
'''
|
||
|
|
return self.rotate_y_rad(rotation_deg * (pi/180), in_place)
|
||
|
|
|
||
|
|
def rotate_z_deg(self, rotation_deg, in_place=False):
|
||
|
|
'''
|
||
|
|
Rotates the current rotation matrix around the z-axis
|
||
|
|
|
||
|
|
Parameters
|
||
|
|
----------
|
||
|
|
rotation_rad : float
|
||
|
|
rotation in degrees
|
||
|
|
in_place: bool, optional
|
||
|
|
* True: the internal matrix is changed in-place (default)
|
||
|
|
* False: a new matrix is returned and the current one is not changed
|
||
|
|
|
||
|
|
Returns
|
||
|
|
-------
|
||
|
|
result : Matrix_3x3
|
||
|
|
self is returned if in_place is True
|
||
|
|
'''
|
||
|
|
return self.rotate_z_rad(rotation_deg * (pi/180), in_place)
|
||
|
|
|
||
|
|
def invert(self, in_place=False):
|
||
|
|
'''
|
||
|
|
Inverts the current rotation matrix
|
||
|
|
|
||
|
|
Parameters
|
||
|
|
----------
|
||
|
|
in_place: bool, optional
|
||
|
|
* True: the internal matrix is changed in-place (default)
|
||
|
|
* False: a new matrix is returned and the current one is not changed
|
||
|
|
|
||
|
|
Returns
|
||
|
|
-------
|
||
|
|
result : Matrix_3x3
|
||
|
|
self is returned if in_place is True
|
||
|
|
'''
|
||
|
|
|
||
|
|
if in_place:
|
||
|
|
self.m = np.linalg.inv(self.m)
|
||
|
|
return self
|
||
|
|
else:
|
||
|
|
return Matrix_3x3(np.linalg.inv(self.m))
|
||
|
|
|
||
|
|
def multiply(self,mat, in_place=False, reverse_order=False):
|
||
|
|
'''
|
||
|
|
Multiplies the current rotation matrix by mat
|
||
|
|
|
||
|
|
Parameters
|
||
|
|
----------
|
||
|
|
mat : Matrix_3x3 or array_like
|
||
|
|
multiplier matrix or 3D vector
|
||
|
|
in_place: bool, optional
|
||
|
|
- True: the internal matrix is changed in-place
|
||
|
|
- False: a new matrix is returned and the current one is not changed (default)
|
||
|
|
reverse_order: bool, optional
|
||
|
|
- False: self * mat
|
||
|
|
- True: mat * self
|
||
|
|
|
||
|
|
Returns
|
||
|
|
-------
|
||
|
|
result : Matrix_3x3 | array_like
|
||
|
|
Matrix_3x3 is returned if mat is a matrix (self is returned if in_place is True);
|
||
|
|
a 3D vector is returned if mat is a vector
|
||
|
|
'''
|
||
|
|
# get array from matrix object or convert to numpy array (if needed)
|
||
|
|
mat = mat.m if type(mat) == Matrix_3x3 else np.asarray(mat)
|
||
|
|
|
||
|
|
a,b = (mat, self.m) if reverse_order else (self.m, mat)
|
||
|
|
|
||
|
|
if mat.ndim == 1:
|
||
|
|
return np.matmul(a, b) # multiplication by 3D vector
|
||
|
|
elif in_place:
|
||
|
|
np.matmul(a, b, self.m) # multiplication by matrix, in place
|
||
|
|
return self
|
||
|
|
else: # multiplication by matrix, return new Matrix_3x3
|
||
|
|
return Matrix_3x3(np.matmul(a, b))
|
||
|
|
|
||
|
|
|