Source

ars / tests / utils_geometry.py


# TODO: create a test for calc_rotation_matrix

import numpy as np

from ars.constants import (DOWN_AXIS, IN_AXIS, LEFT_AXIS, OUT_AXIS, pi,
                           RIGHT_AXIS, UP_AXIS)
import ars.utils.geometry as gemut


def _test_Transform():

	pos = (10,20,30)
	rot = (0,0,1,1,0,0,0,1,0)
	t = gemut.Transform(pos, rot)

	print(pos)
	print(rot)
	print(t)
	print(t.get_long_tuple())
	print(t.matrix)
	print(t.get_rotation())
	print(t.get_rotation(True))
	print(t.get_translation())
	print(t.get_translation(True))

def _test_rot_matrix_to_hom_transform():

	rot1 = np.mat([[1,2,3],[4,5,6],[7,8,9]])
	rot2 = ((1,2,3),(4,5,6),(7,8,9))
	rot3 = (1,2,3,4,5,6,7,8,9)
	ht1 = gemut.rot_matrix_to_hom_transform(rot1)
	ht2 = gemut.rot_matrix_to_hom_transform(rot2)
	ht3 = gemut.rot_matrix_to_hom_transform(rot3)

	#print(rot1)
	#print(rot2)
	print(ht1)
	print(ht2)
	print(ht3)

def test_calc_inclination(axis, angle):
	#axis = UP_AXIS
	#angle = pi / 4
	rot = gemut.calc_rotation_matrix(axis, angle)
	pitch, roll = gemut.calc_inclination(rot)
	print('axis: %s, angle: %f' % (axis, angle))
	print('pitch: %f, roll: %f' % (pitch, roll))
	print('')

def test_calc_compass_angle(axis, angle):
	rot = gemut.calc_rotation_matrix(axis, angle)
	yaw = gemut.calc_compass_angle(rot)
	print('axis: %s, angle: %f' % (axis, angle))
	print('compass: %f' % yaw)
	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_calc_inclination(UP_AXIS, pi / 4)
	test_calc_inclination(UP_AXIS, 3 * pi / 9)
	test_calc_inclination(UP_AXIS, -pi / 8)
	test_calc_inclination(DOWN_AXIS, pi / 4)
	test_calc_inclination(DOWN_AXIS, 3 * pi / 9)
	test_calc_inclination(DOWN_AXIS, -pi / 8)

	test_calc_inclination(RIGHT_AXIS, pi / 6)
	test_calc_inclination(RIGHT_AXIS, 3 * pi / 9)
	test_calc_inclination(RIGHT_AXIS, -pi / 8)
	test_calc_inclination(LEFT_AXIS, pi / 8)
	test_calc_inclination(OUT_AXIS, pi / 8)


	test_calc_compass_angle(UP_AXIS, pi / 8)
	test_calc_compass_angle(UP_AXIS, -pi / 8)
	test_calc_compass_angle(UP_AXIS, 3.0 * pi / 2)
	test_calc_compass_angle(DOWN_AXIS, pi / 8)
	test_calc_compass_angle(DOWN_AXIS, -pi / 8)
	test_calc_compass_angle(RIGHT_AXIS, pi / 8)
	test_calc_compass_angle(LEFT_AXIS, pi / 8)
	test_calc_compass_angle(OUT_AXIS, pi / 8)
	test_calc_compass_angle(IN_AXIS, pi / 8)