German Larrain avatar German Larrain committed 98f470c

backport of a346e3f98c53

Comments (0)

Files changed (2)

ars/utils/geometry.py

 	pitch = yaw_z
 	#yaw = pitch_y  # we don't need it
 	return pitch, roll
+
+
+def calc_compass_angle(rot_matrix):
+	"""Returns the angle around the vertical axis with respect to the X+ axis,
+	i.e. the angular orientation inherent of a rotation matrix `rot_matrix`,
+	constrained to the plane aligned with the horizon (XZ, since the vertical
+	axis is Y).
+
+	"""
+	roll_x, pitch_y, yaw_z = _rot_matrix_to_rpy_angles(rot_matrix)
+	yaw = pitch_y
+	return yaw

tests/utils_geometry.py

 	print('pitch: %f, roll: %f' % (pitch, roll))
 	print('')
 
+def test_calc_compass_angle(axis, angle):
+	rot_matrix = gemut.calc_rotation_matrix(axis, angle)
+	yaw = gemut.calc_compass_angle(rot_matrix)
+	print('axis: %s, angle: %f' % (axis, angle))
+	print('compass: %f' % yaw)
+	print('')
+
 if __name__ == "__main__":
 
 	_test_rot_matrix_to_hom_transform()
 	test_calc_inclination(mut.leftAxis, mut.pi/8)
 	test_calc_inclination(mut.bkwdAxis, mut.pi/8)
 
+
+	test_calc_compass_angle(mut.upAxis, mut.pi/8)
+	test_calc_compass_angle(mut.upAxis, -mut.pi/8)
+	test_calc_compass_angle(mut.upAxis, 3.0 * mut.pi / 2)
+	test_calc_compass_angle(mut.downAxis, mut.pi/8)
+	test_calc_compass_angle(mut.downAxis, -mut.pi/8)
+	test_calc_compass_angle(mut.rightAxis, mut.pi/8)
+	test_calc_compass_angle(mut.leftAxis, mut.pi/8)
+	test_calc_compass_angle(mut.bkwdAxis, mut.pi/8)
+	test_calc_compass_angle(mut.fwdAxis, mut.pi/8)
+
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.