o
    i'                     @   sH  d dl Z d dlZd dlZd dlmZ d dlmZ d dlZ	d dl
mZmZ dejd g dg ddg dg d	dg dg d
dg dg ddg dg ddg dg ddg dg ddg dg ddg dg dddZd'ddZG dd deZdd Zd(ddZG d d! d!ejZG d"d# d#eZd$efd%d&ZdS ))    N)	rearrange)override)ComfyExtensioniog      ?   )        r   r   )angleT)r         r   )r         ?r   )r
   r   r   )r   r   r   )r   r   g       @)r   r   g       )r   r   r
   )r   r   r   )base_T_norm
base_angleStaticPan UpPan DownPan Left	Pan RightZoom InZoom OutAnti Clockwise (ACW)ClockWise (CW)         cpuc                    s   dd }	 dd | D }   }|| }||kr* | }	| D ]
}
|	|
j   |
_ qn| }| D ]
}
||
j   |
_q0tj fdd| D tjd}t|d  }|| }t|d  }t|| |dd dd	d
d	 }|d  }t
|dd }|S )Nc                    s   dd | D }dd | D }d}t g dddd| gg dg dg}||d   |g fd	d|dd
 D  }t j|t jd}|S )zQCopied from https://github.com/hehao13/CameraCtrl/blob/main/inference.py
        c                 S      g | ]}|j qS  )w2c_mat.0	cam_paramr   r   B/mnt/c/Users/fbmor/ComfyUI/comfy_extras/nodes_camera_trajectory.py
<listcomp>       zBprocess_pose_params.<locals>.get_relative_pose.<locals>.<listcomp>c                 S   r   r   )c2w_matr   r   r   r"   r#       r$   r   )   r   r   r   r&   )r   r   r&   r   r   r   r   r&   c                    s   g | ]} | qS r   r   )r    abs_c2wabs2relr   r"   r#   )       Ndtype)nparrayfloat32)
cam_paramsabs_w2csabs_c2wscam_to_origintarget_cam_c2w	ret_posesr   r)   r"   get_relative_pose   s    z.process_pose_params.<locals>.get_relative_posec                 S      g | ]}t |qS r   )Camerar   r   r   r"   r#   /   r+   z'process_pose_params.<locals>.<listcomp>c                    s0   g | ]}|j  |j  |j |j  gqS r   )fxfycxcyr   heightwidthr   r"   r#   =   s    r,   devicer   r   r&      zb f c h w -> b f h w c)r:   r;   r.   asarrayr0   torch	as_tensorray_conditionpermute
contiguousr   )r1   r@   r?   original_pose_widthoriginal_pose_heightrB   r7   sample_wh_ratiopose_wh_ratioresized_ori_wr!   resized_ori_h	intrinsicKc2wsplucker_embeddingr   r>   r"   process_pose_params   s2   &rT   c                   @   s   e Zd ZdZdd ZdS )r9   MCopied from https://github.com/hehao13/CameraCtrl/blob/main/inference.py
    c                 C   s^   |dd \}}}}|| _ || _|| _|| _t|dd  dd}|| _tj	|| _
d S )Nr&            )r:   r;   r<   r=   r.   r/   reshaper%   linalginvr   )selfentryr:   r;   r<   r=   r%   r   r   r"   __init__N   s   zCamera.__init__N)__name__
__module____qualname____doc__r^   r   r   r   r"   r9   K   s    r9   c              	   C   s  | j d }tjtjd|d |||jdtjd|d |||jddd\}}|dd|| g|d|| gd }|dd|| g|d|| gd }| jddd	\}}	}
}t|}||
 | | }|| |	 | }|	|}tj
|||fdd	}||jdd
d }||dddddf dd }|ddddf }|dddddf 	|}t||}tj||gdd	}|||j d ||d}|S )rU   r   r&   )rB   r-   ij)indexing      ?rX   dimT)rh   keepdim.Nr      )shaperE   meshgridlinspacer-   rY   expandchunk	ones_like	expand_asstacknorm	transposecrosscat)rQ   c2wHWrB   Bjir:   r;   r<   r=   zsxsys
directionsrays_drays_orays_dxopluckerr   r   r"   rG   X   s,   

((

"rG   Q   c                 C   s   dd }g }t |D ]0}|| | td  |  }||}|| | td  |dd }	tj||	gdd}
||
 q
t|}|S )Nc              	   S   s   | \}}}t g ddt |t | gdt |t |gg}t t |dt |gg dt | dt |gg}t t |t | dgt |t |dgg dg}t |t ||}|S )N)r&   r   r   r   )r   r&   r   )r   r   r&   )r.   r/   cossindot)anglestheta_xtheta_ytheta_zRxRyRzRr   r   r"   compute_R_form_rad_angle}   s   

z3get_camera_motion.<locals>.compute_R_form_rad_angler   r   r   r&   )axis)rangeCAMERA_DICTrY   r.   concatenateappendrs   )r   r	   speednr   RTr}   _angler   _T_RTr   r   r"   get_camera_motion|   s    
r   c                   @   s.   e Zd Zedd Zed	dejfddZdS )
WanCameraEmbeddingc                 C   s  t jddt jjdg dddt jjddd	tjd	d
t jjddd	tjd	d
t jjdddtjdd
t jjdddddddt jjddddddddt jjddddddddt jjddddddddt jjddddddddg	t jj	d d!t jj	dd!t jj	dd!t jj	dd!gd"S )#Nr   cameracamera_pose)	r   r   r   r   r   r   r   r   r   r   )optionsdefaultr@   i@     )r   minmaxstepr?   i  lengthr   r&   rX   r   r   r   g      $@g?T)r   r   r   r   optionalr:   re   g&.>)r   r   r   r   r   advancedr;   r<   g{Gz?r=   camera_embedding)display_name)node_idcategoryinputsoutputs)
r   SchemaComboInputIntnodesMAX_RESOLUTIONFloatr   Output)clsr   r   r"   define_schema   s.   z WanCameraEmbedding.define_schemar   re   returnc
                 C   s  |g}
|}t t|
d  d }t t|
d  d }t||||}g }| D ]+}||||	ddg}||d  ||d  ||d  |g d || q(t dd |D }t t |d	d	d	df |gd}t	|||d
}|
g ddjtj d}tjtj|d	d	d	d	ddf ddd|d	d	d	d	dd	f gdddd}|j\}}}}}| ||d d|||dd}| ||d |d ||dd}t||||S )u   
        Use Camera trajectory as extrinsic parameters to calculate Plücker embeddings (Sitzmannet al., 2021)
        Adapted from https://github.com/aigc-apps/VideoX-Fun/blob/main/comfyui/comfyui_nodes.py
        r   r   r	   r&   rC   r'   c                 S   s   g | ]	}d d |D qS )c                 S   r8   r   )float)r    xr   r   r"   r#      r+   z9WanCameraEmbedding.execute.<locals>.<listcomp>.<listcomp>r   )r    poser   r   r"   r#      s    z.WanCameraEmbedding.execute.<locals>.<listcomp>N)r@   r?   )r   r   r&   rC   rA   rX   )repeatsrh   rg   r   )r.   r/   r   r   tolistextendr   r   
zeros_likerT   rH   	unsqueezetocomfymodel_managementintermediate_devicerE   concatrepeat_interleaveru   rl   rI   viewr   
NodeOutput)r   r   r@   r?   r   r   r:   r;   r<   r=   motion_listr   r	   r   trajscptrajr1   control_camera_videobfchwr   r   r"   execute   s:   &"$$&zWanCameraEmbedding.executeN)r   re   re   re   re   )r_   r`   ra   classmethodr   r   r   r   r   r   r   r"   r      s
    
%r   c                   @   s(   e Zd Zedeeej  fddZdS )CameraTrajectoryExtensionr   c                    s   t gS N)r   )r\   r   r   r"   get_node_list   s   z'CameraTrajectoryExtension.get_node_listN)	r_   r`   ra   r   listtyper   	ComfyNoder   r   r   r   r"   r      s    r   r   c                      s   t  S r   )r   r   r   r   r"   comfy_entrypoint   s   r   )r   r   r   r   r   )r   )r   rE   numpyr.   einopsr   typing_extensionsr   comfy.model_managementr   comfy_api.latestr   r   pir   rT   objectr9   rG   r   r   r   r   r   r   r   r   r"   <module>   s4    
1
$P