TiJmddlZddlmZddlZddlmZmZddlmZddl m Z m Z m Z ddl mZmZmZmZmZmZmZejdZd ejd <Gd d ZGd dZ ddZdS)N)deepcopy)cachingutil) hash_fast) fix_rigidquaternion_matrixrotation_matrix) ArrayLikeHashableNDArrayOptionalSequenceTupleUnionF WRITEABLEc eZdZdZd dZd!dZ d!dedeed ee e j eeffd Z d Z d Zd Zd!dZdZd"dZdZdZdZejdZejdZejdZdeeeeffdZded e fdZ!ded ee e j eeffdZ"dede#fdZ$dZ%dS)# 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. worldh㈵>ct|_||_||_t j|j|_dS)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_rigidrCache__hash___cache)selfrrs R/DATA/AppData/hermes/venv/lib/python3.11/site-packages/trimesh/scene/transforms.py__init__zSceneGraph.__init__s9")**$)mDM22 Nc ||j}d|D}tdi||d<|jj||fi|d|vr|d|jj|d<dSdS)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'). Nc"i|] \}}|dv || S)>geometrymetadata.0kvs r! z%SceneGraph.update..Ss)QQQA18P3P3P13P3P3Pr#matrixr&r()ritemskwargs_to_matrixradd_edge node_data)r frame_to frame_fromkwargsattrs r!updatezSceneGraph.update3s8  JRQQQQ)33F33X ! X>>>>>   >DZ>PDO %h / ; ; ; r#r3r4returnc||j}||f}||jvr |j|S|jj|d}|jj}||kr t }nI||vr||d}n5|j||}|d|ksJ|d|ksJg}t|dd|ddD]\} } || | f} | d| vr| | d>|| | f} d| vr8| tj | dd|D}t|dkrt }n0t|dkr |d}ntj|}|jt#||j}d |jd <||f|j|<||fS) 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.rcvg|]6}tj|tz dk4|7S):0yE>)npabs _identitymax)r*ms r! z"SceneGraph.get..s<RRRarva)m/D/D/H/H/J/JT/Q/Q/Q/Q/Qr#) max_devianceFr)rrrr2get edge_datar@ shortest_pathzipappendr>linalginvlenr multi_dotrrflags) r r3r4keyr&datar.pathmatricesur,forwardbackwards r!rEzSceneGraph.get_s*8  J8$ $+  ;s# #?,X6:::FF(  ! !FF D[[#Yx(FF ?00XFFD7j((((8x''''HD"ItABBx00 G G1((Aq6**&7** (9::: A<x''OOBIMM(82D$E$EFFFRR8RRRH8}}!!"X!##!!11   (vD4EFFFF%* [!#H- Cxr#c4|jSN)rrr s r!rzSceneGraph.__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)r copieds r!copyzSceneGraph.copys6$T_55$T_55 r#ci}|j}|jD]=}||kr |||\}}||d||<>|S)z Export the current transform graph with all transforms baked into world->instance. Returns --------- flat : dict Keyed {node : {transform, geometry} )r3r4) transformr&)rnodesrEtolist)r flatrnoder.r&s r! to_flattenedzSceneGraph.to_flattenedsk_ J N NDz!!#xx*xMM FH'-}}HMMDJJ r#cD|0dt|jD}|j}|j}|j}|j}d|ig}|di|D]2}||kr t||<|d|i3|j } |j } t} |D]} | d}| |g} t| dkrfd| D| d<d||vr||d}||vr ||| d<| r||j jkrd| d <||kr-|j|}|||f}|d }t!j|t$s/|jd | d <| d }|r|}|d d}t1|t2r9|| d<| t|} |d|D|| d<d|i}t| dkrt;| |d<|S)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 Nci|]\}}|| Sr(r()r*inames r!r-z&SceneGraph.to_gltf..sRRRga$RRRr#rfrc g|] }| Sr(r()r*r+lookups r!rCz&SceneGraph.to_gltf..#s#>#>#>!F1I#>#>#>r#childrenr&meshcamerar.r:r'gltf_extensions extensionsc^i|]*\}}t|d||+Sr_hasattrr_r)s r!r-z&SceneGraph.to_gltf..Es6ZZZ41aWQPXEYEYZAHHJJZZZr#extrasr^extensionsUsed) enumerater&keysrr2rFrrLrI has_camerarisetrErkrfparentsrallcloser@Treshaper_r[pop isinstancedictunionr7r/list)r scene mesh_indexgraphr2rFrresultrarvriextensions_usedinfochildsmesh_keyparent node_edger.rrrmgltfrhs @r!to_gltfzSceneGraph.to_gltfs$  SR5>;N;N;P;P1Q1QRRRJO O _ :&'aNN$$ * *Dz!!v;;F4L MM64. ) ) ) )% >%%+ ,+ ,D#>#>#>v#>#>#>Z Yt_,,$T?:6z))#-h#7DL #del&777!"Xz!!t,%vtn5 #8,}VY77C%+X%5%5b%9%9%@%@%B%BDN#z22 ,#[[]]F"(,=t!D!DJ!*d33X-7\**9*?*?JOODUDU@V@V*W*WMMZZ6<<>>ZZZ&,DN    ! # #%)/%:%:D! " r#c\|jj}g}|jjD]~\}}|\}}||}|}d|vr |d|d<|d|D||||g|S)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&c^i|]*\}}t|d||+Srorpr)s r!r-z*SceneGraph.to_edgelist..ks5TTT41awq(?S?STAHHJJTTTr#)rr2rFr/r[r7rI) r r^exportedger6abb_attrattr_news r! to_edgelistzSceneGraph.to_edgelistNs)/399;; , ,JD$DAq1XFyy{{HV##'-j'9$ OOTT8>>+;+;TTT    MM1a* + + + + r#Tc$|D]}t|dkr"|j|d|dfi|d7t|dkr#||d|dm|rtdt|dS)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. r;rrzedge incorrect shape: %sN)rLr7 ValueErrorstr)r edgesstrictrs r! from_edgelistzSceneGraph.from_edgelistps H HD4yyA~~ DGT!W88Q8888Ta DGT!W---- H !;SYYGGG H H Hr#cfddl}|||jS)z Return a `networkx` copy of this graph. Returns ---------- graph : networkx.DiGraph Directed graph. rN) create_using)networkxrrDiGraph)r rs r! to_networkxzSceneGraph.to_networkxs5 %%d&6&6&8&8xGW%XXXr#c ddlm}ddl}ddi}jdifd|D|jdd|i|dS)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_labelsTc$i|] \}}|v || Sr(r()r*r+r,r5s r!r-z#SceneGraph.show..s$NNN$!QavooAooor#Gr()matplotlib.pyplotpyplotrr7r/ draw_networkxrshow)r r5pltrdefaultss ` r!rzSceneGraph.shows ('''''"4( OONNNN(..*:*:NNNOOO>>!1!1!3!3>v>>>  r#c4||ddS)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)r edgelists r!loadzSceneGraph.loads# 8D11111r#c|jjS)z A list of every node in the graph. Returns ------------- nodes : (n,) array All node names. )rr^rXs r!r^zSceneGraph.nodess$$r#cRd|jjDS)z The nodes in the scene graph with geometry attached. Returns ------------ nodes_geometry : (m,) array Node names which have geometry associated c g|] \}}d|v | S)r&r()r*nr6s r!rCz-SceneGraph.nodes_geometry..s'YYYgajTXFXFXFXFXFXr#)rr2r/rXs r!nodes_geometryzSceneGraph.nodes_geometrys*ZY!:!@!@!B!BYYYYr#ctjt}|jjD]*\}}d|vr!||d|+|S)z Which nodes have this geometry? Inverse of `nodes_geometry`. Returns ------------ geometry_nodes : dict Keyed {geometry_name : node name} r&) collections defaultdictrrr2r/rI)r resrar6s r!geometry_nodeszSceneGraph.geometry_nodessg%d++/399;; 3 3JD$T!!D$%,,T222 r# geometriesc4t|tr|g}t|}|jjD]%}d|vr|d|vr|d&|jjddd|j_ dS)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) r}rrwrr2valuesr|rcache_hash)r rattribs r!remove_geometrieszSceneGraph.remove_geometriess j# & & &$J__ o/6688 ' 'FV##z(:j(H(H :&&& .555 $r#rOc||jjvSrW)rr2r rOs r! __contains__zSceneGraph.__contains__sdo///r#c,||SrW)rErs r! __getitem__zSceneGraph.__getitem__sxx}}r#valuectj|tj}|jdkrt d|||S)Ndtype)rrzMatrix must be specified!)r.)r> asanyarrayfloat64shaperr7)r rOrs r! __setitem__zSceneGraph.__setitem__sJ e2:666 ;& 899 9{{3u{---r#c^t|_|jdSrW)rrrclearrXs r!rzSceneGraph.clear s)(** r#)rrrW)T)&__name__ __module__ __qualname____doc__r"r7r rrr r>rrErr[rbrrrrrrrcache_decoratorr^rrrrrwrrboolrrr rrr(r#r!rrsY33334*Q*Q*Q*QZDH^ ^  ^ .6x.@^ wrz"HX$66 7^ ^ ^ ^ @***   ,eeeeN   DHHHH4 Y Y Y* 2 2 2  % % %  Z Z Z  %E#sH2D,E%%%%600T0000 wrz"HX$66 7 .x. .... r#rcbeZdZdZdZdZdZdZedZ edZ dZ d Z d S) 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|_dSrW)rxrrr~rFr2rrXs r!r"zEnforcedForest.__init__s>  %066$066 r#c Jd|_||f|jvri|_n|j||f}tj|dt |dt dr.|d|dkrdS||j|<||j||f<|j| id|vr)|j| d|din |j| idS)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.r=r&FT) rrFrrryrEr@rxr2r7)r rSr,r5rs r!r1zEnforcedForest.add_edge)s$$  q6 ' 'DKK>1a&)D} 8Y//(I1N1NPT 88J''6::j+A+AAAu Q!'1v q  $$$    N1  $ $j&2D%E F F F F N1  $ $R ( ( (tr#c|jvrdSi|_d|_fd|jD}|D] }|j|= |jvr|j=fd|jD}|D] }|j|= |j=dS)z Remove a node from the forest. Parameters ----------- u : any Hashable node key. Returns -------- changed : bool Return if this operation changed anything. FNc&g|] \}}|k |Sr(r()r*childrrSs r!rCz.EnforcedForest.remove_node..ls#UUUoufSTEr#c6g|]\}}|ks|k||fSr(r()r*rrrSs r!rCz.EnforcedForest.remove_node..ss.IIIFQQ!q&&!Q&&&r#T)r2rrrxr/rF)r rSricres ` r! remove_nodezEnforcedForest.remove_nodeUs DN " "5  VUUU1C1C1E1EUUU  A Q    QJIIIdnIII " "Aq!! N1 tr#c ||krgS||f|jvr|j||fS||f|jvr|j||fdddS|j}|g}|g}tt|dzD]}||d}||d}||||||kr||j||f<|cS||kr|ddd}||j||f<|cS||vs|*|'t ||dh t dkrtd|d|dt dkr"t fd|D} | vsJntt } |d| | dz} |d| | }| |dddz} | d|ksJ| d|ksJ| |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` Nr:r;rz No path from z->!c3$K|] }|v|V dSrWr()r*fcommons r! z/EnforcedForest.shortest_path..s'BBaa6kkkkkkBBr#zIteration limit exceeded!) rrxrangerLrErIrw intersection differencernextiterindex) r rSr,rxrTrU_rrlinkrrQrs @r!rGzEnforcedForest.shortest_path|s& 66IVt{ " ";1v& &Vt{ " ";1v&ttt, ,,#3s7||a'(() ) A GBK((A HRL))A NN1    OOA   Avv&- QF#a#DDbD>&. QF#w,,AI!)X33G<<GGOOv;;!##$%>Q%>%>!%>%>%>???[[1__BBBB7BBBBBD6>>>>> V --D5gmmD11A5563x~~d33341TTrT7{Aw!||||Bx1}}}}&* QF# 4555r#c4|jS)z Get a set of every node. Returns ----------- nodes : set Every node currently stored. )r2rurXs r!r^zEnforcedForest.nodess~""$$$r#cd|jvr |jdStjtfd|jDt |jd<|jdS)z Get the children of each node. Returns ---------- children : dict Keyed {node : [child, child, ...]} ricXg|]&\}}||k ||'Sr()rI)r*rSr,rs r!rCz+EnforcedForest.children..s2GGG1Qq  r#)rrrrrxr/r~)r rs @r!rizEnforcedForest.childrens{  $ $;z* *'-- HGGG (:(:(<(<GGGG#'u++ J{:&&r#ch|j}||vr|hS|g}t|}tt|jdzD]l}t|dkr|cS||}|*||||m|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. r;r) rirwrrLr2rEr|extendr7)r rariqueue collectedrrs r! successorszEnforcedForest.successorss = x  6MJJ s4>**Q.// ) )A5zzQ    \\%))++..F! V$$$  (((r#ct|dd}||Stdd|jDdd|jDzddd|jDz}||_|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. rNc3K|]:\}}tt||ddzV;dSr&rN)rhashrEr)s r!rz*EnforcedForest.__hash__..(sY1QLL155R#8#88r#c3hK|]-\}}t||ddzV.dSr)rrEr)s r!rz*EnforcedForest.__hash__..,sP7;q!CFFQUU:r222r#zutf-8r#c3RK|]"}d|v|dV#dS)r.N)tobytes)r*r,s r!rz*EnforcedForest.__hash__..0s@*+(VW--( ##%%----r#) getattrrjoinrFr/r2encoderr)r hasheds r!rzEnforcedForest.__hash__sw--  M $ 4 4 6 6''?C~?S?S?U?U fWoohh/3~/D/D/F/F      r#N) rrrrr"r1rrGpropertyr^rirrr(r#r!rrs"***X%%%NN6N6N6` % %X %''X'($$$L"""""r#rc | tj|tjS|t|}n)||t ||}ntjd}||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 translationaxisangler5s r!r0r08sxbj1111  ":.. e/ -- rr1u $ Mr#)NNNNN)rr[rnumpyr>rrrrtransformationsrr r typedr r r rrrrrr@rNrrr0r(r#r!r sQKKKKKKKKKKRRRRRRRRRRRRRRRRRR BF1II $  ||||||||~ffffffffT FJr#