# Commits

committed 6899487

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

• Participants
• Parent commits 1be8b50
• Branches dev

# File ars/utils/geometry.py

• Ignore whitespace
` 	"""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`

# File tests/utilities_geometry.py

• Ignore whitespace
` 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)`