From 5d7a8413803dff00c93e05db8377c57a6596fd9f Mon Sep 17 00:00:00 2001 From: Dion Moult Date: Wed, 13 May 2020 22:08:30 +1000 Subject: [PATCH] Rewrite IFCClash to get good results --- src/ifcblenderexport/ifcclash/collision.py | 715 +++++++++++++++++++++ src/ifcblenderexport/ifcclash/ifcclash | 172 ++--- 2 files changed, 764 insertions(+), 123 deletions(-) create mode 100644 src/ifcblenderexport/ifcclash/collision.py diff --git a/src/ifcblenderexport/ifcclash/collision.py b/src/ifcblenderexport/ifcclash/collision.py new file mode 100644 index 0000000000..fc17138f2e --- /dev/null +++ b/src/ifcblenderexport/ifcclash/collision.py @@ -0,0 +1,715 @@ +# 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.float32) + 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=100000, 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 diff --git a/src/ifcblenderexport/ifcclash/ifcclash b/src/ifcblenderexport/ifcclash/ifcclash index 478a10c079..0803390a67 100755 --- a/src/ifcblenderexport/ifcclash/ifcclash +++ b/src/ifcblenderexport/ifcclash/ifcclash @@ -1,14 +1,20 @@ #!python +import collision import ifcopenshell import ifcopenshell.geom import multiprocessing import numpy as np -import fcl import json import argparse import logging + +class Mesh: + faces: [] + vertices: [] + + class IfcClasher: def __init__(self, a_file, b_file, settings): self.settings = settings @@ -18,78 +24,52 @@ class IfcClasher: self.b = None self.a_file = a_file self.b_file = b_file - self.a_geoms = [] - self.b_geoms = [] - self.a_objs = [] - self.b_objs = [] - self.a_global_ids = [] - self.b_global_ids = [] - self.a_geom_to_global_id = {} - self.b_geom_to_global_id = {} - self.a_manager = None - self.b_manager = None - self.clashes = [] - self.meshes = {} + self.clashes = {} + self.a_meshes = {} + self.b_meshes = {} def clash(self): - self.load_files() - for ab in ('a', 'b'): - if self.settings.should_use_legacy: - self.create_collision_objects_legacy(ab) - else: - self.create_collision_objects(ab) - self.create_manager(ab) - self.create_data_maps(ab) + for ab in ['a', 'b']: + self.settings.logger.info(f'Loading file {ab} ...') + setattr(self, ab, ifcopenshell.open(getattr(self, f'{ab}_file'))) + self.settings.logger.info(f'Purging unnecessary elements {ab} ...') + self.purge_elements(ab) + self.settings.logger.info(f'Creating collision manager {ab} ...') + setattr(self, f'{ab}_cm', collision.CollisionManager()) + self.add_collision_objects(ab) + results = self.a_cm.in_collision_other(self.b_cm, return_data=True) - self.settings.logger.info('Colliding models') - req = fcl.CollisionRequest(num_max_contacts=1, enable_contact=True) - rdata = fcl.CollisionData(request = req) - self.a_manager.collide(self.b_manager, rdata, fcl.defaultCollisionCallback) - for contact in rdata.result.contacts: - if contact.penetration_depth < self.tolerance: - continue - a_global_id = self.a_geom_to_global_id[id(contact.o1)] - b_global_id = self.b_geom_to_global_id[id(contact.o2)] + if not results[0]: + return + + for contact in results[1]: + a_global_id, b_global_id = contact.names a = self.a.by_guid(a_global_id) b = self.b.by_guid(b_global_id) - self.clashes.append({ + if contact.raw.penetration_depth < self.tolerance: + continue + self.clashes[f'{a_global_id}-{b_global_id}'] = { 'a_global_id': a_global_id, 'b_global_id': b_global_id, 'a_ifc_class': a.is_a(), 'b_ifc_class': b.is_a(), 'a_name': a.Name, 'b_name': b.Name, - 'normal': list(contact.normal), - 'position': list(contact.pos), - 'penetration_depth': contact.penetration_depth - }) + 'normal': list(contact.raw.normal), + 'position': list(contact.raw.pos), + 'penetration_depth': contact.raw.penetration_depth + } - def load_files(self): - self.settings.logger.info('Loading files') - self.a = ifcopenshell.open(self.a_file) - self.b = ifcopenshell.open(self.b_file) + def purge_elements(self, ab): + # TODO: more filtering abilities + for element in getattr(self, ab).by_type('IfcSpace'): + getattr(self, ab).remove(element) - def create_collision_objects_legacy(self, ab): - self.settings.logger.info('Creating legacy collision data for {}'.format(ab)) - elements = getattr(self, ab).by_type('IfcElement') - for element in elements: - try: - shape = ifcopenshell.geom.create_shape(self.geom_settings, element) - except: - self.settings.logger.error('Failed to generate shape for {}'.format(element)) - continue - mesh = self.create_mesh(element, shape) - transform = self.get_transform(self.get_local_placement(element.ObjectPlacement)) - getattr(self, '{}_geoms'.format(ab)).append(mesh) - getattr(self, '{}_objs'.format(ab)).append(fcl.CollisionObject(mesh, transform)) - getattr(self, '{}_global_ids'.format(ab)).append(element.GlobalId) - - def create_collision_objects(self, ab): + def add_collision_objects(self, ab): self.settings.logger.info('Creating collision data for {}'.format(ab)) iterator = ifcopenshell.geom.iterator(self.geom_settings, getattr(self, ab), multiprocessing.cpu_count()) valid_file = iterator.initialize() if not valid_file: - self.create_collision_objects_legacy() return False old_progress = -1 while True: @@ -97,21 +77,21 @@ class IfcClasher: if progress > old_progress: print("\r[" + "#" * progress + " " * (50 - progress) + "]", end="") old_progress = progress - self.create_collision_object(ab, iterator.get()) + self.add_collision_object(ab, iterator.get()) if not iterator.next(): break - def create_collision_object(self, ab, shape): + def add_collision_object(self, ab, shape): if shape is None: return element = getattr(self, ab).by_id(shape.guid) self.settings.logger.info('Creating object {}'.format(element)) mesh_name = f'mesh-{shape.geometry.id}' - if mesh_name in self.meshes: - mesh = self.meshes[mesh_name] + if mesh_name in getattr(self, f'{ab}_meshes'): + mesh = getattr(self, f'{ab}_meshes')[mesh_name] else: - mesh = self.create_mesh(element, shape) - self.meshes[mesh_name] = mesh + mesh = self.create_mesh(shape) + getattr(self, f'{ab}_meshes')[mesh_name] = mesh m = shape.transformation.matrix.data mat = np.array( @@ -123,72 +103,18 @@ class IfcClasher: ] ) mat.transpose() - transform = self.get_transform(mat) - getattr(self, '{}_geoms'.format(ab)).append(mesh) - getattr(self, '{}_objs'.format(ab)).append(fcl.CollisionObject(mesh, transform)) - getattr(self, '{}_global_ids'.format(ab)).append(element.GlobalId) + getattr(self, f'{ab}_cm').add_object(shape.guid, mesh, mat) - def create_manager(self, ab): - name = '{}_manager'.format(ab) - setattr(self, name, fcl.DynamicAABBTreeCollisionManager()) - getattr(self, name).registerObjects(getattr(self, '{}_objs'.format(ab))) - getattr(self, name).setup() - - def create_data_maps(self, ab): - setattr(self, '{}_geom_to_global_id'.format(ab), - { - id(geom) : global_id - for geom, global_id - in zip( - getattr(self, '{}_geoms'.format(ab)), - getattr(self, '{}_global_ids'.format(ab)) - ) - } - ) - - def get_transform(self, m): - R = np.array( - [ - [m[0][0], m[1][0], m[2][0]], - [m[0][1], m[1][1], m[2][1]], - [m[0][2], m[1][2], m[2][2]] - ] - ) - T = np.array([m[0][3], m[1][3], m[2][3]]) - return fcl.Transform(R, T) - - def create_mesh(self, element, shape): + def create_mesh(self, shape): f = shape.geometry.faces v = shape.geometry.verts - vertices = np.array([[v[i], v[i + 1], v[i + 2]] + mesh = Mesh() + mesh.vertices = np.array([[v[i], v[i + 1], v[i + 2]] for i in range(0, len(v), 3)]) - faces = np.array([[f[i], f[i + 1], f[i + 2]] + mesh.faces = np.array([[f[i], f[i + 1], f[i + 2]] for i in range(0, len(f), 3)]) - m = fcl.BVHModel() - m.beginModel(len(vertices), len(faces)) - m.addSubModel(vertices, faces) - m.endModel() - return m + return mesh - def get_local_placement(self, plc): - if plc.PlacementRelTo is None: - parent = np.eye(4) - else: - parent = self.get_local_placement(plc.PlacementRelTo) - return np.dot(self.get_axis2placement(plc.RelativePlacement), parent) - - def a2p(self, o, z, x): - y = np.cross(z, x) - r = np.eye(4) - r[:-1,:-1] = x,y,z - r[-1,:-1] = o - return r.T - - def get_axis2placement(self, plc): - z = np.array(plc.Axis.DirectionRatios if plc.Axis else (0,0,1)) - x = np.array(plc.RefDirection.DirectionRatios if plc.RefDirection else (1,0,0)) - o = plc.Location.Coordinates - return self.a2p(o,z,x) parser = argparse.ArgumentParser( description='Clashes geometry between two IFC files') @@ -231,4 +157,4 @@ ifc_clasher = IfcClasher(args.a, args.b, settings) ifc_clasher.clash() with open(args.output, 'w', encoding='utf-8') as clashes_file: - json.dump(ifc_clasher.clashes, clashes_file, indent=4) + json.dump(list(ifc_clasher.clashes.values()), clashes_file, indent=4)