Commit d4515bf2 authored by Matt Clarkson's avatar Matt Clarkson

Merge branch '12-generate-rigid-transformation' into 'master'

Resolve "Agree a 'standard' ordering for R_x, R_y, R_z, and give a function to generate a Rigid Body Transformation."

Closes #12

See merge request WEISS/SoftwareRepositories/SNAPPY/scikit-surgerycore!7
parents 0e4b0bd3 d7c412db
Pipeline #1519 passed with stages
in 10 minutes and 32 seconds
"""
Construct 3x3 rotation matrices for rotating around the x, y, z axes.
Construct 3x3 rotation matrices for rotating around the x, y and z axes
individually, as well as 3x3 rotation matrices from sequences of three
Euler angles.
"""
import numpy as np
......@@ -9,9 +11,9 @@ def construct_rx_matrix(angle, is_in_radians=True):
"""
Construct a rotation matrix for rotation around the x axis.
:param angle: the angle to rotate
:param is_in_radians: if angle is in radians or not, default being True
:returns: rot_x -- the 3x3 rotation matrix
:param angle: the angle to rotate, float
:param is_in_radians: if angle is in radians, default being True, bool
:returns: rot_x -- the 3x3 rotation matrix constructed, numpy array
"""
if not is_in_radians:
angle = np.pi * angle / 180
......@@ -28,9 +30,9 @@ def construct_ry_matrix(angle, is_in_radians=True):
"""
Construct a rotation matrix for rotation around the y axis.
:param angle: the angle to rotate
:param is_in_radians: if angle is in radians or not, default being True
:returns: rot_y -- the 3x3 rotation matrix
:param angle: the angle to rotate, float
:param is_in_radians: if angle is in radians, default being True, bool
:returns: rot_y -- the 3x3 rotation matrix constructed, numpy array
"""
if not is_in_radians:
angle = np.pi * angle / 180
......@@ -47,9 +49,9 @@ def construct_rz_matrix(angle, is_in_radians=True):
"""
Construct a rotation matrix for rotation around the z axis.
:param angle: the angle to rotate
:param is_in_radians: if angle is in radians or not, default being True
:returns: rot_z -- the 3x3 rotation matrix
:param angle: the angle to rotate, float
:param is_in_radians: if angle is in radians, default being True, bool
:returns: rot_z -- the 3x3 rotation matrix constructed, numpy array
"""
if not is_in_radians:
angle = np.pi * angle / 180
......@@ -60,3 +62,103 @@ def construct_rz_matrix(angle, is_in_radians=True):
rot_z = np.array([[cos_z, -sin_z, 0], [sin_z, cos_z, 0], [0, 0, 1]])
return rot_z
def construct_rotm_from_euler(
angle_a, angle_b, angle_c,
sequence, is_in_radians=True):
"""
Construct a rotation matrix from a sequence of three Euler angles, to
pre-multiply column vectors. In the case of tracking, the column vector is
under the local coordinate system and the resulting vector is under the
world (reference) coordinate system.
The three elemental rotations represented by the Euler angles are about the
INTRINSIC axes. They can also be interpreted to be about the EXTRINSIC axes,
in the reverse order.
:param angle_a: first Euler angle, float
:param angle_b: second Euler angle, float
:param angle_c: third Euler angle, float
:param sequence: the sequence of axes the three elemental rotations are
about, with respect to the intrinsic axes, string
:param is_in_radians: if the angles are in radians, default being True,
bool
:returns: rot_m -- the 3x3 rotation matrix, numpy array
"""
while True:
if sequence == 'zxz':
rot_a = construct_rz_matrix(angle_a, is_in_radians)
rot_b = construct_rx_matrix(angle_b, is_in_radians)
rot_c = construct_rz_matrix(angle_c, is_in_radians)
break
if sequence == 'zyz':
rot_a = construct_rz_matrix(angle_a, is_in_radians)
rot_b = construct_ry_matrix(angle_b, is_in_radians)
rot_c = construct_rz_matrix(angle_c, is_in_radians)
break
if sequence == 'xyx':
rot_a = construct_rx_matrix(angle_a, is_in_radians)
rot_b = construct_ry_matrix(angle_b, is_in_radians)
rot_c = construct_rx_matrix(angle_c, is_in_radians)
break
if sequence == 'xzx':
rot_a = construct_rx_matrix(angle_a, is_in_radians)
rot_b = construct_rz_matrix(angle_b, is_in_radians)
rot_c = construct_rx_matrix(angle_c, is_in_radians)
break
if sequence == 'yxy':
rot_a = construct_ry_matrix(angle_a, is_in_radians)
rot_b = construct_rx_matrix(angle_b, is_in_radians)
rot_c = construct_ry_matrix(angle_c, is_in_radians)
break
if sequence == 'yzy':
rot_a = construct_ry_matrix(angle_a, is_in_radians)
rot_b = construct_rz_matrix(angle_b, is_in_radians)
rot_c = construct_ry_matrix(angle_c, is_in_radians)
break
if sequence == 'xyz':
rot_a = construct_rx_matrix(angle_a, is_in_radians)
rot_b = construct_ry_matrix(angle_b, is_in_radians)
rot_c = construct_rz_matrix(angle_c, is_in_radians)
break
if sequence == 'zyx':
rot_a = construct_rz_matrix(angle_a, is_in_radians)
rot_b = construct_ry_matrix(angle_b, is_in_radians)
rot_c = construct_rx_matrix(angle_c, is_in_radians)
break
raise ValueError(sequence + " is not a valid sequence.")
rot_tmp = np.matmul(rot_a, rot_b)
rot_m = np.matmul(rot_tmp, rot_c)
return rot_m
def construct_rigid_transformation(rot_m, trans_v):
"""Construct a 4x4 rigid-body transformation from a 3x3 rotation matrix and
a 3x1 vector as translation
:param rot_m: 3x3 rotation matrix, numpy array
:param trans_v: 3x1 vector as translation, numpy array
:returns: rigid_transformation -- 4x4 rigid transformation matrix,
numpy array
"""
rigid_transformation = np.identity(4)
for i in range(3):
for j in range(3):
rigid_transformation[i][j] = rot_m[i][j]
rigid_transformation[0][3] = trans_v[0]
rigid_transformation[1][3] = trans_v[1]
rigid_transformation[2][3] = trans_v[2]
return rigid_transformation
This diff is collapsed.
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