o
    *jE                     @   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prefixZ
name_labelZchannel_labelZoffset_labelZ	has_childy	endsiteZfile_stringnamesoffsetorderparentrotationseqwrite_static r/var/www/html/Deteccion_Ine/venv/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   Z	frametimer   pathr   fileframeZ	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)selfr*   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   )r+   rotZquatZ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   r-   r.   )r   r         r         r.            	                                 rD      A   )r0   r   listr   torchzeroslen)r+   r1   n_boneZsmpl_mapperZ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 )Nr-   r.   r   r   )
r   r0   r   cloner   rH   matmulZinverser   r2   )r+   r1   	rest_poseZ	node_listrK   Z	frame_numrL   kZ
action_rotr%   Zrot1Zrot2ZnrotZ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   r-   r.   r?   rA   )gGr5@gcZBgtf0@)gec]4g㥛 p%@gs!r   )printr0   r   r2   rH   tensor)r+   r1   rP   Zsmpl_left_arm_idxZsmpl_right_arm_idxZstd_arm_rotxdeltar   r   r   rS      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   r.      r<   rA   )r   r   r.   r3   r:   r6   r8   r;   r>   rX   r<   .c                 S   s   g | ]}d | qS )z%02dr   ).0r%   r   r   r   
<listcomp>   s    z'WriterWrapper.write.<locals>.<listcomp>g镲?r/   )
isinstancerH   ZTensorrU   r   Z	unsqueezenprI   rM   rT   rR   r2   repeatcatr   r'   r*   )r+   filenamer   r1   Z
action_locrP   Zcorrect_armrK   posZnode_list_chagerestZ
frames_addZ
action_lenr   r   r   r   r       s<   





"zWriterWrapper.write)NNNr   )	__name__
__module____qualname__r,   r2   rM   rR   rS   r    r   r   r   r   r(   N   s    r(   r)   )
numpyr\   rH   Z
transformsr   r   r   r   r'   r(   r   r   r   r   <module>   s   
F