Coverage for src\attipy\_transforms.py: 17%

1import numpy as np 

2from numba import njit 

3from numpy.typing import NDArray 

4 

5 

6@njit # type: ignore[misc] 

7def _rot_matrix_from_quaternion(q: NDArray[np.float64]) -> NDArray[np.float64]: 

8 """ 

9 Compute the rotation matrix from a unit quaternion. 

10 

11 Parameters 

12 ---------- 

13 q : numpy.ndarray, shape (4,) 

14 Unit quaternion. 

15 

16 Returns 

17 ------- 

18 rot : numpy.ndarray, shape (3, 3) 

19 Rotation matrix. 

20 """ 

21 q0, q1, q2, q3 = q 

22 

23 _2q1 = q1 + q1 

24 _2q2 = q2 + q2 

25 _2q3 = q3 + q3 

26 

27 _2q1q1 = q1 * _2q1 

28 _2q1q2 = q1 * _2q2 

29 _2q1q3 = q1 * _2q3 

30 _2q2q2 = q2 * _2q2 

31 _2q2q3 = q2 * _2q3 

32 _2q3q3 = q3 * _2q3 

33 _2q0q1 = q0 * _2q1 

34 _2q0q2 = q0 * _2q2 

35 _2q0q3 = q0 * _2q3 

36 

37 rot_00 = 1.0 - (_2q2q2 + _2q3q3) 

38 rot_01 = _2q1q2 - _2q0q3 

39 rot_02 = _2q1q3 + _2q0q2 

40 

41 rot_10 = _2q1q2 + _2q0q3 

42 rot_11 = 1.0 - (_2q1q1 + _2q3q3) 

43 rot_12 = _2q2q3 - _2q0q1 

44 

45 rot_20 = _2q1q3 - _2q0q2 

46 rot_21 = _2q2q3 + _2q0q1 

47 rot_22 = 1.0 - (_2q1q1 + _2q2q2) 

48 

49 rot = np.array( 

50 [ 

51 [rot_00, rot_01, rot_02], 

52 [rot_10, rot_11, rot_12], 

53 [rot_20, rot_21, rot_22], 

54 ] 

55 ) 

56 return rot