commit f14838a475c2a0a571fc5fcdbc0bd03699ff546d Author: forty-twoo <1013417276@qq.com> Date: Sun Dec 17 11:07:41 2023 +0800 first add diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..382f58e --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,42 @@ +cmake_minimum_required (VERSION 3.2.0) + +# Project name +project (OCCTMeshless CXX) + +list(APPEND CMAKE_PREFIX_PATH "F:/OCC/demo/opencascade-install" ) + +# Enable C++17 +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +# OpenCascade +find_package(OpenCASCADE REQUIRED) + +# Eigen3 +set(EIGEN3_INCLUDE_DIR "F:/eigen-3.4.0") + +# Configure C++ compiler's includes dir +include_directories (SYSTEM ${OpenCASCADE_INCLUDE_DIR} ) +include_directories(${EIGEN3_INCLUDE_DIR}) +include_directories(${PROJECT_SOURCE_DIR}/include) + + +file(GLOB HEADER_FILES "include/*.h*") +file(GLOB SRC_FILES "src/*.cpp") +source_group("include" FILES HEADER_FILES) + +# Add executable +add_executable (${PROJECT_NAME} + ${SRC_FILES} + ${HEADER_FILES} +) + +# Add linker options +foreach (LIB ${OpenCASCADE_LIBRARIES}) + target_link_libraries(${PROJECT_NAME} PUBLIC ${OpenCASCADE_LIBRARY_DIR}/${LIB}.lib) + target_link_libraries(${PROJECT_NAME} PUBLIC ${OpenCASCADE_LIBRARY_DIR}d/${LIB}.lib) +endforeach() + +# Adjust runtime environment +set_property(TARGET ${PROJECT_NAME} PROPERTY VS_DEBUGGER_ENVIRONMENT "PATH=$<$:${OpenCASCADE_BINARY_DIR}d>$<$>:${OpenCASCADE_BINARY_DIR}>;%PATH%") diff --git a/include/DomainDiscretization.hpp b/include/DomainDiscretization.hpp new file mode 100644 index 0000000..c6f2e90 --- /dev/null +++ b/include/DomainDiscretization.hpp @@ -0,0 +1,96 @@ +#pragma once +#include "assert.hpp" +#include "DomainDiscretization_fwd.hpp" + +namespace meshless { + template + const vec& DomainDiscretization::normal(int i) const { + assert_msg(0 <= i && i <= size(), "Index %d out of range [0, %d)", i, size()); + assert_msg(types_[i] < 0, "Node %d must be a boundary node, got type %d.", i, types_[i]); + assert_msg(boundaryMap_[i] != -1, "Node %d does not have a normal. Maybe you manually set" + " supports and positions instead of using addInternalNode* methods?", + i); + return normals_[boundaryMap_[i]]; + + } + template + vec& DomainDiscretization::normal(int i) { + assert_msg(0 <= i && i <= size(), "Index %d out of range [0, %d)", i, size()); + assert_msg(types_[i] < 0, "Node %d must be a boundary node, got type %d.", i, types_[i]); + assert_msg(boundaryMap_[i] != -1, "Node %d does not have a normal. Maybe you manually set" + " supports and positions instead of using addInternalNode* methods?", + i); + return normals_[boundaryMap_[i]]; + + } + + template + int DomainDiscretization::addInternalNode(const vec& point, int type) { + + assert_msg(type > 0, "This function is for adding internal points, but got type %d, which is " + "not positive. Use addBoundaryNode to add boundary nodes.", + type); + return addNode(point, type); + } + + template + int DomainDiscretization::addInternalNodeWithT(const vec& point, double t, int type) { + + assert_msg(type > 0, "This function is for adding internal points, but got type %d, which is " + "not positive. Use addBoundaryNode to add boundary nodes.", + type); + return addNodeWithT(point, t, type); + } + + /** + * Adds a boundary node with given type and normal to the domain. + * @param point Coordinates of the node to add. + * @param type Type of the point, must be negative. + * @param normal Outside unit normal to the boundary at point `point`. + * @return The index of the new node. + * @sa addInternalNode + */ + template + int DomainDiscretization::addBoundaryNode(const vec& point, int type, const vec& normal) { + assert_msg(type < 0, "Type of boundary points must be negative, got %d.", type); + int idx = addNode(point, type); + boundaryMap_[idx] = normals_.size(); + normals_.push_back(normal); + return idx; + + } + + template + int DomainDiscretization::addBoundaryNodeWithT(const vec& point, double t, int type, const vec& normal) { + assert_msg(type < 0, "Type of boundary points must be negative, got %d.", type); + int idx = addNodeWithT(point, t, type); + boundaryMap_[idx] = normals_.size(); + normals_.push_back(normal); + return idx; + + } + + template + int DomainDiscretization::addNode(const vec& point, int type) { + positions_.push_back(point); + types_.push_back(type); + support_.emplace_back(); + boundaryMap_.push_back(-1); + return positions_.size() - 1; + } + + template + int DomainDiscretization::addNodeWithT(const vec& point, double t, int type) { + positions_.push_back(point); + inCurve_.push_back(t); + types_.push_back(type); + support_.emplace_back(); + boundaryMap_.push_back(-1); + return positions_.size() - 1; + } + //bool DomainDiscretization::contains(const vec& point)const; + + + + +}; \ No newline at end of file diff --git a/include/DomainDiscretization_fwd.hpp b/include/DomainDiscretization_fwd.hpp new file mode 100644 index 0000000..0b897a0 --- /dev/null +++ b/include/DomainDiscretization_fwd.hpp @@ -0,0 +1,197 @@ +#pragma once +#include +#include +#include "OccShape.hpp" + +namespace meshless { + + + + template + class DomainDiscretization { + protected: + std::vector positions_; + std::vector types_; + std::vector> support_; + std::vector boundaryMap_; + std::vector normals_; + OccShape shape_; + std::vector inCurve_; + + public: + DomainDiscretization() {} + DomainDiscretization(const OccShape& shape) { + shape_.max_points = shape.max_points; + shape_.seed_ = shape.seed_; + shape_.n_samples = shape.n_samples; + shape_.zeta = shape.zeta; + shape_.myShape = shape.myShape; + + } + const double getT(int i)const { + return inCurve_[i]; + } + const std::vector& positions()const { + return positions_; + } + + const vec& pos(int i)const { + return positions_[i]; + } + + vec& pos(int i) { + return positions_[i]; + } + double pos(int i, int j) const { + return positions_[i][j]; + } + + const std::vector >& supports()const { + return support_; + } + + const std::vector& support(int i)const { + return support_[i]; + } + + std::vector& support(int i) { + return support_[i]; + } + + int support(int i, int j) const { + return support_[i][j]; + } + + std::vector supportNodes(int i) const { + auto sp = support_[i]; + std::vector retsP; + for(auto it : sp) { + retsP.push_back(positions_[it]); + + } + return retsP; + } + /// Returns position of `j`-th support node of `i`-th node. + vec supportNode(int i, int j) const { + return positions_[support_[i][j]]; + } + /// Returns Euclidean distance to the second support node. + double dr(int i) const { + return (positions_[i] - positions_[support_[i][1]]).norm(); + } + /// Returns size of `i`-th node support. + int supportSize(int i) const { + return support_[i].size(); + } + /// Returns a vector of support sizes for each node. + //std::vector supportSizes() const; + + /// Returns types of all nodes. + const std::vector& types() const { + return types_; + } + /// Returns mutable types of all nodes. + std::vector& types() { + return types_; + } + /// Returns type of `i`-th node. + int type(int i) const { + return types_[i]; + } + /// Returns writeable type of `i`-th node. + int& type(int i) { + return types_[i]; + } + + + /// Returns boundary map. @sa boundary_map_ + const std::vector& bmap() const { + return boundaryMap_; + } + /** + * Returns index of node `node` among only boundary nodes. The returned index is + * in range `[0, boundary().size())` if `node` is a boundary node, and `-1` otherwise. + */ + int bmap(int node) const { + return boundaryMap_[node]; + } + /// Returns normals of all boundary nodes. + const std::vector& normals() const { + return normals_; + } + /** + * Returns outside unit normal of `i`-th node. The node must be a boundary node. + * @throw Assertion fails if the noe is not a boundary node, i.e.\ `type(i) < 0` must hold. + */ + const vec& normal(int i) const; + /// Returns writable outside unit normal of `i`-th node. @sa normal + vec& normal(int i); + + /// Returns indexes of all boundary nodes. + std::vector boundary() const { + std::vector ret; + for(int i = 0; i < types_.size(); i++) { + if(types_[i] < 0) { + ret.push_back(i); + } + } + return ret; + } + /// Returns indexes of all internal nodes. + std::vector interior() const { + std::vector ret; + for(int i = 0; i < types_.size(); i++) { + if(types_[i] > 0) { + ret.push_back(i); + } + } + return ret; + } + /// Returns indexes of all nodes, i.e.\ `{0, 1, ..., N-1}`. + std::vector all() const { + std::vector ret; + for(int i = 0; i < types_.size(); i++) { + if(types_[i] != 0) { + ret.push_back(i); + } + } + return ret; + } + + /// Returns `N`, the number of nodes in this discretization. + int size() const { + return positions_.size(); + } + + /** + * Adds a single interior node with specified type to this discretization. + * @param point Coordinates of the node to add. + * @param type Type of the node to add. Must be positive. + * @return The index of the new node. + * @sa addBoundaryNode + */ + int addInternalNode(const vec& point, int type); + + /** + * Adds a boundary node with given type and normal to the domain. + * @param point Coordinates of the node to add. + * @param type Type of the point, must be negative. + * @param normal Outside unit normal to the boundary at point `point`. + * @return The index of the new node. + * @sa addInternalNode + */ + int addBoundaryNode(const vec& point, int type, const vec& normal); + + int addBoundaryNodeWithT(const vec& point, double t, int type, const vec& normal); + int addInternalNodeWithT(const vec& point, double t, int type); + private: + int addNode(const vec& point, int type); + int addNodeWithT(const vec& point, double t, int type); + + public: + //bool discreteContains(const vec& point, )const; + }; + + + +}; \ No newline at end of file diff --git a/include/FillBoundary.hpp b/include/FillBoundary.hpp new file mode 100644 index 0000000..59b4c8c --- /dev/null +++ b/include/FillBoundary.hpp @@ -0,0 +1,193 @@ +#pragma once +#include "FillBoundary_fwd.hpp" +#include "KDTree.hpp" +#include "KDTreeMutable.hpp" +#include +#include +#include + +namespace meshless { + + DomainDiscretization discretizeBoundaryWithDensity(const OccShape& shape, const std::function& h, int type) { + if(type == 0)type = -1; + std::mt19937 gen(shape.seed_); + DomainDiscretization domainGlobal(shape); + KDTreeMutable treeGlobal; + TopExp_Explorer expFace(shape.myShape, TopAbs_FACE); + int numFace = 0; + + std::cout.precision(10); + + for(; expFace.More(); expFace.Next()) { + std::cout << "正在离散化第" << ++numFace << "张曲面" << std::endl; + //if(numFace != 5)continue; + bool reverseNormal = false; + const TopoDS_Face& face = TopoDS::Face(expFace.Current()); + Handle(Geom_Surface) geomSurface = BRep_Tool::Surface(face); + if(face.Orientable() == TopAbs_REVERSED) { + std::cout << "该面法线需反向\n"; + reverseNormal = true; + } + + //开始离散化每个wire + auto outerWire = BRepTools::OuterWire(face); + int numWire = 0; + TopExp_Explorer expWire(face, TopAbs_WIRE); + for(; expWire.More(); expWire.Next()) { + std::cout << "正在离散化第" << ++numWire << "个Wire" << std::endl; + const TopoDS_Wire& wire = TopoDS::Wire(expWire.Current()); + if(wire == outerWire) { + std::cout << "此wire为外环\n"; + } + + + DomainDiscretization domainParamLocalPatch(shape); + + TopExp_Explorer expEdge(wire, TopAbs_EDGE); + int numEdge = 0; + int genPoints = 0; + for(; expEdge.More(); expEdge.Next()) { + + std::cout << "正在离散化第" << ++numEdge << "条Edge" << std::endl; + //if(numEdge != 5)continue; + const TopoDS_Edge& edge = TopoDS::Edge(expEdge.Current()); + + KDTreeMutable treeLocal; + Standard_Real firstParam, lastParam; + Handle(Geom_Curve) geomCurve = BRep_Tool::Curve(edge, firstParam, lastParam); + //std::cout << "param: " << firstParam << " -> " << lastParam << '\n'; + + //曲线参数点在曲面参数域上的坐标 + Standard_Real pCurveFirstParam, pCurveLastParam; + Handle(Geom2d_Curve) geomPCurve = BRep_Tool::CurveOnSurface(edge, face, pCurveFirstParam, pCurveLastParam); + Handle(Geom2d_TrimmedCurve) geomTrimmedCurve = new Geom2d_TrimmedCurve(geomPCurve, pCurveFirstParam, pCurveLastParam); + + //GeomTools::Dump(geomCurve, std::cout); + //std::cout << "In param Pcurve:\n"; + //GeomTools::Dump(geomPCurve, std::cout); + std::cout << "GeomPcurve P:" << pCurveFirstParam << ',' << pCurveLastParam << '\n'; + + std::uniform_real_distribution<> dis(pCurveFirstParam, pCurveLastParam); + //曲线参数点 + double tSeedInParam = dis(gen); + gp_Pnt2d tSeedInPCurve; + geomPCurve->D0(tSeedInParam, tSeedInPCurve); + + + //std::cout << "t in param: " << tSeedInParam << "\n"; + //std::cout << "t in PCurve: " << tSeedInPCurve.Coord().X() << "," << tSeedInPCurve.Coord().Y() << "\n"; + + //!这里两种计算方法得到的结果不一致，第一种更准确 + gp_Pnt tSeedInCurve; + geomCurve->D0(tSeedInParam, tSeedInCurve); + gp_Pnt tSeedInSurf; + geomSurface->D0(tSeedInPCurve.X(), tSeedInPCurve.Y(), tSeedInSurf); + //! Tip: use GeomLib::NormEstim() to calculate surface normal at specified (U, V) point. + + std::cout << "t in Surface:#1 " << tSeedInCurve.Coord().X() << "," << tSeedInCurve.Coord().Y() << "," << tSeedInCurve.Coord().Z() << "\n"; + std::cout << "t in Surface:#2 " << tSeedInSurf.Coord().X() << "," << tSeedInSurf.Coord().Y() << "," << tSeedInSurf.Coord().Z() << "\n"; + + Eigen::Vector3d eigenSeedPnt(tSeedInCurve.X(), tSeedInCurve.Y(), tSeedInCurve.Z()); + Eigen::Vector2d eigenSeedPntParam(tSeedInPCurve.X(), tSeedInPCurve.Y()); + domainParamLocalPatch.addInternalNodeWithT(transPnt2d(tSeedInPCurve), tSeedInParam, 1); + treeLocal.insert(transPnt(tSeedInCurve)); + + + //Insert into global structures. + double checkRadiusSeed = h(eigenSeedPnt); + + double d_sq = treeGlobal.size() == 0 ? 10 * checkRadiusSeed * checkRadiusSeed : treeGlobal.query(eigenSeedPnt).second[0]; + if(d_sq >= (shape.zeta * checkRadiusSeed) * (shape.zeta * checkRadiusSeed)) { + //domainGlobal.addBoundaryNode(eigenSeedPnt, -1); + gp_Dir tSeedNormal; + auto retStatus = GeomLib::NormEstim(geomSurface, tSeedInPCurve, 1e-6, tSeedNormal); + if(retStatus >= 2) { + std::cout << "calculate wrong!\n"; + exit(-1); + } + if(reverseNormal) tSeedNormal = -tSeedNormal; + Eigen::Vector3d eigenNormal(tSeedNormal.X(), tSeedNormal.Y(), tSeedNormal.Z()); + domainGlobal.addBoundaryNode(eigenSeedPnt, type, eigenNormal); + treeGlobal.insert(eigenSeedPnt); + } + + int curNode = domainParamLocalPatch.size() - 1; + int endNode = domainParamLocalPatch.size(); + while(curNode < endNode && endNode < shape.max_points) { + genPoints++; + //std::cout << "curNode / endNode : " << curNode << '/' << endNode << '\n'; + auto param = domainParamLocalPatch.pos(curNode); + double t = domainParamLocalPatch.getT(curNode); + std::vector candidates{ 1.0, -1.0 }; + + gp_Pnt pt; + gp_Vec der; + geomCurve->D1(t, pt, der); + std::cout << "pt: " << pt.X() << "," << pt.Y() << "," << pt.Z() << '\n'; + std::cout << "der: " << der.X() << "," << der.Y() << "," << der.Z() << '\n'; + + + std::cout << "der.Mag:" << der.Magnitude() << '\n'; + double alpha = h(transPnt(pt)) / der.Magnitude(); + for(const auto& uCan : candidates) { + double tNew = t + alpha * uCan; + if(tNew > pCurveLastParam || tNew < pCurveFirstParam) { + std::cout << "排除\n"; + continue; + } + std::cout << "tNew:" << tNew << "\n"; + gp_Pnt2d tNewSeedInPCurve; + geomPCurve->D0(tNew, tNewSeedInPCurve); + gp_Pnt ptNew; + geomCurve->D0(tNew, ptNew); + gp_Pnt ptNew2; + geomSurface->D0(tNewSeedInPCurve.X(), tNewSeedInPCurve.Y(), ptNew2); + std::cout << "ptNew: " << ptNew.X() << "," << ptNew.Y() << "," << ptNew.Z() << '\n'; + std::cout << "ptNew2: " << ptNew2.X() << "," << ptNew2.Y() << "," << ptNew2.Z() << '\n'; + double checkradius = pt.SquareDistance(ptNew); + std::cout << "CheckRadius: " << checkradius << '\n'; + double d_sq_loc = treeLocal.query(transPnt(ptNew)).second[0]; + std::cout << "d_sq_loc: " << d_sq_loc << '\n'; + if(d_sq_loc >= (shape.zeta * shape.zeta * checkradius)) { + domainParamLocalPatch.addInternalNodeWithT(transPnt2d(tNewSeedInPCurve), tNew, 1); + treeLocal.insert(transPnt(ptNew)); + endNode++; + double d_sq_global = treeGlobal.query(transPnt(ptNew)).second[0]; + if(d_sq_global >= (shape.zeta * shape.zeta * checkradius)) { + gp_Dir tSeedNormalNew; + auto retStatus = GeomLib::NormEstim(geomSurface, tNewSeedInPCurve, 1e-6, tSeedNormalNew); + if(retStatus >= 2) { + std::cout << "calculate wrong!\n"; + exit(-1); + } + if(reverseNormal) { + tSeedNormalNew = -tSeedNormalNew; + } + domainGlobal.addBoundaryNode(transPnt(ptNew), type, transVec(tSeedNormalNew)); + treeGlobal.insert(transPnt(ptNew)); + } else { + std::cout << "全局不添加点\n"; + } + } else { + std::cout << "局部不添加点\n"; + } + } + curNode++; + } + std::cout << "该边已经生成：" << genPoints << "个点\n"; + } + } + + //fill boundary surface! + KDTree paramBoundarySearch; + + + + + + + } + return domainGlobal; + } + +}; \ No newline at end of file diff --git a/include/FillBoundary_fwd.hpp b/include/FillBoundary_fwd.hpp new file mode 100644 index 0000000..5c58432 --- /dev/null +++ b/include/FillBoundary_fwd.hpp @@ -0,0 +1,23 @@ +#pragma once +#include "OccHelper.hpp" +#include "OccShape.hpp" +#include "DomainDiscretization.hpp" +#include +#include + +namespace meshless { + + + DomainDiscretization discretizeBoundaryWithDensity(const OccShape& shape, const std::function& h, int type); + + DomainDiscretization discretizeBoundaryWithStep(const OccShape& shape, double step, int type) { + auto f = [=](Eigen::Vector3d) {return step; }; + return discretizeBoundaryWithDensity(shape, f, type); + + } + + + + + +}; \ No newline at end of file diff --git a/include/KDTree.hpp b/include/KDTree.hpp new file mode 100644 index 0000000..ce09179 --- /dev/null +++ b/include/KDTree.hpp @@ -0,0 +1,30 @@ +#pragma once +#include +#include + +namespace meshless { + std::pair, std::vector> KDTree::query(const Eigen::Vector3d& point, int k)const { + + assert_msg(point.array().isFinite().prod() == 1, "Invalid point."); + std::vector ret_index(k); + std::vector out_dist_sqr(k); + int actual_k = tree.knnSearch(point.data(), k, &ret_index[0], &out_dist_sqr[0]); + assert_msg(actual_k == k, "There were not enough points in the tree, you requested %d " + "points, the tree only contains %d points.", k, actual_k); + return { ret_index, out_dist_sqr }; + } + + std::pair, std::vector> KDTree::query(const Eigen::Vector3d& point, const double& radius_squared)const { + assert_msg(point.array().isFinite().prod() == 1, "Invalid point."); + std::vector> idx_dist; + int k = tree.radiusSearch(point.data(), radius_squared, idx_dist, nanoflann::SearchParams()); + std::vector idx(k); std::vector dists(k); + for(int i = 0; i < k; ++i) { + std::tie(idx[i], dists[i]) = idx_dist[i]; + } + return { idx, dists }; + } + + + +}; \ No newline at end of file diff --git a/include/KDTreeMutable.hpp b/include/KDTreeMutable.hpp new file mode 100644 index 0000000..67ad44f --- /dev/null +++ b/include/KDTreeMutable.hpp @@ -0,0 +1,40 @@ +#pragma once +#include "assert.hpp" +#include "KDTreeMutable_fwd.hpp" + +namespace meshless { + + void KDTreeMutable::insert(const Eigen::Vector3d& point) { + assert_msg(point.array().isFinite().prod() == 1, "Invalid point."); + auto n = points_.kdtree_get_point_count(); + points_.add(point); + tree.addPoints(n, n); + ++size_; + + } + + void KDTreeMutable::insert(const std::vector& points) { + auto n = points_.kdtree_get_point_count(); + for(const auto& p : points) { + assert_msg(p.array().isFinite().prod() == 1, "One of the points is invalid."); + points_.add(p); + } + size_ += points.size(); + tree.addPoints(n, n + points.size() - 1); + + } + + std::pair, std::vector> KDTreeMutable::query(const Eigen::Vector3d& point, int k) { + assert_msg(point.array().isFinite().prod() == 1, "Invalid query point."); + nanoflann::KNNResultSet resultSet(k); + std::vector ret_index(k); + std::vector out_dist_sqr(k); + resultSet.init(&ret_index[0], &out_dist_sqr[0]); + tree.findNeighbors(resultSet, point.data(), nanoflann::SearchParams(k)); + assert_msg(resultSet.full(), "Not enough points in the tree, you requested %d points, " + "but the tree contains only %d points.", + k, size()); + return { ret_index, out_dist_sqr }; + } + +}; \ No newline at end of file diff --git a/include/KDTreeMutable_fwd.hpp b/include/KDTreeMutable_fwd.hpp new file mode 100644 index 0000000..761496e --- /dev/null +++ b/include/KDTreeMutable_fwd.hpp @@ -0,0 +1,69 @@ +#pragma once +#include "nanoflann.hpp" +#include +#include +#include "PointCloud.hpp" + +namespace meshless { + class KDTreeMutable { + + private: + typedef nanoflann::KDTreeSingleIndexDynamicAdaptor, PointCloud, 3, int> kd_tree_t; + int size_; + PointCloud points_; + kd_tree_t tree; + + public: + explicit KDTreeMutable(const std::vector& points) :points_(points), size_(points.size()), tree(3, points_, nanoflann::KDTreeSingleIndexAdaptorParams(20)) {} + + KDTreeMutable() :points_(), size_(0), tree(3, points_, nanoflann::KDTreeSingleIndexAdaptorParams(20)) {} + + void reset(const std::vector& points) { + points_.setPts(points); + size_ = points.size(); + tree.reset(); + + } + + void insert(const Eigen::Vector3d& point); + void insert(const std::vector& points); + + /// Check if any point exists in sphere centered at `p` with radius `r`. + bool existsPointInSphere(const Eigen::Vector3d& p, double r) { + if(size_ == 0) + return false; + return query(p).second[0] <= r * r; + } + + /** + * Removes a point with given index from the tree. The indexes of the points are given + * sequentially at insertion and do not change. The removal is lazy and point still takes + * up memory. + */ + void remove(int index) { + size_ -= tree.removePoint(index); + } + + /// Returns number of points in the tree. + int size() const { + return size_; + } + + /** + * Find `k` nearest neighbors to given point. + * @param point Find closest points to this point. + * @param k How many nearest points to find. + * + * @return A pair of two vectors of size `k` containing + * indices of nearest neighbors and squared distances to said neighbors. + * @throw Assertion fails if there are not enough points in the tree. + */ + std::pair, std::vector> query(const Eigen::Vector3d& point, int k = 1); + + + + }; + + + +}; \ No newline at end of file diff --git a/include/KDTree_fwd.hpp b/include/KDTree_fwd.hpp new file mode 100644 index 0000000..c5a7de9 --- /dev/null +++ b/include/KDTree_fwd.hpp @@ -0,0 +1,49 @@ +#pragma once +#include +#include +#include + +namespace meshless { + class KDTree { + private: + typedef nanoflann::KDTreeSingleIndexAdaptor, PointCloud, 3, int> kd_tree_t; + PointCloud points_; + kd_tree_t tree; + public: + explicit KDTree(const std::vector& points) :points_(points), tree(3, points_, nanoflann::KDTreeSingleIndexAdaptorParams(20)) { + tree.buildIndex(); + } + + KDTree() :points_(), tree(3, points_, nanoflann::KDTreeSingleIndexAdaptorParams(20)) {} + + void reset(const std::vector& points) { + points_.setPts(points); + tree.buildIndex(); + } + std::pair, std::vector> query(const Eigen::Vector3d& point, int k = 1)const; + + std::pair, std::vector> query(const Eigen::Vector3d& point, const double& radius_squared) const; + + Eigen::Vector3d get(int index) const { + return points_.get(index); + } + + std::vector get(const std::vector& indexes) const { + const int n = indexes.size(); + std::vector result(n); + for(int i = 0; i < n; i++) { + result[i] = points_.get(indexes[i]); + } + return result; + } + + int size() const { + return points_.kdtree_get_point_count(); + } + + + }; + + + +}; \ No newline at end of file diff --git a/include/OccHelper.hpp b/include/OccHelper.hpp new file mode 100644 index 0000000..bf40075 --- /dev/null +++ b/include/OccHelper.hpp @@ -0,0 +1,65 @@ +#pragma once + +// OpenCascade includes +#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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +Eigen::Vector3d transVec(const gp_Vec& vec) { + return Eigen::Vector3d(vec.X(), vec.Y(), vec.Z()); +} + +Eigen::Vector3d transPnt(const gp_Pnt& Pnt) { + return Eigen::Vector3d(Pnt.X(), Pnt.Y(), Pnt.Z()); +} + +Eigen::Vector2d transPnt2d(const gp_Pnt2d& Pnt) { + return Eigen::Vector2d(Pnt.X(), Pnt.Y()); +} diff --git a/include/OccShape.hpp b/include/OccShape.hpp new file mode 100644 index 0000000..83b9137 --- /dev/null +++ b/include/OccShape.hpp @@ -0,0 +1,57 @@ +#pragma once +#include "OccHelper.hpp" +#include "assert.hpp" + +namespace meshless { + class OccShape { + public: + int max_points = 500000; + int seed_; + int n_samples = 15; + double zeta = 1 - 1e-10; + double epsilon = 0; + TopoDS_Shape myShape; + public: + OccShape() {} + OccShape(const TopoDS_Shape& myShape_) { + max_points = 500000; + seed_; + n_samples = 15; + zeta = 1 - 1e-10; + epsilon = 0; + myShape = myShape_; + } + OccShape& maxPoints(int max_points) { + this->max_points = max_points; + return *this; + } + + OccShape& seed(int seed) { + seed_ = seed; + return *this; + } + + OccShape& numSamples(int n_samples) { + this->n_samples = n_samples; + return *this; + } + + + OccShape& proximityTolerance(double zeta) { + assert_msg((0 < zeta && zeta < 1), "Zeta must be between 0 and 1, got %f.", zeta); + this->zeta = zeta; + return *this; + } + + OccShape& boundaryProximity(double epsilon) { + assert_msg((0 < epsilon && epsilon < 1), "Epsilon must be between 0 and 1, got %f.", epsilon); + this->epsilon = epsilon; + return *this; + } + + + }; + + + +}; \ No newline at end of file diff --git a/include/PointCloud.hpp b/include/PointCloud.hpp new file mode 100644 index 0000000..31518c1 --- /dev/null +++ b/include/PointCloud.hpp @@ -0,0 +1,56 @@ +#pragma once + +/** + * @file + * Implementation of KDTree storage class. + */ + +#include +#include + +namespace meshless { + + /// Helper class for KDTree with appropriate accessors containing a set of points. @ingroup utils + struct PointCloud { + std::vector pts; ///< Points, contained in the tree. + + /// Construct an empty point set. + PointCloud() = default; + + /// Construct from an array of points. + PointCloud(const std::vector& pts) : pts(pts) {} + + /// Reset contained points. + void setPts(const std::vector& pts) { + PointCloud::pts = pts; + } + + /// Interface requirement: returns number of data points. + inline int kdtree_get_point_count() const { + return pts.size(); + } + + /// Interface requirement: returns `dim`-th coordinate of `idx`-th point. + inline typename double kdtree_get_pt(const size_t idx, int dim) const { + return pts[idx][dim]; + } + + /// Access the points. + inline Eigen::Vector3d get(const size_t idx) const { + return pts[idx]; + } + + /// Add a point to the cloud. + inline void add(const Eigen::Vector3d& p) { + pts.push_back(p); + } + + /// Comply with the interface. + template + bool kdtree_get_bbox(BBOX& /* bb */) const { + return false; + } + }; + +} // namespace mm + diff --git a/include/assert.hpp b/include/assert.hpp new file mode 100644 index 0000000..4cc2fae --- /dev/null +++ b/include/assert.hpp @@ -0,0 +1,110 @@ +#pragma once + +/** + * @file + * Implementation of custom assert and debug utilities. + */ + +#include +#include "print.hpp" + +namespace meshless { + +#ifdef _MSC_VER // Check if using MSVC +#define FUNCTION_NAME __FUNCSIG__ // Use __FUNCSIG__ for function name +#else +#define FUNCTION_NAME __PRETTY_FUNCTION__ // Use __PRETTY_FUNCTION__ for other compilers +#endif + + + // print macro + /// @cond +#define VA_NUM_ARGS(...) VA_NUM_ARGS_IMPL(__VA_ARGS__, 5, 4, 3, 2, 1, 0) +#define VA_NUM_ARGS_IMPL(_1, _2, _3, _4, _5, N, ...) N +#define macro_dispatcher(func, ...) macro_dispatcher_(func, VA_NUM_ARGS(__VA_ARGS__)) +#define macro_dispatcher_(func, nargs) macro_dispatcher__(func, nargs) +#define macro_dispatcher__(func, nargs) func ## nargs +#define addflag(a) {std::cerr << "flags=[flags, " << (a) << "];" << std::endl;} +#define prnv2(a, b) {std::cerr << a << " = " << (b) << ";" << std::endl;} +#define prnv1(a) {std::cerr << #a << " = " << (a) << ";" << std::endl;} +/// @endcond +/** + * Prints a variable name and value to standard output. Can take one or two parameters. + * Example: + * @code + * int a = 6; + * prn(a) // prints 'a = 6;' + * prn("value", a) // prints 'value = 6;' + * @endcode + */ +#define prn(...) macro_dispatcher(prnv, __VA_ARGS__)(__VA_ARGS__) + + using tinyformat::printf; + using tinyformat::format; + + /// Namespace holding custom assert implementation. + namespace assert_internal { + /** + * Actual assert implementation. + * @param condition Condition to test, e.g.\ `n > 0`. + * @param file File where the assertion failed. + * @param func_name Function name where the assertion failed. + * @param line Line on which the assertion failed. + * @param message Message as specified in the `assert_msg` macro. + * @param format_list List of format field values to pass to `tinyformat::format` function. + */ + bool assert_handler_implementation(const char* condition, const char* file, const char* func_name, + int line, const char* message, tfm::FormatListRef format_list); + /// Assert handler that unpacks varargs. + template + bool assert_handler(const char* condition, const char* file, const char* func_name, int line, + const char* message, const Args&... args) { // unpacks first argument + tfm::FormatListRef arg_list = tfm::makeFormatList(args...); + return assert_handler_implementation(condition, file, func_name, line, message, arg_list); + } + } // namespace assert_internal + +#ifdef NDEBUG +#define assert_msg(cond, ...) ((void)sizeof(cond)) +#else +/** + * @brief Assert with better error reporting. + * @param cond Conditions to test. + * @param ... The second parameter is also required and represents the message to print on failure. + * For every %* field in message one additional parameter must be present. + * + * Example: + * @code + * assert_msg(n > 0, "n must be positive, got %d.", n); + * @endcode + */ +#define assert_msg(cond, ...) ((void)(!(cond) && \ + meshless::assert_internal::assert_handler( \ + #cond, __FILE__, FUNCTION_NAME, __LINE__, __VA_ARGS__) && (assert(0), 1))) + // #cond, __FILE__, __PRETTY_FUNCTION__, __LINE__, __VA_ARGS__) && (exit(1), 1))) +#endif + +/** + * Prints given text in bold red. + * @param s text to print. + */ + inline void print_red(const std::string& s) { + std::cout << "\x1b[31;1m" << s << "\x1b[37;0m"; + } + /** + * Prints given text in bold white. + * @param s text to print. + */ + inline void print_white(const std::string& s) { + std::cout << "\x1b[37;1m" << s << "\x1b[37;0m"; + } + /** + * Prints given text in bold green. + * @param s text to print. + */ + inline void print_green(const std::string& s) { + std::cout << "\x1b[32;1m" << s << "\x1b[37;0m"; + } + +} // namespace meshless + diff --git a/include/nanoflann.hpp b/include/nanoflann.hpp new file mode 100644 index 0000000..4ed8444 --- /dev/null +++ b/include/nanoflann.hpp @@ -0,0 +1,2095 @@ +/*********************************************************************** + * Software License Agreement (BSD License) + * + * Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved. + * Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved. + * Copyright 2011-2016 Jose Luis Blanco (joseluisblancoc@gmail.com). + * All rights reserved. + * + * THE BSD LICENSE + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR + * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES + * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF + * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *************************************************************************/ + + /** \mainpage nanoflann C++ API documentation + * nanoflann is a C++ header-only library for building KD-Trees, mostly + * optimized for 2D or 3D point clouds. + * + * nanoflann does not require compiling or installing, just an + * #include in your code. + * + * See: + * - C++ API organized by modules + * - Online README + * - Doxygen + * documentation + */ + +#ifndef NANOFLANN_HPP_ +#define NANOFLANN_HPP_ + +#include +#include +#include +#include // for abs() +#include // for fwrite() +#include // for abs() +#include +#include // std::reference_wrapper +#include +#include + + /** Library version: 0xMmP (M=Major,m=minor,P=patch) */ +#define NANOFLANN_VERSION 0x130 + +// Avoid conflicting declaration of min/max macros in windows headers +#if !defined(NOMINMAX) && \ + (defined(_WIN32) || defined(_WIN32_) || defined(WIN32) || defined(_WIN64)) +#define NOMINMAX +#ifdef max +#undef max +#undef min +#endif +#endif + +namespace nanoflann { + /** @addtogroup nanoflann_grp nanoflann C++ library for ANN + * @{ */ + + /** the PI constant (required to avoid MSVC missing symbols) */ + template T pi_const() { + return static_cast(3.14159265358979323846); + } + + /** + * Traits if object is resizable and assignable (typically has a resize | assign + * method) + */ + template struct has_resize : std::false_type { + }; + + template + struct has_resize().resize(1), 0)> + : std::true_type { + }; + + template struct has_assign : std::false_type { + }; + + template + struct has_assign().assign(1, 0), 0)> + : std::true_type { + }; + + /** + * Free function to resize a resizable object + */ + template + inline typename std::enable_if::value, void>::type + resize(Container& c, const size_t nElements) { + c.resize(nElements); + } + + /** + * Free function that has no effects on non resizable containers (e.g. + * std::array) It raises an exception if the expected size does not match + */ + template + inline typename std::enable_if::value, void>::type + resize(Container& c, const size_t nElements) { + if(nElements != c.size()) + throw std::logic_error("Try to change the size of a std::array."); + } + + /** + * Free function to assign to a container + */ + template + inline typename std::enable_if::value, void>::type + assign(Container& c, const size_t nElements, const T& value) { + c.assign(nElements, value); + } + + /** + * Free function to assign to a std::array + */ + template + inline typename std::enable_if::value, void>::type + assign(Container& c, const size_t nElements, const T& value) { + for(size_t i = 0; i < nElements; i++) + c[i] = value; + } + + /** @addtogroup result_sets_grp Result set classes + * @{ */ + template + class KNNResultSet { + public: + typedef _DistanceType DistanceType; + typedef _IndexType IndexType; + typedef _CountType CountType; + + private: + IndexType* indices; + DistanceType* dists; + CountType capacity; + CountType count; + + public: + inline KNNResultSet(CountType capacity_) + : indices(0), dists(0), capacity(capacity_), count(0) {} + + inline void init(IndexType* indices_, DistanceType* dists_) { + indices = indices_; + dists = dists_; + count = 0; + if(capacity) + dists[capacity - 1] = (std::numeric_limits::max)(); + } + + inline CountType size() const { + return count; + } + + inline bool full() const { + return count == capacity; + } + + /** + * Called during search to add an element matching the criteria. + * @return true if the search should be continued, false if the results are + * sufficient + */ + inline bool addPoint(DistanceType dist, IndexType index) { + CountType i; + for(i = count; i > 0; --i) { +#ifdef NANOFLANN_FIRST_MATCH // If defined and two points have the same + // distance, the one with the lowest-index will be + // returned first. + if((dists[i - 1] > dist) || + ((dist == dists[i - 1]) && (indices[i - 1] > index))) { +#else + if(dists[i - 1] > dist) { +#endif + if(i < capacity) { + dists[i] = dists[i - 1]; + indices[i] = indices[i - 1]; + } + } else + break; + } + if(i < capacity) { + dists[i] = dist; + indices[i] = index; + } + if(count < capacity) + count++; + + // tell caller that the search shall continue + return true; + } + + inline DistanceType worstDist() const { + return dists[capacity - 1]; + } + }; + + /** operator "<" for std::sort() */ + struct IndexDist_Sorter { + /** PairType will be typically: std::pair */ + template + inline bool operator()(const PairType& p1, const PairType& p2) const { + return p1.second < p2.second; + } + }; + + /** + * A result-set class used when performing a radius based search. + */ + template + class RadiusResultSet { + public: + typedef _DistanceType DistanceType; + typedef _IndexType IndexType; + + public: + const DistanceType radius; + + std::vector>& m_indices_dists; + + inline RadiusResultSet( + DistanceType radius_, + std::vector>& indices_dists) + : radius(radius_), m_indices_dists(indices_dists) { + init(); + } + + inline void init() { + clear(); + } + inline void clear() { + m_indices_dists.clear(); + } + + inline size_t size() const { + return m_indices_dists.size(); + } + + inline bool full() const { + return true; + } + + /** + * Called during search to add an element matching the criteria. + * @return true if the search should be continued, false if the results are + * sufficient + */ + inline bool addPoint(DistanceType dist, IndexType index) { + if(dist < radius) + m_indices_dists.push_back(std::make_pair(index, dist)); + return true; + } + + inline DistanceType worstDist() const { + return radius; + } + + /** + * Find the worst result (furtherest neighbor) without copying or sorting + * Pre-conditions: size() > 0 + */ + std::pair worst_item() const { + if(m_indices_dists.empty()) + throw std::runtime_error("Cannot invoke RadiusResultSet::worst_item() on " + "an empty list of results."); + typedef + typename std::vector>::const_iterator + DistIt; + DistIt it = std::max_element(m_indices_dists.begin(), m_indices_dists.end(), + IndexDist_Sorter()); + return *it; + } + }; + + /** @} */ + + /** @addtogroup loadsave_grp Load/save auxiliary functions + * @{ */ + template + void save_value(FILE* stream, const T& value, size_t count = 1) { + fwrite(&value, sizeof(value), count, stream); + } + + template + void save_value(FILE* stream, const std::vector& value) { + size_t size = value.size(); + fwrite(&size, sizeof(size_t), 1, stream); + fwrite(&value[0], sizeof(T), size, stream); + } + + template + void load_value(FILE* stream, T& value, size_t count = 1) { + size_t read_cnt = fread(&value, sizeof(value), count, stream); + if(read_cnt != count) { + throw std::runtime_error("Cannot read from file"); + } + } + + template void load_value(FILE* stream, std::vector& value) { + size_t size; + size_t read_cnt = fread(&size, sizeof(size_t), 1, stream); + if(read_cnt != 1) { + throw std::runtime_error("Cannot read from file"); + } + value.resize(size); + read_cnt = fread(&value[0], sizeof(T), size, stream); + if(read_cnt != size) { + throw std::runtime_error("Cannot read from file"); + } + } + /** @} */ + + /** @addtogroup metric_grp Metric (distance) classes + * @{ */ + + struct Metric { + }; + + /** Manhattan distance functor (generic version, optimized for + * high-dimensionality data sets). Corresponding distance traits: + * nanoflann::metric_L1 \tparam T Type of the elements (e.g. double, float, + * uint8_t) \tparam _DistanceType Type of distance variables (must be signed) + * (e.g. float, double, int64_t) + */ + template + struct L1_Adaptor { + typedef T ElementType; + typedef _DistanceType DistanceType; + + const DataSource& data_source; + + L1_Adaptor(const DataSource& _data_source) : data_source(_data_source) {} + + inline DistanceType evalMetric(const T* a, const size_t b_idx, size_t size, + DistanceType worst_dist = -1) const { + DistanceType result = DistanceType(); + const T* last = a + size; + const T* lastgroup = last - 3; + size_t d = 0; + + /* Process 4 items with each loop for efficiency. */ + while(a < lastgroup) { + const DistanceType diff0 = + std::abs(a[0] - data_source.kdtree_get_pt(b_idx, d++)); + const DistanceType diff1 = + std::abs(a[1] - data_source.kdtree_get_pt(b_idx, d++)); + const DistanceType diff2 = + std::abs(a[2] - data_source.kdtree_get_pt(b_idx, d++)); + const DistanceType diff3 = + std::abs(a[3] - data_source.kdtree_get_pt(b_idx, d++)); + result += diff0 + diff1 + diff2 + diff3; + a += 4; + if((worst_dist > 0) && (result > worst_dist)) { + return result; + } + } + /* Process last 0-3 components. Not needed for standard vector lengths. */ + while(a < last) { + result += std::abs(*a++ - data_source.kdtree_get_pt(b_idx, d++)); + } + return result; + } + + template + inline DistanceType accum_dist(const U a, const V b, const size_t) const { + return std::abs(a - b); + } + }; + + /** Squared Euclidean distance functor (generic version, optimized for + * high-dimensionality data sets). Corresponding distance traits: + * nanoflann::metric_L2 \tparam T Type of the elements (e.g. double, float, + * uint8_t) \tparam _DistanceType Type of distance variables (must be signed) + * (e.g. float, double, int64_t) + */ + template + struct L2_Adaptor { + typedef T ElementType; + typedef _DistanceType DistanceType; + + const DataSource& data_source; + + L2_Adaptor(const DataSource& _data_source) : data_source(_data_source) {} + + inline DistanceType evalMetric(const T* a, const size_t b_idx, size_t size, + DistanceType worst_dist = -1) const { + DistanceType result = DistanceType(); + const T* last = a + size; + const T* lastgroup = last - 3; + size_t d = 0; + + /* Process 4 items with each loop for efficiency. */ + while(a < lastgroup) { + const DistanceType diff0 = a[0] - data_source.kdtree_get_pt(b_idx, d++); + const DistanceType diff1 = a[1] - data_source.kdtree_get_pt(b_idx, d++); + const DistanceType diff2 = a[2] - data_source.kdtree_get_pt(b_idx, d++); + const DistanceType diff3 = a[3] - data_source.kdtree_get_pt(b_idx, d++); + result += diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3; + a += 4; + if((worst_dist > 0) && (result > worst_dist)) { + return result; + } + } + /* Process last 0-3 components. Not needed for standard vector lengths. */ + while(a < last) { + const DistanceType diff0 = *a++ - data_source.kdtree_get_pt(b_idx, d++); + result += diff0 * diff0; + } + return result; + } + + template + inline DistanceType accum_dist(const U a, const V b, const size_t) const { + return (a - b) * (a - b); + } + }; + + /** Squared Euclidean (L2) distance functor (suitable for low-dimensionality + * datasets, like 2D or 3D point clouds) Corresponding distance traits: + * nanoflann::metric_L2_Simple \tparam T Type of the elements (e.g. double, + * float, uint8_t) \tparam _DistanceType Type of distance variables (must be + * signed) (e.g. float, double, int64_t) + */ + template + struct L2_Simple_Adaptor { + typedef T ElementType; + typedef _DistanceType DistanceType; + + const DataSource& data_source; + + L2_Simple_Adaptor(const DataSource& _data_source) + : data_source(_data_source) {} + + inline DistanceType evalMetric(const T* a, const size_t b_idx, + size_t size) const { + DistanceType result = DistanceType(); + for(size_t i = 0; i < size; ++i) { + const DistanceType diff = a[i] - data_source.kdtree_get_pt(b_idx, i); + result += diff * diff; + } + return result; + } + + template + inline DistanceType accum_dist(const U a, const V b, const size_t) const { + return (a - b) * (a - b); + } + }; + + /** SO2 distance functor + * Corresponding distance traits: nanoflann::metric_SO2 + * \tparam T Type of the elements (e.g. double, float) + * \tparam _DistanceType Type of distance variables (must be signed) (e.g. + * float, double) orientation is constrained to be in [-pi, pi] + */ + template + struct SO2_Adaptor { + typedef T ElementType; + typedef _DistanceType DistanceType; + + const DataSource& data_source; + + SO2_Adaptor(const DataSource& _data_source) : data_source(_data_source) {} + + inline DistanceType evalMetric(const T* a, const size_t b_idx, + size_t size) const { + return accum_dist(a[size - 1], data_source.kdtree_get_pt(b_idx, size - 1), + size - 1); + } + + /** Note: this assumes that input angles are already in the range [-pi,pi] */ + template + inline DistanceType accum_dist(const U a, const V b, const size_t) const { + DistanceType result = DistanceType(), PI = pi_const(); + result = b - a; + if(result > PI) + result -= 2 * PI; + else if(result < -PI) + result += 2 * PI; + return result; + } + }; + + /** SO3 distance functor (Uses L2_Simple) + * Corresponding distance traits: nanoflann::metric_SO3 + * \tparam T Type of the elements (e.g. double, float) + * \tparam _DistanceType Type of distance variables (must be signed) (e.g. + * float, double) + */ + template + struct SO3_Adaptor { + typedef T ElementType; + typedef _DistanceType DistanceType; + + L2_Simple_Adaptor distance_L2_Simple; + + SO3_Adaptor(const DataSource& _data_source) + : distance_L2_Simple(_data_source) {} + + inline DistanceType evalMetric(const T* a, const size_t b_idx, + size_t size) const { + return distance_L2_Simple.evalMetric(a, b_idx, size); + } + + template + inline DistanceType accum_dist(const U a, const V b, const size_t idx) const { + return distance_L2_Simple.accum_dist(a, b, idx); + } + }; + + /** Metaprogramming helper traits class for the L1 (Manhattan) metric */ + struct metric_L1 : public Metric { + template struct traits { + typedef L1_Adaptor distance_t; + }; + }; + /** Metaprogramming helper traits class for the L2 (Euclidean) metric */ + struct metric_L2 : public Metric { + template struct traits { + typedef L2_Adaptor distance_t; + }; + }; + /** Metaprogramming helper traits class for the L2_simple (Euclidean) metric */ + struct metric_L2_Simple : public Metric { + template struct traits { + typedef L2_Simple_Adaptor distance_t; + }; + }; + /** Metaprogramming helper traits class for the SO3_InnerProdQuat metric */ + struct metric_SO2 : public Metric { + template struct traits { + typedef SO2_Adaptor distance_t; + }; + }; + /** Metaprogramming helper traits class for the SO3_InnerProdQuat metric */ + struct metric_SO3 : public Metric { + template struct traits { + typedef SO3_Adaptor distance_t; + }; + }; + + /** @} */ + + /** @addtogroup param_grp Parameter structs + * @{ */ + + /** Parameters (see README.md) */ + struct KDTreeSingleIndexAdaptorParams { + KDTreeSingleIndexAdaptorParams(size_t _leaf_max_size = 10) + : leaf_max_size(_leaf_max_size) {} + + size_t leaf_max_size; + }; + + /** Search options for KDTreeSingleIndexAdaptor::findNeighbors() */ + struct SearchParams { + /** Note: The first argument (checks_IGNORED_) is ignored, but kept for + * compatibility with the FLANN interface */ + SearchParams(int checks_IGNORED_ = 32, float eps_ = 0, bool sorted_ = true) + : checks(checks_IGNORED_), eps(eps_), sorted(sorted_) {} + + int checks; //!< Ignored parameter (Kept for compatibility with the FLANN + //!< interface). + float eps; //!< search for eps-approximate neighbours (default: 0) + bool sorted; //!< only for radius search, require neighbours sorted by + //!< distance (default: true) + }; + /** @} */ + + /** @addtogroup memalloc_grp Memory allocation + * @{ */ + + /** + * Allocates (using C's malloc) a generic type T. + * + * Params: + * count = number of instances to allocate. + * Returns: pointer (of type T*) to memory buffer + */ + template inline T* allocate(size_t count = 1) { + T* mem = static_cast(::malloc(sizeof(T) * count)); + return mem; + } + + /** + * Pooled storage allocator + * + * The following routines allow for the efficient allocation of storage in + * small chunks from a specified pool. Rather than allowing each structure + * to be freed individually, an entire pool of storage is freed at once. + * This method has two advantages over just using malloc() and free(). First, + * it is far more efficient for allocating small objects, as there is + * no overhead for remembering all the information needed to free each + * object or consolidating fragmented memory. Second, the decision about + * how long to keep an object is made at the time of allocation, and there + * is no need to track down all the objects to free them. + * + */ + + const size_t WORDSIZE = 16; + const size_t BLOCKSIZE = 8192; + + class PooledAllocator { + /* We maintain memory alignment to word boundaries by requiring that all + allocations be in multiples of the machine wordsize. */ + /* Size of machine word in bytes. Must be power of 2. */ + /* Minimum number of bytes requested at a time from the system. Must be + * multiple of WORDSIZE. */ + + size_t remaining; /* Number of bytes left in current block of storage. */ + void* base; /* Pointer to base of current block of storage. */ + void* loc; /* Current location in block to next allocate memory. */ + + void internal_init() { + remaining = 0; + base = NULL; + usedMemory = 0; + wastedMemory = 0; + } + + public: + size_t usedMemory; + size_t wastedMemory; + + /** + Default constructor. Initializes a new pool. + */ + PooledAllocator() { + internal_init(); + } + + /** + * Destructor. Frees all the memory allocated in this pool. + */ + ~PooledAllocator() { + free_all(); + } + + /** Frees all allocated memory chunks */ + void free_all() { + while(base != NULL) { + void* prev = + *(static_cast(base)); /* Get pointer to prev block. */ + ::free(base); + base = prev; + } + internal_init(); + } + + /** + * Returns a pointer to a piece of new memory of the given size in bytes + * allocated from the pool. + */ + void* malloc(const size_t req_size) { + /* Round size up to a multiple of wordsize. The following expression + only works for WORDSIZE that is a power of 2, by masking last bits of + incremented size to zero. + */ + const size_t size = (req_size + (WORDSIZE - 1)) & ~(WORDSIZE - 1); + + /* Check whether a new block must be allocated. Note that the first word + of a block is reserved for a pointer to the previous block. + */ + if(size > remaining) { + + wastedMemory += remaining; + + /* Allocate new storage. */ + const size_t blocksize = + (size + sizeof(void*) + (WORDSIZE - 1) > BLOCKSIZE) + ? size + sizeof(void*) + (WORDSIZE - 1) + : BLOCKSIZE; + + // use the standard C malloc to allocate memory + void* m = ::malloc(blocksize); + if(!m) { + fprintf(stderr, "Failed to allocate memory.\n"); + return NULL; + } + + /* Fill first word of new block with pointer to previous block. */ + static_cast(m)[0] = base; + base = m; + + size_t shift = 0; + // int size_t = (WORDSIZE - ( (((size_t)m) + sizeof(void*)) & + // (WORDSIZE-1))) & (WORDSIZE-1); + + remaining = blocksize - sizeof(void*) - shift; + loc = (static_cast(m) + sizeof(void*) + shift); + } + void* rloc = loc; + loc = static_cast(loc) + size; + remaining -= size; + + usedMemory += size; + + return rloc; + } + + /** + * Allocates (using this pool) a generic type T. + * + * Params: + * count = number of instances to allocate. + * Returns: pointer (of type T*) to memory buffer + */ + template T* allocate(const size_t count = 1) { + T* mem = static_cast(this->malloc(sizeof(T) * count)); + return mem; + } + }; + /** @} */ + + /** @addtogroup nanoflann_metaprog_grp Auxiliary metaprogramming stuff + * @{ */ + + /** Used to declare fixed-size arrays when DIM>0, dynamically-allocated vectors + * when DIM=-1. Fixed size version for a generic DIM: + */ + template struct array_or_vector_selector { + typedef std::array container_t; + }; + /** Dynamic size version */ + template struct array_or_vector_selector<-1, T> { + typedef std::vector container_t; + }; + + /** @} */ + + /** kd-tree base-class + * + * Contains the member functions common to the classes KDTreeSingleIndexAdaptor + * and KDTreeSingleIndexDynamicAdaptor_. + * + * \tparam Derived The name of the class which inherits this class. + * \tparam DatasetAdaptor The user-provided adaptor (see comments above). + * \tparam Distance The distance metric to use, these are all classes derived + * from nanoflann::Metric \tparam DIM Dimensionality of data points (e.g. 3 for + * 3D points) \tparam IndexType Will be typically size_t or int + */ + + template + class KDTreeBaseClass { + + public: + /** Frees the previously-built index. Automatically called within + * buildIndex(). */ + void freeIndex(Derived& obj) { + obj.pool.free_all(); + obj.root_node = NULL; + obj.m_size_at_index_build = 0; + } + + typedef typename Distance::ElementType ElementType; + typedef typename Distance::DistanceType DistanceType; + + /*--------------------- Internal Data Structures --------------------------*/ + struct Node { + /** Union used because a node can be either a LEAF node or a non-leaf node, + * so both data fields are never used simultaneously */ + union { + struct leaf { + IndexType left, right; //!< Indices of points in leaf node + } lr; + struct nonleaf { + int divfeat; //!< Dimension used for subdivision. + DistanceType divlow, divhigh; //!< The values used for subdivision. + } sub; + } node_type; + Node* child1, * child2; //!< Child nodes (both=NULL mean its a leaf node) + }; + + typedef Node* NodePtr; + + struct Interval { + ElementType low, high; + }; + + /** + * Array of indices to vectors in the dataset. + */ + std::vector vind; + + NodePtr root_node; + + size_t m_leaf_max_size; + + size_t m_size; //!< Number of current points in the dataset + size_t m_size_at_index_build; //!< Number of points in the dataset when the + //!< index was built + int dim; //!< Dimensionality of each data point + + /** Define "BoundingBox" as a fixed-size or variable-size container depending + * on "DIM" */ + typedef + typename array_or_vector_selector::container_t BoundingBox; + + /** Define "distance_vector_t" as a fixed-size or variable-size container + * depending on "DIM" */ + typedef typename array_or_vector_selector::container_t + distance_vector_t; + + /** The KD-tree used to find neighbours */ + + BoundingBox root_bbox; + + /** + * Pooled memory allocator. + * + * Using a pooled memory allocator is more efficient + * than allocating memory directly when there is a large + * number small of memory allocations. + */ + PooledAllocator pool; + + /** Returns number of points in dataset */ + size_t size(const Derived& obj) const { + return obj.m_size; + } + + /** Returns the length of each point in the dataset */ + size_t veclen(const Derived& obj) { + return static_cast(DIM > 0 ? DIM : obj.dim); + } + + /// Helper accessor to the dataset points: + inline ElementType dataset_get(const Derived& obj, size_t idx, + int component) const { + return obj.dataset.kdtree_get_pt(idx, component); + } + + /** + * Computes the inde memory usage + * Returns: memory used by the index + */ + size_t usedMemory(Derived& obj) { + return obj.pool.usedMemory + obj.pool.wastedMemory + + obj.dataset.kdtree_get_point_count() * + sizeof(IndexType); // pool memory and vind array memory + } + + void computeMinMax(const Derived& obj, IndexType* ind, IndexType count, + int element, ElementType& min_elem, + ElementType& max_elem) { + min_elem = dataset_get(obj, ind[0], element); + max_elem = dataset_get(obj, ind[0], element); + for(IndexType i = 1; i < count; ++i) { + ElementType val = dataset_get(obj, ind[i], element); + if(val < min_elem) + min_elem = val; + if(val > max_elem) + max_elem = val; + } + } + + /** + * Create a tree node that subdivides the list of vecs from vind[first] + * to vind[last]. The routine is called recursively on each sublist. + * + * @param left index of the first vector + * @param right index of the last vector + */ + NodePtr divideTree(Derived& obj, const IndexType left, const IndexType right, + BoundingBox& bbox) { + NodePtr node = obj.pool.template allocate(); // allocate memory + + /* If too few exemplars remain, then make this a leaf node. */ + if((right - left) <= static_cast(obj.m_leaf_max_size)) { + node->child1 = node->child2 = NULL; /* Mark as leaf node. */ + node->node_type.lr.left = left; + node->node_type.lr.right = right; + + // compute bounding-box of leaf points + for(int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { + bbox[i].low = dataset_get(obj, obj.vind[left], i); + bbox[i].high = dataset_get(obj, obj.vind[left], i); + } + for(IndexType k = left + 1; k < right; ++k) { + for(int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { + if(bbox[i].low > dataset_get(obj, obj.vind[k], i)) + bbox[i].low = dataset_get(obj, obj.vind[k], i); + if(bbox[i].high < dataset_get(obj, obj.vind[k], i)) + bbox[i].high = dataset_get(obj, obj.vind[k], i); + } + } + } else { + IndexType idx; + int cutfeat; + DistanceType cutval; + middleSplit_(obj, &obj.vind[0] + left, right - left, idx, cutfeat, cutval, + bbox); + + node->node_type.sub.divfeat = cutfeat; + + BoundingBox left_bbox(bbox); + left_bbox[cutfeat].high = cutval; + node->child1 = divideTree(obj, left, left + idx, left_bbox); + + BoundingBox right_bbox(bbox); + right_bbox[cutfeat].low = cutval; + node->child2 = divideTree(obj, left + idx, right, right_bbox); + + node->node_type.sub.divlow = left_bbox[cutfeat].high; + node->node_type.sub.divhigh = right_bbox[cutfeat].low; + + for(int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { + bbox[i].low = std::min(left_bbox[i].low, right_bbox[i].low); + bbox[i].high = std::max(left_bbox[i].high, right_bbox[i].high); + } + } + + return node; + } + + void middleSplit_(Derived& obj, IndexType* ind, IndexType count, + IndexType& index, int& cutfeat, DistanceType& cutval, + const BoundingBox& bbox) { + const DistanceType EPS = static_cast(0.00001); + ElementType max_span = bbox[0].high - bbox[0].low; + for(int i = 1; i < (DIM > 0 ? DIM : obj.dim); ++i) { + ElementType span = bbox[i].high - bbox[i].low; + if(span > max_span) { + max_span = span; + } + } + ElementType max_spread = -1; + cutfeat = 0; + for(int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { + ElementType span = bbox[i].high - bbox[i].low; + if(span > (1 - EPS) * max_span) { + ElementType min_elem, max_elem; + computeMinMax(obj, ind, count, i, min_elem, max_elem); + ElementType spread = max_elem - min_elem; + ; + if(spread > max_spread) { + cutfeat = i; + max_spread = spread; + } + } + } + // split in the middle + DistanceType split_val = (bbox[cutfeat].low + bbox[cutfeat].high) / 2; + ElementType min_elem, max_elem; + computeMinMax(obj, ind, count, cutfeat, min_elem, max_elem); + + if(split_val < min_elem) + cutval = min_elem; + else if(split_val > max_elem) + cutval = max_elem; + else + cutval = split_val; + + IndexType lim1, lim2; + planeSplit(obj, ind, count, cutfeat, cutval, lim1, lim2); + + if(lim1 > count / 2) + index = lim1; + else if(lim2 < count / 2) + index = lim2; + else + index = count / 2; + } + + /** + * Subdivide the list of points by a plane perpendicular on axe corresponding + * to the 'cutfeat' dimension at 'cutval' position. + * + * On return: + * dataset[ind[0..lim1-1]][cutfeat]cutval + */ + void planeSplit(Derived& obj, IndexType* ind, const IndexType count, + int cutfeat, DistanceType& cutval, IndexType& lim1, + IndexType& lim2) { + /* Move vector indices for left subtree to front of list. */ + IndexType left = 0; + IndexType right = count - 1; + for(;;) { + while(left <= right && dataset_get(obj, ind[left], cutfeat) < cutval) + ++left; + while(right && left <= right && + dataset_get(obj, ind[right], cutfeat) >= cutval) + --right; + if(left > right || !right) + break; // "!right" was added to support unsigned Index types + std::swap(ind[left], ind[right]); + ++left; + --right; + } + /* If either list is empty, it means that all remaining features + * are identical. Split in the middle to maintain a balanced tree. + */ + lim1 = left; + right = count - 1; + for(;;) { + while(left <= right && dataset_get(obj, ind[left], cutfeat) <= cutval) + ++left; + while(right && left <= right && + dataset_get(obj, ind[right], cutfeat) > cutval) + --right; + if(left > right || !right) + break; // "!right" was added to support unsigned Index types + std::swap(ind[left], ind[right]); + ++left; + --right; + } + lim2 = left; + } + + DistanceType computeInitialDistances(const Derived& obj, + const ElementType* vec, + distance_vector_t& dists) const { + assert(vec); + DistanceType distsq = DistanceType(); + + for(int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { + if(vec[i] < obj.root_bbox[i].low) { + dists[i] = obj.distance.accum_dist(vec[i], obj.root_bbox[i].low, i); + distsq += dists[i]; + } + if(vec[i] > obj.root_bbox[i].high) { + dists[i] = obj.distance.accum_dist(vec[i], obj.root_bbox[i].high, i); + distsq += dists[i]; + } + } + return distsq; + } + + void save_tree(Derived& obj, FILE* stream, NodePtr tree) { + save_value(stream, *tree); + if(tree->child1 != NULL) { + save_tree(obj, stream, tree->child1); + } + if(tree->child2 != NULL) { + save_tree(obj, stream, tree->child2); + } + } + + void load_tree(Derived& obj, FILE* stream, NodePtr& tree) { + tree = obj.pool.template allocate(); + load_value(stream, *tree); + if(tree->child1 != NULL) { + load_tree(obj, stream, tree->child1); + } + if(tree->child2 != NULL) { + load_tree(obj, stream, tree->child2); + } + } + + /** Stores the index in a binary file. + * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when + * loading the index object it must be constructed associated to the same + * source of data points used while building it. See the example: + * examples/saveload_example.cpp \sa loadIndex */ + void saveIndex_(Derived& obj, FILE* stream) { + save_value(stream, obj.m_size); + save_value(stream, obj.dim); + save_value(stream, obj.root_bbox); + save_value(stream, obj.m_leaf_max_size); + save_value(stream, obj.vind); + save_tree(obj, stream, obj.root_node); + } + + /** Loads a previous index from a binary file. + * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the + * index object must be constructed associated to the same source of data + * points used while building the index. See the example: + * examples/saveload_example.cpp \sa loadIndex */ + void loadIndex_(Derived& obj, FILE* stream) { + load_value(stream, obj.m_size); + load_value(stream, obj.dim); + load_value(stream, obj.root_bbox); + load_value(stream, obj.m_leaf_max_size); + load_value(stream, obj.vind); + load_tree(obj, stream, obj.root_node); + } + }; + + /** @addtogroup kdtrees_grp KD-tree classes and adaptors + * @{ */ + + /** kd-tree static index + * + * Contains the k-d trees and other information for indexing a set of points + * for nearest-neighbor matching. + * + * The class "DatasetAdaptor" must provide the following interface (can be + * non-virtual, inlined methods): + * + * \code + * // Must return the number of data poins + * inline size_t kdtree_get_point_count() const { ... } + * + * + * // Must return the dim'th component of the idx'th point in the class: + * inline T kdtree_get_pt(const size_t idx, const size_t dim) const { ... } + * + * // Optional bounding-box computation: return false to default to a standard + * bbox computation loop. + * // Return true if the BBOX was already computed by the class and returned + * in "bb" so it can be avoided to redo it again. + * // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 + * for point clouds) template bool kdtree_get_bbox(BBOX &bb) const + * { + * bb[0].low = ...; bb[0].high = ...; // 0th dimension limits + * bb[1].low = ...; bb[1].high = ...; // 1st dimension limits + * ... + * return true; + * } + * + * \endcode + * + * \tparam DatasetAdaptor The user-provided adaptor (see comments above). + * \tparam Distance The distance metric to use: nanoflann::metric_L1, + * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam DIM + * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will + * be typically size_t or int + */ + template + class KDTreeSingleIndexAdaptor + : public KDTreeBaseClass< + KDTreeSingleIndexAdaptor, + Distance, DatasetAdaptor, DIM, IndexType> { + public: + /** Deleted copy constructor*/ + KDTreeSingleIndexAdaptor( + const KDTreeSingleIndexAdaptor + &) = delete; + + /** + * The dataset used by this index + */ + const DatasetAdaptor& dataset; //!< The source of our data + + const KDTreeSingleIndexAdaptorParams index_params; + + Distance distance; + + typedef typename nanoflann::KDTreeBaseClass< + nanoflann::KDTreeSingleIndexAdaptor, + Distance, DatasetAdaptor, DIM, IndexType> + BaseClassRef; + + typedef typename BaseClassRef::ElementType ElementType; + typedef typename BaseClassRef::DistanceType DistanceType; + + typedef typename BaseClassRef::Node Node; + typedef Node* NodePtr; + + typedef typename BaseClassRef::Interval Interval; + /** Define "BoundingBox" as a fixed-size or variable-size container depending + * on "DIM" */ + typedef typename BaseClassRef::BoundingBox BoundingBox; + + /** Define "distance_vector_t" as a fixed-size or variable-size container + * depending on "DIM" */ + typedef typename BaseClassRef::distance_vector_t distance_vector_t; + + /** + * KDTree constructor + * + * Refer to docs in README.md or online in + * https://github.com/jlblancoc/nanoflann + * + * The KD-Tree point dimension (the length of each point in the datase, e.g. 3 + * for 3D points) is determined by means of: + * - The \a DIM template parameter if >0 (highest priority) + * - Otherwise, the \a dimensionality parameter of this constructor. + * + * @param inputData Dataset with the input features + * @param params Basically, the maximum leaf node size + */ + KDTreeSingleIndexAdaptor(const int dimensionality, + const DatasetAdaptor& inputData, + const KDTreeSingleIndexAdaptorParams& params = + KDTreeSingleIndexAdaptorParams()) + : dataset(inputData), index_params(params), distance(inputData) { + BaseClassRef::root_node = NULL; + BaseClassRef::m_size = dataset.kdtree_get_point_count(); + BaseClassRef::m_size_at_index_build = BaseClassRef::m_size; + BaseClassRef::dim = dimensionality; + if(DIM > 0) + BaseClassRef::dim = DIM; + BaseClassRef::m_leaf_max_size = params.leaf_max_size; + + // Create a permutable array of indices to the input vectors. + init_vind(); + } + + /** + * Builds the index + */ + void buildIndex() { + BaseClassRef::m_size = dataset.kdtree_get_point_count(); + BaseClassRef::m_size_at_index_build = BaseClassRef::m_size; + init_vind(); + this->freeIndex(*this); + BaseClassRef::m_size_at_index_build = BaseClassRef::m_size; + if(BaseClassRef::m_size == 0) + return; + computeBoundingBox(BaseClassRef::root_bbox); + BaseClassRef::root_node = + this->divideTree(*this, 0, BaseClassRef::m_size, + BaseClassRef::root_bbox); // construct the tree + } + + /** \name Query methods + * @{ */ + + /** + * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored + * inside the result object. + * + * Params: + * result = the result object in which the indices of the + * nearest-neighbors are stored vec = the vector for which to search the + * nearest neighbors + * + * \tparam RESULTSET Should be any ResultSet + * \return True if the requested neighbors could be found. + * \sa knnSearch, radiusSearch + */ + template + bool findNeighbors(RESULTSET& result, const ElementType* vec, + const SearchParams& searchParams) const { + assert(vec); + if(this->size(*this) == 0) + return false; + if(!BaseClassRef::root_node) + throw std::runtime_error( + "[nanoflann] findNeighbors() called before building the index."); + float epsError = 1 + searchParams.eps; + + distance_vector_t + dists; // fixed or variable-sized container (depending on DIM) + auto zero = static_cast(0); + assign(dists, (DIM > 0 ? DIM : BaseClassRef::dim), + zero); // Fill it with zeros. + DistanceType distsq = this->computeInitialDistances(*this, vec, dists); + searchLevel(result, vec, BaseClassRef::root_node, distsq, dists, + epsError); // "count_leaf" parameter removed since was neither + // used nor returned to the user. + return result.full(); + } + + /** + * Find the "num_closest" nearest neighbors to the \a query_point[0:dim-1]. + * Their indices are stored inside the result object. \sa radiusSearch, + * findNeighbors \note nChecks_IGNORED is ignored but kept for compatibility + * with the original FLANN interface. \return Number `N` of valid points in + * the result set. Only the first `N` entries in `out_indices` and + * `out_distances_sq` will be valid. Return may be less than `num_closest` + * only if the number of elements in the tree is less than `num_closest`. + */ + size_t knnSearch(const ElementType* query_point, const size_t num_closest, + IndexType* out_indices, DistanceType* out_distances_sq, + const int /* nChecks_IGNORED */ = 10) const { + nanoflann::KNNResultSet resultSet(num_closest); + resultSet.init(out_indices, out_distances_sq); + this->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); + return resultSet.size(); + } + + /** + * Find all the neighbors to \a query_point[0:dim-1] within a maximum radius. + * The output is given as a vector of pairs, of which the first element is a + * point index and the second the corresponding distance. Previous contents of + * \a IndicesDists are cleared. + * + * If searchParams.sorted==true, the output list is sorted by ascending + * distances. + * + * For a better performance, it is advisable to do a .reserve() on the vector + * if you have any wild guess about the number of expected matches. + * + * \sa knnSearch, findNeighbors, radiusSearchCustomCallback + * \return The number of points within the given radius (i.e. indices.size() + * or dists.size() ) + */ + size_t + radiusSearch(const ElementType* query_point, const DistanceType& radius, + std::vector>& IndicesDists, + const SearchParams& searchParams) const { + RadiusResultSet resultSet(radius, IndicesDists); + const size_t nFound = + radiusSearchCustomCallback(query_point, resultSet, searchParams); + if(searchParams.sorted) + std::sort(IndicesDists.begin(), IndicesDists.end(), IndexDist_Sorter()); + return nFound; + } + + /** + * Just like radiusSearch() but with a custom callback class for each point + * found in the radius of the query. See the source of RadiusResultSet<> as a + * start point for your own classes. \sa radiusSearch + */ + template + size_t radiusSearchCustomCallback( + const ElementType* query_point, SEARCH_CALLBACK& resultSet, + const SearchParams& searchParams = SearchParams()) const { + this->findNeighbors(resultSet, query_point, searchParams); + return resultSet.size(); + } + + /** @} */ + + public: + /** Make sure the auxiliary list \a vind has the same size than the current + * dataset, and re-generate if size has changed. */ + void init_vind() { + // Create a permutable array of indices to the input vectors. + BaseClassRef::m_size = dataset.kdtree_get_point_count(); + if(BaseClassRef::vind.size() != BaseClassRef::m_size) + BaseClassRef::vind.resize(BaseClassRef::m_size); + for(size_t i = 0; i < BaseClassRef::m_size; i++) + BaseClassRef::vind[i] = i; + } + + void computeBoundingBox(BoundingBox& bbox) { + resize(bbox, (DIM > 0 ? DIM : BaseClassRef::dim)); + if(dataset.kdtree_get_bbox(bbox)) { + // Done! It was implemented in derived class + } else { + const size_t N = dataset.kdtree_get_point_count(); + if(!N) + throw std::runtime_error("[nanoflann] computeBoundingBox() called but " + "no data points found."); + for(int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) { + bbox[i].low = bbox[i].high = this->dataset_get(*this, 0, i); + } + for(size_t k = 1; k < N; ++k) { + for(int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) { + if(this->dataset_get(*this, k, i) < bbox[i].low) + bbox[i].low = this->dataset_get(*this, k, i); + if(this->dataset_get(*this, k, i) > bbox[i].high) + bbox[i].high = this->dataset_get(*this, k, i); + } + } + } + } + + /** + * Performs an exact search in the tree starting from a node. + * \tparam RESULTSET Should be any ResultSet + * \return true if the search should be continued, false if the results are + * sufficient + */ + template + bool searchLevel(RESULTSET& result_set, const ElementType* vec, + const NodePtr node, DistanceType mindistsq, + distance_vector_t& dists, const float epsError) const { + /* If this is a leaf node, then do check and return. */ + if((node->child1 == NULL) && (node->child2 == NULL)) { + // count_leaf += (node->lr.right-node->lr.left); // Removed since was + // neither used nor returned to the user. + DistanceType worst_dist = result_set.worstDist(); + for(IndexType i = node->node_type.lr.left; i < node->node_type.lr.right; + ++i) { + const IndexType index = BaseClassRef::vind[i]; // reorder... : i; + DistanceType dist = distance.evalMetric( + vec, index, (DIM > 0 ? DIM : BaseClassRef::dim)); + if(dist < worst_dist) { + if(!result_set.addPoint(dist, BaseClassRef::vind[i])) { + // the resultset doesn't want to receive any more points, we're done + // searching! + return false; + } + } + } + return true; + } + + /* Which child branch should be taken first? */ + int idx = node->node_type.sub.divfeat; + ElementType val = vec[idx]; + DistanceType diff1 = val - node->node_type.sub.divlow; + DistanceType diff2 = val - node->node_type.sub.divhigh; + + NodePtr bestChild; + NodePtr otherChild; + DistanceType cut_dist; + if((diff1 + diff2) < 0) { + bestChild = node->child1; + otherChild = node->child2; + cut_dist = distance.accum_dist(val, node->node_type.sub.divhigh, idx); + } else { + bestChild = node->child2; + otherChild = node->child1; + cut_dist = distance.accum_dist(val, node->node_type.sub.divlow, idx); + } + + /* Call recursively to search next level down. */ + if(!searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError)) { + // the resultset doesn't want to receive any more points, we're done + // searching! + return false; + } + + DistanceType dst = dists[idx]; + mindistsq = mindistsq + cut_dist - dst; + dists[idx] = cut_dist; + if(mindistsq * epsError <= result_set.worstDist()) { + if(!searchLevel(result_set, vec, otherChild, mindistsq, dists, + epsError)) { + // the resultset doesn't want to receive any more points, we're done + // searching! + return false; + } + } + dists[idx] = dst; + return true; + } + + public: + /** Stores the index in a binary file. + * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when + * loading the index object it must be constructed associated to the same + * source of data points used while building it. See the example: + * examples/saveload_example.cpp \sa loadIndex */ + void saveIndex(FILE* stream) { + this->saveIndex_(*this, stream); + } + + /** Loads a previous index from a binary file. + * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the + * index object must be constructed associated to the same source of data + * points used while building the index. See the example: + * examples/saveload_example.cpp \sa loadIndex */ + void loadIndex(FILE* stream) { + this->loadIndex_(*this, stream); + } + + }; // class KDTree + + /** kd-tree dynamic index + * + * Contains the k-d trees and other information for indexing a set of points + * for nearest-neighbor matching. + * + * The class "DatasetAdaptor" must provide the following interface (can be + * non-virtual, inlined methods): + * + * \code + * // Must return the number of data poins + * inline size_t kdtree_get_point_count() const { ... } + * + * // Must return the dim'th component of the idx'th point in the class: + * inline T kdtree_get_pt(const size_t idx, const size_t dim) const { ... } + * + * // Optional bounding-box computation: return false to default to a standard + * bbox computation loop. + * // Return true if the BBOX was already computed by the class and returned + * in "bb" so it can be avoided to redo it again. + * // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 + * for point clouds) template bool kdtree_get_bbox(BBOX &bb) const + * { + * bb[0].low = ...; bb[0].high = ...; // 0th dimension limits + * bb[1].low = ...; bb[1].high = ...; // 1st dimension limits + * ... + * return true; + * } + * + * \endcode + * + * \tparam DatasetAdaptor The user-provided adaptor (see comments above). + * \tparam Distance The distance metric to use: nanoflann::metric_L1, + * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam DIM + * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will + * be typically size_t or int + */ + template + class KDTreeSingleIndexDynamicAdaptor_ + : public KDTreeBaseClass, + Distance, DatasetAdaptor, DIM, IndexType> { + public: + /** + * The dataset used by this index + */ + const DatasetAdaptor& dataset; //!< The source of our data + + KDTreeSingleIndexAdaptorParams index_params; + + std::vector& treeIndex; + + Distance distance; + + typedef typename nanoflann::KDTreeBaseClass< + nanoflann::KDTreeSingleIndexDynamicAdaptor_, + Distance, DatasetAdaptor, DIM, IndexType> + BaseClassRef; + + typedef typename BaseClassRef::ElementType ElementType; + typedef typename BaseClassRef::DistanceType DistanceType; + + typedef typename BaseClassRef::Node Node; + typedef Node* NodePtr; + + typedef typename BaseClassRef::Interval Interval; + /** Define "BoundingBox" as a fixed-size or variable-size container depending + * on "DIM" */ + typedef typename BaseClassRef::BoundingBox BoundingBox; + + /** Define "distance_vector_t" as a fixed-size or variable-size container + * depending on "DIM" */ + typedef typename BaseClassRef::distance_vector_t distance_vector_t; + + /** + * KDTree constructor + * + * Refer to docs in README.md or online in + * https://github.com/jlblancoc/nanoflann + * + * The KD-Tree point dimension (the length of each point in the datase, e.g. 3 + * for 3D points) is determined by means of: + * - The \a DIM template parameter if >0 (highest priority) + * - Otherwise, the \a dimensionality parameter of this constructor. + * + * @param inputData Dataset with the input features + * @param params Basically, the maximum leaf node size + */ + KDTreeSingleIndexDynamicAdaptor_( + const int dimensionality, const DatasetAdaptor& inputData, + std::vector& treeIndex_, + const KDTreeSingleIndexAdaptorParams& params = + KDTreeSingleIndexAdaptorParams()) + : dataset(inputData), index_params(params), treeIndex(treeIndex_), + distance(inputData) { + BaseClassRef::root_node = NULL; + BaseClassRef::m_size = 0; + BaseClassRef::m_size_at_index_build = 0; + BaseClassRef::dim = dimensionality; + if(DIM > 0) + BaseClassRef::dim = DIM; + BaseClassRef::m_leaf_max_size = params.leaf_max_size; + } + + /** Assignment operator definiton */ + KDTreeSingleIndexDynamicAdaptor_ + operator=(const KDTreeSingleIndexDynamicAdaptor_& rhs) { + KDTreeSingleIndexDynamicAdaptor_ tmp(rhs); + std::swap(BaseClassRef::vind, tmp.BaseClassRef::vind); + std::swap(BaseClassRef::m_leaf_max_size, tmp.BaseClassRef::m_leaf_max_size); + std::swap(index_params, tmp.index_params); + std::swap(treeIndex, tmp.treeIndex); + std::swap(BaseClassRef::m_size, tmp.BaseClassRef::m_size); + std::swap(BaseClassRef::m_size_at_index_build, + tmp.BaseClassRef::m_size_at_index_build); + std::swap(BaseClassRef::root_node, tmp.BaseClassRef::root_node); + std::swap(BaseClassRef::root_bbox, tmp.BaseClassRef::root_bbox); + std::swap(BaseClassRef::pool, tmp.BaseClassRef::pool); + return *this; + } + + /** + * Builds the index + */ + void buildIndex() { + BaseClassRef::m_size = BaseClassRef::vind.size(); + this->freeIndex(*this); + BaseClassRef::m_size_at_index_build = BaseClassRef::m_size; + if(BaseClassRef::m_size == 0) + return; + computeBoundingBox(BaseClassRef::root_bbox); + BaseClassRef::root_node = + this->divideTree(*this, 0, BaseClassRef::m_size, + BaseClassRef::root_bbox); // construct the tree + } + + /** \name Query methods + * @{ */ + + /** + * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored + * inside the result object. + * + * Params: + * result = the result object in which the indices of the + * nearest-neighbors are stored vec = the vector for which to search the + * nearest neighbors + * + * \tparam RESULTSET Should be any ResultSet + * \return True if the requested neighbors could be found. + * \sa knnSearch, radiusSearch + */ + template + bool findNeighbors(RESULTSET& result, const ElementType* vec, + const SearchParams& searchParams) const { + assert(vec); + if(this->size(*this) == 0) + return false; + if(!BaseClassRef::root_node) + return false; + float epsError = 1 + searchParams.eps; + + // fixed or variable-sized container (depending on DIM) + distance_vector_t dists; + // Fill it with zeros. + assign(dists, (DIM > 0 ? DIM : BaseClassRef::dim), + static_cast(0)); + DistanceType distsq = this->computeInitialDistances(*this, vec, dists); + searchLevel(result, vec, BaseClassRef::root_node, distsq, dists, + epsError); // "count_leaf" parameter removed since was neither + // used nor returned to the user. + return result.full(); + } + + /** + * Find the "num_closest" nearest neighbors to the \a query_point[0:dim-1]. + * Their indices are stored inside the result object. \sa radiusSearch, + * findNeighbors \note nChecks_IGNORED is ignored but kept for compatibility + * with the original FLANN interface. \return Number `N` of valid points in + * the result set. Only the first `N` entries in `out_indices` and + * `out_distances_sq` will be valid. Return may be less than `num_closest` + * only if the number of elements in the tree is less than `num_closest`. + */ + size_t knnSearch(const ElementType* query_point, const size_t num_closest, + IndexType* out_indices, DistanceType* out_distances_sq, + const int /* nChecks_IGNORED */ = 10) const { + nanoflann::KNNResultSet resultSet(num_closest); + resultSet.init(out_indices, out_distances_sq); + this->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); + return resultSet.size(); + } + + /** + * Find all the neighbors to \a query_point[0:dim-1] within a maximum radius. + * The output is given as a vector of pairs, of which the first element is a + * point index and the second the corresponding distance. Previous contents of + * \a IndicesDists are cleared. + * + * If searchParams.sorted==true, the output list is sorted by ascending + * distances. + * + * For a better performance, it is advisable to do a .reserve() on the vector + * if you have any wild guess about the number of expected matches. + * + * \sa knnSearch, findNeighbors, radiusSearchCustomCallback + * \return The number of points within the given radius (i.e. indices.size() + * or dists.size() ) + */ + size_t + radiusSearch(const ElementType* query_point, const DistanceType& radius, + std::vector>& IndicesDists, + const SearchParams& searchParams) const { + RadiusResultSet resultSet(radius, IndicesDists); + const size_t nFound = + radiusSearchCustomCallback(query_point, resultSet, searchParams); + if(searchParams.sorted) + std::sort(IndicesDists.begin(), IndicesDists.end(), IndexDist_Sorter()); + return nFound; + } + + /** + * Just like radiusSearch() but with a custom callback class for each point + * found in the radius of the query. See the source of RadiusResultSet<> as a + * start point for your own classes. \sa radiusSearch + */ + template + size_t radiusSearchCustomCallback( + const ElementType* query_point, SEARCH_CALLBACK& resultSet, + const SearchParams& searchParams = SearchParams()) const { + this->findNeighbors(resultSet, query_point, searchParams); + return resultSet.size(); + } + + /** @} */ + + public: + void computeBoundingBox(BoundingBox& bbox) { + resize(bbox, (DIM > 0 ? DIM : BaseClassRef::dim)); + + if(dataset.kdtree_get_bbox(bbox)) { + // Done! It was implemented in derived class + } else { + const size_t N = BaseClassRef::m_size; + if(!N) + throw std::runtime_error("[nanoflann] computeBoundingBox() called but " + "no data points found."); + for(int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) { + bbox[i].low = bbox[i].high = + this->dataset_get(*this, BaseClassRef::vind[0], i); + } + for(size_t k = 1; k < N; ++k) { + for(int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) { + if(this->dataset_get(*this, BaseClassRef::vind[k], i) < bbox[i].low) + bbox[i].low = this->dataset_get(*this, BaseClassRef::vind[k], i); + if(this->dataset_get(*this, BaseClassRef::vind[k], i) > bbox[i].high) + bbox[i].high = this->dataset_get(*this, BaseClassRef::vind[k], i); + } + } + } + } + + /** + * Performs an exact search in the tree starting from a node. + * \tparam RESULTSET Should be any ResultSet + */ + template + void searchLevel(RESULTSET& result_set, const ElementType* vec, + const NodePtr node, DistanceType mindistsq, + distance_vector_t& dists, const float epsError) const { + /* If this is a leaf node, then do check and return. */ + if((node->child1 == NULL) && (node->child2 == NULL)) { + // count_leaf += (node->lr.right-node->lr.left); // Removed since was + // neither used nor returned to the user. + DistanceType worst_dist = result_set.worstDist(); + for(IndexType i = node->node_type.lr.left; i < node->node_type.lr.right; + ++i) { + const IndexType index = BaseClassRef::vind[i]; // reorder... : i; + if(treeIndex[index] == -1) + continue; + DistanceType dist = distance.evalMetric( + vec, index, (DIM > 0 ? DIM : BaseClassRef::dim)); + if(dist < worst_dist) { + if(!result_set.addPoint( + static_cast(dist), + static_cast( + BaseClassRef::vind[i]))) { + // the resultset doesn't want to receive any more points, we're done + // searching! + return; // false; + } + } + } + return; + } + + /* Which child branch should be taken first? */ + int idx = node->node_type.sub.divfeat; + ElementType val = vec[idx]; + DistanceType diff1 = val - node->node_type.sub.divlow; + DistanceType diff2 = val - node->node_type.sub.divhigh; + + NodePtr bestChild; + NodePtr otherChild; + DistanceType cut_dist; + if((diff1 + diff2) < 0) { + bestChild = node->child1; + otherChild = node->child2; + cut_dist = distance.accum_dist(val, node->node_type.sub.divhigh, idx); + } else { + bestChild = node->child2; + otherChild = node->child1; + cut_dist = distance.accum_dist(val, node->node_type.sub.divlow, idx); + } + + /* Call recursively to search next level down. */ + searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError); + + DistanceType dst = dists[idx]; + mindistsq = mindistsq + cut_dist - dst; + dists[idx] = cut_dist; + if(mindistsq * epsError <= result_set.worstDist()) { + searchLevel(result_set, vec, otherChild, mindistsq, dists, epsError); + } + dists[idx] = dst; + } + + public: + /** Stores the index in a binary file. + * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when + * loading the index object it must be constructed associated to the same + * source of data points used while building it. See the example: + * examples/saveload_example.cpp \sa loadIndex */ + void saveIndex(FILE* stream) { + this->saveIndex_(*this, stream); + } + + /** Loads a previous index from a binary file. + * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the + * index object must be constructed associated to the same source of data + * points used while building the index. See the example: + * examples/saveload_example.cpp \sa loadIndex */ + void loadIndex(FILE* stream) { + this->loadIndex_(*this, stream); + } + }; + + /** kd-tree dynaimic index + * + * class to create multiple static index and merge their results to behave as + * single dynamic index as proposed in Logarithmic Approach. + * + * Example of usage: + * examples/dynamic_pointcloud_example.cpp + * + * \tparam DatasetAdaptor The user-provided adaptor (see comments above). + * \tparam Distance The distance metric to use: nanoflann::metric_L1, + * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam DIM + * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will + * be typically size_t or int + */ + template + class KDTreeSingleIndexDynamicAdaptor { + public: + typedef typename Distance::ElementType ElementType; + typedef typename Distance::DistanceType DistanceType; + + protected: + size_t m_leaf_max_size; + size_t treeCount; + size_t pointCount; + + /** + * The dataset used by this index + */ + const DatasetAdaptor& dataset; //!< The source of our data + + std::vector treeIndex; //!< treeIndex[idx] is the index of tree in which + //!< point at idx is stored. treeIndex[idx]=-1 + //!< means that point has been removed. + + KDTreeSingleIndexAdaptorParams index_params; + + int dim; //!< Dimensionality of each data point + + typedef KDTreeSingleIndexDynamicAdaptor_ + index_container_t; + std::vector index; + + public: + /** Get a const ref to the internal list of indices; the number of indices is + * adapted dynamically as the dataset grows in size. */ + const std::vector& getAllIndices() const { + return index; + } + + private: + /** finds position of least significant unset bit */ + int First0Bit(IndexType num) { + int pos = 0; + while(num & 1) { + num = num >> 1; + pos++; + } + return pos; + } + + /** Creates multiple empty trees to handle dynamic support */ + void init() { + typedef KDTreeSingleIndexDynamicAdaptor_ + my_kd_tree_t; + std::vector index_( + treeCount, my_kd_tree_t(dim /*dim*/, dataset, treeIndex, index_params)); + index = index_; + } + + public: + Distance distance; + + /** + * KDTree constructor + * + * Refer to docs in README.md or online in + * https://github.com/jlblancoc/nanoflann + * + * The KD-Tree point dimension (the length of each point in the datase, e.g. 3 + * for 3D points) is determined by means of: + * - The \a DIM template parameter if >0 (highest priority) + * - Otherwise, the \a dimensionality parameter of this constructor. + * + * @param inputData Dataset with the input features + * @param params Basically, the maximum leaf node size + */ + KDTreeSingleIndexDynamicAdaptor(const int dimensionality, + const DatasetAdaptor& inputData, + const KDTreeSingleIndexAdaptorParams& params = + KDTreeSingleIndexAdaptorParams(), + const size_t maximumPointCount = 1000000000U) + : dataset(inputData), index_params(params), distance(inputData) { + treeCount = static_cast(std::log2(maximumPointCount)); + pointCount = 0U; + dim = dimensionality; + treeIndex.clear(); + if(DIM > 0) + dim = DIM; + m_leaf_max_size = params.leaf_max_size; + init(); + const size_t num_initial_points = dataset.kdtree_get_point_count(); + if(num_initial_points > 0) { + addPoints(0, num_initial_points - 1); + } + } + + /** Deleted copy constructor*/ + KDTreeSingleIndexDynamicAdaptor( + const KDTreeSingleIndexDynamicAdaptor&) = delete; + + void reset() { + pointCount = 0U; + treeIndex.clear(); + index.clear(); + init(); + const size_t num_initial_points = dataset.kdtree_get_point_count(); + if(num_initial_points > 0) { + addPoints(0, num_initial_points - 1); + } + } + + /** Add points to the set, Inserts all points from [start, end] */ + void addPoints(IndexType start, IndexType end) { + size_t count = end - start + 1; + treeIndex.resize(treeIndex.size() + count); + for(IndexType idx = start; idx <= end; idx++) { + int pos = First0Bit(pointCount); + index[pos].vind.clear(); + treeIndex[pointCount] = pos; + for(int i = 0; i < pos; i++) { + for(int j = 0; j < static_cast(index[i].vind.size()); j++) { + index[pos].vind.push_back(index[i].vind[j]); + if(treeIndex[index[i].vind[j]] != -1) + treeIndex[index[i].vind[j]] = pos; + } + index[i].vind.clear(); + index[i].freeIndex(index[i]); + } + index[pos].vind.push_back(idx); + index[pos].buildIndex(); + pointCount++; + } + } + + /** Remove a point from the set (Lazy Deletion) */ + bool removePoint(size_t idx) { + if(idx >= pointCount) + return false; + bool exists = treeIndex[idx] != -1; + treeIndex[idx] = -1; + return exists; + } + + /** + * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored + * inside the result object. + * + * Params: + * result = the result object in which the indices of the + * nearest-neighbors are stored vec = the vector for which to search the + * nearest neighbors + * + * \tparam RESULTSET Should be any ResultSet + * \return True if the requested neighbors could be found. + * \sa knnSearch, radiusSearch + */ + template + bool findNeighbors(RESULTSET& result, const ElementType* vec, + const SearchParams& searchParams) const { + for(size_t i = 0; i < treeCount; i++) { + index[i].findNeighbors(result, &vec[0], searchParams); + } + return result.full(); + } + }; + + /** An L2-metric KD-tree adaptor for working with data directly stored in an + * Eigen Matrix, without duplicating the data storage. Each row in the matrix + * represents a point in the state space. + * + * Example of usage: + * \code + * Eigen::Matrix mat; + * // Fill out "mat"... + * + * typedef KDTreeEigenMatrixAdaptor< Eigen::Matrix > + * my_kd_tree_t; const int max_leaf = 10; my_kd_tree_t mat_index(mat, max_leaf + * ); mat_index.index->buildIndex(); mat_index.index->... \endcode + * + * \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality + * for the points in the data set, allowing more compiler optimizations. \tparam + * Distance The distance metric to use: nanoflann::metric_L1, + * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. + */ + template + struct KDTreeEigenMatrixAdaptor { + typedef KDTreeEigenMatrixAdaptor self_t; + typedef typename MatrixType::Scalar num_t; + typedef typename MatrixType::Index IndexType; + typedef + typename Distance::template traits::distance_t metric_t; + typedef KDTreeSingleIndexAdaptor + index_t; + + index_t* index; //! The kd-tree index for the user to call its methods as + //! usual with any other FLANN index. + + /// Constructor: takes a const ref to the matrix object with the data points + KDTreeEigenMatrixAdaptor(const size_t dimensionality, + const std::reference_wrapper& mat, + const int leaf_max_size = 10) + : m_data_matrix(mat) { + const auto dims = mat.get().cols(); + if(size_t(dims) != dimensionality) + throw std::runtime_error( + "Error: 'dimensionality' must match column count in data matrix"); + if(DIM > 0 && int(dims) != DIM) + throw std::runtime_error( + "Data set dimensionality does not match the 'DIM' template argument"); + index = + new index_t(static_cast(dims), *this /* adaptor */, + nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size)); + index->buildIndex(); + } + + public: + /** Deleted copy constructor */ + KDTreeEigenMatrixAdaptor(const self_t&) = delete; + + ~KDTreeEigenMatrixAdaptor() { + delete index; + } + + const std::reference_wrapper m_data_matrix; + + /** Query for the \a num_closest closest points to a given point (entered as + * query_point[0:dim-1]). Note that this is a short-cut method for + * index->findNeighbors(). The user can also call index->... methods as + * desired. \note nChecks_IGNORED is ignored but kept for compatibility with + * the original FLANN interface. + */ + inline void query(const num_t* query_point, const size_t num_closest, + IndexType* out_indices, num_t* out_distances_sq, + const int /* nChecks_IGNORED */ = 10) const { + nanoflann::KNNResultSet resultSet(num_closest); + resultSet.init(out_indices, out_distances_sq); + index->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); + } + + /** @name Interface expected by KDTreeSingleIndexAdaptor + * @{ */ + + const self_t& derived() const { + return *this; + } + self_t& derived() { + return *this; + } + + // Must return the number of data points + inline size_t kdtree_get_point_count() const { + return m_data_matrix.get().rows(); + } + + // Returns the dim'th component of the idx'th point in the class: + inline num_t kdtree_get_pt(const IndexType idx, size_t dim) const { + return m_data_matrix.get().coeff(idx, IndexType(dim)); + } + + // Optional bounding-box computation: return false to default to a standard + // bbox computation loop. + // Return true if the BBOX was already computed by the class and returned in + // "bb" so it can be avoided to redo it again. Look at bb.size() to find out + // the expected dimensionality (e.g. 2 or 3 for point clouds) + template bool kdtree_get_bbox(BBOX& /*bb*/) const { + return false; + } + + /** @} */ + + }; // end of KDTreeEigenMatrixAdaptor + /** @} */ + + /** @} */ // end of grouping + } // namespace nanoflann + +#endif /* NANOFLANN_HPP_ */ diff --git a/include/print.hpp b/include/print.hpp new file mode 100644 index 0000000..e754a0a --- /dev/null +++ b/include/print.hpp @@ -0,0 +1,103 @@ +#ifndef MEDUSA_BITS_UTILS_PRINT_HPP_ +#define MEDUSA_BITS_UTILS_PRINT_HPP_ + +/** + * @file + * Printing helpers for std types. + */ + +#include +#include +#include +#include +#include + + // additional ostream operators +namespace std { + + /// Output pairs as `(1, 2)`. @ingroup utils + template + std::ostream& operator<<(std::ostream& xx, const std::pair& par) { + return xx << "(" << par.first << "," << par.second << ")"; + } + + /// Output arrays as `[1, 2, 3]`. @ingroup utils + template + std::ostream& operator<<(std::ostream& xx, const std::array& arr) { + xx << "["; + for(size_t i = 0; i < N; ++i) { + xx << arr[i]; + if(i < N - 1) + xx << ", "; + } + xx << "]"; + return xx; + } + + /// Output vectors as `[1, 2, 3]`. @ingroup utils + template + std::ostream& operator<<(std::ostream& xx, const std::vector& arr) { + // do it like the matlab does it. + xx << "["; + for(size_t i = 0; i < arr.size(); ++i) { + xx << arr[i]; + if(i < arr.size() - 1) + xx << ", "; + } + xx << "]"; + return xx; + } + + /// Output nested vectors as `[[1, 2]; [3, 4]]`. @ingroup utils + template + std::ostream& operator<<(std::ostream& xx, const std::vector>& arr) { + xx << "["; + for(size_t i = 0; i < arr.size(); ++i) { + for(size_t j = 0; j < arr[i].size(); ++j) { + xx << arr[i][j]; + if(j < arr[i].size() - 1) + xx << ", "; + } + if(i < arr.size() - 1) + xx << "; "; + } + xx << "]"; + return xx; + } + + /// @cond + namespace tuple_print_internal { + template + struct TuplePrinter { + static void print(std::ostream& os, const Tuple& t) { // recursive + TuplePrinter::print(os, t); + os << ", " << std::get(t); + } + }; + + template + struct TuplePrinter { + static void print(std::ostream& os, const Tuple& t) { // one element + os << std::get<0>(t); + } + }; + + template + struct TuplePrinter { + static void print(std::ostream&, const Tuple&) {} + }; // zero elt + + } // namespace tuple_print_internal + /// @endcond + + /// Print a tuple as (1, 4.5, abc). @ingroup utils + template + std::ostream& operator<<(std::ostream& os, const std::tuple& t) { + os << "("; + tuple_print_internal::TuplePrinter::print(os, t); + return os << ")"; + } + +} // namespace std + +#endif // MEDUSA_BITS_UTILS_PRINT_HPP_ diff --git a/include/tinyformat.h b/include/tinyformat.h new file mode 100644 index 0000000..cd40624 --- /dev/null +++ b/include/tinyformat.h @@ -0,0 +1,1056 @@ +// tinyformat.h +// Copyright (C) 2011, Chris Foster [chris42f (at) gmail (d0t) com] +// +// Boost Software License - Version 1.0 +// +// Permission is hereby granted, free of charge, to any person or organization +// obtaining a copy of the software and accompanying documentation covered by +// this license (the "Software") to use, reproduce, display, distribute, +// execute, and transmit the Software, and to prepare derivative works of the +// Software, and to permit third-parties to whom the Software is furnished to +// do so, all subject to the following: +// +// The copyright notices in the Software and this entire statement, including +// the above license grant, this restriction and the following disclaimer, +// must be included in all copies of the Software, in whole or in part, and +// all derivative works of the Software, unless such copies or derivative +// works are solely in the form of machine-executable object code generated by +// a source language processor. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT +// SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE +// FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, +// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +// DEALINGS IN THE SOFTWARE. + +//------------------------------------------------------------------------------ +// Tinyformat: A minimal type safe printf replacement +// +// tinyformat.h is a type safe printf replacement library in a single C++ +// header file. Design goals include: +// +// * Type safety and extensibility for user defined types. +// * C99 printf() compatibility, to the extent possible using std::ostream +// * Simplicity and minimalism. A single header file to include and distribute +// with your projects. +// * Augment rather than replace the standard stream formatting mechanism +// * C++98 support, with optional C++11 niceties +// +// +// Main interface example usage +// ---------------------------- +// +// To print a date to std::cout: +// +// std::string weekday = "Wednesday"; +// const char* month = "July"; +// size_t day = 27; +// long hour = 14; +// int min = 44; +// +// tfm::printf("%s, %s %d, %.2d:%.2d\n", weekday, month, day, hour, min); +// +// The strange types here emphasize the type safety of the interface; it is +// possible to print a std::string using the "%s" conversion, and a +// size_t using the "%d" conversion. A similar result could be achieved +// using either of the tfm::format() functions. One prints on a user provided +// stream: +// +// tfm::format(std::cerr, "%s, %s %d, %.2d:%.2d\n", +// weekday, month, day, hour, min); +// +// The other returns a std::string: +// +// std::string date = tfm::format("%s, %s %d, %.2d:%.2d\n", +// weekday, month, day, hour, min); +// std::cout << date; +// +// These are the three primary interface functions. There is also a +// convenience function printfln() which appends a newline to the usual result +// of printf() for super simple logging. +// +// +// User defined format functions +// ----------------------------- +// +// Simulating variadic templates in C++98 is pretty painful since it requires +// writing out the same function for each desired number of arguments. To make +// this bearable tinyformat comes with a set of macros which are used +// internally to generate the API, but which may also be used in user code. +// +// The three macros TINYFORMAT_ARGTYPES(n), TINYFORMAT_VARARGS(n) and +// TINYFORMAT_PASSARGS(n) will generate a list of n argument types, +// type/name pairs and argument names respectively when called with an integer +// n between 1 and 16. We can use these to define a macro which generates the +// desired user defined function with n arguments. To generate all 16 user +// defined function bodies, use the macro TINYFORMAT_FOREACH_ARGNUM. For an +// example, see the implementation of printf() at the end of the source file. +// +// Sometimes it's useful to be able to pass a list of format arguments through +// to a non-template function. The FormatList class is provided as a way to do +// this by storing the argument list in a type-opaque way. Continuing the +// example from above, we construct a FormatList using makeFormatList(): +// +// FormatListRef formatList = tfm::makeFormatList(weekday, month, day, hour, min); +// +// The format list can now be passed into any non-template function and used +// via a call to the vformat() function: +// +// tfm::vformat(std::cout, "%s, %s %d, %.2d:%.2d\n", formatList); +// +// +// Additional API information +// -------------------------- +// +// Error handling: Define TINYFORMAT_ERROR to customize the error handling for +// format strings which are unsupported or have the wrong number of format +// specifiers (calls assert() by default). +// +// User defined types: Uses operator<< for user defined types by default. +// Overload formatValue() for more control. + + +#ifndef TINYFORMAT_H_INCLUDED +#define TINYFORMAT_H_INCLUDED + +namespace tinyformat {} +//------------------------------------------------------------------------------ +// Config section. Customize to your liking! + +// Namespace alias to encourage brevity +namespace tfm = tinyformat; + +// Error handling; calls assert() by default. +// #define TINYFORMAT_ERROR(reasonString) your_error_handler(reasonString) + +// Define for C++11 variadic templates which make the code shorter & more +// general. If you don't define this, C++11 support is autodetected below. +// #define TINYFORMAT_USE_VARIADIC_TEMPLATES + + +//------------------------------------------------------------------------------ +// Implementation details. +#include +#include +#include + +#ifndef TINYFORMAT_ASSERT +# include +# define TINYFORMAT_ASSERT(cond) assert(cond) +#endif + +#ifndef TINYFORMAT_ERROR +# include +# define TINYFORMAT_ERROR(reason) assert(0 && reason) +#endif + +#if !defined(TINYFORMAT_USE_VARIADIC_TEMPLATES) && !defined(TINYFORMAT_NO_VARIADIC_TEMPLATES) +# ifdef __GXX_EXPERIMENTAL_CXX0X__ +# define TINYFORMAT_USE_VARIADIC_TEMPLATES +# endif +#endif + +#if defined(__GLIBCXX__) && __GLIBCXX__ < 20080201 +// std::showpos is broken on old libstdc++ as provided with OSX. See +// http://gcc.gnu.org/ml/libstdc++/2007-11/msg00075.html +# define TINYFORMAT_OLD_LIBSTDCPLUSPLUS_WORKAROUND +#endif + +#ifdef __APPLE__ +// Workaround OSX linker warning: xcode uses different default symbol +// visibilities for static libs vs executables (see issue #25) +# define TINYFORMAT_HIDDEN __attribute__((visibility("hidden"))) +#else +# define TINYFORMAT_HIDDEN +#endif + +namespace tinyformat { + +//------------------------------------------------------------------------------ +namespace detail { + +// Test whether type T1 is convertible to type T2 +template +struct is_convertible +{ + private: + // two types of different size + struct fail { char dummy[2]; }; + struct succeed { char dummy; }; + // Try to convert a T1 to a T2 by plugging into tryConvert + static fail tryConvert(...); + static succeed tryConvert(const T2&); + static const T1& makeT1(); + public: +# ifdef _MSC_VER + // Disable spurious loss of precision warnings in tryConvert(makeT1()) +# pragma warning(push) +# pragma warning(disable:4244) +# pragma warning(disable:4267) +# endif + // Standard trick: the (...) version of tryConvert will be chosen from + // the overload set only if the version taking a T2 doesn't match. + // Then we compare the sizes of the return types to check which + // function matched. Very neat, in a disgusting kind of way :) + static const bool value = + sizeof(tryConvert(makeT1())) == sizeof(succeed); +# ifdef _MSC_VER +# pragma warning(pop) +# endif +}; + + +// Detect when a type is not a wchar_t string +template struct is_wchar { typedef int tinyformat_wchar_is_not_supported; }; +template<> struct is_wchar {}; +template<> struct is_wchar {}; +template struct is_wchar {}; +template struct is_wchar {}; + + +// Format the value by casting to type fmtT. This default implementation +// should never be called. +template::value> +struct formatValueAsType +{ + static void invoke(std::ostream& /*out*/, const T& /*value*/) { TINYFORMAT_ASSERT(0); } +}; +// Specialized version for types that can actually be converted to fmtT, as +// indicated by the "convertible" template parameter. +template +struct formatValueAsType +{ + static void invoke(std::ostream& out, const T& value) + { out << static_cast(value); } +}; + +#ifdef TINYFORMAT_OLD_LIBSTDCPLUSPLUS_WORKAROUND +template::value> +struct formatZeroIntegerWorkaround +{ + static bool invoke(std::ostream& /**/, const T& /**/) { return false; } +}; +template +struct formatZeroIntegerWorkaround +{ + static bool invoke(std::ostream& out, const T& value) + { + if (static_cast(value) == 0 && out.flags() & std::ios::showpos) + { + out << "+0"; + return true; + } + return false; + } +}; +#endif // TINYFORMAT_OLD_LIBSTDCPLUSPLUS_WORKAROUND + +// Convert an arbitrary type to integer. The version with convertible=false +// throws an error. +template::value> +struct convertToInt +{ + static int invoke(const T& /*value*/) + { + TINYFORMAT_ERROR("tinyformat: Cannot convert from argument type to " + "integer for use as variable width or precision"); + return 0; + } +}; +// Specialization for convertToInt when conversion is possible +template +struct convertToInt +{ + static int invoke(const T& value) { return static_cast(value); } +}; + +// Format at most ntrunc characters to the given stream. +template +inline void formatTruncated(std::ostream& out, const T& value, int ntrunc) +{ + std::ostringstream tmp; + tmp << value; + std::string result = tmp.str(); + out.write(result.c_str(), (std::min)(ntrunc, static_cast(result.size()))); +} +#define TINYFORMAT_DEFINE_FORMAT_TRUNCATED_CSTR(type) \ +inline void formatTruncated(std::ostream& out, type* value, int ntrunc) \ +{ \ + std::streamsize len = 0; \ + while(len < ntrunc && value[len] != 0) \ + ++len; \ + out.write(value, len); \ +} +// Overload for const char* and char*. Could overload for signed & unsigned +// char too, but these are technically unneeded for printf compatibility. +TINYFORMAT_DEFINE_FORMAT_TRUNCATED_CSTR(const char) +TINYFORMAT_DEFINE_FORMAT_TRUNCATED_CSTR(char) +#undef TINYFORMAT_DEFINE_FORMAT_TRUNCATED_CSTR + +} // namespace detail + + +//------------------------------------------------------------------------------ +// Variable formatting functions. May be overridden for user-defined types if +// desired. + + +/// Format a value into a stream, delegating to operator<< by default. +/// +/// Users may override this for their own types. When this function is called, +/// the stream flags will have been modified according to the format string. +/// The format specification is provided in the range [fmtBegin, fmtEnd). For +/// truncating conversions, ntrunc is set to the desired maximum number of +/// characters, for example "%.7s" calls formatValue with ntrunc = 7. +/// +/// By default, formatValue() uses the usual stream insertion operator +/// operator<< to format the type T, with special cases for the %c and %p +/// conversions. +template +inline void formatValue(std::ostream& out, const char* /*fmtBegin*/, + const char* fmtEnd, int ntrunc, const T& value) +{ +#ifndef TINYFORMAT_ALLOW_WCHAR_STRINGS + // Since we don't support printing of wchar_t using "%ls", make it fail at + // compile time in preference to printing as a void* at runtime. + typedef typename detail::is_wchar::tinyformat_wchar_is_not_supported DummyType; + (void) DummyType(); // avoid unused type warning with gcc-4.8 +#endif + // The mess here is to support the %c and %p conversions: if these + // conversions are active we try to convert the type to a char or const + // void* respectively and format that instead of the value itself. For the + // %p conversion it's important to avoid dereferencing the pointer, which + // could otherwise lead to a crash when printing a dangling (const char*). + const bool canConvertToChar = detail::is_convertible::value; + const bool canConvertToVoidPtr = detail::is_convertible::value; + if(canConvertToChar && *(fmtEnd-1) == 'c') + detail::formatValueAsType::invoke(out, value); + else if(canConvertToVoidPtr && *(fmtEnd-1) == 'p') + detail::formatValueAsType::invoke(out, value); +#ifdef TINYFORMAT_OLD_LIBSTDCPLUSPLUS_WORKAROUND + else if(detail::formatZeroIntegerWorkaround::invoke(out, value)) /**/; +#endif + else if(ntrunc >= 0) + { + // Take care not to overread C strings in truncating conversions like + // "%.4s" where at most 4 characters may be read. + detail::formatTruncated(out, value, ntrunc); + } + else + out << value; +} + + +// Overloaded version for char types to support printing as an integer +#define TINYFORMAT_DEFINE_FORMATVALUE_CHAR(charType) \ +inline void formatValue(std::ostream& out, const char* /*fmtBegin*/, \ + const char* fmtEnd, int /**/, charType value) \ +{ \ + switch(*(fmtEnd-1)) \ + { \ + case 'u': case 'd': case 'i': case 'o': case 'X': case 'x': \ + out << static_cast(value); break; \ + default: \ + out << value; break; \ + } \ +} +// per 3.9.1: char, signed char and unsigned char are all distinct types +TINYFORMAT_DEFINE_FORMATVALUE_CHAR(char) +TINYFORMAT_DEFINE_FORMATVALUE_CHAR(signed char) +TINYFORMAT_DEFINE_FORMATVALUE_CHAR(unsigned char) +#undef TINYFORMAT_DEFINE_FORMATVALUE_CHAR + + +//------------------------------------------------------------------------------ +// Tools for emulating variadic templates in C++98. The basic idea here is +// stolen from the boost preprocessor metaprogramming library and cut down to +// be just general enough for what we need. + +#define TINYFORMAT_ARGTYPES(n) TINYFORMAT_ARGTYPES_ ## n +#define TINYFORMAT_VARARGS(n) TINYFORMAT_VARARGS_ ## n +#define TINYFORMAT_PASSARGS(n) TINYFORMAT_PASSARGS_ ## n +#define TINYFORMAT_PASSARGS_TAIL(n) TINYFORMAT_PASSARGS_TAIL_ ## n + +// To keep it as transparent as possible, the macros below have been generated +// using python via the excellent cog.py code generation script. This avoids +// the need for a bunch of complex (but more general) preprocessor tricks as +// used in boost.preprocessor. +// +// To rerun the code generation in place, use `cog.py -r tinyformat.h` +// (see http://nedbatchelder.com/code/cog). Alternatively you can just create +// extra versions by hand. + +/*[[[cog +maxParams = 16 + +def makeCommaSepLists(lineTemplate, elemTemplate, startInd=1): + for j in range(startInd,maxParams+1): + list = ', '.join([elemTemplate % {'i':i} for i in range(startInd,j+1)]) + cog.outl(lineTemplate % {'j':j, 'list':list}) + +makeCommaSepLists('#define TINYFORMAT_ARGTYPES_%(j)d %(list)s', + 'class T%(i)d') + +cog.outl() +makeCommaSepLists('#define TINYFORMAT_VARARGS_%(j)d %(list)s', + 'const T%(i)d& v%(i)d') + +cog.outl() +makeCommaSepLists('#define TINYFORMAT_PASSARGS_%(j)d %(list)s', 'v%(i)d') + +cog.outl() +cog.outl('#define TINYFORMAT_PASSARGS_TAIL_1') +makeCommaSepLists('#define TINYFORMAT_PASSARGS_TAIL_%(j)d , %(list)s', + 'v%(i)d', startInd = 2) + +cog.outl() +cog.outl('#define TINYFORMAT_FOREACH_ARGNUM(m) \\\n ' + + ' '.join(['m(%d)' % (j,) for j in range(1,maxParams+1)])) +]]]*/ +#define TINYFORMAT_ARGTYPES_1 class T1 +#define TINYFORMAT_ARGTYPES_2 class T1, class T2 +#define TINYFORMAT_ARGTYPES_3 class T1, class T2, class T3 +#define TINYFORMAT_ARGTYPES_4 class T1, class T2, class T3, class T4 +#define TINYFORMAT_ARGTYPES_5 class T1, class T2, class T3, class T4, class T5 +#define TINYFORMAT_ARGTYPES_6 class T1, class T2, class T3, class T4, class T5, class T6 +#define TINYFORMAT_ARGTYPES_7 class T1, class T2, class T3, class T4, class T5, class T6, class T7 +#define TINYFORMAT_ARGTYPES_8 class T1, class T2, class T3, class T4, class T5, class T6, class T7, class T8 +#define TINYFORMAT_ARGTYPES_9 class T1, class T2, class T3, class T4, class T5, class T6, class T7, class T8, class T9 +#define TINYFORMAT_ARGTYPES_10 class T1, class T2, class T3, class T4, class T5, class T6, class T7, class T8, class T9, class T10 +#define TINYFORMAT_ARGTYPES_11 class T1, class T2, class T3, class T4, class T5, class T6, class T7, class T8, class T9, class T10, class T11 +#define TINYFORMAT_ARGTYPES_12 class T1, class T2, class T3, class T4, class T5, class T6, class T7, class T8, class T9, class T10, class T11, class T12 +#define TINYFORMAT_ARGTYPES_13 class T1, class T2, class T3, class T4, class T5, class T6, class T7, class T8, class T9, class T10, class T11, class T12, class T13 +#define TINYFORMAT_ARGTYPES_14 class T1, class T2, class T3, class T4, class T5, class T6, class T7, class T8, class T9, class T10, class T11, class T12, class T13, class T14 +#define TINYFORMAT_ARGTYPES_15 class T1, class T2, class T3, class T4, class T5, class T6, class T7, class T8, class T9, class T10, class T11, class T12, class T13, class T14, class T15 +#define TINYFORMAT_ARGTYPES_16 class T1, class T2, class T3, class T4, class T5, class T6, class T7, class T8, class T9, class T10, class T11, class T12, class T13, class T14, class T15, class T16 + +#define TINYFORMAT_VARARGS_1 const T1& v1 +#define TINYFORMAT_VARARGS_2 const T1& v1, const T2& v2 +#define TINYFORMAT_VARARGS_3 const T1& v1, const T2& v2, const T3& v3 +#define TINYFORMAT_VARARGS_4 const T1& v1, const T2& v2, const T3& v3, const T4& v4 +#define TINYFORMAT_VARARGS_5 const T1& v1, const T2& v2, const T3& v3, const T4& v4, const T5& v5 +#define TINYFORMAT_VARARGS_6 const T1& v1, const T2& v2, const T3& v3, const T4& v4, const T5& v5, const T6& v6 +#define TINYFORMAT_VARARGS_7 const T1& v1, const T2& v2, const T3& v3, const T4& v4, const T5& v5, const T6& v6, const T7& v7 +#define TINYFORMAT_VARARGS_8 const T1& v1, const T2& v2, const T3& v3, const T4& v4, const T5& v5, const T6& v6, const T7& v7, const T8& v8 +#define TINYFORMAT_VARARGS_9 const T1& v1, const T2& v2, const T3& v3, const T4& v4, const T5& v5, const T6& v6, const T7& v7, const T8& v8, const T9& v9 +#define TINYFORMAT_VARARGS_10 const T1& v1, const T2& v2, const T3& v3, const T4& v4, const T5& v5, const T6& v6, const T7& v7, const T8& v8, const T9& v9, const T10& v10 +#define TINYFORMAT_VARARGS_11 const T1& v1, const T2& v2, const T3& v3, const T4& v4, const T5& v5, const T6& v6, const T7& v7, const T8& v8, const T9& v9, const T10& v10, const T11& v11 +#define TINYFORMAT_VARARGS_12 const T1& v1, const T2& v2, const T3& v3, const T4& v4, const T5& v5, const T6& v6, const T7& v7, const T8& v8, const T9& v9, const T10& v10, const T11& v11, const T12& v12 +#define TINYFORMAT_VARARGS_13 const T1& v1, const T2& v2, const T3& v3, const T4& v4, const T5& v5, const T6& v6, const T7& v7, const T8& v8, const T9& v9, const T10& v10, const T11& v11, const T12& v12, const T13& v13 +#define TINYFORMAT_VARARGS_14 const T1& v1, const T2& v2, const T3& v3, const T4& v4, const T5& v5, const T6& v6, const T7& v7, const T8& v8, const T9& v9, const T10& v10, const T11& v11, const T12& v12, const T13& v13, const T14& v14 +#define TINYFORMAT_VARARGS_15 const T1& v1, const T2& v2, const T3& v3, const T4& v4, const T5& v5, const T6& v6, const T7& v7, const T8& v8, const T9& v9, const T10& v10, const T11& v11, const T12& v12, const T13& v13, const T14& v14, const T15& v15 +#define TINYFORMAT_VARARGS_16 const T1& v1, const T2& v2, const T3& v3, const T4& v4, const T5& v5, const T6& v6, const T7& v7, const T8& v8, const T9& v9, const T10& v10, const T11& v11, const T12& v12, const T13& v13, const T14& v14, const T15& v15, const T16& v16 + +#define TINYFORMAT_PASSARGS_1 v1 +#define TINYFORMAT_PASSARGS_2 v1, v2 +#define TINYFORMAT_PASSARGS_3 v1, v2, v3 +#define TINYFORMAT_PASSARGS_4 v1, v2, v3, v4 +#define TINYFORMAT_PASSARGS_5 v1, v2, v3, v4, v5 +#define TINYFORMAT_PASSARGS_6 v1, v2, v3, v4, v5, v6 +#define TINYFORMAT_PASSARGS_7 v1, v2, v3, v4, v5, v6, v7 +#define TINYFORMAT_PASSARGS_8 v1, v2, v3, v4, v5, v6, v7, v8 +#define TINYFORMAT_PASSARGS_9 v1, v2, v3, v4, v5, v6, v7, v8, v9 +#define TINYFORMAT_PASSARGS_10 v1, v2, v3, v4, v5, v6, v7, v8, v9, v10 +#define TINYFORMAT_PASSARGS_11 v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11 +#define TINYFORMAT_PASSARGS_12 v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12 +#define TINYFORMAT_PASSARGS_13 v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13 +#define TINYFORMAT_PASSARGS_14 v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14 +#define TINYFORMAT_PASSARGS_15 v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15 +#define TINYFORMAT_PASSARGS_16 v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16 + +#define TINYFORMAT_PASSARGS_TAIL_1 +#define TINYFORMAT_PASSARGS_TAIL_2 , v2 +#define TINYFORMAT_PASSARGS_TAIL_3 , v2, v3 +#define TINYFORMAT_PASSARGS_TAIL_4 , v2, v3, v4 +#define TINYFORMAT_PASSARGS_TAIL_5 , v2, v3, v4, v5 +#define TINYFORMAT_PASSARGS_TAIL_6 , v2, v3, v4, v5, v6 +#define TINYFORMAT_PASSARGS_TAIL_7 , v2, v3, v4, v5, v6, v7 +#define TINYFORMAT_PASSARGS_TAIL_8 , v2, v3, v4, v5, v6, v7, v8 +#define TINYFORMAT_PASSARGS_TAIL_9 , v2, v3, v4, v5, v6, v7, v8, v9 +#define TINYFORMAT_PASSARGS_TAIL_10 , v2, v3, v4, v5, v6, v7, v8, v9, v10 +#define TINYFORMAT_PASSARGS_TAIL_11 , v2, v3, v4, v5, v6, v7, v8, v9, v10, v11 +#define TINYFORMAT_PASSARGS_TAIL_12 , v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12 +#define TINYFORMAT_PASSARGS_TAIL_13 , v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13 +#define TINYFORMAT_PASSARGS_TAIL_14 , v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14 +#define TINYFORMAT_PASSARGS_TAIL_15 , v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15 +#define TINYFORMAT_PASSARGS_TAIL_16 , v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16 + +#define TINYFORMAT_FOREACH_ARGNUM(m) \ + m(1) m(2) m(3) m(4) m(5) m(6) m(7) m(8) m(9) m(10) m(11) m(12) m(13) m(14) m(15) m(16) +//[[[end]]] + + + +namespace detail { + +// Type-opaque holder for an argument to format(), with associated actions on +// the type held as explicit function pointers. This allows FormatArg's for +// each argument to be allocated as a homogenous array inside FormatList +// whereas a naive implementation based on inheritance does not. +class FormatArg +{ + public: + FormatArg() + : m_value(NULL), + m_formatImpl(NULL), + m_toIntImpl(NULL) + { } + + template + FormatArg(const T& value) + : m_value(static_cast(&value)), + m_formatImpl(&formatImpl), + m_toIntImpl(&toIntImpl) + { } + + void format(std::ostream& out, const char* fmtBegin, + const char* fmtEnd, int ntrunc) const + { + TINYFORMAT_ASSERT(m_value); + TINYFORMAT_ASSERT(m_formatImpl); + m_formatImpl(out, fmtBegin, fmtEnd, ntrunc, m_value); + } + + int toInt() const + { + TINYFORMAT_ASSERT(m_value); + TINYFORMAT_ASSERT(m_toIntImpl); + return m_toIntImpl(m_value); + } + + private: + template + TINYFORMAT_HIDDEN static void formatImpl(std::ostream& out, const char* fmtBegin, + const char* fmtEnd, int ntrunc, const void* value) + { + formatValue(out, fmtBegin, fmtEnd, ntrunc, *static_cast(value)); + } + + template + TINYFORMAT_HIDDEN static int toIntImpl(const void* value) + { + return convertToInt::invoke(*static_cast(value)); + } + + const void* m_value; + void (*m_formatImpl)(std::ostream& out, const char* fmtBegin, + const char* fmtEnd, int ntrunc, const void* value); + int (*m_toIntImpl)(const void* value); +}; + + +// Parse and return an integer from the string c, as atoi() +// On return, c is set to one past the end of the integer. +inline int parseIntAndAdvance(const char*& c) +{ + int i = 0; + for(;*c >= '0' && *c <= '9'; ++c) + i = 10*i + (*c - '0'); + return i; +} + +// Print literal part of format string and return next format spec +// position. +// +// Skips over any occurrences of '%%', printing a literal '%' to the +// output. The position of the first % character of the next +// nontrivial format spec is returned, or the end of string. +inline const char* printFormatStringLiteral(std::ostream& out, const char* fmt) +{ + const char* c = fmt; + for(;; ++c) + { + switch(*c) + { + case '\0': + out.write(fmt, c - fmt); + return c; + case '%': + out.write(fmt, c - fmt); + if(*(c+1) != '%') + return c; + // for "%%", tack trailing % onto next literal section. + fmt = ++c; + break; + default: + break; + } + } +} + + +// Parse a format string and set the stream state accordingly. +// +// The format mini-language recognized here is meant to be the one from C99, +// with the form "%[flags][width][.precision][length]type". +// +// Formatting options which can't be natively represented using the ostream +// state are returned in spacePadPositive (for space padded positive numbers) +// and ntrunc (for truncating conversions). argIndex is incremented if +// necessary to pull out variable width and precision . The function returns a +// pointer to the character after the end of the current format spec. +inline const char* streamStateFromFormat(std::ostream& out, bool& spacePadPositive, + int& ntrunc, const char* fmtStart, + const detail::FormatArg* formatters, + int& argIndex, int numFormatters) +{ + if(*fmtStart != '%') + { + TINYFORMAT_ERROR("tinyformat: Not enough conversion specifiers in format string"); + return fmtStart; + } + // Reset stream state to defaults. + out.width(0); + out.precision(6); + out.fill(' '); + // Reset most flags; ignore irrelevant unitbuf & skipws. + out.unsetf(std::ios::adjustfield | std::ios::basefield | + std::ios::floatfield | std::ios::showbase | std::ios::boolalpha | + std::ios::showpoint | std::ios::showpos | std::ios::uppercase); + bool precisionSet = false; + bool widthSet = false; + int widthExtra = 0; + const char* c = fmtStart + 1; + // 1) Parse flags + for(;; ++c) + { + switch(*c) + { + case '#': + out.setf(std::ios::showpoint | std::ios::showbase); + continue; + case '0': + // overridden by left alignment ('-' flag) + if(!(out.flags() & std::ios::left)) + { + // Use internal padding so that numeric values are + // formatted correctly, eg -00010 rather than 000-10 + out.fill('0'); + out.setf(std::ios::internal, std::ios::adjustfield); + } + continue; + case '-': + out.fill(' '); + out.setf(std::ios::left, std::ios::adjustfield); + continue; + case ' ': + // overridden by show positive sign, '+' flag. + if(!(out.flags() & std::ios::showpos)) + spacePadPositive = true; + continue; + case '+': + out.setf(std::ios::showpos); + spacePadPositive = false; + widthExtra = 1; + continue; + default: + break; + } + break; + } + // 2) Parse width + if(*c >= '0' && *c <= '9') + { + widthSet = true; + out.width(parseIntAndAdvance(c)); + } + if(*c == '*') + { + widthSet = true; + int width = 0; + if(argIndex < numFormatters) + width = formatters[argIndex++].toInt(); + else + TINYFORMAT_ERROR("tinyformat: Not enough arguments to read variable width"); + if(width < 0) + { + // negative widths correspond to '-' flag set + out.fill(' '); + out.setf(std::ios::left, std::ios::adjustfield); + width = -width; + } + out.width(width); + ++c; + } + // 3) Parse precision + if(*c == '.') + { + ++c; + int precision = 0; + if(*c == '*') + { + ++c; + if(argIndex < numFormatters) + precision = formatters[argIndex++].toInt(); + else + TINYFORMAT_ERROR("tinyformat: Not enough arguments to read variable precision"); + } + else + { + if(*c >= '0' && *c <= '9') + precision = parseIntAndAdvance(c); + else if(*c == '-') // negative precisions ignored, treated as zero. + parseIntAndAdvance(++c); + } + out.precision(precision); + precisionSet = true; + } + // 4) Ignore any C99 length modifier + while(*c == 'l' || *c == 'h' || *c == 'L' || + *c == 'j' || *c == 'z' || *c == 't') + ++c; + // 5) We're up to the conversion specifier character. + // Set stream flags based on conversion specifier (thanks to the + // boost::format class for forging the way here). + bool intConversion = false; + switch(*c) + { + case 'u': case 'd': case 'i': + out.setf(std::ios::dec, std::ios::basefield); + intConversion = true; + break; + case 'o': + out.setf(std::ios::oct, std::ios::basefield); + intConversion = true; + break; + case 'X': + out.setf(std::ios::uppercase); + // Falls through + case 'x': case 'p': + out.setf(std::ios::hex, std::ios::basefield); + intConversion = true; + break; + case 'E': + out.setf(std::ios::uppercase); + // Falls through + case 'e': + out.setf(std::ios::scientific, std::ios::floatfield); + out.setf(std::ios::dec, std::ios::basefield); + break; + case 'F': + out.setf(std::ios::uppercase); + // Falls through + case 'f': + out.setf(std::ios::fixed, std::ios::floatfield); + break; + case 'G': + out.setf(std::ios::uppercase); + // Falls through + case 'g': + out.setf(std::ios::dec, std::ios::basefield); + // As in boost::format, let stream decide float format. + out.flags(out.flags() & ~std::ios::floatfield); + break; + case 'a': case 'A': + TINYFORMAT_ERROR("tinyformat: the %a and %A conversion specs " + "are not supported"); + break; + case 'c': + // Handled as special case inside formatValue() + break; + case 's': + if(precisionSet) + ntrunc = static_cast(out.precision()); + // Make %s print booleans as "true" and "false" + out.setf(std::ios::boolalpha); + break; + case 'n': + // Not supported - will cause problems! + TINYFORMAT_ERROR("tinyformat: %n conversion spec not supported"); + break; + case '\0': + TINYFORMAT_ERROR("tinyformat: Conversion spec incorrectly " + "terminated by end of string"); + return c; + default: + break; + } + if(intConversion && precisionSet && !widthSet) + { + // "precision" for integers gives the minimum number of digits (to be + // padded with zeros on the left). This isn't really supported by the + // iostreams, but we can approximately simulate it with the width if + // the width isn't otherwise used. + out.width(out.precision() + widthExtra); + out.setf(std::ios::internal, std::ios::adjustfield); + out.fill('0'); + } + return c+1; +} + + +//------------------------------------------------------------------------------ +inline void formatImpl(std::ostream& out, const char* fmt, + const detail::FormatArg* formatters, + int numFormatters) +{ + // Saved stream state + std::streamsize origWidth = out.width(); + std::streamsize origPrecision = out.precision(); + std::ios::fmtflags origFlags = out.flags(); + char origFill = out.fill(); + + for (int argIndex = 0; argIndex < numFormatters; ++argIndex) + { + // Parse the format string + fmt = printFormatStringLiteral(out, fmt); + bool spacePadPositive = false; + int ntrunc = -1; + const char* fmtEnd = streamStateFromFormat(out, spacePadPositive, ntrunc, fmt, + formatters, argIndex, numFormatters); + if (argIndex >= numFormatters) + { + // Check args remain after reading any variable width/precision + TINYFORMAT_ERROR("tinyformat: Not enough format arguments"); + return; + } + const FormatArg& arg = formatters[argIndex]; + // Format the arg into the stream. + if(!spacePadPositive) + arg.format(out, fmt, fmtEnd, ntrunc); + else + { + // The following is a special case with no direct correspondence + // between stream formatting and the printf() behaviour. Simulate + // it crudely by formatting into a temporary string stream and + // munging the resulting string. + std::ostringstream tmpStream; + tmpStream.copyfmt(out); + tmpStream.setf(std::ios::showpos); + arg.format(tmpStream, fmt, fmtEnd, ntrunc); + std::string result = tmpStream.str(); // allocates... yuck. + for(size_t i = 0, iend = result.size(); i < iend; ++i) + if(result[i] == '+') result[i] = ' '; + out << result; + } + fmt = fmtEnd; + } + + // Print remaining part of format string. + fmt = printFormatStringLiteral(out, fmt); + if(*fmt != '\0') + TINYFORMAT_ERROR("tinyformat: Too many conversion specifiers in format string"); + + // Restore stream state + out.width(origWidth); + out.precision(origPrecision); + out.flags(origFlags); + out.fill(origFill); +} + +} // namespace detail + + +/// List of template arguments format(), held in a type-opaque way. +/// +/// A const reference to FormatList (typedef'd as FormatListRef) may be +/// conveniently used to pass arguments to non-template functions: All type +/// information has been stripped from the arguments, leaving just enough of a +/// common interface to perform formatting as required. +class FormatList +{ + public: + FormatList(detail::FormatArg* formatters, int N) + : m_formatters(formatters), m_N(N) { } + + friend void vformat(std::ostream& out, const char* fmt, + const FormatList& list); + + private: + const detail::FormatArg* m_formatters; + int m_N; +}; + +/// Reference to type-opaque format list for passing to vformat() +typedef const FormatList& FormatListRef; + + +namespace detail { + +// Format list subclass with fixed storage to avoid dynamic allocation +template +class FormatListN : public FormatList +{ + public: +#ifdef TINYFORMAT_USE_VARIADIC_TEMPLATES + template + FormatListN(const Args&... args) + : FormatList(&m_formatterStore[0], N), + m_formatterStore { FormatArg(args)... } + { static_assert(sizeof...(args) == N, "Number of args must be N"); } +#else // C++98 version + void init(int) {} +# define TINYFORMAT_MAKE_FORMATLIST_CONSTRUCTOR(n) \ + \ + template \ + FormatListN(TINYFORMAT_VARARGS(n)) \ + : FormatList(&m_formatterStore[0], n) \ + { TINYFORMAT_ASSERT(n == N); init(0, TINYFORMAT_PASSARGS(n)); } \ + \ + template \ + void init(int i, TINYFORMAT_VARARGS(n)) \ + { \ + m_formatterStore[i] = FormatArg(v1); \ + init(i+1 TINYFORMAT_PASSARGS_TAIL(n)); \ + } + + TINYFORMAT_FOREACH_ARGNUM(TINYFORMAT_MAKE_FORMATLIST_CONSTRUCTOR) +# undef TINYFORMAT_MAKE_FORMATLIST_CONSTRUCTOR +#endif + + private: + FormatArg m_formatterStore[N]; +}; + +// Special 0-arg version - MSVC says zero-sized C array in struct is nonstandard +template<> class FormatListN<0> : public FormatList +{ + public: FormatListN() : FormatList(0, 0) {} +}; + +} // namespace detail + + +//------------------------------------------------------------------------------ +// Primary API functions + +#ifdef TINYFORMAT_USE_VARIADIC_TEMPLATES + +/// Make type-agnostic format list from list of template arguments. +/// +/// The exact return type of this function is an implementation detail and +/// shouldn't be relied upon. Instead it should be stored as a FormatListRef: +/// +/// FormatListRef formatList = makeFormatList( /*...*/ ); +template +detail::FormatListN makeFormatList(const Args&... args) +{ + return detail::FormatListN(args...); +} + +#else // C++98 version + +inline detail::FormatListN<0> makeFormatList() +{ + return detail::FormatListN<0>(); +} +#define TINYFORMAT_MAKE_MAKEFORMATLIST(n) \ +template \ +detail::FormatListN makeFormatList(TINYFORMAT_VARARGS(n)) \ +{ \ + return detail::FormatListN(TINYFORMAT_PASSARGS(n)); \ +} +TINYFORMAT_FOREACH_ARGNUM(TINYFORMAT_MAKE_MAKEFORMATLIST) +#undef TINYFORMAT_MAKE_MAKEFORMATLIST + +#endif + +/// Format list of arguments to the stream according to the given format string. +/// +/// The name vformat() is chosen for the semantic similarity to vprintf(): the +/// list of format arguments is held in a single function argument. +inline void vformat(std::ostream& out, const char* fmt, FormatListRef list) +{ + detail::formatImpl(out, fmt, list.m_formatters, list.m_N); +} + + +#ifdef TINYFORMAT_USE_VARIADIC_TEMPLATES + +/// Format list of arguments to the stream according to given format string. +template +void format(std::ostream& out, const char* fmt, const Args&... args) +{ + vformat(out, fmt, makeFormatList(args...)); +} + +/// Format list of arguments according to the given format string and return +/// the result as a string. +template +std::string format(const char* fmt, const Args&... args) +{ + std::ostringstream oss; + format(oss, fmt, args...); + return oss.str(); +} + +/// Format list of arguments to std::cout, according to the given format string +template +void printf(const char* fmt, const Args&... args) +{ + format(std::cout, fmt, args...); +} + +template +void printfln(const char* fmt, const Args&... args) +{ + format(std::cout, fmt, args...); + std::cout << '\n'; +} + + +#else // C++98 version + +inline void format(std::ostream& out, const char* fmt) +{ + vformat(out, fmt, makeFormatList()); +} + +inline std::string format(const char* fmt) +{ + std::ostringstream oss; + format(oss, fmt); + return oss.str(); +} + +inline void printf(const char* fmt) +{ + format(std::cout, fmt); +} + +inline void printfln(const char* fmt) +{ + format(std::cout, fmt); + std::cout << '\n'; +} + +#define TINYFORMAT_MAKE_FORMAT_FUNCS(n) \ + \ +template \ +void format(std::ostream& out, const char* fmt, TINYFORMAT_VARARGS(n)) \ +{ \ + vformat(out, fmt, makeFormatList(TINYFORMAT_PASSARGS(n))); \ +} \ + \ +template \ +std::string format(const char* fmt, TINYFORMAT_VARARGS(n)) \ +{ \ + std::ostringstream oss; \ + format(oss, fmt, TINYFORMAT_PASSARGS(n)); \ + return oss.str(); \ +} \ + \ +template \ +void printf(const char* fmt, TINYFORMAT_VARARGS(n)) \ +{ \ + format(std::cout, fmt, TINYFORMAT_PASSARGS(n)); \ +} \ + \ +template \ +void printfln(const char* fmt, TINYFORMAT_VARARGS(n)) \ +{ \ + format(std::cout, fmt, TINYFORMAT_PASSARGS(n)); \ + std::cout << '\n'; \ +} + +TINYFORMAT_FOREACH_ARGNUM(TINYFORMAT_MAKE_FORMAT_FUNCS) +#undef TINYFORMAT_MAKE_FORMAT_FUNCS + +#endif + + +} // namespace tinyformat + +#endif // TINYFORMAT_H_INCLUDED diff --git a/include/writeVTK.hpp b/include/writeVTK.hpp new file mode 100644 index 0000000..2ff0f19 --- /dev/null +++ b/include/writeVTK.hpp @@ -0,0 +1,54 @@ +#pragma once +#include +#include +namespace meshless { + template + void writePntVTK(const std::string& path, const Eigen::MatrixXd& pnts, const std::vector& attri) { + std::ofstream out(path); + out << "# vtk DataFile Version 3.0\n" + "Volume Mesh\n" + "ASCII\n" + "DATASET UNSTRUCTURED_GRID" + << std::endl; + out << "POINTS " << pnts.rows() << " float" << std::endl; + for(int i = 0; i < pnts.rows(); ++i) { + for(int j = 0; j < dim; ++j) { + out << std::setprecision(4) << pnts(i, j) << " "; + } + for(int j = dim; j < 3; ++j) { + out << "0 "; + } + out << std::endl; + } + int innersize = 0; + int interSize = 0; + int bounSize = 0; + + out << "CELLS " << pnts.rows() << " " << pnts.rows() * (1 + 1) << std::endl; + for(int i = 0; i < pnts.rows(); ++i) { + out << "1 " << i << std::endl; + } + out << "CELL_TYPES " << pnts.rows() << std::endl; + for(int i = 0; i < pnts.rows(); ++i) { + out << 1 << std::endl; + } + + if(!attri.empty()) { + out << "POINT_DATA " << attri.size() << "\n" + << "SCALARS point_scalars double 1\n" + << "LOOKUP_TABLE default" << std::endl; + for(auto& d : attri) { + out << d << std::endl; + if(d == 1) + innersize++; + else + bounSize++; + } + } + std::cout << "内部点： " << innersize << "\n"; + std::cout << "边界点： " << bounSize << "\n"; + } + + + +}; \ No newline at end of file diff --git a/src/assert.cpp b/src/assert.cpp new file mode 100644 index 0000000..29b5d71 --- /dev/null +++ b/src/assert.cpp @@ -0,0 +1,26 @@ +#include "assert.hpp" + +/** + * @file + * Implementation of custom assert utilities. + */ + +namespace meshless { + + namespace assert_internal { + + bool assert_handler_implementation(const char* condition, const char* file, const char* func_name, + int line, const char* message, tfm::FormatListRef format_list) { + std::cerr << "\x1b[37;1m"; // white bold + tfm::format(std::cerr, "%s:%d: %s: Assertion `%s' failed with message:\n", + file, line, func_name, condition); + std::cerr << "\x1b[31;1m"; // red bold + tfm::vformat(std::cerr, message, format_list); + std::cerr << "\x1b[37;0m\n"; // no color + std::cerr.flush(); + return true; + } + + } // namespace assert_internal + +} // namespace meshless diff --git a/src/main.cpp b/src/main.cpp new file mode 100644 index 0000000..8073c76 --- /dev/null +++ b/src/main.cpp @@ -0,0 +1,38 @@ +#include +#include +#include +#include "FillBoundary.hpp" +#include "writeVTK.hpp" + +using namespace std; +using namespace meshless; + +int main() { + Eigen::MatrixXd mat; + BRep_Builder brepBuilder; + TopoDS_Shape myShape; + //auto status = BRepTools::Read(myShape, "F:/CADMeshless/StepAndBrepModel/DWE_ADM102_ASSIGN.brep", brepBuilder); + auto status = BRepTools::Read(myShape, "F:/CADMeshless/StepAndBrepModel/Exhaust_Manifold.brep", brepBuilder); + OccShape occShape(myShape); + auto domain = discretizeBoundaryWithStep(occShape, 1, -1); + Eigen::MatrixXd quaPoints; + + quaPoints.resize(domain.positions().size(), 3); + std::vector attri; + for(int i = 0; i < domain.positions().size(); i++) { + if(domain.types()[i] == 1) { + attri.push_back(1.0); + } else { + attri.push_back(0); + } + quaPoints(i, 0) = domain.positions()[i].x(); + quaPoints(i, 1) = domain.positions()[i].y(); + quaPoints(i, 2) = domain.positions()[i].z(); + } + writePntVTK<3>("Exhaust_Manifold.vtk", quaPoints, attri); + + + + + return 0; +} \ No newline at end of file