# This code is taken from the trimesh project at https://github.com/mikedh/trimesh/blob/master/trimesh/collision.py

# License MIT https://github.com/mikedh/trimesh/blob/master/LICENSE.md

import numpy as np

import collections

try:

# pip install python-fcl

import fcl

except BaseException:

fcl = None

class ContactData(object):

"""

Data structure for holding information about a collision contact.

"""

def __init__(self, names, contact):

"""

Initialize a ContactData.

Parameters

----------

names : list of str

The names of the two objects in order.

contact : fcl.Contact

The contact in question.

"""

self.names = names

self._inds = {names[0]: contact.b1, names[1]: contact.b2}

self._point = contact.pos

self.raw = contact

@property

def point(self):

"""

The 3D point of intersection for this contact.

Returns

-------

point : (3,) float

The intersection point.

"""

return self._point

def index(self, name):

"""

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 collison

"""

return self._inds[name]

class DistanceData(object):

"""

Data structure for holding information about a distance query.

"""

def __init__(self, names, result):

"""

Initialize a DistanceData.

Parameters

----------

names : list of str

The names of the two objects in order.

contact : fcl.DistanceResult

The distance query result.

"""

self.names = set(names)

self._inds = {names[0]: result.b1, names[1]: result.b2}

self._points = {names[0]: result.nearest_points[0], names[1]: result.nearest_points[1]}

self._distance = result.min_distance

@property

def distance(self):

"""

Returns the distance between the two objects.

Returns

-------

distance : float

The euclidean distance between the objects.

"""

return self._distance

def index(self, name):

"""

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.

"""

return self._inds[name]

def point(self, name):

"""

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.

"""

return self._points[name]

class CollisionManager(object):

"""

A mesh-mesh collision manager.

"""

def __init__(self):

"""

Initialize a mesh-mesh collision manager.

"""

if fcl is None:

raise ValueError("No FCL Available!")

# {name: {geom:, obj}}

self._objs = {}

# {id(bvh) : str, name}

# unpopulated values will return None

self._names = collections.defaultdict(lambda: None)

# cache BVH objects

# {mesh.md5(): fcl.BVHModel object}

self._bvh = {}

self._manager = fcl.DynamicAABBTreeCollisionManager()

self._manager.setup()

def add_object(self, name, mesh, transform=None):

"""

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

"""

# if no transform passed, assume identity transform

if transform is None:

transform = np.eye(4)

transform = np.asanyarray(transform, dtype=np.float64)

if transform.shape != (4, 4):

raise ValueError("transform must be (4,4)!")

# create or recall from cache BVH

bvh = self._get_BVH(mesh)

# create the FCL transform from (4,4) matrix

t = fcl.Transform(transform[:3, :3], transform[:3, 3])

o = fcl.CollisionObject(bvh, t)

# Add collision object to set

if name in self._objs:

self._manager.unregisterObject(self._objs[name])

self._objs[name] = {"obj": o, "geom": bvh}

# store the name of the geometry

self._names[id(bvh)] = name

self._manager.registerObject(o)

self._manager.update()

return o

def remove_object(self, name):

"""

Delete an object from the collision manager.

Parameters

----------

name : str

The identifier for the object

"""

if name in self._objs:

self._manager.unregisterObject(self._objs[name]["obj"])

self._manager.update(self._objs[name]["obj"])

# remove objects from _objs

geom_id = id(self._objs.pop(name)["geom"])

# remove names

self._names.pop(geom_id)

else:

raise ValueError("{} not in collision manager!".format(name))

def set_transform(self, name, transform):

"""

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

"""

if name in self._objs:

o = self._objs[name]["obj"]

o.setRotation(transform[:3, :3])

o.setTranslation(transform[:3, 3])

self._manager.update(o)

else:

raise ValueError("{} not in collision manager!".format(name))

def in_collision_single(self, mesh, transform=None, return_names=False, return_data=False):

"""

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

"""

if transform is None:

transform = np.eye(4)

# Create FCL data

b = self._get_BVH(mesh)

t = fcl.Transform(transform[:3, :3], transform[:3, 3])

o = fcl.CollisionObject(b, t)

# Collide with manager's objects

cdata = fcl.CollisionData()

if return_names or return_data:

cdata = fcl.CollisionData(request=fcl.CollisionRequest(num_max_contacts=100000, enable_contact=True))

self._manager.collide(o, cdata, fcl.defaultCollisionCallback)

result = cdata.result.is_collision

# If we want to return the objects that were collision, collect them.

objs_in_collision = set()

contact_data = []

if return_names or return_data:

for contact in cdata.result.contacts:

cg = contact.o1

if cg == b:

cg = contact.o2

name = self._extract_name(cg)

names = (name, "__external")

if cg == contact.o2:

names = reversed(names)

if return_names:

objs_in_collision.add(name)

if return_data:

contact_data.append(ContactData(names, contact))

if return_names and return_data:

return result, objs_in_collision, contact_data

elif return_names:

return result, objs_in_collision

elif return_data:

return result, contact_data

else:

return result

def in_collision_internal(self, return_names=False, return_data=False):

"""

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

"""

cdata = fcl.CollisionData()

if return_names or return_data:

cdata = fcl.CollisionData(request=fcl.CollisionRequest(num_max_contacts=1000000, enable_contact=True))

self._manager.collide(cdata, fcl.defaultCollisionCallback)

result = cdata.result.is_collision

objs_in_collision = set()

contact_data = []

if return_names or return_data:

for contact in cdata.result.contacts:

names = (self._extract_name(contact.o1), self._extract_name(contact.o2))

if return_names:

objs_in_collision.add(tuple(sorted(names)))

if return_data:

contact_data.append(ContactData(names, contact))

if return_names and return_data:

return result, objs_in_collision, contact_data

elif return_names:

return result, objs_in_collision

elif return_data:

return result, contact_data

else:

return result

def in_collision_other(self, other_manager, return_names=False, return_data=False):

"""

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

"""

cdata = fcl.CollisionData()

if return_names or return_data:

cdata = fcl.CollisionData(request=fcl.CollisionRequest(num_max_contacts=100000, enable_contact=True))

self._manager.collide(other_manager._manager, cdata, fcl.defaultCollisionCallback)

result = cdata.result.is_collision

objs_in_collision = set()

contact_data = []

if return_names or return_data:

for contact in cdata.result.contacts:

reverse = False

names = (self._extract_name(contact.o1), other_manager._extract_name(contact.o2))

if names[0] is None:

names = (self._extract_name(contact.o2), other_manager._extract_name(contact.o1))

reverse = True

if return_names:

objs_in_collision.add(names)

if return_data:

if reverse:

names = reversed(names)

contact_data.append(ContactData(names, contact))

if return_names and return_data:

return result, objs_in_collision, contact_data

elif return_names:

return result, objs_in_collision

elif return_data:

return result, contact_data

else:

return result

def min_distance_single(self, mesh, transform=None, return_name=False, return_data=False):

"""

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

"""

if transform is None:

transform = np.eye(4)

# Create FCL data

b = self._get_BVH(mesh)

t = fcl.Transform(transform[:3, :3], transform[:3, 3])

o = fcl.CollisionObject(b, t)

# Collide with manager's objects

ddata = fcl.DistanceData()

if return_data:

ddata = fcl.DistanceData(fcl.DistanceRequest(enable_nearest_points=True), fcl.DistanceResult())

self._manager.distance(o, ddata, fcl.defaultDistanceCallback)

distance = ddata.result.min_distance

# If we want to return the objects that were collision, collect them.

name, data = None, None

if return_name or return_data:

cg = ddata.result.o1

if cg == b:

cg = ddata.result.o2

name = self._extract_name(cg)

names = (name, "__external")

if cg == ddata.result.o2:

names = reversed(names)

data = DistanceData(names, ddata.result)

if return_name and return_data:

return distance, name, data

elif return_name:

return distance, name

elif return_data:

return distance, data

else:

return distance

def min_distance_internal(self, return_names=False, return_data=False):

"""

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

"""

ddata = fcl.DistanceData()

if return_data:

ddata = fcl.DistanceData(fcl.DistanceRequest(enable_nearest_points=True), fcl.DistanceResult())

self._manager.distance(ddata, fcl.defaultDistanceCallback)

distance = ddata.result.min_distance

names, data = None, None

if return_names or return_data:

names = (self._extract_name(ddata.result.o1), self._extract_name(ddata.result.o2))

data = DistanceData(names, ddata.result)

names = tuple(sorted(names))

if return_names and return_data:

return distance, names, data

elif return_names:

return distance, names

elif return_data:

return distance, data

else:

return distance

def min_distance_other(self, other_manager, return_names=False, return_data=False):

"""

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

"""

ddata = fcl.DistanceData()

if return_data:

ddata = fcl.DistanceData(fcl.DistanceRequest(enable_nearest_points=True), fcl.DistanceResult())

self._manager.distance(other_manager._manager, ddata, fcl.defaultDistanceCallback)

distance = ddata.result.min_distance

names, data = None, None

if return_names or return_data:

reverse = False

names = (self._extract_name(ddata.result.o1), other_manager._extract_name(ddata.result.o2))

if names[0] is None:

reverse = True

names = (self._extract_name(ddata.result.o2), other_manager._extract_name(ddata.result.o1))

dnames = tuple(names)

if reverse:

dnames = reversed(dnames)

data = DistanceData(dnames, ddata.result)

if return_names and return_data:

return distance, names, data

elif return_names:

return distance, names

elif return_data:

return distance, data

else:

return distance

def _get_BVH(self, mesh):

"""

Get a BVH for a mesh.

Parameters

-------------

mesh : Trimesh

Mesh to create BVH for

Returns

--------------

bvh : fcl.BVHModel

BVH object of source mesh

"""

bvh = mesh_to_BVH(mesh)

return bvh

def _extract_name(self, geom):

"""

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

"""

return self._names[id(geom)]

def mesh_to_BVH(mesh):

"""

Create a BVHModel object from a Trimesh object

Parameters

-----------

mesh : Trimesh

Input geometry

Returns

------------

bvh : fcl.BVHModel

BVH of input geometry

"""

bvh = fcl.BVHModel()

bvh.beginModel(num_tris_=len(mesh.faces), num_vertices_=len(mesh.vertices))

bvh.addSubModel(verts=mesh.vertices, triangles=mesh.faces)

bvh.endModel()

return bvh

def scene_to_collision(scene):

"""

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

"""

manager = CollisionManager()

objects = {}

for node in scene.graph.nodes_geometry:

T, geometry = scene.graph[node]

objects[node] = manager.add_object(name=node, mesh=scene.geometry[geometry], transform=T)

return manager, objects