#algebra.py# 2.72 KB
Newer Older
Galo Maldonado's avatar
update  
Galo Maldonado committed
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
def quaternion_from_matrix(matrix, isprecise=False):
    """Return quaternion from rotation matrix.                                                                   
                                                                                                                 
    If isprecise is True, the input matrix is assumed to be a precise rotation                                   
    matrix and a faster algorithm is used.                                                                       
                                                                                                                 
    >>> q = quaternion_from_matrix(np.identity(4), True)                                                         
    >>> np.allclose(q, [1, 0, 0, 0])                                                                             
    True                                                                                                         
    >>> q = quaternion_from_matrix(np.diag([1, -1, -1, 1]))                                                      
    >>> np.allclose(q, [0, 1, 0, 0]) or np.allclose(q, [0, -1, 0, 0])                                            
    True                                                                                                         
    >>> R = rotation_matrix(0.123, (1, 2, 3))                                                                    
    >>> q = quaternion_from_matrix(R, True)                                                                      
    >>> np.allclose(q, [0.9981095, 0.0164262, 0.0328524, 0.0492786])                                             
    True                                                                                                         
    >>> R = [[-0.545, 0.797, 0.260, 0], [0.733, 0.603, -0.313, 0],                                               
    ...      [-0.407, 0.021, -0.913, 0], [0, 0, 0, 1]]                                                           
    >>> q = quaternion_from_matrix(R)                                                                            
    >>> np.allclose(q, [0.19069, 0.43736, 0.87485, -0.083611])                                                   
    True                                                                                                         
    >>> R = [[0.395, 0.362, 0.843, 0], [-0.626, 0.796, -0.056, 0],                                               
    ...      [-0.677, -0.498, 0.529, 0], [0, 0, 0, 1]]                                                           
    >>> q = quaternion_from_matrix(R)                                                                            
    >>> np.allclose(q, [0.82336615, -0.13610694, 0.46344705, -0.29792603])