o
    -tjΈ                  	   @   s  d Z ddlZddlZddlZddlZddlZddlZddlZddl	m
Z
 ejejeZejejeddddZejde ejdejeddd ddlZddlmZ dd	lmZ dd
lmZ ddlmZm Z!m"Z#m$Z%m&Z' ddl(m)Z*m+Z,m-Z. d\Z/Z0g de1e* Z2e3dd e4e2D Z5e3dd e5D Z6d ddZ7dd Z8e0fddZ9G dd dZ:G dd dZ;dd Z<dd Z=dS )!u  Resident (warm) body-motion engine — the kemix velocity codec+flow, loaded ONCE.

Why: `infer_turn` currently shells out to gen_fulltake_tag3.py per turn, which reloads
the whole model every time (~4.7s, mostly load). This keeps the model resident so a turn
pays only real inference, and — crucially — exposes `gen_window()`, the single-window
primitive that v2 real-time streaming will call every ~500ms.

It reuses gen_fulltake_tag3.py's *exact* classes/constants/decode by importing that module
(the top-level imports run; main() does not). The generation here is the audio_manifest
path of gen_fulltake (arbitrary wav + emotion + proto seed): the same window loop
(fixed 4.27s window, autoregressive seed carry `seed = g[:, -PRE:, :]`, latent overlap-add
stitch) — just callable and warm instead of a one-shot subprocess.

    eng = BodyEngine(ckpt=..., vqvae_dir=..., meanstd_dir=..., npz=..., proto_seed=...)
    quats, T = eng.generate_take(wav_path, "excitement")       # (T,44,4) basis quats
    eng.write_vrma(quats, "out.vrma")
    N)stride_tricksz..zmotion-blenderexperimentsz
wave-handslib)VrmLSM)	OmegaConf)CHILDRENREST_OFFSETS
write_vrmawrite_vrma_from_template_read_glb_json)FINGER_BONES_qinv_qmul)   i>  )hipsspinechest
upperChestneckheadleftShoulderleftUpperArmleftLowerArmleftHandrightShoulderrightUpperArmrightLowerArm	rightHandleftUpperLegleftLowerLegleftFootrightUpperLegrightLowerLeg	rightFootc                 c        | ]\}}|t v r|V  qd S N_VRMA_REST_OFFSETS.0ibone r,   U/home/gglabs/Desktop/service/kemix-engine/package/avatar-E2E-inference/body_engine.py	<genexpr>;       r.   c                 c   s    | ]}t | V  qd S r%   )_BONES44r)   r*   r,   r,   r-   r.   >   s    c                    sj   |durt |nt}dd t D tjg dtjdi   fddtfdd	|D S )
zIBuild the source VRMA rig's cumulative rest rotations in ``bones`` order.Nc                 S   s    i | ]\}}|D ]}||qqS r,   r,   )r)   r+   childrenchildr,   r,   r-   
<dictcomp>D   s    z2_source_world_rest_quaternions.<locals>.<dictcomp>        r6   r6         ?dtypec                    s   |  v r |  S t j| t jd}|ttt j|d }| }|r/t||n|}|ttt j|d }| | < |S )Nr8   -q=)	npasarraygetfloat64maxfloatlinalgnorm	_v2n_qmul)r+   localparent_bonevaluecacheidentity
local_restparentworldr,   r-   rL   L   s   
z-_source_world_rest_quaternions.<locals>.worldc                    s   g | ]} |qS r,   r,   )r)   r+   )rL   r,   r-   
<listcomp>W       z2_source_world_rest_quaternions.<locals>.<listcomp>)listr0   _VRMA_CHILDRENitemsr;   arrayr>   stack)rJ   bonesr,   rG   r-   _source_world_rest_quaternionsA   s   rU   c                 C   s   t j| t jd}t j|t jd}|jdd |jd dfks%|jd dkr1td|j d|j tt||t|}|t t jj	|ddd	d
 }|
t jS )a  Convert local source-rig basis rotations to VRM normalized-humanoid rotations.

    GestureVRM training stores ``basis = inverse(localRest) * channel``.  The
    VRMAnimation loader converts the reconstructed source channel into normalized
    humanoid space as ``worldRest * basis * inverse(worldRest)``.  Streaming skips
    that loader, so it must perform this conjugation explicitly before the browser
    writes quaternions to normalized bone nodes.
    r8   N   z+quaternion/world-rest shapes do not match: z vs Taxiskeepdimsr:   )r;   r<   r>   shape
ValueErrorrC   	_v2n_qinvmaximumrA   rB   astypefloat32)quatsworld_rest_quatsqrL   outr,   r,   r-   basis_to_humanoid_spaceZ   s   	*rf   c                 C   s<  t j| t jdd} | jdkrt jdt jdS d}| j|k r;t | d|| j f}t j| jt t 	|t jd}n4| j| d |f}| j
d | j
d f}tj| ||d}t jt 	|dd}t j|d|d fd	|d d
}tjj| |dd}t|}	|	|	| jk  }	t j| jt jd}
d|
|	< t j||
gddt jS )a_  In-memory equivalent of GestureVRM's ``load_onset_amplitude``.

    Streaming audio never exists as a complete wav file, so the worker keeps a
    rolling PCM window and computes the same two conditioning channels directly.
    The returned channel order intentionally matches the training loader:
    ``[amplitude, onset]`` at audio sample rate.
    r8   rX   r   r      i      )r\   stridesrZ   constant)modeconstant_valuesframes)ysrunitsr7   )r;   r<   ra   reshapesizezerospadfullr?   absrj   r   
as_stridedlibrosaonsetonset_detectframes_to_samplesrS   r`   )
audio_dataaudio_srframe_lengthpadded	amplituder\   rj   rollingonset_framesonset_samplesr{   r,   r,   r-   audio_to_onset_amplituden   s(   

"
r   c                   @   s   e Zd ZeeZeZeZ						d dd	Z
d
d Zd!ddZdd Zd!ddZdd Ze d"ddZe d#ddZe d$ddZd#ddZd%ddZdS )&
BodyEngineNT   testcuda:0tag3Fc           !   
      s  t |	_|_|
_|_|_|
dvrtd|
 d_|rUtt	
tj|djd }|d r=td| d|d _jttkrUtd| dtt ttd d	 ttd j  _tj_td
d tjD _tfddjD _d _|rt|\}}|d d d d }|d fdd| D _|dpi d}|d urt	j| dg dt	j d_nt	j
tjt!dddd" _t	#fddjD _$t%jj_&t'
tjt(j)d dg _*|rd j+_,|rt-| D ]\}}| j+j.j/|< qi }|rAt0|}t1
|}W d    n	1 s8w   Y  |d  }d!D ]:}||tj|d"| d# d$| d%< tj|d&| d' d(| d%< tj|d&| d) d*| d%< qC|r|d+tj|d, d-< tj|d d.< tj|d/ d0< |dkr| j+_2tjt!dd1tj3 }| j+j4j/_5| j6_7d j6_8| j6_9tjt!dd2 j6_:d3 j6_; _<|
d4krt(j=nt>}| ?j_+t j
|jd5}d6d |d7| D }j+j@||
d8kd9 j+A  j+jB_Ct jD_EjCjE _FjFd: _GjCd: _HtjHtI tJ _Kg d;|rDd+jd fgng  _Li _MjLD ]'\}}tNO|} | @t j
 d$| d% d<d5d=  | A ?jjM|< qN fd>djLD _P jQ_Rt0tj|d?d@dA}tS
|jT_UW d    n	1 sw   Y  d _V|rt	
|fdBdjWD _VdCtjL _Xd S )DN)r   basezunknown body model kind: r   zvrm_fingers_mean.npy   zfingers mean dim z is not a multiple of 6z exceeds known bones    c                 s   r$   r%   r&   r(   r,   r,   r-   r.      r/   z&BodyEngine.__init__.<locals>.<genexpr>c                 3   s    | ]} j | V  qd S r%   )
bone_namesr1   selfr,   r-   r.      s    
extensionsVRMC_vrm_animationhumanoid
humanBonesnodesc              	      sB   i | ]\}}| d dur|dd  |d    dg dD qS )nodeNc                 S   s   g | ]}t |qS r,   )r@   )r)   vr,   r,   r-   rM      rN   z2BodyEngine.__init__.<locals>.<dictcomp>.<listcomp>rotation)r   r   r   ri   )r=   )r)   be)r   r,   r-   r4      s    z'BodyEngine.__init__.<locals>.<dictcomp>r   r   translation)r6   r6   r6   r8   outputszkemix_rest_table.npyT)allow_picklec                    s8   g | ]}| j v rt j | tjntg d qS )r5   )_RESTr;   r<   r>   rR   )r)   r   r   r,   r-   rM      s
    z'BodyEngine.__init__.<locals>.<listcomp>zconfigs/shortcut_vrm.yamlcheckpointsr   armslegsbest_z.pthvqvae__pathvrm_z	_mean.npymean_z_std.npystd_fingerszbest_fingers.pthvqvae_fingers_pathmean_fingers_pathzvrm_fingers_std.npystd_fingers_path
dummy_langkemix_wave16krh   r   )map_locationc                 S   s   i | ]\}}| d d|qS )zmodule. )replace)r)   kr   r,   r,   r-   r4      s    model_state_dictr   )strictrW   ))r   $   )r   0   )r   '   cpunetc                    sZ   i | ])\}}|t t d | d jt t d| d jfqS )r   r   r   )torch
from_numpyr;   loadtodevice)r)   p_)cfgr   r,   r-   r4     s
    $"weightsz	vocab.pklrbc                    s.   i | ]}|t  |  d jqS )r   )r   r   r@   	unsqueezer   r   )r)   r   )npzfr   r,   r-   r4     s    &   )Yr   r   r   
model_kindcontinuous_decodetemplate_vrmar]   	n_fingersintr;   r   ospathjoinr\   RuntimeErrorlen_FBtupler0   rO   r   n_bones	enumeratestream_bone_indicesstream_bone_names_hips_rest_trans	_read_glbrQ   r   r=   r<   ra   WHitemrS   	_REST_ARRrU   _WORLD_REST_ARRr   GGVRMgpusmodeluse_expdictdenoiserparamsopenjsonn_stepssepmodality_encoder	data_pathdatabeat_data_path	ours_modeour_npz_pathour_wave_pathour_speaker_idr   
Tag3VrmLSM
BaseVrmLSMr   load_state_dictevalseq_lenSEQ
pre_framesPREHOP_LATHOP_POSEWIN_POSEAUDIO_SRFPSWIN_SAMP_partsvqivcreate_vqvaemsdvqvae_latent_scalescalepickle	PAD_tokenPADprotofilesC)!r   ckpt	vqvae_dirmeanstd_dirnpz
proto_seedr   stepssplitr   r   codec_selectionr   r   denoiser_paramsfdimgltfr   hbr   r   r   codec_pathsf	selectionr   _lang	model_clscksdpartdimmr,   )r   r   r   r   r-   __init__   s   
$




"&



$
zBodyEngine.__init__c                 C   s@   | j r| j| || j dddS | j| || j d S )Nr   rh   ri   )r   r  decoderr  permutelatent2origin)r   r   latr,   r,   r-   _dec$  s    zBodyEngine._decc           	         s  |dddf |dddf |dddf d j r,|jd dkr,|dddf  d	<  fd
d D }|d d    }|d jd }d	|v rPjnd}t||dftj}|d d    	|dd|ddt
jf< |d d    	|dd|ddt
jf< |ddddf 	|dd|ddt
jf< d	|v r|d	 d    	|jd|ddd|f< t|}|r||ddddf tjfS |S )uy  -> (T,B,4) basis quats. with_trans=True also returns (T,3) 이동 속도.

        legs 코덱의 마지막 3차원은 hips 이동 속도인데, 종전엔 `legs[:, :-3]`로
        **버리고 있었다** — 배우가 실제로 걷는데(구간 median 84cm, 최대 4m)
        루트가 고정돼 발이 러닝머신처럼 쓸린다. 호출자가 적분해서 쓴다.
        .Nr      i  r   rX   i   r   c                    s:   i | ]}| | | j| d   j| d  qS )ri   r   )r(  r  )r)   r   partsr   r,   r-   r4   3  s   : z-BodyEngine._decode_latent.<locals>.<dictcomp>r   r   r   ri   r   r   r   r   )r   r\   detachr   numpyr   r;   ru   ra   rs   r   SPINEARMSLEGSr   r  rotation_6d_to_quaternionr`   )	r   g
with_transrecr   Tnbr6rb   r,   r*  r-   _decode_latent)  s"   000,
(zBodyEngine._decode_latentc                 C   s    t | jd  |tjtjS r%   )rC   r   r`   r;   r>   ra   )r   rd   r,   r,   r-   _basis_to_channelA  s    zBodyEngine._basis_to_channelc                 C   s(   t || j}|r|d| jddf }|S )a  Return loader-equivalent rotations for normalized VRM humanoid nodes.

        ``mapped_only=True`` also removes channels absent from the source VRMA
        humanoid map.  Today that is only ``upperChest``; batch VRMA playback
        ignores the same channel.
        .N)rf   r   r   )r   rd   mapped_onlyre   r,   r,   r-   basis_to_humanoidD  s   zBodyEngine.basis_to_humanoidc                 C   s8   | j dur|| j v r| j | S tjd| j| jf| jdS )z@First-window seed: prototype opener for the emotion, else zeros.Nri   r   )r
  r   ru   r   r  r   r   emotionr,   r,   r-   seed_forP  s   
zBodyEngine.seed_forgeneralc                 C   s  t j|t jd}|jdkrt j| jdft jd}t|| jk r0t j|d| jt| fdfdd}t	|
 d| j}tjd| jf| jtj| jd}tjd| jfdtj| jd}||||d	d
}	|d	urmt
||	d< | jdkr|	tjtj|dg| jdtjtj|dg| jdd |d	urt
||	d< d	| j_t	t | jdt jd	 | j| j_d|	i}
| |
d }|ddddS )u  onset_win: (WIN_SAMP, 2) np; seed: (1,PRE,C). Returns latent g (1,SEQ,C).

        ``guidance=None`` → 모델 기본값(base는 yaml ``guidance_scale``, tag3은
        모듈 ``GUIDANCE``). 종전 기본값은 3.0이었지만 **base 경로에서는 이 인자가
        읽히지도 않아** 실제로는 늘 2.0(yaml)이 쓰였다 (2026-08-06 진단). 기본값을
        3.0으로 둔 채 배관만 고치면 기존 발행물이 전부 달라지므로 None으로 바꾼다.
        r8   r   rh   r   r   edgerm   ri   )r9   r   N)audio_onsetwordidseedstyle_featureguidancer   r=  )	style_tagemo_tagtag_guidancerp   latents) r;   r<   ra   rt   ru   r   r   rv   r   r   r@   r   r   r   rw   r   r	  longr   updatetensorr   STYLE_IDr=   EMO_IDr   _cur_energyr   int64	_cur_restsqueezer%  )r   	onset_winrH  r?  stylerJ  a_trF  sid	body_condcondx_tr,   r,   r-   
gen_windowW  sJ   	

zBodyEngine.gen_windowc                 C   sb   |durt |S | jdkr&tjt| jjj}t	|dd}|dur&t |S t t	| jdt dS )u  실제로 적용되는 guidance — 발행 메타에 거짓값이 남지 않게 한다.

        tag3 기본값은 ``Tag2VrmLSM.forward``가 읽는 **그 모듈의** 전역
        ``GUIDANCE``다(train_ours_tag2, 4.0). ``G``(gen_fulltake_tag3)에는 없어서
        초판이 조용히 nan을 돌려줬다 — 그 nan이 샘플러까지 흘러가 latent 전체를
        nan으로 만들었다. 정의된 자리에서 직접 찾는다.
        Nr   GUIDANCEguidance_scalenan)
r@   r   sysmodulesr=   typer   forward
__module__getattr)r   rJ  modr3  r,   r,   r-   effective_guidance}  s   	
zBodyEngine.effective_guidancec              	   C   s  t |t}|du rtt|t t }tdtt|| j	 | j
 d }| |}g }	t|D ]3}
t|
| j
 t t }| |||| j  ||||}|	| |dd| j dddf  }q1|	d jd }| j|d  | j }tjd||f| jd}tjd|df| jd}tjd| jd | jd | jd  }t|	D ]`\}
}tj| j| jd}|
dkr||d| j< |
|d k r|d|| j d< |
| j }|dd||| j f  ||ddddf  7  < |dd||| j f  |ddddf 7  < q||d }| j|dd\}}|d| |d| }}t|}tj |dd dd	|dd< | j!dur@|| j! }||jd |fS )
u   Batch (turn-based) full-take generation — warm.

        Returns (quats (T,B,4) basis, T, root (T,3)). `root`은 코덱이 만든 이동
        속도를 원점에서 적분한 hips 위치 — 종전엔 이 값을 버려 발이 쓸렸다.
        Nri   r   rX   r=  ư>T)r4  rk   )"r   load_onset_amplituder   r   r   r   r?   r;   ceilr   r   r@  ranger_  r   appendr   r-  r\   r   r   r   ru   r   aranger@   r   onesflip	clamp_minr9  
zeros_likecumsumr   )r   wav_pathr?  T_fullrY  rJ  r{   n_winrH  winswastartr3  r  Ltotaccwsumrampwtsg_fullrd   trans_vrootr,   r,   r-   generate_take  s>   $
 
$&"
42

zBodyEngine.generate_takec              	   C   sJ   | j rt| j |t| jt||ddS t| |t| jt||| jd dS )uO  basis 회전을 VRMA로 발행.

        template_vrma가 있으면 **그 데이터셋의 베이크 템플릿 구조 위에** 쓴다
        (노드 계층·humanoid 맵·노드 rest가 베이크와 동일 — 베이크 산출물 채널을
        1.2e-07로 재현 검증). 없으면 종전 제네릭 리그 경로 그대로.
        T)root_translationquats_are_basis)r  
rest_quatsN)r   _write_vrma_tplrO   r   r   _write_vrma44r:  r   )r   rb   out_pathr  r,   r,   r-   r	     s   
zBodyEngine.write_vrma      @c                 C   s   t | ||||S r%   )
BodyStream)r   r?  rY  rJ  emit_framesr,   r,   r-   
new_stream  s   zBodyEngine.new_stream)
NTr   r   r   r   NFNNF)rA  Nr%   )NrA  N)rA  r  N)__name__rg  __qualname__r   r0   r   _STREAM_BONE_INDICESr   _STREAM_BONESr   r#  r(  r9  r:  r<  r@  r   no_gradr_  rj  r  r	   r  r,   r,   r,   r-   r      s0    
 

%
'r   c                   @   sz   e Zd ZdZddddefddZdd	 Zd
d ZdddZdd Z	dd Z
dddZdddZdd Zdd Zdd ZdS )r  u  Stateful real-time streaming with one generated-window of lookahead.

    v2 primitive. Audio is kept in a 4.27s rolling window whose RIGHT edge is "now";
    missing history is padded on the LEFT. Each regular step advances by
    ``emit_frames / 4`` latent frames.

    A rollout's newest poses are not emitted immediately.  The following rollout
    predicts the same global interval in its interior, so the two predictions are
    quaternion-SLERPed over that interval.  Holding one rollout makes the end of
    one output packet and the beginning of the next packet adjacent frames from
    the same rollout, instead of unrelated tail predictions from two rollouts.

    The seed must be aligned with the *new window's beginning*. For a
    ``shift_lat`` rolling shift that is
    ``previous_g[:, shift_lat:shift_lat+PRE]`` — not the previous tail (the tail
    is only correct for the original 28-latent batch hop). This distinction is what
    keeps the generated motion and the rolling audio on the same timeline.
    rA  r  Nr   c           	      C   s   || _ |||| _| _| _t|p|jd | _| jdks"| jd r&td| jd | _| j|j |j	kr9td|}|| _
tdtj| _tdtj| _tdtj| _d| _d | _d | _d | _d| _d| _d| _d S )NrW   r   z?emit_frames must be a positive multiple of the codec stride (4)z;emit_frames is too large to carry an aligned PRE-frame seedrg   )r   r?  rY  rJ  r   r   emitr]   	shift_latr   
feature_fnr;   ru   ra   _pcm_pending_pcm_window_feature_window_step_index_previous_g_pending_segment_last_samples_received_frames_scheduledframes_emitted)	r   enginer?  rY  rJ  r  blendr  r   r,   r,   r-   r#    s(   
zBodyStream.__init__c                 C   s
   || _ d S r%   )r?  r>  r,   r,   r-   set_emotion  s   
zBodyStream.set_emotionc                 C   sl   t j|t jddd}t | j|g| jj d | _| | j}| 	|| j
| j}|  jt|7  _|S )zHLow-level feature input used by tests; production uses :meth:`push_pcm`.r8   rX   rh   N)r;   r<   ra   rs   concatenater  r   r   _left_pad_features	_generater  r  r  r   )r   onset_chunkchunkwindowre   r,   r,   r-   tick  s    zBodyStream.tickFc                 C   s  t j|t jdd}|jrt | j|g| _|  j|j7  _g }	 | | j	}| jj|k r/n#| jd| }| j|d | _|
| || j| j |  j	d7  _	q"|r| jjr| j}t dt j| _tt| jt t }t| jtd|| j }|dkr| | j	}	t j|d|	|j fdd}
|
| j|
|| j| jd	 |r|
|   |st jdt| jjd
ft jdS t j|dd}|  jt|7  _|S )a  Consume mono float32 PCM at 16kHz and return newly generated basis quaternions.

        Arbitrary input chunk sizes are accepted. Regular generation is quantized to
        a codec-aligned pose count. Output is delayed by one rollout so the next
        rollout can smooth the pending interval. ``flush=True`` right-pads a short
        final audio step to preserve codec alignment, then emits only its real prefix.
        r8   rX   TNri   r   rl   rD  advance_framesrW   rk   )r;   r<   ra   rs   rt   r  r  r  _samples_for_stepr  ro  _push_segmentr  r  ru   r   roundr   r   minr?   r  rv   _flush_pendingr   r   r   r  )r   	pcm_chunkflushpcmr   needsegmenttarget_framesn_framesregular_samplesr   re   r,   r,   r-   push_pcm  sD   	zBodyStream.push_pcmc                 C   s:   | j t t }tt|| }tt|d | }|| S )Nri   )r  r   r   r   r  )r   
step_indexsamples_per_steplohir,   r,   r-   r  8  s   zBodyStream._samples_for_stepc                 C   sF   t || jjkr|| jj d  S tj|| jjt | dfdfddS )Nr   rB  rl   rD  )r   r   r   r;   rv   )r   featuresr,   r,   r-   r  >  s
   zBodyStream._left_pad_featuresc                 C   s   t | j|g| jj d  | _| jj| jjk r*t j| j| jj| jj dfdd}n| j}| j| |t	|||d}|  j
|7  _
|S )Nr   rl   rD  r  )r;   r  r  r   r   rt   rv   r  r  r   r  )r   r  
out_framesr  r  audiore   r,   r,   r-   r  D  s    zBodyStream._push_segmentc           
      C   s4  t |p|}|dks||krtd| d| | jd u r%| j| j}n| jd d ||| jj d d f  }| j||| j| j	| j
}| | _tj| j|tjd}|t|krjtdt| d| d| jd u rtjdt| jjdftjd}n| | j||}t|| }	||	|	|   | _|S )	Nr   zretained poses must be in 1..z, got r8   zrollout has z poses, cannot advance by z posesrW   )r   r]   r  r   r@  r?  r   r-  r_  rY  rJ  r;   r<   r9  ra   r   r  ru   r   _stitch_pendingcopy)
r   onset_windowr  r  r  rH  r3  rd   re   newest_startr,   r,   r-   r  R  s6   
(

 
zBodyStream._generatec                 C   s   t |t|}}t || }|| }|dk r%td||  dt | t| }t|||  }|dkrBtjdtjd}	ntjdd|tjd}
|
|
 d	d
|
   ddddf }	| 	t
|||	S )a  Emit the pending interval using its duplicate prediction as lookahead.

        If the pending segment has duration ``P`` and the current rollout advances
        by ``C`` poses, these slices describe the same global time:

        ``previous == current_full[-(P+C):-C]``.
        r   z1rollout is too short for lookahead overlap: need z poses, got ri   )ri   ri   ri   r8   r6   r7   r  g       @N)r   r   r]   _normalize_quaternionsr  r;   rq  ra   linspace_finish_seam_slerp_frames)r   previouscurrent_fullcurrent_advance_framesr   coverlap_endoverlap_startcurrentalphaur,   r,   r-   r  r  s(   "zBodyStream._stitch_pendingc                 C   sB   | j d u rtjdt| jjdftjdS | j }d | _ | | S )Nr   rW   r8   )	r  r;   ru   r   r   r   ra   r  r  )r   r  r,   r,   r-   r    s
   
zBodyStream._flush_pendingc                 C   s   t |s
|tjS t|}| j}tt |D ]+}|dkr|n||d  }|d urBt|||  jddddk ||  || ||< q|d 	 | _|tjS )Nr   ri   rX   Tr[   )
r   r`   r;   ra   r  r  rn  wheresumr  )r   re   prevr*   refr,   r,   r-   r    s   zBodyStream._finish_seamr  r%   )r  rg  r  __doc__r   r#  r  r  r  r  r  r  r  r  r  r  r,   r,   r,   r-   r    s    

	5

 r  c                 C   s.   t j| t jd} | t t jj| dddd S )Nr8   rX   TrY   g:0yE>)r;   r<   ra   r_   rA   rB   )rd   r,   r,   r-   r    s   r  c           
      C   s   t | } t |}t| | jddddk | |}t| | jddddd}t|}t|}tt|dk|d}td| | | |  t|| | |  }d| |  ||  }tt|dk||}	t |	S )z3Framewise, per-bone shortest-path quaternion SLERP.rX   Tr  r   g      r7   rk  )r  r;   r  r  cliparccossinrx   )
qaqbr  dottheta	sin_thetasafe_sin	sphericallinearre   r,   r,   r-   r    s   "

r  r%   )>r  r   r   r  rc  rz   r.  r;   r   	numpy.libr   r   dirnameabspath__file__HEREr   r   insertgen_fulltake_tag3r   scripts.infer_vrma
infer_vrmar  models.vrm_lsmr   r   	omegaconfr   vrma_writerr   rP   r   r'   r	   r  r
   r  r   r   vrma_to_npzr   r   r   r^   r   rC   r   r   rO   r0   r   r   r  r  rU   rf   r   r   r  r  r  r,   r,   r,   r-   <module>   sJ    
   > a