l0j6XddlZddlZ ddlZn #e$rdZYnwxYwGddZGddZGddZdZd Z d Z dS) Nc`eZdZdZdZedZedZedZdZ dS) ContactDatazK Data structure for holding information about a collision contact. ct||_|d|j|d|ji|_|j|_|j|_|j |_ dS)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 T/home/wildlama/miniconda3/envs/lam/lib/python3.11/site-packages/trimesh/collision.py__init__zContactData.__init__sMZZ Ah E!HgjA ~ k / 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.indexFz$rN) __name__ __module__ __qualname____doc__rpropertyr rrr"rrrr s000"  X   X   X      rrc:eZdZdZdZedZdZdZdS) DistanceDatazH Data structure for holding information about a distance query. ct||_|d|j|d|ji|_|d|jd|d|jdi|_|j|_dS)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__]shZZ Ah 58VY? !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+Xsf---&  X    """"""rr+cleZdZdZdZddZdZdZ ddZdd Z dd Z dd Z dd Z dd Z dZdZdS)CollisionManagerz( A mesh-mesh collision manager. cttdi|_tjd|_tj|_|jdS)z; Initialize a mesh-mesh collision manager. Nz7No FCL Available! Please install the python-fcl librarycdSNr)r)rrz+CollisionManager.__init__..sdr) fcl ValueError_objs collections defaultdict_namesDynamicAABBTreeCollisionManager_managersetuprs rrzCollisionManager.__init__s_ ;VWW W "-ll;; ;==  rNcZ|tjd}tj|tj}|jdkrt d||}tj|ddddf|dddf}tj ||}||j vr%|j |j |||d|j |<||j t|<|j ||j |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 IM)2:>>> ?f $ $788 8  && M)BQBF+Yrr1u-= > >  a ( ( 4:   M * *4:d+; < < <#$d33 4 $ BtHH $$Q''' rcv||jvr|j|j|d|j|j|dt |j|d}|j|dSt|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:   M * *4:d+;E+B C C C M D!1%!8 9 9 9--f566G KOOG $ $ $ $ $@@@AA Arc||jvro|j|d}||ddddf||dddf|j|dSt |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- . . . M  # # # # #@@@AA ArFcP|tjd}||}tj|ddddf|dddf}tj||}tj}|s|r)tjtjdd}|j ||tj |j j } t} g} |s|r|j jD]} | j} | |kr| j} || }|df}| | jkrt%t'|}|r| ||r#| 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  && M)BQBF+Yrr1u-= > >  a ( (!##  ; %,fUYZZZE a(DEEE* EE  E; E <0 E EZ:: B))"--|,##!(5//22E0%))$///E '' E7(C(CDDD  K ,l: :  ,, ,  <' 'Mrcltj}|s|r)tjtjdd}|j|tj|jj}t}g}|s|r|jj D]}| |j | |j f}|r/| tt||r#|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_internalPso0!##  ; %,fUYZZZE eS%ABBB*EE  E; E <0 E E++GJ779K9KGJ9W9WX@%))%u *>*>???E '' E7(C(CDDD  K ,l: :  ,, ,  <' 'Mrctj}|s|r)tjtjdd}|j|j|tj|jj}t}g}|s|r|jj D]}d} | |j | |j f} | d6| |j | |j f} d} |r| | |rA| rtt| } |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_others8!##  ; %,fUYZZZE m4eS=YZZZ*EE  E; E <0 E E&&wz22!// ;;8#**7:66%33GJ??E#G1%))%000E7 %huoo 6 6 '' E7(C(CDDD  K ,l: :  ,, ,  <' 'Mrc2|tjd}||}tj|ddddf|dddf}tj||}tjtjd}|r:tjtjddtj}|j ||tj |j j } d\} } |s|rx|j j} | |kr |j j} || } | df} | |j jkrt#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  && M)BQBF+Yrr1u-= > >  a ( ( !4D!Q!Q!QRR  $#*.t"$$ E q%)DEEE<, d  5+ 5BTzz\_%%b))D<(EU\_$$huoo..u|44D  ; T4' '  T> !  T> !OrcHtjtjd}|r:tjtjddtj}|j|tj|jj}d\}}|s|ro| |jj | |jj f}t||j}tt|}|r|r|||fS|r||fS|r||fS|S)aW Get the minimum distance between any pair of objects in the manager. Parameters ------------- 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 any two managed objects names : (2,) str The names of the closest objects data : DistanceData Extra data about the distance query Trrr)r<r+rrrCr3rr1r/rvrtrurwr)rr{r|rr3rrs rmin_distance_internalz&CollisionManager.min_distance_internals@* !4D!Q!Q!QRR  $#*.+/"$$ E uc&ABBB<, t  ); )""5 !Orc tjtjd}|r:tjtjddtj}|j|j|tj|jj}d\}}|s|rd}| |jj | |jj f}|d@d}| |jj | |jj f}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_otherIs6 !4D!Q!Q!QRR  $#*.+/"$$ E }5uc>YZZZ<, t  6; 6G""5 !OrcR|jrt|}nt|}|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/ > $ &&CCd##C rc6|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{2d88$$rr:)NFF)FF)r$r%r&r'rr\rarerrrrrrrQrvr)rrr7r7s    ((((TBBB&BBB*EJKKKKZ4444lBBBBJDIHHHHT3333jCCCCJ*%%%%%rr7c tj}|t|jt|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 rrrsc ,..CNNS__C >>LLNNN Jrctjdtjt|jdftjz|jfd}t j|jt|| S)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 rrrso  RWc$*ooq) : : : :DJGa   B :dmSWWbjjll ; ;;rct}i}|jjD]8}|j|\}}|||j||||<9||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_collisionrss   GG *  k$' 8**EN84+    G r) r?numpyrLr< BaseExceptionrr+r7rrrr)rrrsJJJJ CCCI I I I I I I I XC"C"C"C"C"C"C"C"LT%T%T%T%T%T%T%T%n*<<<(s