:vj qddlZddlZddlmZddlZddlmZmZddlm Z ddl m Z m Z m Z ddlmZmZmZmZmZej(dZd ej,d <d ZGd d ZGddZ ddZy)N)deepcopy)cachingutil) hash_fast) fix_rigidquaternion_matrixrotation_matrix) ArrayLikeFloatingHashableNDArraySequenceF WRITEABLEworldc eZdZdZ ddedzdedzfdZd dZ d dededzd ee e jedzffd Z d Z d Zd Zd dZdZd!dZdZdZdZej.dZej.dZej.dZdeezezfdZded efdZ ded ee e jedzffdZ!dede"fdZ#dZ$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. N base_frame repair_rigidct|_||nt|_||_t j |j|_y)a! Create a scene graph, holding homogeneous transformation matrices and instance information about geometry. Parameters ----------- base_frame The root node transforms will be positioned from. repair_rigid 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 transformsDEFAULT_BASE_FRAMErrrCache__hash___cache)selfrrs i/workspace/moltex-experiments/.pixi/envs/default/lib/python3.12/site-packages/trimesh/scene/transforms.py__init__zSceneGraph.__init__s=()*)3(>*DV)mmDMM2 c | |j}|jDcic] \}}|dvs ||}}}tdi||d<|jj||fi|d|vr |d|jj |d<yycc}}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>geometrymetadatamatrixr#)ritemskwargs_to_matrixradd_edge node_data)rframe_to frame_fromkwargskvattrs rupdatezSceneGraph.update;s8  J"(QA18P3P1Q)3F3X !  X>>  >DZ>PDOO % %h / ; Rs BBr+r,returnc| |j}||f}||jvr|j|S|jj|j d}|jj }||k(rt }nC||vr ||d}n5|jj||}|d|k(sJ|d|k(sJg}tj|D]p\} } |j | | f} | d| vr|j| d4|| | f} d| vs@|jtjj| dr|D cgc]2} tj| t z jdkDs1| 4}} t!|dk(rt }n)t!|dk(r|d}nt#j$|}|j&t)||j&}d|j*d <||f|j|<||fScc} 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. r#r%r:0yE>) max_devianceFr)rrrr*get edge_data _identity shortest_path itertoolspairwiseappendnplinalginvabsmaxlenr multi_dotrrflags)rr+r,keyr#datar%pathmatricesur/forwardbackwardms rr8zSceneGraph.getgs8  J8$ $++ ;;s# #??,,X6:::F((  !F D[#Yx(F ??00XFD7j( ((8x' ''H!**401((Aq6*&7* (9: A<x'OOBIIMM(82D$EF1$,R8arvva)m/D/H/H/JT/Q8HR8}!"X!#!!1    (vD4E4EFF%* [!#H- Cx'Ss 2H:Hc6|jjSN)rrrs rrzSceneGraph.__hash__s''))r!ct}t|j|_t|j|_|S)z Return a copy of the current TransformForest. Returns ------------ copied : TransformForest Copy of current object. )rrrr)rcopieds rcopyzSceneGraph.copys2$T__5$T__5 r!ci}|j}|jD]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#)rnodesr8tolist)rflatrnoder%r#s r to_flattenedzSceneGraph.to_flatteneds]__ JJDz!#xx*xM FH'-}}HMDJ  r!c \|6t|jjDcic]\}}|| }}}|j}|j}|j }|j }|j} |j} d|j|ivxr>| xr||jjk( xrt| j|gdkD} | rg} i} n d|ig} |di} |jD])}||k(r t| | |<| jd|i+t}| D]}|d}| j|g}t|dkDr|Dcgc]}| | c}|d<d||vr||d}||vr|||d<| r||jjk(rd|d<||k7s|j|}|||f}|d}t!j"|t$s,|j&j)d j+|d<|jd }|s|j-}|j/d d}t1|t2r-||d <|j5t|j}|j7|j9Dcic]#\}}t;|d s||j+%c}}||d<| r| |Dcgc]}| | }}n| |g}| |d}t|dkDrt=||d<|Scc}}wcc}wcc}}wcc}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 and 'scene_roots' referencing the node indices the scene should start from. Nr#rnamechildrenmeshcamerar%r4r$gltf_extensions extensionsrXextras)rW scene_rootsextensionsUsed) enumerater#keysrr*r9r has_camerar^r8r`r]rDr>setparentsrallcloser:TreshaperXrTpop isinstancedictunionr1r'hasattrlist)rscene mesh_indexir]graphr*r9rrhr^ skip_baseresultlookuprZextensions_usedinfochildsr.mesh_keyparent node_edger%rcrbr/crootsgltfs rto_gltfzSceneGraph.to_gltfsT&  2;5>>;N;N;P1QR1Qga$'1QJROO OO __ %% >> immJ; ; 6CJ%,,2C2C$CD 6HLLR01A5  FFz*+F !_FNN$Dz!v;F4L MM64. ) %%Dv!F1Iv#>Z Yt_,$T?:6z)#-h#7DLdell&7&77!"Xz!t,%vtn5 #8,}}VY7%+XX%5%5b%9%@%@%BDN#z2#[[]F"(,=t!DJ!*d3-7\**9*?*?JOODU@V*WMM39<<>Z>41aWQPXEYAHHJ>Z&,DNWZ (0(<=(<1VAY(s L LL# 4L# L)c |jj}g}|jjjD]\}}|\}}||}|j }d|vr|d|d<|j |jD cic]#\} } t | ds| | j%c} } |j|||g|Scc} } 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#rX) rr*r9r'rTr1rrrXr>) rrWexportedger0abb_attrattr_newr.r/s r to_edgelistzSceneGraph.to_edgelistgs))//3399;JD$DAq1XFyy{HV#'-j'9$ OO+3>>+;T+;41awq(?SAHHJ+;T  MM1a* +<  Us C C c|D]l}t|dk(r|j|d|dfi|d/t|dk(r|j|d|dV|sYtdt|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. r6rrzedge incorrect shape: %sN)rDr1 ValueErrorstr)redgesstrictrs r from_edgelistzSceneGraph.from_edgelistspD4yA~ DGT!W8Q8Ta DGT!W- !;SYGGr!c`ddl}|j|j|jS)z Return a `networkx` copy of this graph. Returns ---------- graph : networkx.DiGraph Directed graph. rN) create_using)networkxrrDiGraph)rrs r to_networkxzSceneGraph.to_networkxs, %%d&6&6&8xGWGW%XXr!c ddlm}ddl}ddi}|jdi|j Dcic] \}}||vs ||c}}|j dd|j i||jycc}}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` rN with_labelsTGr&)matplotlib.pyplotpyplotrr1r' draw_networkxrshow)rr-pltrdefaultsr.r/s rrzSceneGraph.shows} ("4( O(..*:N*:$!QavoA*:NO>!1!1!3>v>  Os A;A;c*|j|dy)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)rN)r)redgelists rloadzSceneGraph.loads 8D1r!c.|jjS)z A list of every node in the graph. Returns ------------- nodes : (n,) array All node names. )rrWrQs rrWzSceneGraph.nodess$$$r!c|jjjDcgc] \}}d|vs |c}}Scc}}w)z The nodes in the scene graph with geometry attached. Returns ------------ nodes_geometry : (m,) array Node names which have geometry associated r#)rr*r')rnr0s rnodes_geometryzSceneGraph.nodes_geometrys="&!:!:!@!@!BY!BgajTXFX!BYYYs ==ctjt}|jjj D]!\}}d|vs ||dj |#|S)z Which nodes have this geometry? Inverse of `nodes_geometry`. Returns ------------ geometry_nodes : dict Keyed {geometry_name : node name} r#) collections defaultdictrsrr*r'r>)rresrZr0s rgeometry_nodeszSceneGraph.geometry_nodess\%%d+//3399;JD$T!D$%,,T2< r! geometriesc<t|tr|g}t|}|jjj D] }d|vs|d|vs|j d"|jjj ddd|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#rN) rorrirr*valuesrnrcache_hash)rrattribs rremove_geometrieszSceneGraph.remove_geometriess j# &$J_ oo//668FV#z(:j(H :&9 .5 $r!rGc2||jjvSrP)rr*rrGs r __contains__zSceneGraph.__contains__sdoo////r!c$|j|SrP)r8rs r __getitem__zSceneGraph.__getitem__sxx}r!valuectj|tj}|jdk7r t d|j ||S)Ndtype)rrzMatrix must be specified!)r%)r? asanyarrayfloat64shaperr1)rrGrs r __setitem__zSceneGraph.__setitem__sA e2::6 ;;& 89 9{{3u{--r!cVt|_|jjyrP)rrrclearrQs rrzSceneGraph.clear!s(* r!)Ngh㈵>rP)T)%__name__ __module__ __qualname____doc__r r r r1tuplerr?rr8rrTr[rrrrrrrcache_decoratorrWrrrrirrboolrrr rrr&r!rrrscSW3"T/3@H43<*QZAE^  ^ .6o^ wrzz"HtO3 4^ @* ,vp DH4 Y* 2  % %  Z Z  %C#I,@%600T0xE'"**2ExRV2V,W.x. . r!rcTeZdZdZdZdZdZdZedZ edZ dZ d Z y ) ra 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." ci|_tjt|_tjt|_i|_yrP)rjrrrpr9r*rrQs rr zEnforcedForest.__init__/s:  %006$006 r!c 2d|_||f|jvri|_nt|j||f}tj|j dt |j dt dr$|j d|j dk(ry||j|<||j||f<|j|jid|vr$|j|jd|diy|j|jiy)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%r5r#FT) rr9rrrkr8r:rjr*r1)rrKr/r-rs rr)zEnforcedForest.add_edge@s$  q6 'DK>>1a&)D}} 8Y/(I1NPT88J'6::j+AA Q!'1v q  $   NN1  $ $j&2D%E F NN1  $ $R (r!c||jvryi|_d|_|jj Dcgc] \}}||k(s |}}}|D]}|j|=||jvr |j|=|j Dcgc]\}}||k(s||k(s||f}}}|D]} |j | =|j|=ycc}}wcc}}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*rrrjr'r9) rrKchildrr^rrrres r remove_nodezEnforcedForest.remove_nodels DNN "  261C1C1EU1EoufSTE1EUA Q   Q'+nnInFQQ!q&!QnIAq! NN1 VJs C  C C!Cc ||k(rgS||f|jvr|j||fS||f|jvr|j||fdddS|j}|g}|g}tt|dzD]v}|j |d}|j |d}|j ||j |||k(r||j||f<|cS||k(r|ddd}||j||f<|cS||vs||t |j|jdh t dk(rtd|d|dt dkDrt fd|D} | vsJtt } |d|j| dz} |d|j| }| |dddz} | d|k(sJ| d|k(sJ| |j||f<| cStd ) 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` Nr4r6rz No path from z->!c3,K|] }|vs| ywrPr&).0fcommons r z/EnforcedForest.shortest_path..sB7aa6k7s zIteration limit exceeded!) rrjrangerDr8r>ri intersection differencernextiterindex) rrKr/rjrLrM_rrlinkrrIrs @rr;zEnforcedForest.shortest_paths4& 6IVt{{ ";;1v& &Vt{{ ";;1v&tt, ,,,#3s7|a'(A GBK(A HRL)A NN1  OOA Av&- QF#a#DbD>&. QF#w,AI!)X33G<GGOv;!#$}QCr!A%>??[1_B7BBD6>)> V -D5gmmD1A563x~~d341TrT7{Aw!|#|Bx1}$}&* QF# S)V455r!c6|jjS)z Get a set of every node. Returns ----------- nodes : set Every node currently stored. )r*rgrQs rrWzEnforcedForest.nodess~~""$$r!cRd|jvr|jdStjt}|jj Dcgc]\}}||k7s ||j |!c}}t||jd<|jdScc}}w)z Get the children of each node. Returns ---------- children : dict Keyed {node : [child, child, ...]} r^)rrrrsrjr'r>rp)rrrKr/s rr^zEnforcedForest.childrens  $;;z* *''-)- (:(:(<G(<1Qq (<G#'u+ J{{:&& Hs  B#"B#c@|j}||vr|hS|g}t|}tt|jdzD]X}t|dk(r|cS|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. r6r) r^rirrDr*r8rnextendr1)rrZr^queue collectedrr}s r successorszEnforcedForest.successorss == x 6MJ s4>>*Q./A5zQ  \\%))+.F! V$  (0r!ct|dd}||Stdjd|jj Ddjd|j j Dzj ddjd|jjDz}||_|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. rNc3pK|].\}}tt||jddz0ywr#rN)rhashr8rr.r/s rrz*EnforcedForest.__hash__..?s4 61QL155R#88 6s46c3^K|]%\}}t||jddz'ywr)rr8rs rrz*EnforcedForest.__hash__..Cs-?Utq!CFQUU:r22?Us+-zutf-8r!c3LK|]}d|vs|djyw)r%N)tobytes)rr/s rrz*EnforcedForest.__hash__..Gs)/F!(VW-( ##%/Fs $$) getattrrjoinr9r'r*encoderr)rhasheds rrzEnforcedForest.__hash__*sw-  M $ 4 4 6''?C~~?S?S?U fWohh/3~~/D/D/F     r!N) rrrrr r)rr;propertyrWr^rrr&r!rrr&sR"*X%NN6` % %''($L"r!rc |%tj|tjS| t|}n&|| t ||}ntj d}||dddfxx|z cc<|S)z Take multiple keyword arguments and parse them into a homogeneous transformation matrix. Returns --------- matrix : (4, 4) float Homogeneous transformation matrix. Nrrr)r?arrayrr r eye)r% quaternion translationaxisangler-s rr(r(Osvxxbjj11  ":. e/ - rr1u $ Mr!)NNNNN)rr<rTrnumpyr?rrrrtransformationsrr r typedr r r rrrr:rFrrrr(r&r!rrspKKDD BFF1I $  OOdffT FJr!