
    jJm                         d dl Z d dlmZ d dlZddlmZmZ ddlmZ ddl	m
Z
mZmZ ddlmZmZmZmZmZmZmZ  ej*                  d      Zd	ej.                  d
<    G d d      Z G d d      Z	 ddZy)    N)deepcopy   )cachingutil)	hash_fast)	fix_rigidquaternion_matrixrotation_matrix)	ArrayLikeHashableNDArrayOptionalSequenceTupleUnion   F	WRITEABLEc            
          e Zd ZdZddZddZ	 ddedee   dee	e
j                     ee   f   fdZd	 Zd
 Zd ZddZd ZddZd Zd Zd Zej.                  d        Zej.                  d        Zej.                  d        Zdeeeef   fdZdede fdZ!dedee	e
j                     ee   f   fdZ"dede#fdZ$d Z%y) 
SceneGraphz
    Hold data about positions and instances of geometry
    in a scene. This includes a forest (i.e. multi-root tree)
    of transforms and information on which node is the base
    frame, and which geometries are affiliated with which
    nodes.
    c                     t               | _        || _        || _        t	        j
                  | j                        | _        y)a7  
        Create a scene graph, holding homogeneous transformation
        matrices and instance information about geometry.

        Parameters
        -----------
        base_frame : any
          The root node transforms will be positioned from.
        repair_rigid : None or float
          If a float will attempt to repair rotation matrices
          where `M @ M.T` differs from an identity matrix by
          more than floating point zero but less than this value.
          This can happen in a deep tree with a lot of matrix
          multiplies.
        N)EnforcedForest
transforms
base_framerepair_rigidr   Cache__hash___cache)selfr   r   s      E/DATA/.local/lib/python3.12/site-packages/trimesh/scene/transforms.py__init__zSceneGraph.__init__   s3    " )*$ )mmDMM2    Nc                    || j                   }|j                         D ci c]  \  }}|dv s|| }}}t        di ||d<    | j                  j                  ||fi | d|v r |d   | j                  j
                  |   d<   yyc c}}w )aB  
        Update a transform in the tree.

        Parameters
        ------------
        frame_from : hashable object
          Usually a string (eg 'world').
          If left as None it will be set to self.base_frame
        frame_to :  hashable object
          Usually a string (eg 'mesh_0')
        matrix : (4,4) float
          Homogeneous transformation matrix
        quaternion :  (4,) float
          Quaternion ordered [w, x, y, z]
        axis : (3,) float
          Axis of rotation
        angle :  float
          Angle of rotation, in radians
        translation : (3,) float
          Distance to translate
        geometry : hashable
          Geometry object name, e.g. 'mesh_0'
        metadata: dictionary
          Optional metadata attached to the new frame
          (exports to glTF node 'extras').
        N>   geometrymetadatamatrixr#    )r   itemskwargs_to_matrixr   add_edge	node_data)r   frame_to
frame_fromkwargskvattrs          r   updatezSceneGraph.update3   s    8 J "(QA18P3P1Q)3F3X 	!  X>> >DZ>PDOO%%h/
;   Rs
   BBr+   r,   returnc                    || j                   }||f}|| j                  v r| j                  |   S | j                  j                  |   j	                  d      }| j                  j
                  }||k(  rt        }n@||v r
||   d   }n2| j                  j                  ||      }|d   |k(  sJ |d   |k(  sJ g }t        |dd |dd       D ]p  \  }	}
|j	                  |	|
f      }|d|v r|j                  |d          4||
|	f   }d|v s@|j                  t        j                  j                  |d                r |D cg c]2  }t        j                  |t        z
        j                         dkD  s1|4 }}t        |      dk(  rt        }n)t        |      dk(  r|d   }nt!        j"                  |      }| j$                  t'        || j$                        }d	|j(                  d
<   ||f| j                  |<   ||fS c c}w )aP  
        Get the transform from one frame to another.

        Parameters
        ------------
        frame_to : hashable
          Node name, usually a string (eg 'mesh_0')
        frame_from : hashable
          Node name, usually a string (eg 'world').
          If None it will be set to self.base_frame

        Returns
        ----------
        transform : (4, 4) float
          Homogeneous transformation matrix
        geometry
          The name of the geometry if it exists

        Raises
        -----------
        ValueError
          If the frames aren't connected.
        Nr#   r%   r      :0yE>)max_devianceFr   )r   r   r   r*   get	edge_data	_identityshortest_pathzipappendnplinalginvabsmaxlenr   	multi_dotr   r   flags)r   r+   r,   keyr#   datar%   pathmatricesur/   forwardbackwardms                 r   r8   zSceneGraph.get_   s   8 J 8$$++;;s## ??,,X6:::F ((!FD[#Yx(F
 ??00XFD7j(((8x''' HD"ItABx01((Aq6*&7* (9:  A<x'OOBIIMM(82D$EF 1 $,R8arvva)m/D/H/H/JT/Q8HR8}!"X!#!! 1 (vD4E4EFF %*[! #H-Cx' Ss   2H7Hc                 6    | j                   j                         S N)r   r   r   s    r   r   zSceneGraph.__hash__   s    ''))r!   c                     t               }t        | j                        |_        t        | j                        |_        |S )z
        Return a copy of the current TransformForest.

        Returns
        ------------
        copied : TransformForest
          Copy of current object.
        )r   r   r   r   )r   copieds     r   copyzSceneGraph.copy   s2     $T__5$T__5r!   c                     i }| j                   }| j                  D ]4  }||k(  r	| j                  ||      \  }}|j                         |d||<   6 |S )z
        Export the current transform graph with all
        transforms baked into world->instance.

        Returns
        ---------
        flat : dict
          Keyed {node : {transform, geometry}
        )r+   r,   )	transformr#   )r   nodesr8   tolist)r   flatr   noder%   r#   s         r   to_flattenedzSceneGraph.to_flattened   s]     __
JJDz!#xx*xMFH'-}}HMDJ  r!   c                 ^   |6t        |j                  j                               D ci c]  \  }}||
 }}}| j                  }|j                  }|j
                  }| j                  }d|ig}	|di}
|j                         D ])  }||k(  r	t        |	      |
|<   |	j                  d|i       + |j                  }|j                  }t               }|	D ]  }|d   }|j                  |g       }t        |      dkD  r|D cg c]  }|
|   	 c}|d<   d||   v r||   d   }||v r||   |d<   |r||j                  j                  k(  rd|d<   ||k7  s|j                  |   }|||f   }|d   }t!        j"                  |t$              s,|j&                  j)                  d	      j+                         |d<   |j                  d
      }|s|j-                         }|j/                  dd      }t1        |t2              r-||d<   |j5                  t        |j                                     }|j7                  |j9                         D ci c]#  \  }}t;        |d      s||j+                         % c}}       ||d<    d|	i}t        |      dkD  rt=        |      |d<   |S c c}}w c c}w c c}}w )a  
        Export a transforms as the 'nodes' section of the
        GLTF header dict.

        Parameters
        ------------
        scene : trimesh.Scene
          Scene with geometry.
        mesh_index : dict or None
          Mapping { key in scene.geometry : int }

        Returns
        --------
        gltf : dict
          With 'nodes' referencing a list of dicts
        Nnamer   childrenr#   meshcamerar%   r4   r$   gltf_extensions
extensionsrW   extrasrV   extensionsUsed)	enumerater#   keysr   r*   r9   r   rC   r=   
has_camerar]   setr8   r_   r\   parentsr   allcloser:   TreshaperW   rS   pop
isinstancedictunionr1   r'   hasattrlist)r   scene
mesh_indexir\   graphr*   r9   r   resultlookuprY   rf   r]   extensions_usedinfochildsr.   mesh_keyparent	node_edger%   rb   ra   r/   gltfs                             r   to_gltfzSceneGraph.to_gltf   s   $  2;5>>;N;N;P1QR1Qga$'1QJR OO	OO	__
 :&'a NN$Dz!v;F4LMM64.) % %%
>>% D<D \\$+F6{Q7=#>v!F1Iv#>Z  Yt_,$T?:6z)#-h#7DLdell&7&77!"Xz!t,%vtn5	 #8,}}VY7%+XX%5%5b%9%@%@%BDN #z2#[[]F "(,=t!DJ!*d3-7\**9*?*?JOODU@V*W MM39<<>Z>41aWQPXEYAHHJ>Z &,DNW Z  !#%)/%:D!"a SN $?D [s   J?J$J)J)c                    | j                   j                  }g }| j                   j                  j                         D ]  \  }}|\  }}||   }|j	                         }d|v r|d   |d<   |j                  |j                         D 	
ci c]#  \  }	}
t        |
d      s|	|
j                         % c}
}	       |j                  |||g        |S c c}
}	w )z
        Export the current transforms as a list of
        edge tuples, with each tuple having the format:
        (node_a, node_b, {metadata})

        Returns
        ---------
        edgelist : (n,) list
          Of edge tuples
        r#   rW   )	r   r*   r9   r'   rS   r1   rp   rW   r=   )r   rV   exportedger0   abb_attrattr_newr.   r/   s              r   to_edgelistzSceneGraph.to_edgelistN  s     ))//3399;JD$DAq 1XFyy{HV#'-j'9$OO+3>>+;T+;41awq(?SAHHJ+;T MM1a*+ <   Us   CCc                     |D ]l  }t        |      dk(  r | j                  |d   |d   fi |d    /t        |      dk(  r| j                  |d   |d          V|sYt        dt        |             y)aF  
        Load transform data from an edge list into the current
        scene graph.

        Parameters
        -------------
        edgelist : (n,) tuples
          Keyed (node_a, node_b, {key: value})
        strict : bool
          If True raise a ValueError when a
          malformed edge is passed in a tuple.
           r5   r   r   zedge incorrect shape: %sN)rC   r1   
ValueErrorstr)r   edgesstrictr   s       r   from_edgelistzSceneGraph.from_edgelistp  sp     D4yA~DGT!W8Q8TaDGT!W- !;SYGG r!   c                 `    ddl }|j                  | j                         |j                        S )z
        Return a `networkx` copy of this graph.

        Returns
        ----------
        graph : networkx.DiGraph
          Directed graph.
        r   N)create_using)networkxr   r   DiGraph)r   r   s     r   to_networkxzSceneGraph.to_networkx  s,     	%%d&6&6&8xGWGW%XXr!   c           
         ddl m} ddl}ddi} |j                  di |j	                         D ci c]  \  }}||vs|| c}}  |j
                  dd| j                         i| |j                          yc c}}w )z
        Plot the scene graph using `networkx.draw_networkx`
        which uses matplotlib to display the graph.

        Parameters
        -----------
        kwargs : dict
          Passed to `networkx.draw_networkx`
        r   Nwith_labelsTGr&   )matplotlib.pyplotpyplotr   r1   r'   draw_networkxr   show)r   r-   pltr   defaultsr.   r/   s          r   r   zSceneGraph.show  s}     	( "4(O(..*:N*:$!QavoA*:NO>!1!1!3>v>
 Os
   A;A;c                 *    | j                  |d       y)z
        Load transform data from an edge list into the current
        scene graph.

        Parameters
        -------------
        edgelist : (n,) tuples
          Structured (node_a, node_b, {key: value})
        T)r   N)r   )r   edgelists     r   loadzSceneGraph.load  s     	8D1r!   c                 .    | j                   j                  S )z
        A list of every node in the graph.

        Returns
        -------------
        nodes : (n,) array
          All node names.
        )r   rV   rP   s    r   rV   zSceneGraph.nodes  s     $$$r!   c                     | j                   j                  j                         D cg c]  \  }}d|v s| c}}S c c}}w )z
        The nodes in the scene graph with geometry attached.

        Returns
        ------------
        nodes_geometry : (m,) array
          Node names which have geometry associated
        r#   )r   r*   r'   )r   nr0   s      r   nodes_geometryzSceneGraph.nodes_geometry  s=     "&!:!:!@!@!BY!BgajTXFX!BYYYs   ==c                     t        j                  t              }| j                  j                  j                         D ]!  \  }}d|v s||d      j                  |       # |S )z
        Which nodes have this geometry? Inverse
        of `nodes_geometry`.

        Returns
        ------------
        geometry_nodes : dict
          Keyed {geometry_name : node name}
        r#   )collectionsdefaultdictrq   r   r*   r'   r=   )r   resrY   r0   s       r   geometry_nodeszSceneGraph.geometry_nodes  s\     %%d+//3399;JD$T!D$%,,T2 < 
r!   
geometriesc                 <   t        |t              r|g}t        |      }| j                  j                  j                         D ]   }d|v s|d   |v s|j                  d       " | j                  j                  j                  dd       d| j                  _	        y)z
        Remove the reference for specified geometries
        from nodes without deleting the node.

        Parameters
        ------------
        geometries : list or str
          Name of scene.geometry to dereference.
        r#   r   N)
rm   r   rg   r   r*   valuesrl   r   cache_hash)r   r   attribs      r   remove_geometrieszSceneGraph.remove_geometries  s     j#&$J_
 oo//668FV#z(:j(H

:& 9 	.5 $r!   rF   c                 2    || j                   j                  v S rO   )r   r*   r   rF   s     r   __contains__zSceneGraph.__contains__  s    doo////r!   c                 $    | j                  |      S rO   )r8   r   s     r   __getitem__zSceneGraph.__getitem__  s     xx}r!   valuec                     t        j                  |t         j                        }|j                  dk7  rt	        d      | j                  ||      S )Ndtype)r   r   zMatrix must be specified!)r%   )r>   
asanyarrayfloat64shaper   r1   )r   rF   r   s      r   __setitem__zSceneGraph.__setitem__  sA    e2::6;;& 899{{3u{--r!   c                 V    t               | _        | j                  j                          y rO   )r   r   r   clearrP   s    r   r   zSceneGraph.clear
  s    (*r!   )worldgh㈵>rO   )T)&__name__
__module____qualname____doc__r    r1   r   r   r   r   r>   r   r8   r   rS   rZ   r   r   r   r   r   r   r   cache_decoratorrV   r   r   r   r   rg   r   r   boolr   r   r   r   r   r&   r!   r   r   r      sK   34*QZ DH^  ^ .6x.@^ 	wrzz"HX$66	7^ @*,eN DH4Y*
2 	% 	% 	Z 	Z   %E#sH2D,E %60 0T 0	wrzz"HX$66	7
.x .	 .r!   r   c                   T    e Zd ZdZd Zd Zd Zd Zed        Z	ed        Z
d Zd	 Zy
)r   a  
    A simple forest graph data structure: every node
    is allowed to have exactly one parent. This makes
    traversal and implementation much simpler than a
    full graph data type; by storing only one parent
    reference, it enforces the structure for "free."
    c                     i | _         t        j                  t              | _        t        j                  t              | _        i | _        y rO   )rh   r   r   rn   r9   r*   r   rP   s    r   r    zEnforcedForest.__init__  s:    
  %006$006 r!   c                 2   d| _         ||f| j                  vri | _        nt| j                  ||f   }t        j                  |j                  dt              |j                  dt              d      r$|j                  d      |j                  d      k(  ry|| j                  |<   || j                  ||f<   | j                  |   j                  i        d|v r$| j                  |   j                  d|d   i       y| j                  |   j                  i        y)aa  
        Add an edge to the forest cleanly.

        Parameters
        -----------
        u : any
          Hashable node key.
        v : any
          Hashable node key.
        kwargs : dict
           Stored as (u, v) edge data.

        Returns
        --------
        changed : bool
          Return if this operation changed anything.
        Nr%   r6   r#   FT)
r   r9   r   r   ri   r8   r:   rh   r*   r1   )r   rJ   r/   r-   r   s        r   r)   zEnforcedForest.add_edge)  s   $ 
 q6'DK >>1a&)D}}

8Y/(I1NPT88J'6::j+AA Q!'1vq  $NN1$$j&2D%EF  NN1$$R(r!   c                    || j                   vryi | _        d| _        | j                  j	                         D cg c]  \  }}||k(  s| }}}|D ]  }| j                  |=  || j                  v r| j                  |= | j
                  D cg c]  \  }}||k(  s||k(  s||f }}}|D ]  }	| j
                  |	=  | j                   |= yc c}}w c c}}w )z
        Remove a node from the forest.

        Parameters
        -----------
        u : any
          Hashable node key.

        Returns
        --------
        changed : bool
          Return if this operation changed anything.
        FNT)r*   r   r   rh   r'   r9   )
r   rJ   childr|   r]   cr   r   r   es
             r   remove_nodezEnforcedForest.remove_nodeU  s     DNN" 
 261C1C1EU1EoufSTE1EUAQ Q '+nnInFQQ!q&!QnIAq!  NN1 V Js   C	CC!Cc                    ||k(  rg S ||f| j                   v r| j                   ||f   S ||f| j                   v r| j                   ||f   ddd   S | j                  }|g}|g}t        t        |      dz         D ]v  }|j	                  |d         }|j	                  |d         }|j                  |       |j                  |       ||k(  r|| j                   ||f<   |c S ||k(  r|ddd   }|| j                   ||f<   |c S ||v s||t        |      j                  |      j                  dh      t              dk(  rt        d| d| d      t              dkD  rt        fd|D              }	|	v sJ t        t                    }	|d|j                  |	      dz    }
|d|j                  |	       }|
|ddd   z   }|d   |k(  sJ |d   |k(  sJ || j                   ||f<   |c S  t        d	      )
a  
        Find the shortest path between `u` and `v`, returning
        a path where the first element is always `u` and the
        last element is always `v`, disregarding edge direction.

        Parameters
        -----------
        u : any
          Hashable node key.
        v : any
          Hashable node key.

        Returns
        -----------
        path : (n,)
          Path between `u` and `v`
        Nr4   r5   r   zNo path from z->!c              3   ,   K   | ]  }|v s|  y wrO   r&   ).0fcommons     r   	<genexpr>z/EnforcedForest.shortest_path.<locals>.<genexpr>  s     B7aa6k7s   	zIteration limit exceeded!)r   rh   rangerC   r8   r=   rg   intersection
differencer   nextiterindex)r   rJ   r/   rh   rK   rL   _r   r   linkr   rH   r   s               @r   r;   zEnforcedForest.shortest_path|  s4   & 6IVt{{";;1v&&Vt{{";;1v&tt,, ,,#3 s7|a'(AGBK(AHRL)ANN1OOAAv&-QF#a#DbD>&.QF#w,AI!) X33G<GGOv;!#$}QCr!A%>??[1_B7BBD6>)>  V-D 5gmmD1A563x~~d341TrT7{ Aw!|#|Bx1}$}&*QF#S )V 455r!   c                 6    | j                   j                         S )z
        Get a set of every node.

        Returns
        -----------
        nodes : set
          Every node currently stored.
        )r*   re   rP   s    r   rV   zEnforcedForest.nodes  s     ~~""$$r!   c                 R   d| j                   v r| j                   d   S t        j                  t              }| j                  j                         D cg c]  \  }}||k7  s||   j                  |      ! c}} t        |      | j                   d<   | j                   d   S c c}}w )z
        Get the children of each node.

        Returns
        ----------
        children : dict
          Keyed {node : [child, child, ...]}
        r]   )r   r   r   rq   rh   r'   r=   rn   )r   r   rJ   r/   s       r   r]   zEnforcedForest.children  s     $;;z**''- )-(:(:(<G(<1Qq	(<G #'u+J{{:&&	 	Hs   B#"B#c                 @   | j                   }||vr|hS |g}t        |      }t        t        | j                        dz         D ]X  }t        |      dk(  r|c S |j                  |j                               }|7|j                  |       |j                  |       Z |S )a8  
        Get all nodes that are successors to specified node,
        including the specified node.

        Parameters
        -------------
        node : any
          Hashable key for a node.

        Returns
        ------------
        successors : set
          Nodes that succeed specified node.
        r5   r   )	r]   rg   r   rC   r*   r8   rl   extendr1   )r   rY   r]   queue	collectedr   rz   s          r   
successorszEnforcedForest.successors  s      ==x6M J	 s4>>*Q./A5zQ  \\%))+.F!V$  ( 0 r!   c                    t        | dd      }||S t        dj                  d | j                  j	                         D              dj                  d | j
                  j	                         D              z   j                  d      dj                  d | j                  j                         D              z         }|| _        |S )	ao  
        Actually hash all of the data, but use a "dirty" mechanism
        in functions that modify the data, which MUST
        # all invalidate the hash by setting `self._hash = None`

        This was optimized a bit, and is evaluating on an
        older laptop on a scene with 77 nodes and 76 edges
        10,000 times in 0.7s which seems fast enough.
        r   N c              3   p   K   | ].  \  }}t        t        |            |j                  d d      z    0 ywr#   r   N)r   hashr8   r   r.   r/   s      r   r   z*EnforcedForest.__hash__.<locals>.<genexpr>(  s4       61 QL155R#88 6s   46c              3   ^   K   | ]%  \  }}t        |      |j                  d d      z    ' ywr   )r   r8   r   s      r   r   z*EnforcedForest.__hash__.<locals>.<genexpr>,  s-      ?Utq!CFQUU:r22?Us   +-zutf-8r!   c              3   L   K   | ]  }d |v s|d    j                           yw)r%   N)tobytes)r   r/   s     r   r   z*EnforcedForest.__hash__.<locals>.<genexpr>0  s)      /F!(VW-(##%/Fs   	$$)	getattrr   joinr9   r'   r*   encoder   r   )r   hasheds     r   r   zEnforcedForest.__hash__  s     w-M  $ 4 4 6  '' ?C~~?S?S?U 	 fWohh /3~~/D/D/F 
 
r!   N)r   r   r   r   r    r)   r   r;   propertyrV   r]   r   r   r&   r!   r   r   r     sR    "*X%NN6` 	% 	% ' '($L"r!   r   c                     | %t        j                  | t         j                        S |t        |      } n&||t	        ||      } nt        j
                  d      } || dddfxx   |z  cc<   | S )z
    Take multiple keyword arguments and parse them
    into a homogeneous transformation matrix.

    Returns
    ---------
    matrix : (4, 4) float
      Homogeneous transformation matrix.
    Nr   r   r   )r>   arrayr   r	   r
   eye)r%   
quaterniontranslationaxisangler-   s         r   r(   r(   8  sv     xxbjj11		":.		e/ - 	rr1u$Mr!   )NNNNN)r   rS   r   numpyr>   r   r   r   r   transformationsr   r	   r
   typedr   r   r   r   r   r   r   r   r:   rE   r   r   r(   r&   r!   r   <module>r      sh         K K R R R BFF1I	$	 | |~f fT	 FJr!   