ars / tests / utils_geometry.py

 ``` 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89``` ``` # 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) ```