j\ddlZddlZ ddlZGddZGddZGddZdZd Z d Z y#e$rdZY1wxYw) NcLeZdZdZdZedZedZedZdZ y) ContactDatazK Data structure for holding information about a collision contact. ct||_|d|j|d|ji|_|j |_|j|_|j|_ y)z Initialize a ContactData. Parameters ---------- names : list of str The names of the two objects in order. contact : fcl.Contact The contact in question. rN) setnamesb1b2_indsnormal_normalpos_pointpenetration_depth_depth)selfrcontacts >/DATA/.local/lib/python3.12/site-packages/trimesh/collision.py__init__zContactData.__init__sRZ Ah E!HgjjA ~~ kk // c|jS)z The 3D intersection normal for this contact. Returns ------- normal : (3,) float The intersection normal. )r rs rr zContactData.normal"s||rc|jS)z The 3D point of intersection for this contact. Returns ------- point : (3,) float The intersection point. )rrs rpointzContactData.point.{{rc|jS)z The penetration depth of the 3D point of intersection for this contact. Returns ------- depth : float The penetration depth. )rrs rdepthzContactData.depth:rrc |j|S)a) Returns the index of the face in contact for the mesh with the given name. Parameters ---------- name : str The name of the target object. Returns ------- index : int The index of the face in collision r rnames rindexzContactData.indexFzz$rN) __name__ __module__ __qualname____doc__rpropertyr rrr"rrrr sM0"       rrc2eZdZdZdZedZdZdZy) DistanceDatazH Data structure for holding information about a distance query. ct||_|d|j|d|ji|_|d|j d|d|j di|_|j|_y)z Initialize a DistanceData. Parameters ---------- names : list of str The names of the two objects in order. contact : fcl.DistanceResult The distance query result. rrN) rrr r r nearest_points_points min_distance _distance)rrresults rrzDistanceData.__init__]soZ Ah 58VYY? !Hf++A. !Hf++A.   ,,rc|jS)z Returns the distance between the two objects. Returns ------- distance : float The euclidean distance between the objects. )r0rs rdistancezDistanceData.distanceps~~rc |j|S)a' Returns the index of the closest face for the mesh with the given name. Parameters ---------- name : str The name of the target object. Returns ------- index : int The index of the face in collisoin. rr s rr"zDistanceData.index|r#rc |j|S)a The 3D point of closest distance on the mesh with the given name. Parameters ---------- name : str The name of the target object. Returns ------- point : (3,) float The closest point. )r.r s rrzDistanceData.points||D!!rN) r$r%r&r'rr(r3r"rr)rrr+r+Xs*-&   ""rr+cjeZdZdZdZddZdZdZ ddZddZ dd Z dd Z dd Z dd Z d ZdZy)CollisionManagerz( A mesh-mesh collision manager. ct tdi|_tjd|_tj |_|jjy)z; Initialize a mesh-mesh collision manager. Nz7No FCL Available! Please install the python-fcl librarycyNr)r)rrz+CollisionManager.__init__..sdr) fcl ValueError_objs collections defaultdict_namesDynamicAABBTreeCollisionManager_managersetuprs rrzCollisionManager.__init__sP ;VW W "--l; ;;=  rNcv|tjd}tj|tj}|jdk7r t d|j |}tj|ddddf|dddf}tj||}||jvr(|jj|j|||d|j|<||jt|<|jj||jj!|S)a Add an object to the collision manager. If an object with the given name is already in the manager, replace it. Parameters ---------- name : str An identifier for the object mesh : Trimesh object The geometry of the collision object transform : (4,4) float Homogeneous transform matrix for the object Ndtype)rFrFztransform must be (4,4)!)objgeom)npeye asanyarrayfloat32shaper= _get_fcl_objr< TransformCollisionObjectr>rCunregisterObjectrAidregisterObjectupdate)rr!mesh transformrKtos r add_objectzCollisionManager.add_objects $  q IMM)2::> ??f $78 8  & MM)BQBF+Yrr1u-= >   a ( 4::  MM * *4::d+; <#$d3 4 $ BtH $$Q' rcl||jvr|jj|j|d|jj|j|dt |jj |d}|j j |yt|d)z Delete an object from the collision manager. Parameters ---------- name : str The identifier for the object rJrK not in collision manager!N)r>rCrTrWrUpoprAr=)rr!geom_ids r remove_objectzCollisionManager.remove_objects 4::  MM * *4::d+;E+B C MM D!1%!8 9-f56G KKOOG $v%?@A Arc||jvr`|j|d}|j|ddddf|j|dddf|jj |yt |d)aD Set the transform for one of the manager's objects. This replaces the prior transform. Parameters ---------- name : str An identifier for the object already in the manager transform : (4,4) float A new homogeneous transform matrix for the object rJNrIr^)r> setRotationsetTranslationrCrWr=)rr!rYr[s r set_transformzCollisionManager.set_transforms 4::  4 'A MM)BQBF+ ,  Yrr1u- . MM  #v%?@A Arcb|tjd}|j|}tj|ddddf|dddf}tj ||}tj }|s|r+tj tjdd}|jj||tj|jj} t} g} |s|r|jjD]} | j} | |k(r | j } |j#| }|df}| | j k(rt%t'|}|r| j)||sn| j+t-|| |r|r| | | fS|r| | fS|r| | fS| S) aI Check a single object for collisions against all objects in the manager. Parameters ---------- mesh : Trimesh object The geometry of the collision object transform : (4,4) float Homogeneous transform matrix return_names : bool If true, a set is returned containing the names of all objects in collision with the object return_data : bool If true, a list of ContactData is returned as well Returns ------------ is_collision : bool True if a collision occurs and False otherwise names : set of str [OPTIONAL] The set of names of objects that collided with the provided one contacts : list of ContactData [OPTIONAL] All contacts detected NrFrI順Tnum_max_contactsenable_contactrequest __external)rLrMrQr<rRrS CollisionDataCollisionRequestrCcollidedefaultCollisionCallbackr1 is_collisionrcontactso1o2 _extract_nametuplereversedaddappendr)rrXrY return_names return_datarKrZr[cdatar1objs_in_collision contact_datarcgr!rs rin_collision_singlez$CollisionManager.in_collision_singles:  q I  & MM)BQBF+Yrr1u-= >   a (!!# ;%%,,fUYZE a(D(DE** E ; <<00ZZ: B))"-|,#!(5/2E%))$/ '' E7(CD1 K,l: : ,, , <' 'Mrc~tj}|s|r+tjtjdd}|jj |tj |j j}t}g}|s|r|j jD]{}|j|j|j|jf}|r#|jtt||sa|j!t#||}|r|r|||fS|r||fS|r||fS|S)a Check if any pair of objects in the manager collide with one another. Parameters ---------- return_names : bool If true, a set is returned containing the names of all pairs of objects in collision. return_data : bool If true, a list of ContactData is returned as well Returns ------- is_collision : bool True if a collision occurred between any pair of objects and False otherwise names : set of 2-tup The set of pairwise collisions. Each tuple contains two names in alphabetical order indicating that the two corresponding objects are in collision. contacts : list of ContactData All contacts detected rgTrhrk)r<rnrorCrprqr1rrrrsrvrtruryrwsortedrzr) rr{r|r}r1r~rrrs rin_collision_internalz&CollisionManager.in_collision_internalPs0!!# ;%%,,fUYZE eS%A%AB**E ; <<00++GJJ79K9KGJJ9WX%))%u *>? '' E7(CD 1 K,l: : ,, , <' 'Mrctj}|s|r+tjtjdd}|jj |j|tj |j j}t}g}|s|r|j jD]}d} |j|j|j|jf} | d8|j|j|j|jf} d} |r|j| |s| rtt| } |j!t#| ||r|r|||fS|r||fS|r||fS|S)a Check if any object from this manager collides with any object from another manager. Parameters ------------------- other_manager : CollisionManager Another collision manager object return_names : bool If true, a set is returned containing the names of all pairs of objects in collision. return_data : bool If true, a list of ContactData is returned as well Returns ------------- is_collision : bool True if a collision occurred between any pair of objects and False otherwise names : set of 2-tup The set of pairwise collisions. Each tuple contains two names (first from this manager, second from the other_manager) indicating that the two corresponding objects are in collision. contacts : list of ContactData All contacts detected rgTrhrkFr)r<rnrorCrprqr1rrrrsrvrtruryrwrxrzr) r other_managerr{r|r}r1r~rrreversers rin_collision_otherz#CollisionManager.in_collision_othersp8!!# ;%%,,fUYZE m44eS=Y=YZ**E ; <<00&&wzz2!// ;8#**7::6%33GJJ?E#G%))%0 %huo 6 '' E7(CD%1( K,l: : ,, , <' 'Mrcn|tjd}|j|}tj|ddddf|dddf}tj ||}tj tjd}|r=tj tjddtj}|jj||tj|jj} d\} } |s|r|jj} | |k(r|jj} |j!| } | df} | |jjk(rt#t%| } t | |j} |r|r| | | fS|r| | fS|r| | fS| S) a Get the minimum distance between a single object and any object in the manager. Parameters --------------- mesh : Trimesh object The geometry of the collision object transform : (4,4) float Homogeneous transform matrix for the object return_names : bool If true, return name of the closest object return_data : bool If true, a DistanceData object is returned as well Returns ------------- distance : float Min distance between mesh and any object in the manager name : str The name of the object in the manager that was closest data : DistanceData Extra data about the distance query NrFrITenable_signed_distanceenable_nearest_pointsrNNrm)rLrMrQr<rRrSr+DistanceRequestDistanceResultrCr3defaultDistanceCallbackr1r/rtrurvrwrx)rrXrY return_namer|rKrZr[ddatar3r!datarrs rmin_distance_singlez$CollisionManager.min_distance_singles6  q I  & MM)BQBF+Yrr1u-= >   a (  !4!4D!QR $$##*.t""$ E q%)D)DE<<,, d +BTz\\__%%b)D<(EU\\__$huo.u||4D ;T4' ' T> ! T> !Orctjtjd}|r=tjtjddtj}|||jvrt |d|j|d}|j j||j j||j j||tj|j j||j jn*|j j|tj|jj}d\}}|s|rt|j|jj|j|jj f}t||j}t#t%|}|r|r|||fS|r||fS|r||fS|S)a Get the minimum distance between objects in the manager. If name is provided, computes the minimum distance between the specified object and any other object in the manager. If name is None, computes the minimum distance between any pair of objects in the manager. Parameters ------------- name : str or None If provided, the identifier for the object already in the manager to compute distances from. If None, computes distances between all pairs of objects. return_names : bool If true, a 2-tuple is returned containing the names of the closest objects. return_data : bool If true, a DistanceData object is returned as well Returns ----------- distance : float Min distance between objects names : (2,) str The names of the closest objects data : DistanceData Extra data about the distance query Trrr^rJr)r<r+rrr>r=rCrTrWr3rrVr1r/rvrtrurwr) rr!r{r|rrJr3rrs rmin_distance_internalz&CollisionManager.min_distance_internals<  !4!4D!QR $$##*.+/""$ E  4::% D6)C!DEE**T"5)C MM * *3 / MM  % MM " "3s/J/J K MM ( ( - MM " MM " "5#*E*E F<<,, t ;""5< !OrcPtjtjd}|r=tjtjddtj}|jj |j|tj |jj}d\}}|s|rd}|j|jj|j|jjf}|dLd}|j|jj|j|jjf}t|} |rtt| } t| |j}|r|r|||fS|r||fS|r||fS|S)aI Get the minimum distance between any pair of objects, one in each manager. Parameters ---------- other_manager : CollisionManager Another collision manager object return_names : bool If true, a 2-tuple is returned containing the names of the closest objects. return_data : bool If true, a DistanceData object is returned as well Returns ----------- distance : float The min distance between a pair of objects, one from each manager. names : 2-tup of str A 2-tuple containing two names (first from this manager, second from the other_manager) indicating the two closest objects. data : DistanceData Extra data about the distance query TrrrFr)r<r+rrrCr3rr1r/rvrtrurwrx) rrr{r|rr3rrrdnamess rmin_distance_otherz#CollisionManager.min_distance_otherdso6  !4!4D!QR $$##*.+/""$ E }55uc>Y>YZ<<,, t ;G""5< !OrcN|jr t|}|St|}|S)a Get a BVH or Convex for a mesh. Parameters ------------- mesh : Trimesh Mesh to create BVH/Convex for Returns -------------- obj : fcl.BVHModel or fcl.Convex BVH/Convex object of source mesh ) is_convexmesh_to_convex mesh_to_BVH)rrXrJs rrQzCollisionManager._get_fcl_objs- >> &C d#C rc2|jt|S)aG Retrieve the name of an object from the manager by its CollisionObject, or return None if not found. Parameters ----------- geom : CollisionObject or BVHModel Input model Returns ------------ names : hashable Name of input geometry )rArU)rrKs rrvzCollisionManager._extract_names{{2d8$$rr:)NFF)FF)r$r%r&r'rr\rarerrrrrrrQrvr)rrr7r7s_ (TB&B*EJKZ4lBJDIHTN`CJ*%rr7ctj}|jt|jt|j |j |j |j|j|S)z Create a BVHModel object from a Trimesh object Parameters ----------- mesh : Trimesh Input geometry Returns ------------ bvh : fcl.BVHModel BVH of input geometry ) num_tris_ num_vertices_)verts triangles)r<BVHModel beginModellenfacesvertices addSubModelendModel)rXbvhs rrrsV ,,.CNNS_C LLN Jrc.tjdtjt|jdftj z|jfd}t j|jt||jS)z Create a Convex object from a Trimesh object Parameters ----------- mesh : Trimesh Input geometry Returns ------------ convex : fcl.Convex Convex of input geometry rIrrG)axis) rL concatenateonesrrint64r<Convexrflatten)rXfss rrrsd  RWWc$**oq) : :DJJGa B ::dmmSWbjjl ;;rct}i}|jjD]8}|j|\}}|j||j||||<:||fS)ad Create collision objects from a trimesh.Scene object. Parameters ------------ scene : trimesh.Scene Scene to create collision objects for Returns ------------ manager : CollisionManager CollisionManager for objects in scene objects: {node name: CollisionObject} Collision objects for nodes in scene )r!rXrY)r7graphnodes_geometryr\geometry)scenemanagerobjectsnodeTrs rscene_to_collisionrsm  GG **kk$' 8**ENN84+  + G r) r?numpyrLr< BaseExceptionrr+r7rrrr)rrrs` I I XC"C"Lo%o%d*<(c Cs6AA