From 6bc99544c58f00215ac341c8a6d1acb642ee2006 Mon Sep 17 00:00:00 2001 From: Thomas Krijnen Date: Fri, 1 Mar 2024 13:29:56 +0100 Subject: [PATCH] Move free functions out to bvh_utils; delete h5 related bits; try to work on sharing bvh clash prologue --- src/ifcgeom_schema_agnostic/IfcGeomTree.h | 755 ++++++---------------- src/ifcgeom_schema_agnostic/bvh_utils.h | 309 +++++++++ 2 files changed, 512 insertions(+), 552 deletions(-) create mode 100644 src/ifcgeom_schema_agnostic/bvh_utils.h diff --git a/src/ifcgeom_schema_agnostic/IfcGeomTree.h b/src/ifcgeom_schema_agnostic/IfcGeomTree.h index 6d8373087c..1fd92c407f 100644 --- a/src/ifcgeom_schema_agnostic/IfcGeomTree.h +++ b/src/ifcgeom_schema_agnostic/IfcGeomTree.h @@ -27,6 +27,8 @@ #include "../ifcgeom_schema_agnostic/IfcGeomMaterial.h" #include "../ifcgeom_schema_agnostic/Kernel.h" #include "../ifcgeom_schema_agnostic/base_utils.h" +#include "../ifcgeom_schema_agnostic/clash_utils.h" +#include "../ifcgeom_schema_agnostic/bvh_utils.h" #include #include @@ -43,7 +45,6 @@ #include #include #include -#include #include #include #include @@ -60,7 +61,6 @@ #include #include #include -#include "clash_utils.h" #include "H5Cpp.h" @@ -134,335 +134,218 @@ namespace IfcGeom { 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); + class tree { - 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(); + template + bool process_bvh_intersections(const T& tA, const T& tB, Fn&& f) { + // 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]; + + f(bvh_a, bvh_b, tris_a, tris_b, verts_a, verts_b, normals_a, normals_b, i, tri, bvh_b_is); + } } - 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(); + clash test_intersection_2(const T& tA, const T& tB, double tolerance, bool check_all = true) { + auto obb_b = obbs_.find(tB)->second; + obb_b.Enlarge(-tolerance); - 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] - } + // ~10% faster? + std::unordered_set points_in_b_cache; + std::unordered_set points_not_in_b_cache; - 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]); + double protrusion = -std::numeric_limits::infinity(); + std::array protrusion_point; + std::array surface_point; - int total_intersections = 0; + double pierce = -std::numeric_limits::infinity(); + std::array pierce_point1; + std::array pierce_point2; - std::stack stack; - stack.push(0); + // No need to search beyond the distance of the max protrusion. + const double max_protrusion = max_protrusions_.find(tB)->second; - while ( ! stack.empty()) { - int i = stack.top(); - stack.pop(); + process_bvh_intersections(tA, tB, [ + &obb_b, + &points_in_b_cache, + &points_not_in_b_cache, + &protrusion, + &protrusion_point, + &surface_point, + &pierce, + &pierce_point1, + &pierce_point2, + &max_protrusion + ]( + opencascade::handle> bvh_a, + opencascade::handle> bvh_b, + const std::vector>& tris_a, + const std::vector>& tris_b, + const std::vector& verts_a, + const std::vector& verts_b, + const std::vector& normals_a, + const std::vector& normals_b, + int i, + const std::array& tri, + const std::vector& bvh_b_is + ) { + std::vector points_in_b; - 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)) { + for (int v_id : tri) { + if (points_not_in_b_cache.find(v_id) != points_not_in_b_cache.end()) { 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}; - } + + 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 (IfcGeom::util::is_point_in_shape(v, bvh_b, tris_b, verts_b) + && IfcGeom::util::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 { - stack.push(bvh_b->Child<0>(j)); - stack.push(bvh_b->Child<1>(j)); + points_not_in_b_cache.insert(v_id); } } - } - return bvh_clashes; + + // 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 = { + IfcGeom::util::pierce_shape(v1_a_vec, v2_a_vec, bvh_b, tris_b, verts_b, normals_b), + IfcGeom::util::pierce_shape(v1_a_vec, v3_a_vec, bvh_b, tris_b, verts_b, normals_b), + IfcGeom::util::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 }; + } + } + } + }); } clash test_intersection(const T& tA, const T& tB, double tolerance, bool check_all = true) const { @@ -1053,7 +936,7 @@ namespace IfcGeom { } bvhs_[t] = bvh; - is_manifold_[t] = is_shape_manifold(s); + is_manifold_[t] = IfcGeom::util::is_manifold(s); tris_[t] = std::move(tris); verts_[t] = std::move(verts); normals_[t] = std::move(normals); @@ -1637,245 +1520,13 @@ namespace IfcGeom { } } - 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(val); }); - - // Write faces - size_t total_verts = verts.size() / 3; - hsize_t faces_dims[1] = {faces.size()}; - H5::DataSpace faces_dataspace(1, faces_dims); - H5::DSetCreatPropList faces_propList; - faces_propList.setChunk(1, faces_dims); - faces_propList.setDeflate(9); - if (total_verts < (1 << 8)) { - H5::DataType dtype = H5::PredType::NATIVE_UINT8; - std::vector faces_dtype(faces.begin(), faces.end()); - H5::DataSet faces_dataset = group.createDataSet("faces", dtype, faces_dataspace, faces_propList); - faces_dataset.write(faces_dtype.data(), dtype); - } else if (total_verts < (1 << 16)) { - H5::DataType dtype = H5::PredType::NATIVE_UINT16; - std::vector faces_dtype(faces.begin(), faces.end()); - H5::DataSet faces_dataset = group.createDataSet("faces", dtype, faces_dataspace, faces_propList); - faces_dataset.write(faces_dtype.data(), dtype); - } else { - H5::DataType dtype = H5::PredType::NATIVE_UINT32; - H5::DataSet faces_dataset = group.createDataSet("faces", dtype, faces_dataspace, faces_propList); - faces_dataset.write(faces.data(), dtype); - } - - // Write verts - H5::DataType dtype = H5::PredType::NATIVE_FLOAT; - hsize_t dims[1] = {verts.size()}; - H5::DataSpace dataspace(1, dims); - H5::DSetCreatPropList propList; - propList.setChunk(1, dims); - propList.setDeflate(9); - H5::DataSet dataset = group.createDataSet("verts", dtype, dataspace, propList); - dataset.write(verts_float.data(), H5::PredType::NATIVE_FLOAT); - - // Write materials - std::vector material_keys; - for (const auto& material : materials) { - float alpha = 1.0; - if (material.hasTransparency() && material.transparency() > 0) { - alpha = 1.0 - material.transparency(); - } - - int i = 0; - bool is_existing_colour = false; - for (const auto& colour : colours) { - if (std::abs(colour[0] - static_cast(material.diffuse()[0])) < tolerance - && std::abs(colour[1] - static_cast(material.diffuse()[1])) < tolerance - && std::abs(colour[2] - static_cast(material.diffuse()[2])) < tolerance - && std::abs(colour[3] - alpha) < tolerance) { - is_existing_colour = true; - break; - } - i++; - } - - if ( ! is_existing_colour) { - colours.push_back({material.diffuse()[0], material.diffuse()[1], material.diffuse()[2], alpha}); - } - material_keys.push_back(i); - } - - size_t total_material_keys = material_keys.size(); - if (total_material_keys) { - hsize_t dims[1] = {material_keys.size()}; - H5::DataSpace dataspace(1, dims); - H5::DSetCreatPropList propList; - propList.setChunk(1, dims); - propList.setDeflate(9); - H5::DataType dtype = H5::PredType::NATIVE_UINT8; - H5::DataSet dataset = group.createDataSet("materials", dtype, dataspace, propList); - dataset.write(material_keys.data(), dtype); - } - - if (total_material_keys > 1) { - hsize_t dims[1] = {material_ids.size()}; - H5::DataSpace dataspace(1, dims); - H5::DSetCreatPropList propList; - propList.setChunk(1, dims); - propList.setDeflate(9); - H5::DataType dtype = H5::PredType::NATIVE_UINT8; - H5::DataSet dataset = group.createDataSet("material_ids", dtype, dataspace, propList); - std::vector data_dtype(material_ids.begin(), material_ids.end()); - dataset.write(data_dtype.data(), dtype); - } - } - - // Write GlobalIds - std::vector uuids_array; - for (const auto& id_str : global_ids) { - for (size_t i = 0; i < id_str.length(); i += 2) { - uuids_array.push_back(std::stoi(id_str.substr(i, 2), 0, 16)); - } - } - - hsize_t global_ids_dims[2] = {global_ids.size(), 16}; // 16 bytes per UUID - H5::DataSpace global_ids_dataspace(2, global_ids_dims); - H5::DataSet global_ids_dataset = file.createDataSet("element_global_ids", H5::PredType::NATIVE_UINT8, global_ids_dataspace); - global_ids_dataset.write(uuids_array.data(), H5::PredType::NATIVE_UINT8); - - // Write names - H5::StrType strType(H5::PredType::C_S1, H5T_VARIABLE); - hsize_t names_dims[1] = {names.size()}; - H5::DataSpace names_dataspace(1, names_dims); - H5::DataSet names_dataset = file.createDataSet("element_names", strType, names_dataspace); - std::vector cstr_names; - for (const auto& name : names) { - cstr_names.push_back(name.c_str()); - } - names_dataset.write(&cstr_names[0], strType); - - // Write matrices - std::vector flat_matrices; - for (const auto& matrix : matrices) { - flat_matrices.insert(flat_matrices.end(), matrix.begin(), matrix.end()); - } - hsize_t dims[2] = {matrices.size(), matrices[0].size()}; - H5::DataSpace dataspace(2, dims); - H5::DSetCreatPropList propList; - propList.setChunk(2, dims); - propList.setDeflate(9); - H5::DataSet dataset = file.createDataSet("element_matrices", H5::PredType::NATIVE_FLOAT, dataspace, propList); - dataset.write(flat_matrices.data(), H5::PredType::NATIVE_FLOAT); - - // Write element_shape_ids - hsize_t element_shape_ids_dims[1] = {element_shape_ids.size()}; - H5::DataSpace element_shape_ids_dataspace(1, element_shape_ids_dims); - H5::DSetCreatPropList element_shape_ids_propList; - element_shape_ids_propList.setChunk(1, element_shape_ids_dims); - element_shape_ids_propList.setDeflate(9); - if (geometry_index < (1 << 8)) { - H5::DataType dtype = H5::PredType::NATIVE_UINT8; - std::vector element_shape_ids_dtype(element_shape_ids.begin(), element_shape_ids.end()); - H5::DataSet element_shape_ids_dataset = file.createDataSet("element_shape_ids", dtype, element_shape_ids_dataspace, element_shape_ids_propList); - element_shape_ids_dataset.write(element_shape_ids_dtype.data(), dtype); - } else if (geometry_index < (1 << 16)) { - H5::DataType dtype = H5::PredType::NATIVE_UINT16; - std::vector element_shape_ids_dtype(element_shape_ids.begin(), element_shape_ids.end()); - H5::DataSet element_shape_ids_dataset = file.createDataSet("element_shape_ids", dtype, element_shape_ids_dataspace, element_shape_ids_propList); - element_shape_ids_dataset.write(element_shape_ids_dtype.data(), dtype); - } else if (geometry_index < (1UL << 32)) { - H5::DataType dtype = H5::PredType::NATIVE_UINT32; - std::vector element_shape_ids_dtype(element_shape_ids.begin(), element_shape_ids.end()); - H5::DataSet element_shape_ids_dataset = file.createDataSet("element_shape_ids", dtype, element_shape_ids_dataspace, element_shape_ids_propList); - element_shape_ids_dataset.write(element_shape_ids_dtype.data(), dtype); - } - - // Write colours - if (colours.size()) { - std::vector flat_colours; - for (const auto& colour : colours) { - flat_colours.insert(flat_colours.end(), colour.begin(), colour.end()); - } - hsize_t colours_dims[2] = {colours.size(), colours[0].size()}; - H5::DataSpace colours_dataspace(2, colours_dims); - H5::DSetCreatPropList colours_propList; - colours_propList.setChunk(2, colours_dims); - colours_propList.setDeflate(9); - H5::DataSet colours_dataset = file.createDataSet("materials", H5::PredType::NATIVE_FLOAT, colours_dataspace, colours_propList); - colours_dataset.write(flat_colours.data(), H5::PredType::NATIVE_FLOAT); - } - } - - void apply_matrix_to_flat_verts(const std::vector& flat_list, const std::vector& matrix, std::vector& result) { - result.clear(); - result.reserve(flat_list.size()); - - for (size_t i = 0; i < flat_list.size(); i += 3) { - float x = flat_list[i]; - float y = flat_list[i + 1]; - float z = flat_list[i + 2]; - result.push_back(x * matrix[0] + y * matrix[3] + z * matrix[6] + matrix[9]); - result.push_back(x * matrix[1] + y * matrix[4] + z * matrix[7] + matrix[10]); - result.push_back(x * matrix[2] + y * matrix[5] + z * matrix[8] + matrix[11]); - } - } - - std::string uint8_to_b64(const std::vector& uuids_array) { - std::string hex_str; - for (auto byte : uuids_array) { - // Convert each byte to a two-digit hexadecimal string and append it to the result - char hex[3]; // Two characters for the hex value and one for the null terminator - snprintf(hex, sizeof(hex), "%02x", byte); - hex_str.append(hex); - } - return hex_str; - } - - void add_triangulation_element(IfcGeom::TriangulationElement* elem, std::string name, std::string global_id) { + void add_triangulation_element(IfcGeom::TriangulationElement* elem) { triangulation_elements_.push_back(elem); const auto& t = elem->product(); const auto geometry_id = elem->geometry().id(); placements_[t] = elem->transformation().matrix().data(); - names_[t] = name; - global_ids_[t] = global_id; + names_[t] = elem->name(); + global_ids_[t] = elem->guid(); if (local_verts_.find(geometry_id) != local_verts_.end()) { return; diff --git a/src/ifcgeom_schema_agnostic/bvh_utils.h b/src/ifcgeom_schema_agnostic/bvh_utils.h new file mode 100644 index 0000000000..5a0232d14a --- /dev/null +++ b/src/ifcgeom_schema_agnostic/bvh_utils.h @@ -0,0 +1,309 @@ +#ifndef BVH_UTILS_H +#define BVH_UTILS_H + +#include +#include + +namespace IfcGeom { +namespace util { + +template +bool is_point_on_line(const T& point, const T& lineStart, const T& lineEnd) { + // 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 +} + +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 +) { + ray v_ray; + v_ray.origin[0] = static_cast(v.X()); + v_ray.origin[1] = static_cast(v.Y()); + v_ray.origin[2] = static_cast(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] = static_cast(min_point[0] - 1.e-5); + box.corners[0][1] = static_cast(min_point[1] - 1.e-5); + box.corners[0][2] = static_cast(min_point[2] - 1.e-5); + box.corners[1][0] = static_cast(max_point[0] + 1.e-5); + box.corners[1][1] = static_cast(max_point[1] + 1.e-5); + box.corners[1][2] = static_cast(max_point[2] + 1.e-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 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] = static_cast(ray_origin.X()); + v_ray.origin[1] = static_cast(ray_origin.Y()); + v_ray.origin[2] = static_cast(ray_origin.Z()); + + v_ray.dir[0] = static_cast(ray_vector.X()); + v_ray.dir[1] = static_cast(ray_vector.Y()); + v_ray.dir[2] = static_cast(ray_vector.Z()); + v_ray.dir_inv[0] = 1.0f / static_cast(ray_vector.X()); + v_ray.dir_inv[1] = 1.0f / static_cast(ray_vector.Y()); + v_ray.dir_inv[2] = 1.0f / static_cast(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] = static_cast(min_point[0] - 1.e-5); + box.corners[0][1] = static_cast(min_point[1] - 1.e-5); + box.corners[0][2] = static_cast(min_point[2] - 1.e-5); + box.corners[1][0] = static_cast(max_point[0] + 1.e-5); + box.corners[1][1] = static_cast(max_point[1] + 1.e-5); + box.corners[1][2] = static_cast(max_point[2] + 1.e-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); +} + +std::unordered_map> clash_bvh( + opencascade::handle> bvh_a, + opencascade::handle> bvh_b, + double extend = 0.0 +) { + std::unordered_map> bvh_clashes; + for (int i = 0; i < bvh_a->Length(); ++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; +} + +} +} + +#endif