o
    igj                  	   @   s  d Z ddlZddlZd\ZZd\ZZd\ZZ	d Z
 ZZe
e e ZdZdZd	ZdZg d
Zg dZdZdejfddZdejdejfddZdejdejfddZdejdejdejfddZefdejdedejfddZd%dejd ed!edejfd"d#Zd$ZdS )&u   Shared bounds + voxel-center + projection helpers for the volume AR model.

Bounds locked-in 2026-05-17 (symmetric around robot base, verified visually):
  X[-0.40, 0.40]  Y[-0.40, 0.40]  Z[0.90, 1.25]
  32 × 32 × 32 voxels  →  cells ~25mm xy, ~11mm z
    N)g?g?)g      пffffff?)gr                )gٿgɿg333333ÿ)皙?r   g333333?i  returnc                  C   s   t ttt dt   ttt dt   t} t ttt dt   ttt dt   t}t tt	t dt
   t	t	t dt
   t
}t j| ||dd\}}}t j|||gdddd}|S )zT(V, 3) tensor of voxel-center world coords, flat-indexed as iz * Ny*Nx + iy*Nx + ix.   ij)indexingdim   )torchlinspaceX_MINX_MAXN_XY_MINY_MAXN_YZ_MINZ_MAXN_Zmeshgridstackreshape)xsyszsgxgygzZcenters r$   )/data/cameron/para/libero/robot_volume.pyvoxel_centers_world!   s   r&   	pts_worldc                 C   s   t t t }tt t }tt t }| d t | 	 
dtd }| d t | 	 
dtd }| d t | 	 
dtd }|tt  |t  | S )uO   (..., 3) world coords → (...,) flat voxel index. Clamps to inside the volume.).r   r   r   ).r   ).r	   )r   r   r   r   r   r   r   r   r   longclamp)r'   cell_xcell_ycell_zixiyizr$   r$   r%   world_to_voxel_idx0   s      r0   idxc           
      C   s   | t  }| t  t }| tt   }tt t }tt t }tt t  }t|	 d |  }t|	 d |  }t|	 d |  }	t
j|||	gddS )uC   Inverse of world_to_voxel_idx — returns voxel center world coord.g      ?r   r   )r   r   r   r   r   r   r   r   r   floatr   r   )
r1   r/   r.   r-   r*   r+   r,   xyzr$   r$   r%   voxel_idx_to_world<   s   r6   world_to_camerac                 C   sv   | j dd \}}tj||d| j| jd}tj| |gdd}td||}|ddd	f jd
d}|dddf | S )u  Vectorized world→pixel using a robosuite-style 4×4 world_to_camera matrix.

    pts_world: (B, M, 3)
    world_to_camera: (B, 4, 4) — the matrix returned by get_camera_transform_matrix
                                  (already encodes the agentview flip; pixel coords on
                                  flipud(obs) image, see existing data.py comment.)
    returns: (B, M, 2) (u, v) pixel coords.
    Nr	   r   devicedtyper   r   zbij,bmj->bmi.r   gMbP?)min)shaper   onesr9   r:   cateinsumr)   )r'   r7   BMr=   Zpts_hprojr5   r$   r$   r%   world_to_pixel_torchJ   s   	rC   pix_uv
image_sizec                 C   s   | |d  d d S )uI   (B, M, 2) pixel uv → (B, M, 2) normalized in [-1, 1] for F.grid_sample.r          @      ?r$   )rD   rE   r$   r$   r%   pixel_to_normalized_grid^   s   rH      rG   xyzn_bandsscalec                 C   s\   | j }| j}dtj|||d }| d| tj|  }tj| |	 gdd}|
dS )uS   (B, M, 3) → (B, M, 3 * 2 * n_bands). Frequencies geometric in [1, 2^(n_bands-1)].rF   r8   r   r   )r9   r:   r   arange	unsqueezenppir>   sincosflatten)rJ   rK   rL   r9   r:   Zfreqsanglesoutr$   r$   r%   sincos_pe_3dc   s   
rW   $   )rI   rG   )__doc__numpyrP   r   r   r   r   r   r   r   r   r   r   ZN_VOX
N_PAST_EEFT_FUTUREZN_GRIPPER_CLASSZ
N_ROT_BINSZMIN_ROTZMAX_ROT
IMAGE_SIZETensorr&   r0   r6   rC   intrH   r2   rW   ZPE_DIMr$   r$   r$   r%   <module>   s,     