o ॵi @s,ddlZddZddZddZdd ZdS) NcCsF|jddks Jdt|}|dddf |dddf<||S)Nz"q must be a tensor of shape (*, 4).)shapetorch ones_like)qmaskr c/home/ubuntu/.local/lib/python3.10/site-packages/modelscope/utils/cv/motion_utils/motion_process.pyqinvs r cCs|jddks J|jddksJ|jdd|jddks"Jt|j}|dd}|dd}|ddddf}tj||dd}tj||dd}|d|ddddf|||S)z Rotate vector(s) v about the rotation described by quaternion(s) q. Expects a tensor of shape (*, 4) for q and a tensor of shape (*, 3) for v, where * denotes any number of dimensions. Returns a tensor of shape (*, 3). rrNrdim)rlist contiguousviewrcross)rvoriginal_shapeqvecuvuuvr r r qrots  *rcCs|d}t||j} |dddf|dddf<tj|dd}t|jddd|j}t||d<t||d<t|jddd|j}|ddddd f|dddd d gf< t t ||}tj|d d}|d |d<||fS)N.r.rrrr.r)r r rr).r ).r) r zeros_liketodevicecumsumzerosrcossinrr )datarot_vel r_rot_ang r_rot_quatr_posr r r recover_root_rot_pos$s  ( r+cCst|\}}|dd|dddf}||jddd} tt|ddddf|jddd|} |d|dd df7<|d |dd df7< tj|d |gd d }|S)N.rrr r)rr rrrrrrr) r+rrrr expandrcat unsqueeze)r& joints_numr)r* positionsr r r recover_from_ric:s (r1)rr rr+r1r r r r s