o
    ߥiE                     @   sF   d dl Zd dlZddlmZmZmZmZ 	dddZG dd dZ	dS )	    N   )aa2quatbatch_rodriguesmat2aa
quat2eulerc	              	      s  t |d}	jd }
jd } dg  f	dddd dd	|
 d
|  7 t|
D ]A}d|| d || d || d f 7 t|D ]!}| }d| | d | | d | | d f 7 qVd7 q;|	 S )Nwr   r   z
HIERARCHY
c                    s:   |  | dkrd|   }dj }nd|   }dj }d|  d |  d |  d f }|| d	 7 |d
 7 |d | d	 7 |d | d	 7 d}t| d jd D ]}| | krrd}||d  qa|s|d d 7 |d d
 7 |d d 7 |d d 7 |d 7 d S )Nr   zROOT zUCHANNELS 6 Xposition Yposition Zposition             {}rotation {}rotation {}rotationzJOINT z7CHANNELS 3 {}rotation {}rotation             {}rotationzOFFSET %.6f %.6f %.6fr      
z{
	FTz	End Site
z		zOFFSET 0 0 0
z}
)appendformatrangeshape)idxprefix
name_labelchannel_labeloffset_label	has_childy	endsitefile_stringnamesoffsetorderparentrotationseqwrite_static e/home/ubuntu/.local/lib/python3.10/site-packages/modelscope/models/cv/human3d_animation/bvh_writer.pyr      s:   
zwrite_bvh.<locals>.write_static zMOTION
zFrames: {}
zFrame Time: %.8f
z%.6f %.6f %.6f r   r	   )openr   upperr   r   write)r   r   r   positionr   	frametimer   pathr   fileframe	joint_numijr   r    r   r!   	write_bvh   s2   
	


!
*


r.   c                   @   sF   e Zd Zdd Zdd Zdd Zdd Zd	d
 Z				dddZdS )WriterWrapperc                 C   s
   || _ d S N)parents)selfr1   r    r    r!   __init__P   s   
zWriterWrapper.__init__c                 C   s0   | |jd dd}t|}t|dd}|}|S )Nr      xyz)r   )reshaper   r   r   )r2   rotquateulerr    r    r!   
axis2eulerS   s
   zWriterWrapper.axis2eulerc                 C   s   | |jd dd}g d}|dkrttdd}||7 }t|jd |df}|d d |d d f |d d d t|d d f< |S )Nr   r4   r5   )r   r         r         r5            	                                 rM      A   )r7   r   listr   torchzeroslen)r2   r8   n_bonesmpl_mapperhand_mappernew_rotr    r    r!   mapper_rot_mixamoZ   s   0zWriterWrapper.mapper_rot_mixamoc                 C   s   t |ddd|dd}|jd }||jd dd}| }t|D ]E}t || ddd|dd}|D ]0}	|d|	d d d d f }
|d|	d d d d f }t|t|
}t|}||||	d d f< q9q%| 	|}|S )Nr4   r5   r   r   )
r   r7   r   cloner   rQ   matmulinverser   r;   )r2   r8   	rest_pose	node_listrT   	frame_numrW   k
action_rotr,   rot1rot2nrotnvecr    r    r!   transform_rot_with_restposek   s&   

z)WriterWrapper.transform_rot_with_restposec                 C   s   t d ||jd dd}| |}t |j t |j d}d}tg dg dg}|d d ||d	 d d f }|| }|d d ||d	 d d f  |8  < |S )
Ntransform_rot_with_stdAposer   r4   r5   rH   rJ   )gGr5@gcZBgtf0@)gec]4g㥛 p%@gs!r   )printr7   r   r;   rQ   tensor)r2   r8   r\   smpl_left_arm_idxsmpl_right_arm_idxstd_arm_rotxdeltar    r    r!   rf      s   



&z)WriterWrapper.transform_rot_with_stdAposeNr   c              	   C   sh  t |tjst|}|jd }|d d}|d u r$td|df}nZ|d u r/| ||}n0|dkrS| ||}t	|j ddg}	|jd }t	|ddd d f  ng d}	| 
|||	|}td|d f}
| |
}
d}|
|dd}
t|
|fd}||jd d}|jd }|dd d f || d d d f< d	d
 t|D }t| j||||dd| d S )Nr   r   r5      rE   rJ   )r   r   r5   r<   rC   r?   rA   rD   rG   rn   rE   .c                 S   s   g | ]}d | qS )z%02dr    ).0r,   r    r    r!   
<listcomp>   s    z'WriterWrapper.write.<locals>.<listcomp>g镲?r6   )
isinstancerQ   Tensorrh   r   	unsqueezenprR   rX   rg   re   r;   repeatcatr   r.   r1   )r2   filenamer   r8   
action_locr\   correct_armrT   posnode_list_chagerest
frames_add
action_lenr   r    r    r!   r%      s<   





"zWriterWrapper.write)NNNr   )	__name__
__module____qualname__r3   r;   rX   re   rf   r%   r    r    r    r!   r/   N   s    r/   r0   )
numpyrt   rQ   
transformsr   r   r   r   r.   r/   r    r    r    r!   <module>   s   
F