Commits

German Larrain committed 6899487

utils.geometry: new 'rot_matrix_to_euler_angles' and 'calc_inclination' functions

Comments (0)

Files changed (2)

ars/utils/geometry.py

 	"""Returns the 3-vector vector transformed into the local coordinate system
 	of ODE body 'body'"""
 	return mut.rotate3(mut.transpose3(body.get_rotation()), vector)
+
+
+def rot_matrix_to_euler_angles(rot_matrix):
+	"""Returns the 3-1-3 Euler angles 'phi', 'theta' and 'psi' (using the
+	x-convention) corresponding to the rotation matrix ``rot_matrix``, which
+	is a tuple of three 3-element tuples, where each one is a row (what is
+	called row-major order).
+
+	Using the x-convention, the 3-1-3 Euler angles 'phi', 'theta' and 'psi'
+	(around	the Z, X and again the Z-axis) can be obtained as follows
+		phi = arctan2(A_{31}, A_{32})
+		theta = arccos(A_{33})
+		psi = -arctan2(A_{13}, A_{23})
+ 	http://en.wikipedia.org/wiki/Rotation_representation_(mathematics)#Rotation_matrix_.E2.86.94_Euler_angles
+	"""
+	A = rot_matrix
+	phi = mut.atan2(A[2][0], A[2][1])		# arctan2(A_{31}, A_{32})
+	theta = mut.acos(A[2][2])				# arccos(A_{33})
+	psi = -mut.atan2(A[0][2], A[1][2])		# -arctan2(A_{13}, A_{23})
+	angles = (phi, theta, psi)
+	return angles
+
+
+def calc_inclination(rot_matrix, axis=mut.upAxis):
+	"""Returns the inclination of an orientation -defined by the rotation matrix
+	``rot_matrix``- with respect to a plane orthogonal to the given axis, which
+	defaults to the unit vector pointing upwards as defined in this module.
+
+	Example:
+	If rot_matrix = calc_rotation_matrix(rightAxis, pi/4)
+	"""
+	# TODO: complete docstrings
+	# TODO: check tests
+	rotated_axis = mut.rotate3(rot_matrix, axis)
+	angle = mut.acos_dot3(axis, rotated_axis)
+	return angle

tests/utilities_geometry.py

 import numpy as np
 
 import ars.utils.geometry as gemut
+import ars.utils.mathematical as mut
 
 
 def _test_Transform():
 	print(ht2)
 	print(ht3)
 
+def test_get_inclination(axis, angle, correct):
+	#axis = mut.upAxis
+	#angle = mut.pi/4
+	rot_matrix = gemut.calc_rotation_matrix(axis, angle)
+	res = gemut.calc_inclination(rot_matrix, mut.upAxis)
+	print('angle: %f' % angle)
+	print('tilt: %f' % res)
+	print('error: %f' % (correct - res))
+	print('')
+
 if __name__ == "__main__":
 
 	_test_rot_matrix_to_hom_transform()
 
 	_test_Transform()
+
+	# rotation around axii perpendicular to the ground does not affect
+	# the inclination
+	test_get_inclination(mut.upAxis, mut.pi/4, 0)
+	test_get_inclination(mut.upAxis, 3*mut.pi/9, 0)
+	test_get_inclination(mut.upAxis, -mut.pi/8, 0)
+	test_get_inclination(mut.downAxis, mut.pi/4, 0)
+	test_get_inclination(mut.downAxis, 3*mut.pi/9, 0)
+	test_get_inclination(mut.downAxis, -mut.pi/8, 0)
+
+	test_get_inclination(mut.rightAxis, mut.pi/6, mut.pi/6)
+	test_get_inclination(mut.rightAxis, 3*mut.pi/9, 3*mut.pi/9)
+	test_get_inclination(mut.rightAxis, -mut.pi/8, -mut.pi/8)