
    h'                        d dl Z d dlZd dlmZ d dl mZmZ d dlmZmZ d dl	m
Z d dlmZ d dlZddefd	Zd
edefdZd
eeef         deeef         fdZde j        de j        fdZde j        de j        fdZde j        de j        fdZ	 	 ddeeef         deeef         deeef         deeef         dededeeef         fdZdS )    N)ndarray)TensorFloatTensor)TupleUnion)Rotation)
csc_matrixTreturnc                 F   t          | t                    s!t          j        | t          j                  }n| }t          j        |d          \  }}}}d||z                      d          z  }|j        }|rt          j        d|||z  ||z  z   z  z
  |||z  ||z  z
  z  |||z  ||z  z   z  t          j	        |j
        dd         |t          j                  |||z  ||z  z   z  d|||z  ||z  z   z  z
  |||z  ||z  z
  z  t          j	        |j
        dd         |t          j                  |||z  ||z  z
  z  |||z  ||z  z   z  d|||z  ||z  z   z  z
  t          j	        |j
        dd         |t          j                  t          j	        |j
        dd         |t          j                  t          j	        |j
        dd         |t          j                  t          j	        |j
        dd         |t          j                  t          j        |j
        dd         |t          j                  fd          }	|	                    |j
        dd         dz             S t          j        d|||z  ||z  z   z  z
  |||z  ||z  z
  z  |||z  ||z  z   z  |||z  ||z  z   z  d|||z  ||z  z   z  z
  |||z  ||z  z
  z  |||z  ||z  z
  z  |||z  ||z  z   z  d|||z  ||z  z   z  z
  f	d          }	|	                    |j
        dd         dz             S )	z
    Ref: https://pytorch3d.readthedocs.io/en/latest/_modules/pytorch3d/transforms/rotation_conversions.html#quaternion_to_matrix
    dtype       @   N)devicer   )   r   )   r   )
isinstancer   torchtensorfloat32unbindsumr   stackzerosshapeonesreshape)
xuse_4x4quaternionsrijktwo_sr   os
             0/data/cameron/tmprepos/2unirig/src/data/utils.pyquaternion_to_matrixr)      sm    a   l1EM:::k2..JAq!Q;,11"555EF &:KEQUQU]++QQ'QQ'K-crc26WWWQQ'EQUQU]++QQ'K-crc26WWWQQ'QQ'EQUQU]++K-crc26WWWK-crc26WWWK-crc26WWWK-crc26WWW
;,SbS1&VVV!$ '
 
* yy*3B3/&8999KEQUQU]++QQ'QQ'QQ'EQUQU]++QQ'QQ'QQ'EQUQU]++
 
 
 yy*3B3/&8999    
axis_anglec                 |   t          j        | ddd          }|dz  }d}|                                |k     }t          j        |          }t          j        ||                    ||          z  || <   d||         ||         z  dz  z
  ||<   t          j        t          j        |          | |z  gd          }|S )	z
    Ref: https://pytorch3d.readthedocs.io/en/latest/_modules/pytorch3d/transforms/rotation_conversions.html#axis_angle_to_quaternion
       r   T)pdimkeepdim      ?gư>0   r/   )r   normabs
empty_likesincatcos)r+   angleshalf_anglesepssmall_anglessin_half_angles_over_anglesr!   s          r(   axis_angle_to_quaternionr?   ?   s     Z
aR>>>F3,K
C::<<#%L"'"26":":	+|m,--}0EE  . 	vl#f\&::b@@  - )	;		.I!IJPR  K r*   c                    t          | t                    rt          t          |                     S t	          j        t          j        |                                           ddd          }|j	        dk    sJ d|ddddf<   |S )	z
    Ref: https://pytorch3d.readthedocs.io/en/latest/_modules/pytorch3d/transforms/rotation_conversions.html#axis_angle_to_matrix
    )r   r   r   r   rB   constant)rA   rA   rA   constant_valuesr   r   Nr   )
r   r   r)   r?   nppadRfrom_rotvec	as_matrixndim)r+   ress     r(   axis_angle_to_matrixrM   U   s     *k** #$<Z$H$HIIIfQ]:..88::<TV`  sK  L  L  Lx1}}}}AAAr2I
r*   r   c                     t          j        |           }| dk    }t          j                    rt          j        | |                   ||<   n(t          j        |t          j        |           |          }|S )z[
    Returns torch.sqrt(torch.max(0, x))
    but with a zero subgradient where x is 0.
    r   )r   
zeros_likeis_grad_enabledsqrtwhere)r   retpositive_masks      r(   _sqrt_positive_partrU   a   sj    
 
1

CEM ="Z-(899Mk-A<<Jr*   r!   c                 L    t          j        | dddf         dk     |  |           S )a  
    Convert a unit quaternion to a standard form: one in which the real
    part is non negative.

    Args:
        quaternions: Quaternions with real part first,
            as tensor of shape (..., 4).

    Returns:
        Standardized quaternions as tensor of shape (..., 4).
    .r   r   )r   rR   )r!   s    r(   standardize_quaternionrW   n   s,     ;{3!8,q0;,LLLr*   matrixc                    |                      d          dk    s|                      d          dk    rt          d| j         d          | j        dd         }t          j        |                     |dz             d          \	  }}}}}}}}	}
t          t          j        d	|z   |z   |
z   d	|z   |z
  |
z
  d	|z
  |z   |
z
  d	|z
  |z
  |
z   gd                    }t          j        t          j        |d
         dz  |	|z
  ||z
  ||z
  gd          t          j        |	|z
  |d         dz  ||z   ||z   gd          t          j        ||z
  ||z   |d         dz  ||	z   gd          t          j        ||z
  ||z   |	|z   |d         dz  gd          gd          }t          j        d          	                    |j
        |j                  }|d|d                             |          z  z  }|t          j        j                            |                    d          d          dk    ddf                             |dz             }t#          |          S )z
    Convert rotations given as rotation matrices to quaternions.

    Args:
        matrix: Rotation matrices as tensor of shape (..., 3, 3).

    Returns:
        quaternions with real part first, as tensor of shape (..., 4).
    r   r   zInvalid rotation matrix shape .N)	   r3   g      ?).r   r-   ).r   ).r-   ).r   g?)r   r   r   ).Nr   )num_classesr1   )r   )size
ValueErrorr   r   r   r   rU   r   r   tor   r   maxnn
functionalone_hotargmaxrW   )rX   	batch_dimm00m01m02m10m11m12m20m21m22q_absquat_by_rijkflrquat_candidatesouts                   r(   matrix_to_quaternionru   |   s    {{2!v{{2!33I&,IIIJJJSbS!I27,y4'((b3 3 3/Cc3S#sC  c	C#%c	C#%c	C#%c	C#%	 	
 	
 	

 
E ; Kv!+S3Yc	39MSUVVV KsE&MQ$6c	39MSUVVV KsC#IuV}/A39MSUVVV KsC#IsSy%-1:LMSUVVV	
   L( ,s



U[

F
FC"cE),<,@,@,E,E&EFO ##ELLRL$8$8a#HH3NPQPQPQQgi$  "#&&&r*           vertexmatrix_localskinrG   valuec           
         | j         d         |z   dk    sJ t          | t                    r|                                 }n+t          | t                    r| j        }nt                      |dk    rSt          | t                    sJ |j         d         }|                                t          j	        j
                            | d|ddddf|                              d                              dd                              d|dd          z  }||z  }	|                    dd                              d          |	z  }
|
                    d          }|d	d	ddd	d	f         |                    dd                              d          d
z                       d          z  }|                    ddd          S |dk    rt          | t                    r5|j         d         }|                                t          j	        j
                            | d|ddf|                              d                              dd                              |dd          z  }||z  }	|                    dd                              d          |	z  }
|
                    d          }|ddd	d	f         |                    dd                              d          d
z                       d          z  }|                    dd          S |j         d         }| j         d         }t#          j        | dd|ffdd|f          j        }|t"          j                            |          z  }g }
|dk    j        }t+          |          D ]w}t#          j        d|ft"          j                  }||         |d	d	||         f         z  |j        |||         f         z  |d	d	||         f<   |
                    |           xt#          j        |
          }
t#          j        |
d          }|d	dd	d	f         t#          j        |d          d
z   z  }|j        S J d| j                      )a  
    Args:
        vertex: (B, N, 4-pad) or (N, 4-pad)
        matrix_local: (B, J, 4, 4) or (J, 4, 4)
        matrix: (B, J, 4, 4) or (J, 4, 4)
        skin: (B, N, J) or (N, J), value of pseudo bones should be 0
    Returns:
        (B, N, 3) or (N, 3)
    r   r   r   r   r   )rz   r-   r3   Ng:0yE>rA   rC   rD   r   )axiszunsupported shape: )r   r   r   r/   r   rK   NotImplementedErrorinverser   rb   rc   rG   	unsqueeze	transposerepeatr   permuterF   Tlinalginvranger   r   appendr   )rw   rx   rX   ry   rG   rz   dimsJoffsetper_bone_matrixweighted_per_bone_matrixgfinalNpaddedtransmaskr#   s                     r(   linear_blend_skinningr      sw   " <c!Q&&&&&&!! $zz||	FG	$	$ ${!###qyy&&)))))q!   ""H##FQQ1a,@#NNXXYZ[[eefgijkkrrstvwyz|}~~ 	
 !6/#'>>!Q#7#7#A#A!#D#D#V $((Q(//!!!QqS!!!)q! 4 4 8 8Q 8 ? ?$ FQQRSTTT}}Q1%%%	ff%% 	"1%A$$&&#''CAe'LLVVWXYYccdeghiippqrtuwxyyz  %voO'+~~a';';'E'Ea'H'H?'Z$(,,,33Aac111fI1!5!5!9!9a!9!@!@4!G R RST U UUE==A&&&"1%AQAVFVaX$6
UVX]T^___aFRY]]<888E')$1H<D1XX 8 81a&
;;;&+Ah47
1C&CtvaQUVWQXjGY%Yqqq$q'z"(//7777')x0H'I'I$/a888Abqb!!!eHt! 4 4 4t ;<E7N666666qr*   )T)r   rv   )r   numpyrF   r   r   r   typingr   r   scipy.spatial.transformr   rH   scipy.sparser	   r)   r?   rM   rU   rW   ru   intfloatr    r*   r(   <module>r      s,              % % % % % % % %         1 1 1 1 1 1 # # # # # #    2: 2:[ 2: 2: 2: 2:h     ,
U;+?%@ 
U;X_K_E` 
 
 
 
5< EL    M M M M M M;' ;'%, ;' ;' ;' ;'D I7 I7+w&'I7W,-I7 +w&'I7 W$
%	I7
 
I7 I7 ; I7 I7 I7 I7 I7 I7r*   