/********************************************************************************
* *
* This file is part of IfcOpenShell. *
* *
* IfcOpenShell is free software: you can redistribute it and/or modify *
* it under the terms of the Lesser GNU General Public License as published by *
* the Free Software Foundation, either version 3.0 of the License, or *
* (at your option) any later version. *
* *
* IfcOpenShell is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* Lesser GNU General Public License for more details. *
* *
* You should have received a copy of the Lesser GNU General Public License *
* along with this program. If not, see . *
* *
********************************************************************************/
#ifndef IFCGEOMTREE_H
#define IFCGEOMTREE_H
#include "../ifcparse/IfcFile.h"
#include "../ifcgeom_schema_agnostic/IfcGeomElement.h"
#include "../ifcgeom_schema_agnostic/IfcGeomIterator.h"
#include "../ifcgeom_schema_agnostic/IfcGeomMaterial.h"
#include "../ifcgeom_schema_agnostic/Kernel.h"
#include "../ifcgeom_schema_agnostic/base_utils.h"
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include "clash_utils.h"
#include "H5Cpp.h"
namespace IfcGeom {
struct ray_intersection_result {
double distance;
int style_index;
IfcUtil::IfcBaseEntity* instance;
std::array position;
std::array normal;
double ray_distance;
double dot_product;
};
struct clash {
int clash_type; // 0 = protrusion, 1 = pierce, 2 = collision, 3 = clearance
IfcUtil::IfcBaseClass* a;
IfcUtil::IfcBaseClass* b;
double distance;
std::array p1;
std::array p2;
};
struct h5_shape {
std::vector verts;
std::vector faces;
std::vector materials;
std::vector material_ids;
};
struct chunked_model {
std::vector> materials;
std::vector elements;
};
namespace {
// Approximates the distance `other` protrudes into `volume` by finding the
// max face-vertex distance for every face, and taking the minimal value of
// those. Note that this uses the internal `BRepExtrema_ExtPF` which only
// returns solutions whose when the vertex projected onto the face is contained
// within the face boundaries. In case of concave `volume` this is desirable.
double max_distance_inside(const TopoDS_Shape& volume, const TopoDS_Shape& other) {
TopExp_Explorer exp_v(volume.Reversed(), TopAbs_FACE);
double min_face_vertex_distance = std::numeric_limits::infinity();
for (; exp_v.More(); exp_v.Next()) {
const TopoDS_Face& f = TopoDS::Face(exp_v.Current());
BRepExtrema_ExtPF epf;
epf.Initialize(f, Extrema_ExtFlag_MIN);
double face_vertex_distance = 0.;
TopExp_Explorer exp_o(other, TopAbs_VERTEX);
for (; exp_o.More(); exp_o.Next()) {
const TopoDS_Vertex& v = TopoDS::Vertex(exp_o.Current());
epf.Perform(v, f);
if (epf.IsDone() && epf.NbExt() == 1) {
double d = epf.SquareDistance(1);
if (d > face_vertex_distance) {
face_vertex_distance = d;
}
}
}
if (face_vertex_distance < min_face_vertex_distance) {
min_face_vertex_distance = face_vertex_distance;
}
}
if (min_face_vertex_distance == std::numeric_limits::infinity()) {
return -1.;
} else {
return std::sqrt(min_face_vertex_distance);
}
}
}
namespace impl {
template
class tree {
bool is_shape_manifold(const TopoDS_Shape& s) {
TopExp_Explorer exp(s, TopAbs_SHELL);
bool is_closed = false;
while (exp.More()) {
is_closed = true;
TopoDS_Shell shell = TopoDS::Shell(exp.Current());
TopTools_IndexedDataMapOfShapeListOfShape edgeFaceMap;
TopExp::MapShapesAndAncestors(s, TopAbs_EDGE, TopAbs_FACE, edgeFaceMap);
for (int i = 1; i <= edgeFaceMap.Extent(); ++i) {
if (edgeFaceMap(i).Extent() < 2) {
// This edge is not shared by two faces, indicating a potential opening
return false;
}
}
exp.Next();
}
return is_closed;
}
bool is_point_in_shape(
const gp_Pnt& v,
const opencascade::handle>& bvh,
const std::vector>& tris,
const std::vector& verts,
// In the case of "touching" rays, let's check again!
bool should_check_again = false
) const {
ray v_ray;
v_ray.origin[0] = v.X();
v_ray.origin[1] = v.Y();
v_ray.origin[2] = v.Z();
if (should_check_again) {
// The first check may be incorrect if it intersects
// exactly between triangles or on edges of triangles.
// A second check is used to "double check" the results.
// The second check is perpendicular because AEC objects
// are typically symmetrical along an axis, and goes down
// because there's typically less stuff down there.
v_ray.dir[0] = 0.0f;
v_ray.dir[1] = 0.0f;
v_ray.dir[2] = -1.0f;
v_ray.dir_inv[0] = INFINITY; // 1.0f/dir[0]
v_ray.dir_inv[1] = INFINITY; // 1.0f/dir[1]
v_ray.dir_inv[2] = -1.0f; // 1.0f/dir[2]
} else {
v_ray.dir[0] = 1.0f;
v_ray.dir[1] = 0.0f;
v_ray.dir[2] = 0.0f;
v_ray.dir_inv[0] = 1.0f; // 1.0f/dir[0]
v_ray.dir_inv[1] = INFINITY; // 1.0f/dir[1]
v_ray.dir_inv[2] = INFINITY; // 1.0f/dir[2]
}
gp_Vec ray_origin(v.X(), v.Y(), v.Z());
gp_Vec ray_vector(v_ray.dir[0], v_ray.dir[1], v_ray.dir[2]);
int total_intersections = 0;
std::stack stack;
stack.push(0);
while ( ! stack.empty()) {
int i = stack.top();
stack.pop();
BVH_TreeBase::BVH_VecNt min_point = bvh->MinPoint(i);
BVH_TreeBase::BVH_VecNt max_point = bvh->MaxPoint(i);
box box;
// + 1e-5 for tolerance
box.corners[0][0] = min_point[0] - 1e-5;
box.corners[0][1] = min_point[1] - 1e-5;
box.corners[0][2] = min_point[2] - 1e-5;
box.corners[1][0] = max_point[0] + 1e-5;
box.corners[1][1] = max_point[1] + 1e-5;
box.corners[1][2] = max_point[2] + 1e-5;
/*
std::cout << "Ray "
<< v_ray.origin[0] << " "
<< v_ray.origin[1] << " "
<< v_ray.origin[2] << " "
<< std::endl;
std::cout << "Box "
<< min_point[0] << " "
<< min_point[1] << " "
<< min_point[2] << " "
<< max_point[0] << " "
<< max_point[1] << " "
<< max_point[2] << " "
<< std::endl;
*/
if ( ! is_intersect_ray_box(&v_ray, &box)) {
continue;
}
//std::cout << "Ray hits box" << std::endl;
if (bvh->IsOuter(i)) {
//std::cout << "Ray hits leaf" << std::endl;
// Do ray triangle check.
for (int j=bvh->BegPrimitive(i); j<=bvh->EndPrimitive(i); ++j) {
const std::array& tri = tris[j];
gp_Vec ta(verts[tri[0]].XYZ());
gp_Vec tb(verts[tri[1]].XYZ());
gp_Vec tc(verts[tri[2]].XYZ());
/*
std::cout << "ray origin " << ray_origin.X() << " " << ray_origin.Y() << " " << ray_origin.Z() << std::endl;
std::cout << "inside-tri " << ta.X() << " " << ta.Y() << " " << ta.Z() << std::endl;
std::cout << "inside-tri " << tb.X() << " " << tb.Y() << " " << tb.Z() << std::endl;
std::cout << "inside-tri " << tc.X() << " " << tc.Y() << " " << tc.Z() << std::endl;
*/
double at, au, av;
if (intersectRayTriangle(ray_origin, ray_vector, ta, tb, tc, at, au, av, false)) {
// At is a signed intersection distance (positive is along +ray_vector)
if (at > -1e-5) {
total_intersections++;
}
}
}
} else {
stack.push(bvh->Child<0>(i));
stack.push(bvh->Child<1>(i));
}
}
return total_intersections % 2 != 0;
}
std::tuple<
double,
std::array,
std::array
> pierce_shape(
const gp_Vec& e1,
const gp_Vec& e2,
const opencascade::handle>& bvh,
const std::vector>& tris,
const std::vector& verts,
const std::vector& normals
) const {
const gp_Vec& ray_origin = e1;
gp_Vec ray_vector = e2 - e1;
double edge_length = ray_vector.Magnitude();
std::array min_int;
std::array max_int;
ray_vector.Normalize();
ray v_ray;
v_ray.origin[0] = ray_origin.X();
v_ray.origin[1] = ray_origin.Y();
v_ray.origin[2] = ray_origin.Z();
v_ray.dir[0] = ray_vector.X();
v_ray.dir[1] = ray_vector.Y();
v_ray.dir[2] = ray_vector.Z();
v_ray.dir_inv[0] = 1.0f / ray_vector.X();
v_ray.dir_inv[1] = 1.0f / ray_vector.Y();
v_ray.dir_inv[2] = 1.0f / ray_vector.Z();
double min_distance = std::numeric_limits::infinity();
double max_distance = -std::numeric_limits::infinity();
std::stack stack;
stack.push(0);
while ( ! stack.empty()) {
int i = stack.top();
stack.pop();
BVH_TreeBase::BVH_VecNt min_point = bvh->MinPoint(i);
BVH_TreeBase::BVH_VecNt max_point = bvh->MaxPoint(i);
box box;
// + 1e-5 for tolerance
box.corners[0][0] = min_point[0] - 1e-5;
box.corners[0][1] = min_point[1] - 1e-5;
box.corners[0][2] = min_point[2] - 1e-5;
box.corners[1][0] = max_point[0] + 1e-5;
box.corners[1][1] = max_point[1] + 1e-5;
box.corners[1][2] = max_point[2] + 1e-5;
if ( ! is_intersect_ray_box(&v_ray, &box)) {
continue;
}
if (bvh->IsOuter(i)) {
// Do ray triangle check.
for (int j=bvh->BegPrimitive(i); j<=bvh->EndPrimitive(i); ++j) {
const std::array& tri = tris[j];
const gp_Vec& normal = normals[j];
if (std::abs(normal.Dot(ray_vector)) < 1e-3) {
continue; // This ray is coplanar to the triangle
}
gp_Vec ta(verts[tri[0]].XYZ());
gp_Vec tb(verts[tri[1]].XYZ());
gp_Vec tc(verts[tri[2]].XYZ());
double at, au, av;
// Do box check first?
if (intersectRayTriangle(ray_origin, ray_vector, ta, tb, tc, at, au, av, false)) {
// At is a signed intersection distance (positive is along +ray_vector)
if (at > 0 && at < edge_length) {
double aw = 1.0f - au - av; // Barycentric coordinate for ta
gp_Vec int_vec = aw * ta + au * tb + av * tc; // Intersection point
if (
is_point_on_line(int_vec, ta, tb)
|| is_point_on_line(int_vec, ta, tc)
|| is_point_on_line(int_vec, tb, tc)
|| (ta - int_vec).Magnitude() < 1e-4
|| (tb - int_vec).Magnitude() < 1e-4
|| (tc - int_vec).Magnitude() < 1e-4
) {
continue;
}
if (at < min_distance) {
min_distance = at;
min_int = {int_vec.X(), int_vec.Y(), int_vec.Z()};
}
if (at > max_distance) {
max_distance = at;
max_int = {int_vec.X(), int_vec.Y(), int_vec.Z()};
}
}
}
}
} else {
stack.push(bvh->Child<0>(i));
stack.push(bvh->Child<1>(i));
}
}
if (min_distance == std::numeric_limits::infinity()) {
return std::make_tuple(-1, min_int, max_int);
}
return std::make_tuple(max_distance - min_distance, min_int, max_int);
}
bool is_point_on_line(const gp_Pnt& point, const gp_Pnt& lineStart, const gp_Pnt& lineEnd) const {
// Create vectors
gp_Vec startToPoint(point.XYZ() - lineStart.XYZ());
gp_Vec startToEnd(lineEnd.XYZ() - lineStart.XYZ());
// Check if the point is on the line defined by start and end
// by checking if the cross product is (near) zero vector, indicating collinearity.
gp_Vec crossProduct = startToPoint.Crossed(startToEnd);
if (crossProduct.Magnitude() > Precision::Confusion()) {
return false; // Not collinear, hence not on the line segment
}
return true; // The point is on the line segment
}
// Vec variant? This _Pnt and _Vec difference is annoying.
bool is_point_on_line(const gp_Vec& point, const gp_Vec& lineStart, const gp_Vec& lineEnd) const {
// Create vectors
gp_Vec startToPoint = point - lineStart;
gp_Vec startToEnd = lineEnd - lineStart;
// Check if the point is on the line defined by start and end
// by checking if the cross product is (near) zero vector, indicating collinearity.
gp_Vec crossProduct = startToPoint.Crossed(startToEnd);
if (crossProduct.Magnitude() > Precision::Confusion()) {
return false; // Not collinear, hence not on the line segment
}
return true; // The point is on the line segment
}
std::unordered_map> clash_bvh(
opencascade::handle> bvh_a,
opencascade::handle> bvh_b,
double extend = 0.0
) const {
std::unordered_map> bvh_clashes;
for (int i=0; iLength(); ++i) {
if ( ! bvh_a->IsOuter(i)) {
continue;
}
BVH_TreeBase::BVH_VecNt bvh_a_min = bvh_a->MinPoint(i);
BVH_TreeBase::BVH_VecNt bvh_a_max = bvh_a->MaxPoint(i);
bvh_a_min[0] -= 1e-3;
bvh_a_min[1] -= 1e-3;
bvh_a_min[2] -= 1e-3;
bvh_a_max[0] += 1e-3;
bvh_a_max[1] += 1e-3;
bvh_a_max[2] += 1e-3;
BVH_Box box_a(bvh_a_min, bvh_a_max);
std::stack stack;
stack.push(0);
while ( ! stack.empty()) {
int j = stack.top();
stack.pop();
BVH_TreeBase::BVH_VecNt bvh_b_min = bvh_b->MinPoint(j);
BVH_TreeBase::BVH_VecNt bvh_b_max = bvh_b->MaxPoint(j);
bvh_b_min[0] -= extend + 1e-3;
bvh_b_min[1] -= extend + 1e-3;
bvh_b_min[2] -= extend + 1e-3;
bvh_b_max[0] += extend + 1e-3;
bvh_b_max[1] += extend + 1e-3;
bvh_b_max[2] += extend + 1e-3;
if (box_a.IsOut(bvh_b_min, bvh_b_max)) {
continue;
}
if (bvh_b->IsOuter(j)) {
if (bvh_clashes.find(i) != bvh_clashes.end()) {
bvh_clashes[i].push_back(j);
} else {
bvh_clashes[i] = {j};
}
} else {
stack.push(bvh_b->Child<0>(j));
stack.push(bvh_b->Child<1>(j));
}
}
}
return bvh_clashes;
}
clash test_intersection(const T& tA, const T& tB, double tolerance, bool check_all = true) const {
// If there are verts of A inside shape B (protrusion):
// 1. For each vert, find the shortest distance to the closest face
// 2. Find the innermost vert (i.e. the vert that has the longest distance)
// Otherwise (piercing):
// 1. Intersect each edge with shape B
// 2. Find the longest distance between intersections
auto obb_b = obbs_.find(tB)->second;
obb_b.Enlarge(-tolerance);
// No need to search beyond the distance of the max protrusion.
const double max_protrusion = max_protrusions_.find(tB)->second;
// Collide BVH trees of shape A vs B
opencascade::handle> bvh_a = bvhs_.find(tA)->second;
opencascade::handle> bvh_b = bvhs_.find(tB)->second;
std::unordered_map> bvh_clashes = clash_bvh(bvh_a, bvh_b, max_protrusion);
if (bvh_clashes.empty()) {
return {-1, tA, tB, 0, {0, 0, 0}, {0, 0, 0}};
}
const std::vector>& tris_a = tris_.find(tA)->second;
const std::vector>& tris_b = tris_.find(tB)->second;
const std::vector& verts_a = verts_.find(tA)->second;
const std::vector& verts_b = verts_.find(tB)->second;
const std::vector& normals_a = normals_.find(tA)->second;
const std::vector& normals_b = normals_.find(tB)->second;
// ~10% faster?
std::unordered_set points_in_b_cache;
std::unordered_set points_not_in_b_cache;
double protrusion = -std::numeric_limits::infinity();
std::array protrusion_point;
std::array surface_point;
double pierce = -std::numeric_limits::infinity();
std::array pierce_point1;
std::array pierce_point2;
for (const auto& pair : bvh_clashes) {
const int bvh_a_i = pair.first;
const std::vector& bvh_b_is = pair.second;
for (int i=bvh_a->BegPrimitive(bvh_a_i); i<=bvh_a->EndPrimitive(bvh_a_i); ++i) {
const std::array& tri = tris_a[i];
std::vector points_in_b;
for (int v_id : tri) {
if (points_not_in_b_cache.find(v_id) != points_not_in_b_cache.end()) {
continue;
}
const gp_Pnt& v = verts_a[v_id];
if (points_in_b_cache.find(v_id) != points_in_b_cache.end()) {
points_in_b.push_back(v);
continue;
}
if (obb_b.IsOut(v)) {
points_not_in_b_cache.insert(v_id);
continue;
}
if (is_point_in_shape(v, bvh_b, tris_b, verts_b)
&& is_point_in_shape(v, bvh_b, tris_b, verts_b, true)) {
points_in_b.push_back(v);
points_in_b_cache.insert(v_id);
} else {
points_not_in_b_cache.insert(v_id);
}
}
// If there are no points in b, this may be a "piercing" triangle.
if (points_in_b.empty()) {
gp_Vec v1_a_vec(verts_a[tri[0]].XYZ());
gp_Vec v2_a_vec(verts_a[tri[1]].XYZ());
gp_Vec v3_a_vec(verts_a[tri[2]].XYZ());
// Protrusions take priority over piercings. We only check for piercings if:
// - This is a piercing triangle (e.g. no points in b)
// - No protrusion was already found
// - We haven't yet found a piercing at the max protrusion limit
if (protrusion == -std::numeric_limits::infinity() && pierce != max_protrusion) {
std::array<
std::tuple, std::array>, 3
> pierce_results = {
pierce_shape(v1_a_vec, v2_a_vec, bvh_b, tris_b, verts_b, normals_b),
pierce_shape(v1_a_vec, v3_a_vec, bvh_b, tris_b, verts_b, normals_b),
pierce_shape(v2_a_vec, v3_a_vec, bvh_b, tris_b, verts_b, normals_b)
};
for (const auto& pr : pierce_results) {
auto& p_dist = std::get<0>(pr);
auto& p_min = std::get<1>(pr);
auto& p_max = std::get<2>(pr);
if (p_dist > tolerance && p_dist > pierce) {
// Piercings are capped at max_protrusion for intuitive results
pierce = std::min(p_dist, max_protrusion);
pierce_point1 = p_min;
pierce_point2 = p_max;
if ( ! check_all) {
return {1, tA, tB, pierce, pierce_point1, pierce_point2};
}
}
}
}
// Since there were no points in b, we don't need to check for protrusions.
continue;
}
const gp_Vec& normal_a = normals_a[i];
double v_protrusion = std::numeric_limits::infinity();
std::array v_protrusion_point;
std::array v_surface_point;
// Check for protrusions.
for (const auto& bvh_b_i : bvh_b_is) {
for (int j=bvh_b->BegPrimitive(bvh_b_i); j<=bvh_b->EndPrimitive(bvh_b_i); ++j) {
const std::array& tri = tris_b[j];
const gp_Vec& normal_b = normals_b[j];
tri_count_++;
// We're penetrating _into_ a shape, so don't
// compare distances to faces with roughly the
// same normal as the penetration.
if (normal_a.Dot(normal_b) >= 0.9f) {
continue;
}
gp_Vec ta(verts_b[tri[0]].XYZ());
gp_Vec tb(verts_b[tri[1]].XYZ());
gp_Vec tc(verts_b[tri[2]].XYZ());
for (const auto& v : points_in_b) {
gp_Vec ray_origin(v.XYZ());
/*
std::cout << "POINT IN B " << v.X() << " " << v.Y() << " " << v.Z() << std::endl;
std::cout << "dir-> " << normal_b.X() << " " << normal_b.Y() << " " << normal_b.Z() << std::endl;
std::cout << "->tri " << v1_b[0] << " " << v1_b[1] << " " << v1_b[2] << std::endl;
std::cout << "->tri " << v2_b[0] << " " << v2_b[1] << " " << v2_b[2] << std::endl;
std::cout << "->tri " << v3_b[0] << " " << v3_b[1] << " " << v3_b[2] << std::endl;
*/
// Do (cheaper) line check.
double at, au, av;
if (intersectRayTriangle(ray_origin, normal_b, ta, tb, tc, at, au, av, false)) {
double current_v_protrusion = at;
// std::cout << "We got a current protrusion " << current_v_protrusion << std::endl;
if (current_v_protrusion < v_protrusion) {
double aw = 1.0f - au - av; // Barycentric coordinate for ta
gp_Vec point_on_b = aw * ta + au * tb + av * tc; // Intersection point
// std::cout << "New v_protrusion winner of " << current_v_protrusion << std::endl;
v_protrusion = current_v_protrusion;
v_protrusion_point = {v.X(), v.Y(), v.Z()};
v_surface_point = {point_on_b.X(), point_on_b.Y(), point_on_b.Z()};
if ( ! check_all && v_protrusion > tolerance) {
return {0, tA, tB, v_protrusion, v_protrusion_point, v_surface_point};
}
}
}
}
}
}
if (v_protrusion != std::numeric_limits::infinity()) {
if (v_protrusion > protrusion) {
// std::cout << "New actual protrusion winner of " << v_protrusion << std::endl;
protrusion = v_protrusion;
protrusion_point = v_protrusion_point;
surface_point = v_surface_point;
if (protrusion > (max_protrusion - 1e-3)) {
return {0, tA, tB, protrusion, protrusion_point, surface_point};
}
}
}
}
}
if (protrusion > tolerance) {
return {0, tA, tB, protrusion, protrusion_point, surface_point};
}
if (pierce > tolerance) {
return {1, tA, tB, pierce, pierce_point1, pierce_point2};
}
return {-1, tA, tB, 0, {0, 0, 0}, {0, 0, 0}};
}
clash test_collision(const T& tA, const T& tB, bool allow_touching) const {
// Collide BVH trees of shape A vs B
opencascade::handle> bvh_a = bvhs_.find(tA)->second;
opencascade::handle> bvh_b = bvhs_.find(tB)->second;
std::unordered_map> bvh_clashes = clash_bvh(bvh_a, bvh_b);
if (bvh_clashes.empty()) {
return {-1, tA, tB, 0, {0, 0, 0}, {0, 0, 0}};
}
const std::vector>& tris_a = tris_.find(tA)->second;
const std::vector>& tris_b = tris_.find(tB)->second;
const std::vector& verts_a = verts_.find(tA)->second;
const std::vector& verts_b = verts_.find(tB)->second;
const std::vector& normals_a = normals_.find(tA)->second;
const std::vector& normals_b = normals_.find(tB)->second;
for (const auto& pair : bvh_clashes) {
const int bvh_a_i = pair.first;
const std::vector& bvh_b_is = pair.second;
for (int i=bvh_a->BegPrimitive(bvh_a_i); i<=bvh_a->EndPrimitive(bvh_a_i); ++i) {
const std::array& tri = tris_a[i];
const gp_Pnt& v1_a_pnt = verts_a[tri[0]];
const gp_Pnt& v2_a_pnt = verts_a[tri[1]];
const gp_Pnt& v3_a_pnt = verts_a[tri[2]];
const gp_Vec& normal_a = normals_a[i];
const gp_Vec v1_a_vec(v1_a_pnt.XYZ());
const gp_Vec v2_a_vec(v2_a_pnt.XYZ());
const gp_Vec v3_a_vec(v3_a_pnt.XYZ());
for (const auto& bvh_b_i : bvh_b_is) {
for (int j=bvh_b->BegPrimitive(bvh_b_i); j<=bvh_b->EndPrimitive(bvh_b_i); ++j) {
const std::array& tri = tris_b[j];
const gp_Pnt& v1_b_pnt = verts_b[tri[0]];
const gp_Pnt& v2_b_pnt = verts_b[tri[1]];
const gp_Pnt& v3_b_pnt = verts_b[tri[2]];
const gp_Vec& normal_b = normals_b[j];
tri_count_++;
const gp_Vec v1_b_vec(v1_b_pnt.XYZ());
const gp_Vec v2_b_vec(v2_b_pnt.XYZ());
const gp_Vec v3_b_vec(v3_b_pnt.XYZ());
// Allow a deviation of 0.25 degrees in coplanarity check
if (std::abs(normal_a.Dot(normal_b)) >= 0.99999f) {
continue;
}
gp_Vec int1, int2;
if (trianglesIntersect(v1_a_vec, v2_a_vec, v3_a_vec, v1_b_vec, v2_b_vec, v3_b_vec, int1, int2, ! allow_touching)) {
if (allow_touching) {
return {2, tA, tB, 0, {int1.X(), int1.Y(), int1.Z()}, {int2.X(), int2.Y(), int2.Z()}};
}
// A non-touching collision is defined as two triangles that:
// 1. Are not coplanar
// 2. The point of intersection is not along the edge of triangle A.
// 3. The point of intersection is not a vertex of triangle B.
if (
! is_point_on_line(int1, v1_a_vec, v2_a_vec)
&& ! is_point_on_line(int1, v1_a_vec, v3_a_vec)
&& ! is_point_on_line(int1, v2_a_vec, v3_a_vec)
) {
if (
(v1_b_vec - int1).Magnitude() > 1e-4
&& (v2_b_vec - int1).Magnitude() > 1e-4
&& (v3_b_vec - int1).Magnitude() > 1e-4
) {
return {2, tA, tB, 0, {int1.X(), int1.Y(), int1.Z()}, {int2.X(), int2.Y(), int2.Z()}};
}
}
if (
! is_point_on_line(int1, v1_b_vec, v2_b_vec)
&& ! is_point_on_line(int1, v1_b_vec, v3_b_vec)
&& ! is_point_on_line(int1, v2_b_vec, v3_b_vec)
) {
if (
(v1_a_vec - int1).Magnitude() > 1e-4
&& (v2_a_vec - int1).Magnitude() > 1e-4
&& (v3_a_vec - int1).Magnitude() > 1e-4
) {
return {2, tA, tB, 0, {int1.X(), int1.Y(), int1.Z()}, {int2.X(), int2.Y(), int2.Z()}};
}
}
if (
! is_point_on_line(int2, v1_a_vec, v2_a_vec)
&& ! is_point_on_line(int2, v1_a_vec, v3_a_vec)
&& ! is_point_on_line(int2, v2_a_vec, v3_a_vec)
) {
if (
(v1_b_vec - int2).Magnitude() > 1e-4
&& (v2_b_vec - int2).Magnitude() > 1e-4
&& (v3_b_vec - int2).Magnitude() > 1e-4
) {
return {2, tA, tB, 0, {int2.X(), int2.Y(), int2.Z()}, {int1.X(), int1.Y(), int1.Z()}};
}
}
if (
! is_point_on_line(int2, v1_b_vec, v2_b_vec)
&& ! is_point_on_line(int2, v1_b_vec, v3_b_vec)
&& ! is_point_on_line(int2, v2_b_vec, v3_b_vec)
) {
if (
(v1_a_vec - int2).Magnitude() > 1e-4
&& (v2_a_vec - int2).Magnitude() > 1e-4
&& (v3_a_vec - int2).Magnitude() > 1e-4
) {
return {2, tA, tB, 0, {int2.X(), int2.Y(), int2.Z()}, {int1.X(), int1.Y(), int1.Z()}};
}
}
}
}
}
}
}
return {-1, tA, tB, 0, {0, 0, 0}, {0, 0, 0}};
}
clash test_clearance(const T& tA, const T& tB, double clearance, bool check_all) const {
// Collide BVH trees of shape A vs B
opencascade::handle> bvh_a = bvhs_.find(tA)->second;
opencascade::handle> bvh_b = bvhs_.find(tB)->second;
std::unordered_map> bvh_clashes = clash_bvh(bvh_a, bvh_b, clearance);
if (bvh_clashes.empty()) {
return {-1, tA, tB, 0, {0, 0, 0}, {0, 0, 0}};
}
const std::vector>& tris_a = tris_.find(tA)->second;
const std::vector>& tris_b = tris_.find(tB)->second;
const std::vector& verts_a = verts_.find(tA)->second;
const std::vector& verts_b = verts_.find(tB)->second;
double min_clearance = std::numeric_limits::infinity();
std::array clearance_point1;
std::array clearance_point2;
for (const auto& pair : bvh_clashes) {
const int bvh_a_i = pair.first;
const std::vector& bvh_b_is = pair.second;
for (int i=bvh_a->BegPrimitive(bvh_a_i); i<=bvh_a->EndPrimitive(bvh_a_i); ++i) {
const std::array& tri = tris_a[i];
const gp_Pnt& v1_a_pnt = verts_a[tri[0]];
const gp_Pnt& v2_a_pnt = verts_a[tri[1]];
const gp_Pnt& v3_a_pnt = verts_a[tri[2]];
const gp_Vec v1_a_vec(v1_a_pnt.XYZ());
const gp_Vec v2_a_vec(v2_a_pnt.XYZ());
const gp_Vec v3_a_vec(v3_a_pnt.XYZ());
const std::array p = {v1_a_vec, v2_a_vec, v3_a_vec};
for (const auto& bvh_b_i : bvh_b_is) {
for (int j=bvh_b->BegPrimitive(bvh_b_i); j<=bvh_b->EndPrimitive(bvh_b_i); ++j) {
const std::array& tri = tris_b[j];
const gp_Pnt& v1_b_pnt = verts_b[tri[0]];
const gp_Pnt& v2_b_pnt = verts_b[tri[1]];
const gp_Pnt& v3_b_pnt = verts_b[tri[2]];
tri_count_++;
const gp_Vec v1_b_vec(v1_b_pnt.XYZ());
const gp_Vec v2_b_vec(v2_b_pnt.XYZ());
const gp_Vec v3_b_vec(v3_b_pnt.XYZ());
const std::array q = {v1_b_vec, v2_b_vec, v3_b_vec};
gp_Vec cp;
gp_Vec cq;
// https://stackoverflow.com/questions/53602907/algorithm-to-find-minimum-distance-between-two-triangles
distanceTriangleTriangleSquared(cp, cq, p, q);
double distance = (cq - cp).Magnitude();
if (distance < clearance && distance < min_clearance) {
min_clearance = distance;
clearance_point1 = {cp.X(), cp.Y(), cp.Z()};
clearance_point2 = {cq.X(), cq.Y(), cq.Z()};
if ( ! check_all || min_clearance < 1e-4) {
return {3, tA, tB, min_clearance, clearance_point1, clearance_point2};
}
}
}
}
}
}
if (min_clearance < clearance) {
return {3, tA, tB, min_clearance, clearance_point1, clearance_point2};
}
return {-1, tA, tB, 0, {0, 0, 0}, {0, 0, 0}};
}
bool test(const TopoDS_Shape& A, const TopoDS_Shape& B, bool completely_within, double extend) const {
if (extend > 0.) {
BRepExtrema_DistShapeShape dss(A, B);
if (dss.Perform() && dss.NbSolution() >= 1) {
if (dss.Value() <= extend) {
distances_.push_back(dss.Value());
protrusion_distances_.push_back(max_distance_inside(B, A));
}
return dss.Value() <= extend;
}
} else {
if (util::count(A, TopAbs_SHELL) == 0 ||
util::count(B, TopAbs_SHELL) == 0)
{
return false;
}
if (completely_within) {
BRepAlgoAPI_Cut cut(B, A);
if (cut.IsDone()) {
if (util::count(cut.Shape(), TopAbs_SHELL) == 0) {
return true;
}
}
} else {
BRepAlgoAPI_Common common(A, B);
if (common.IsDone()) {
if (util::count(common.Shape(), TopAbs_SHELL) > 0) {
return true;
}
}
}
}
return false;
}
protected:
// @todo this is ugly, embed this in the return type
mutable std::vector distances_;
mutable std::vector protrusion_distances_;
mutable long long tri_count_ = 0;
public:
void add(const T& t, const Bnd_Box& b) {
tree_.Add(t, b);
}
void add(const T& t, const TopoDS_Shape& s) {
Bnd_Box b;
BRepBndLib::AddClose(s, b);
add(t, b);
shapes_[t] = s;
}
void add_triangulation(const T& t, const TopoDS_Shape& s) {
// Note that the original add function is also used elsewhere (e.g. boolean_utils.cpp)
// We don't want to randomly add triangulated voids in our
// tree, so for now this is a separate function.
Bnd_Box b;
BRepBndLib::AddClose(s, b);
aabbs_[t] = b;
Bnd_OBB obb;
// If IsOptimal = True it doubles the execution time.
BRepBndLib::AddOBB(s, obb, true, false, false);
obbs_[t] = obb;
max_protrusions_[t] = std::min(std::min(obb.XHSize(), obb.YHSize()), obb.ZHSize()) * 2;
int original_tris_index = 0;
std::vector> original_tris;
std::vector verts;
std::vector original_normals;
// Attempt to copy exactly what BRepExtrema_TriangleSet is doing under the hood.
const auto builder = new BVH_LinearBuilder (BVH_Constants_LeafNodeSizeDefault, BVH_Constants_MaxTreeDepth);
BVH_Triangulation triangulation(builder);
BRepExtrema_ShapeList shape_list;
std::vector is_reversed;
TopExp_Explorer exp_f;
for (exp_f.Init(s, TopAbs_FACE); exp_f.More(); exp_f.Next()) {
shape_list.Append(TopoDS::Face(exp_f.Current()));
TopoDS_Face f = TopoDS::Face(exp_f.Current());
is_reversed.push_back(f.Orientation() == TopAbs_REVERSED);
}
// Standard_Boolean BRepExtrema_TriangleSet::Init (const BRepExtrema_ShapeList& theShapes)
Standard_Boolean isOK = Standard_True;
for (Standard_Integer aShapeIdx = 0; aShapeIdx < shape_list.Size() && isOK; ++aShapeIdx)
{
if (shape_list (aShapeIdx).ShapeType() == TopAbs_FACE) {
// isOK = initFace (TopoDS::Face (shape_list(aShapeIdx)), aShapeIdx);
// Standard_Boolean BRepExtrema_TriangleSet::initFace (const TopoDS_Face& theFace, const Standard_Integer theIndex)
TopoDS_Face theFace = TopoDS::Face (shape_list(aShapeIdx));
Standard_Integer theIndex = aShapeIdx;
TopLoc_Location aLocation;
bool is_reversed = theFace.Orientation() == TopAbs_REVERSED;
Handle(Poly_Triangulation) aTriangulation = BRep_Tool::Triangulation (theFace, aLocation);
if (aTriangulation.IsNull())
{
isOK = false;
}
const Standard_Integer aVertOffset = static_cast (verts.size()) - 1;
// initNodes (aTriangulation->MapNodeArray()->ChangeArray1(), aLocation.Transformation(), theIndex);
// void BRepExtrema_TriangleSet::initNodes (const TColgp_Array1OfPnt& theNodes, const gp_Trsf& theTrsf, const Standard_Integer theIndex)
TColgp_Array1OfPnt theNodes = aTriangulation->MapNodeArray()->ChangeArray1();
gp_Trsf theTrsf = aLocation.Transformation();
for (Standard_Integer aVertIdx = 1; aVertIdx <= theNodes.Size(); ++aVertIdx)
{
gp_Pnt aVertex = theNodes.Value (aVertIdx);
aVertex.Transform (theTrsf);
triangulation.Vertices.push_back (BVH_Vec3d (aVertex.X(), aVertex.Y(), aVertex.Z()));
verts.push_back(aVertex);
// myShapeIdxOfVtxVec.Append (theIndex);
}
// myNumVtxInShapeVec.SetValue (theIndex, theNodes.Size());
for (Standard_Integer aTriIdx = 1; aTriIdx <= aTriangulation->NbTriangles(); ++aTriIdx)
{
Standard_Integer aVertex1;
Standard_Integer aVertex2;
Standard_Integer aVertex3;
if (is_reversed) {
aTriangulation->Triangle (aTriIdx).Get (aVertex3, aVertex2, aVertex1);
} else {
aTriangulation->Triangle (aTriIdx).Get (aVertex1, aVertex2, aVertex3);
}
const auto& v1_pnt = verts[aVertex1 + aVertOffset];
const auto& v2_pnt = verts[aVertex2 + aVertOffset];
const auto& v3_pnt = verts[aVertex3 + aVertOffset];
gp_Vec dir1(v1_pnt, v2_pnt);
gp_Vec dir2(v1_pnt, v3_pnt);
gp_Vec cross_product = dir1.Crossed(dir2);
if (cross_product.Magnitude() > Precision::Confusion()) {
triangulation.Elements.push_back (BVH_Vec4i (
aVertex1 + aVertOffset,
aVertex2 + aVertOffset,
aVertex3 + aVertOffset,
original_tris_index));
//theIndex));
original_tris_index++;
original_tris.push_back({
aVertex1 + aVertOffset,
aVertex2 + aVertOffset,
aVertex3 + aVertOffset
});
original_normals.push_back(cross_product.Normalized());
}
}
// myNumTrgInShapeVec.SetValue (theIndex, aTriangulation->NbTriangles());
isOK = true;
} else if (shape_list (aShapeIdx).ShapeType() == TopAbs_EDGE) {
// isOK = initEdge (TopoDS::Edge (shape_list(aShapeIdx)), aShapeIdx);
// Should never occur, we don't pass in edges.
}
}
triangulation.MarkDirty();
const auto bvh = triangulation.BVH();
// After BVH is constructed, triangles are reordered
std::vector> tris(triangulation.Size());
std::vector normals(triangulation.Size());
for (int i=0; i select_box(const T& t, bool completely_within = false, double extend=-1.e-5) const {
typename map_t::const_iterator it = shapes_.find(t);
if (it == shapes_.end()) {
return std::vector();
}
Bnd_Box b;
BRepBndLib::AddClose(it->second, b);
// Gap is assumed to be positive throughout the codebase,
// but at least for IsOut() in the selector a negative
// Gap should work as well.
b.SetGap(b.GetGap() + extend);
return select_box(b, completely_within);
}
std::vector select_box(const gp_Pnt& p, double extend=0.0) const {
Bnd_Box b;
b.Add(p);
b.SetGap(b.GetGap() + extend);
return select_box(b);
}
std::vector select_box(const Bnd_Box& b, bool completely_within = false) const {
selector s(b);
tree_.Select(s);
if (completely_within) {
std::vector ts = s.results();
std::vector ts_filtered;
ts_filtered.reserve(ts.size());
typename std::vector::const_iterator it = ts.begin();
for (; it != ts.end(); ++it) {
const TopoDS_Shape& shp = shapes_.find(*it)->second;
Bnd_Box B;
BRepBndLib::AddClose(shp, B);
// BndBox::CornerMin() /-Max() introduced in OCCT 6.8
double x1, y1, z1, x2, y2, z2;
b.Get(x1, y1, z1, x2, y2, z2);
double gap = B.GetGap();
gp_Pnt p1(x1 - gap, y1 - gap, z1 - gap);
gp_Pnt p2(x2 + gap, y2 + gap, z2 + gap);
if (!b.IsOut(p1) && !b.IsOut(p2)) {
ts_filtered.push_back(*it);
}
}
return ts_filtered;
} else {
return s.results();
}
}
std::unique_ptr> build_box_set(const std::vector& elements) const {
double x, y, z, X, Y, Z;
std::unique_ptr> box_set = std::make_unique>();
for (int i=0; isecond;
aabb.Get(x, y, z, X, Y, Z);
const BVH_Box::BVH_VecNt min(x, y, z);
const BVH_Box::BVH_VecNt max(X, Y, Z);
BVH_Box bvh_box(min, max);
box_set->Add(i, bvh_box);
}
return box_set;
}
struct clash_task {
T a, b;
};
std::vector> allocate_tasks_to_threads(
std::vector& task_queue) const {
int num_threads = std::thread::hardware_concurrency();
std::vector> threaded_tasks(num_threads);
size_t tasks_per_thread = task_queue.size() / num_threads;
for (int i = 0; i < num_threads; ++i) {
auto startIter = std::next(task_queue.begin(), i * tasks_per_thread);
auto endIter = (i == num_threads - 1) ? task_queue.end() : std::next(startIter, tasks_per_thread);
threaded_tasks[i] = std::vector(startIter, endIter);
}
return threaded_tasks;
}
std::vector clash_intersection_many(
const std::vector& set_a, const std::vector& set_b,
double tolerance = 0.002, bool check_all = true
) const {
std::vector task_queue;
std::vector results;
std::unique_ptr> box_set_a = build_box_set(set_a);
std::unique_ptr> box_set_b = build_box_set(set_b);
const opencascade::handle>& bvh_a = box_set_a->BVH();
const opencascade::handle>& bvh_b = box_set_b->BVH();
std::unordered_map> bvh_clashes = clash_bvh(bvh_a, bvh_b, 0.0);
if (bvh_clashes.empty()) {
return results;
}
std::map> tested_pairs;
for (const auto& pair : bvh_clashes) {
const int bvh_a_i = pair.first;
const std::vector& bvh_b_is = pair.second;
for (int i=bvh_a->BegPrimitive(bvh_a_i); i<=bvh_a->EndPrimitive(bvh_a_i); ++i) {
const T& t_a = set_a[box_set_a->Element(i)];
for (const auto& bvh_b_i : bvh_b_is) {
for (int j=bvh_b->BegPrimitive(bvh_b_i); j<=bvh_b->EndPrimitive(bvh_b_i); ++j) {
const T& t_b = set_b[box_set_b->Element(j)];
if (t_a == t_b) {
continue;
}
if (tested_pairs[t_a].insert(t_b).second) {
tested_pairs[t_b].insert(t_a).second;
} else {
continue;
}
task_queue.emplace_back(clash_task{t_a, t_b});
}
}
}
}
std::vector> threaded_tasks = allocate_tasks_to_threads(task_queue);
std::vector threads;
std::mutex results_mutex;
for (auto& tasks : threaded_tasks) {
threads.emplace_back([this, &tasks, &results, &results_mutex, tolerance, check_all] {
std::vector thread_results;
for (auto& task : tasks) {
const auto& obb_a = obbs_.find(task.a)->second;
auto obb_b = obbs_.find(task.b)->second;
obb_b.Enlarge(-tolerance);
if (obb_a.IsOut(obb_b)) {
continue;
}
bool has_clash = false;
bool is_manifold = false;
clash result;
if (is_manifold_.find(task.b)->second) {
is_manifold = true;
clash intersection = test_intersection(task.a, task.b, tolerance, check_all);
if (intersection.clash_type != -1) {
has_clash = true;
result = intersection;
if ( ! check_all) {
thread_results.push_back(result);
continue;
}
}
}
if (is_manifold_.find(task.a)->second) {
is_manifold = true;
clash intersection = test_intersection(task.b, task.a, tolerance, check_all);
if (intersection.clash_type != -1) {
// Replace the clash result if any of these criteria apply:
// - We don't have a clash yet
// - Our previous clash is piercing, and our new one is a protrusion
// - We have the same clash type, but our clash is more severe
if (
! has_clash
|| (result.clash_type == 1 && intersection.clash_type == 0)
|| (
result.clash_type == intersection.clash_type
&& intersection.distance > result.distance
)
) {
has_clash = true;
result = intersection;
}
}
}
if ( ! is_manifold) {
clash collision = test_collision(task.a, task.b, false);
if (collision.clash_type != -1) {
has_clash = true;
result = collision;
}
}
if (has_clash) {
thread_results.push_back(result);
}
}
{
std::lock_guard lock(results_mutex);
results.insert(results.end(), thread_results.begin(), thread_results.end());
}
});
}
for (auto& thread : threads) {
if (thread.joinable()) {
thread.join();
}
}
return results;
}
std::vector clash_collision_many(
const std::vector& set_a, const std::vector& set_b, bool allow_touching = false
) const {
std::vector task_queue;
std::vector results;
std::unique_ptr> box_set_a = build_box_set(set_a);
std::unique_ptr> box_set_b = build_box_set(set_b);
const opencascade::handle>& bvh_a = box_set_a->BVH();
const opencascade::handle>& bvh_b = box_set_b->BVH();
std::unordered_map> bvh_clashes = clash_bvh(bvh_a, bvh_b, 0.0);
if (bvh_clashes.empty()) {
return results;
}
std::map> tested_pairs;
for (const auto& pair : bvh_clashes) {
const int bvh_a_i = pair.first;
const std::vector& bvh_b_is = pair.second;
for (int i=bvh_a->BegPrimitive(bvh_a_i); i<=bvh_a->EndPrimitive(bvh_a_i); ++i) {
const T& t_a = set_a[box_set_a->Element(i)];
for (const auto& bvh_b_i : bvh_b_is) {
for (int j=bvh_b->BegPrimitive(bvh_b_i); j<=bvh_b->EndPrimitive(bvh_b_i); ++j) {
const T& t_b = set_b[box_set_b->Element(j)];
if (t_a == t_b) {
continue;
}
if (tested_pairs[t_a].insert(t_b).second) {
tested_pairs[t_b].insert(t_a).second;
} else {
continue;
}
task_queue.emplace_back(clash_task{t_a, t_b});
}
}
}
}
std::vector> threaded_tasks = allocate_tasks_to_threads(task_queue);
std::vector threads;
std::mutex results_mutex;
for (auto& tasks : threaded_tasks) {
threads.emplace_back([this, &tasks, &results, &results_mutex, allow_touching] {
std::vector thread_results;
for (auto& task : tasks) {
const auto& obb_a = obbs_.find(task.a)->second;
auto obb_b = obbs_.find(task.b)->second;
obb_b.Enlarge(-0.001);
if (obb_a.IsOut(obb_b)) {
continue;
}
clash result = test_collision(task.a, task.b, allow_touching);
if (result.clash_type != -1) {
thread_results.push_back(result);
}
}
{
std::lock_guard lock(results_mutex);
results.insert(results.end(), thread_results.begin(), thread_results.end());
}
});
}
for (auto& thread : threads) {
if (thread.joinable()) {
thread.join();
}
}
return results;
}
std::vector clash_clearance_many(
const std::vector& set_a, const std::vector& set_b,
double clearance = 0.05, bool check_all = false
) const {
std::vector task_queue;
std::vector results;
std::unique_ptr> box_set_a = build_box_set(set_a);
std::unique_ptr> box_set_b = build_box_set(set_b);
const opencascade::handle>& bvh_a = box_set_a->BVH();
const opencascade::handle>& bvh_b = box_set_b->BVH();
std::unordered_map> bvh_clashes = clash_bvh(bvh_a, bvh_b, clearance);
if (bvh_clashes.empty()) {
return results;
}
std::map> tested_pairs;
for (const auto& pair : bvh_clashes) {
const int bvh_a_i = pair.first;
const std::vector& bvh_b_is = pair.second;
for (int i=bvh_a->BegPrimitive(bvh_a_i); i<=bvh_a->EndPrimitive(bvh_a_i); ++i) {
const T& t_a = set_a[box_set_a->Element(i)];
for (const auto& bvh_b_i : bvh_b_is) {
for (int j=bvh_b->BegPrimitive(bvh_b_i); j<=bvh_b->EndPrimitive(bvh_b_i); ++j) {
const T& t_b = set_b[box_set_b->Element(j)];
if (t_a == t_b) {
continue;
}
if (tested_pairs[t_a].insert(t_b).second) {
tested_pairs[t_b].insert(t_a).second;
} else {
continue;
}
task_queue.emplace_back(clash_task{t_a, t_b});
}
}
}
}
std::vector> threaded_tasks = allocate_tasks_to_threads(task_queue);
std::vector threads;
std::mutex results_mutex;
for (auto& tasks : threaded_tasks) {
threads.emplace_back([this, &tasks, &results, &results_mutex, clearance, check_all] {
std::vector thread_results;
for (auto& task : tasks) {
const auto& obb_a = obbs_.find(task.a)->second;
auto obb_b = obbs_.find(task.b)->second;
obb_b.Enlarge(clearance);
if (obb_a.IsOut(obb_b)) {
continue;
}
clash result = test_clearance(task.a, task.b, clearance, check_all);
if (result.clash_type != -1) {
thread_results.push_back(result);
}
}
{
std::lock_guard lock(results_mutex);
results.insert(results.end(), thread_results.begin(), thread_results.end());
}
});
}
for (auto& thread : threads) {
if (thread.joinable()) {
thread.join();
}
}
return results;
}
std::vector select(const T& t, bool completely_within = false, double extend = 0.0) const {
distances_.clear();
protrusion_distances_.clear();
std::vector ts = select_box(t, completely_within, extend);
if (ts.empty()) {
return ts;
}
const TopoDS_Shape& A = shapes_.find(t)->second;
std::vector ts_filtered;
ts_filtered.reserve(ts.size());
typename std::vector::const_iterator it = ts.begin();
for (it = ts.begin(); it != ts.end(); ++it) {
const TopoDS_Shape& B = shapes_.find(*it)->second;
if (test(A, B, completely_within, extend)) {
ts_filtered.push_back(*it);
}
}
return ts_filtered;
}
std::vector select(const TopoDS_Shape& s, bool completely_within = false, double extend = -1.e-5) const {
distances_.clear();
protrusion_distances_.clear();
Bnd_Box bb;
BRepBndLib::AddClose(s, bb);
bb.SetGap(bb.GetGap() + extend);
std::vector ts = select_box(bb, completely_within);
if (ts.empty()) {
return ts;
}
std::vector ts_filtered;
ts_filtered.reserve(ts.size());
typename std::vector::const_iterator it = ts.begin();
for (it = ts.begin(); it != ts.end(); ++it) {
const TopoDS_Shape& B = shapes_.find(*it)->second;
if (test(s, B, completely_within, extend)) {
ts_filtered.push_back(*it);
}
}
return ts_filtered;
}
std::vector select(const IfcGeom::BRepElement* elem, bool completely_within = false, double extend = -1.e-5) const {
auto compound = elem->geometry().as_compound();
compound.Move(elem->transformation().data());
return select(compound, completely_within, extend);
}
std::vector select(const gp_Pnt& p, double extend=0.0) const {
distances_.clear();
protrusion_distances_.clear();
std::vector ts = select_box(p, extend);
if (ts.empty()) {
return ts;
}
std::vector ts_filtered;
ts_filtered.reserve(ts.size());
TopoDS_Vertex v;
if (extend > 0.) {
BRep_Builder B;
B.MakeVertex(v, p, Precision::Confusion());
}
typename std::vector::const_iterator it = ts.begin();
for (it = ts.begin(); it != ts.end(); ++it) {
const TopoDS_Shape& B = shapes_.find(*it)->second;
if (extend > 0.0) {
BRepExtrema_DistShapeShape dss(v, B);
if (dss.Perform() && dss.NbSolution() >= 1 && dss.Value() <= extend) {
distances_.push_back(dss.Value());
protrusion_distances_.push_back(max_distance_inside(B, v));
ts_filtered.push_back(*it);
}
} else {
TopExp_Explorer exp(B, TopAbs_SOLID);
for (; exp.More(); exp.Next()) {
BRepClass3d_SolidClassifier cls(exp.Current(), p, 1e-5);
if (cls.State() != TopAbs_OUT) {
ts_filtered.push_back(*it);
break;
}
}
}
}
return ts_filtered;
}
protected:
typedef NCollection_UBTree tree_t;
typedef std::map map_t;
tree_t tree_;
map_t shapes_;
std::map aabbs_;
std::map obbs_;
std::map max_protrusions_;
std::map>> bvhs_;
std::unordered_map is_manifold_;
std::unordered_map>> tris_;
std::unordered_map> verts_;
std::unordered_map> normals_;
// Temporary structures for H5
std::vector triangulation_elements_;
std::map global_ids_;
std::map names_;
std::map> placements_;
std::map> local_verts_;
std::map> local_faces_;
std::map> local_materials_;
std::map> local_material_ids_;
bool enable_face_styles_ = false;
class selector : public tree_t::Selector
{
public:
selector(const Bnd_Box& b)
: tree_t::Selector()
, bounds_(b)
{}
Standard_Boolean Reject(const Bnd_Box& b) const {
return bounds_.IsOut(b);
}
Standard_Boolean Accept(const T& o) {
results_.push_back(o);
return Standard_True;
}
const std::vector& results() const {
return results_;
}
private:
std::vector results_;
const Bnd_Box& bounds_;
};
};
}
class tree : public impl::tree {
public:
tree() {};
tree(IfcParse::IfcFile& f) {
add_file(f, IfcGeom::IteratorSettings());
}
tree(IfcParse::IfcFile& f, const IfcGeom::IteratorSettings& settings) {
add_file(f, settings);
}
tree(IfcGeom::Iterator& it) {
add_file(it);
}
void add_file(IfcParse::IfcFile& f, const IfcGeom::IteratorSettings& settings) {
IfcGeom::IteratorSettings settings_ = settings;
settings_.set(IfcGeom::IteratorSettings::DISABLE_TRIANGULATION, true);
settings_.set(IfcGeom::IteratorSettings::USE_WORLD_COORDS, true);
settings_.set(IfcGeom::IteratorSettings::SEW_SHELLS, true);
IfcGeom::Iterator it(settings_, &f);
add_file(it);
}
void add_file(IfcGeom::Iterator& it) {
if (it.initialize()) {
do {
add_element(dynamic_cast(it.get()));
} while (it.next());
}
}
uint8_t hexStringToByte(const std::string& hexStr) {
uint8_t byte;
std::stringstream ss;
ss << std::hex << hexStr;
ss >> byte;
return byte;
}
void write_h5() {
H5::H5File file("filename.h5", H5F_ACC_TRUNC);
H5::Group shapes = file.createGroup("/shapes");
std::set processed_geometry_ids;
std::vector element_shape_ids;
std::unordered_map geometry_id_to_shape_id;
int geometry_index = 0;
std::vector> matrices;
std::vector> colours;
std::vector names;
std::vector global_ids;
const float tolerance = 0.01f; // Tolerance value for comparison
for (const auto& elem : triangulation_elements_) {
const auto geometry_id = elem->geometry().id();
const auto& placement = placements_[elem->product()];
matrices.emplace_back(placement.begin(), placement.end());
names.push_back(names_[elem->product()]);
global_ids.push_back(global_ids_[elem->product()]);
if (processed_geometry_ids.find(geometry_id) != processed_geometry_ids.end()) {
element_shape_ids.push_back(geometry_id_to_shape_id[geometry_id]);
continue;
}
processed_geometry_ids.insert(geometry_id);
H5::Group group = shapes.createGroup(std::to_string(geometry_index));
geometry_id_to_shape_id[geometry_id] = geometry_index;
element_shape_ids.push_back(geometry_index);
geometry_index++;
const auto& faces = local_faces_[geometry_id];
const auto& verts = local_verts_[geometry_id];
const auto& materials = local_materials_[geometry_id];
const auto& material_ids = local_material_ids_[geometry_id];
std::vector verts_float(verts.size());
std::transform(verts.begin(), verts.end(), verts_float.begin(),
[](double val) { return static_cast