a
    ij3                     @   s   d Z ddlmZmZmZmZmZ ddlZddlZ	e	j
e	j
eej dddZG dd dZeeef d	d
dZdeeeejf e	j
edddZdeeef eej edddZdS )z!Built-in shapes for USD exporter.    )AnyDictOptionalTupleUnionN)vertices	trianglestexture_typec                    s@  |du rdS g }|t jjkr8 fddt|D }n|t jjkrtj dd}t|D ]} | | \}}}t|t|t|  }	}
}d}d}|dkr|	|
kr|	|kr| |	 }||	 }n|dkr|	|
kr|	|kr||	 }||	 }n|dkr"|
|	kr"|
|kr"||
 }| |
 }n|dkrR|
|	krR|
|krR||
 }||
 }n`|dkr||	kr||
kr|| }|| }n0|dkr||	kr||
kr| | }|| }|d d }|d d }|d }d|kr|d	krd|kr|d	ksJ |||g q^n&|t jj	kr6 fd
dt|D }t
|S )z(Returns UV coordinates for a given mesh.Nc                    s$   g | ]} | d   | d gqS r       .0ir   r   Q/home/ghrups/robot-bench/.venv/lib64/python3.9/site-packages/mujoco/usd/shapes.py
<listcomp>"   s   z$get_triangle_uvs.<locals>.<listcomp>r   Zaxis      ?g       @   r   c                    s$   g | ]} | d   | d gqS r
   r   r   r   r   r   r   K   s   )mujoco
mjtTextureZmjTEXTURE_2DnpZnditerZmjTEXTURE_CUBEmeanabsappendZmjTEXTURE_SKYBOXarray)r   r   r	   triangle_uvscenterZ	vertex_idxyzZabs_xZabs_yZabs_zuvr   r   r   get_triangle_uvs   sT    







,
r$   c                   @   s   e Zd ZdZejejejdddZeeeee	e
j d dddZeee	e
j ed dd	d
Zeee	e
j ed dddZeeee	e
j ed dddZejddddZejeedf ddddZejddddZdd Zdd ZdS )TriangleMeshzStore UV and geometry information for a primitive mesh.

  Attributes:
    vertices: A numpy array of vertices.
    triangles: A numpy array of triangles.
    triangle_uvs: A numpy array of UV coordinates.
  r   r   r   c                 C   s   || _ || _|| _dS )zCreates a TriangleMesh object.

    Args:
      vertices: A numpy array of vertices.
      triangles: A numpy array of triangles.
      triangle_uvs: A numpy array of UV coordinates.
    Nr&   )selfr   r   r   r   r   r   __init__[   s    zTriangleMesh.__init__)widthheightdepthr	   returnc                 C   s   t g d|ddgdd|g|d|gd|dg||dgd||g|||gg}t g dg dg dg dg dg dg d	g d
g dg dg dg dg}t|||}t|||S )zCreates a box.)        r-   r-   r-   )         )r.   r   r/   )r      r.   )r1   r   r.   )r   r   r1   )r      r1   )r   r0   r/   )r   r/   r2   )r1   r2   r/   )r1   r/   r   )r   r.   r   )r   r.   r0   )r   r   r$   r%   )clsr)   r*   r+   r	   r   r   r   r   r   r   
create_boxj   s0    	
	
zTriangleMesh.create_box)radiusr	   
resolutionr,   c                 C   s6  g }g }t d| d D ]}tj| d|  }t |d D ]d}dtj | | }	|t| t|	 }
|t| t|	 }|t| }||
||g q:qt d| D ]Z}t |D ]L}||d  | }|| d }||||d g |||d |d g qqt|}t|}t|||}t|||S )zCreates a sphere.r1   r   )	ranger   pisincosr   r   r$   r%   r3   r5   r	   r6   r   r   r   phijthetar   r    r!   firstsecondr   r   r   r   create_sphere   s(    

zTriangleMesh.create_spherec                 C   sx  g }g }t |d D ]}tjd | | }t |d D ]d}dtj | | }	|t| t|	 }
|t| t|	 }|t| }||
||g q6q|g d t |D ]Z}t |D ]L}||d  | }|| d }||||d g |||d |d g qqt |D ]2}||d  | }|||d t|d g qt|}t|}t|||}t	|||S )zCreates a hemisphere.r   r1   r   r   r   )
r7   r   r8   r9   r:   r   lenr   r$   r%   r;   r   r   r   create_hemisphere   s0     

zTriangleMesh.create_hemisphere)r5   r*   r	   r6   r,   c              	   C   sR  g }g }t dD ]d}|dkr dn|}t |d D ]B}	dtj |	 | }
|t|
 }|t|
 }||||g q0q|g d |dd|g t dD ]B}t |D ]4}	|d | |	 }|||d t|d|  g qqt |D ]<}|d | }||||d g |||d |d g qt|}t|}t|||}t	|||S )zCreates a cylinder.r1   r   r   rB   )
r7   r   r8   r:   r9   r   rC   r   r$   r%   )r3   r5   r*   r	   r6   r   r   r   r!   r=   r>   r   r    r?   r@   r   r   r   r   create_cylinder   s.    	$

zTriangleMesh.create_cylinderN)translationr,   c                 C   s   | j | | _ d S Nr   )r'   rF   r   r   r   	translate  s    zTriangleMesh.translate.)rotationr   r,   c                 C   s    | j | }t||| | _ d S rG   )r   r   dot)r'   rI   r   Ztranslated_pointr   r   r   rotate  s    
zTriangleMesh.rotate)scaler,   c                 C   s   | j | | _ d S rG   r   )r'   rL   r   r   r   rL     s    zTriangleMesh.scalec                 C   s   t j| jdd}|S )Nr   r   )r   r   r   )r'   r   r   r   r   
get_center  s    zTriangleMesh.get_centerc                 C   s~   t |trht| j|jf}|jt| j }t| j|f}d }| jd ur\t| j|jf}t|||S tdt	| d S )NzCannot add TriangleMesh with )

isinstancer%   r   Zvstackr   r   rC   r   	TypeErrortype)r'   otherZnew_verticesZother_trianglesZnew_trianglesZnew_triangle_uvsr   r   r   __add__  s    

zTriangleMesh.__add__)__name__
__module____qualname____doc__r   ndarrayr(   classmethodfloatr   r   r   r4   intrA   rD   rE   rH   r   rK   rL   rM   rR   r   r   r   r   r%   R   sH   	"!&*r%   )configc              
   C   sN   g }|   D ]<\}}|dkrq|d| d d| d d | || i q|S )z2Breaks a shape config into is subcomponent shapes.nameparent_name_)itemsr   copy)r[   Zdecoupled_configkeyvaluer   r   r   decouple_config  s    rc   F)r\   	geom_typesizedecouplec              
   C   s  |t jjkrT| |d dkr&|d d nd|d dkr@|d d nddddd}nd|t jjkrz| d	t|d id
}n>|t jjkrt| t jj|}dddd|d fii|d d< | |d |d dtj	ddfid|d dddd|d  fidd}n|t jj
krDt| t jjtdg}dt|i|d d< | |d d
}nt|t jjkrp| |d |d d dd}nH|t jjkr| |d d |d d |d d dd}nt| d|rt|}|S )z'Creates a config for a particular mesh.r   r1   d   r   gMbP?T)r)   r*   r+   Zmap_texture_to_each_face)r\   boxr5   )r\   sphere	transformrH   cylinderrK   )r5   rj   )r\   rk   Zleft_hemisphereZright_hemispherer   rL   ri   )r5   r*   )r\   rk   )r)   r*   r+   z9 primitive geom type not implemented with USD integration)r   mjtGeomZmjGEOM_PLANEZmjGEOM_SPHERErY   ZmjGEOM_CAPSULEmesh_config_generatorZmjGEOM_CYLINDERr   r8   ZmjGEOM_ELLIPSOIDr   tupleZ
mjGEOM_BOXNotImplementedErrorrc   )r\   rd   re   rf   r[   rk   ri   r   r   r   rm   *  sf    
	



	rm   rg   )mesh_configr	   r6   c                 C   s  d| v sJ d}|   D ]\}}d|v r,qd|v r`tj| | d | | d | | d |d}nxd|v rtj| | d	 ||d
}nVd|v rtj| | d	 ||d
}n4d|v rtj| | d	 | | d ||d}ntdd|v rd|d v rFtd}td}|d d }	d}
t	
||	|
 t	|| |d}|j|dd d|d v rf||d d  d|d v r||d d  |s|}q||7 }q|du rtd| d |fS )z5Generates a mesh given a config consisting of shapes.r\   Nrh   r)   r*   r+   )r)   r*   r+   r	   Z
hemispherer5   )r5   r	   r6   ri   rk   )r5   r*   r	   r6   zShape not supportedrj   rK   	   r.   Zxyz)r2   r2   rB   )r   rL   rH   zMesh not found.)r_   r%   r4   rD   rA   rE   
ValueErrorr   Zzerosr   Zmju_euler2QuatZmju_quat2MatZreshaperK   rL   rH   )rp   r	   r6   Zmeshshaper[   Z	prim_meshrI   ZquatZeulerseqr   r   r   mesh_factoryu  sh    












ru   )F)rg   )rV   typingr   r   r   r   r   r   numpyr   rW   r   r$   r%   strrc   rZ   rl   boolrm   ru   r   r   r   r   <module>   s.   ; J N 
