From c92151a6cd7f5f555f8b50d0dec44f68ed6af25c Mon Sep 17 00:00:00 2001
From: Lucile Remigy <lucile.remigy@epitech.eu>
Date: Thu, 26 Sep 2019 13:25:47 +0200
Subject: [PATCH] File BV_splitter.h and BV_fitter.h private
---
include/hpp/fcl/BVH/BVH_model.h | 4 +-
src/BVH/BV_fitter.cpp | 2 +-
src/BVH/BV_fitter.h | 228 ++++++++++++++++++++
src/BVH/BV_splitter.cpp | 2 +-
src/BVH/BV_splitter.h | 274 +++++++++++++++++++++++++
src/shape/geometric_shapes_utility.cpp | 2 +-
6 files changed, 507 insertions(+), 5 deletions(-)
create mode 100644 src/BVH/BV_fitter.h
create mode 100644 src/BVH/BV_splitter.h
diff --git a/include/hpp/fcl/BVH/BVH_model.h b/include/hpp/fcl/BVH/BVH_model.h
index 7d7f00c8..7612f9f8 100644
--- a/include/hpp/fcl/BVH/BVH_model.h
+++ b/include/hpp/fcl/BVH/BVH_model.h
@@ -41,8 +41,8 @@
#include <hpp/fcl/collision_object.h>
#include <hpp/fcl/BVH/BVH_internal.h>
#include <hpp/fcl/BV/BV_node.h>
-#include <hpp/fcl/BVH/BV_splitter.h>
-#include <hpp/fcl/BVH/BV_fitter.h>
+#include "../../src/BVH/BV_splitter.h"
+#include "../../src/BVH/BV_fitter.h"
#include <vector>
#include <boost/shared_ptr.hpp>
#include <boost/noncopyable.hpp>
diff --git a/src/BVH/BV_fitter.cpp b/src/BVH/BV_fitter.cpp
index d04481fb..df9b26cb 100644
--- a/src/BVH/BV_fitter.cpp
+++ b/src/BVH/BV_fitter.cpp
@@ -35,7 +35,7 @@
/** \author Jia Pan */
-#include <hpp/fcl/BVH/BV_fitter.h>
+#include "BV_fitter.h"
#include <hpp/fcl/BVH/BVH_utility.h>
#include <limits>
#include "../math/tools.h"
diff --git a/src/BVH/BV_fitter.h b/src/BVH/BV_fitter.h
new file mode 100644
index 00000000..d8070d69
--- /dev/null
+++ b/src/BVH/BV_fitter.h
@@ -0,0 +1,228 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * 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.
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "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
+ * COPYRIGHT OWNER OR CONTRIBUTORS 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.
+ */
+
+/** \author Jia Pan */
+
+#ifndef HPP_FCL_BV_FITTER_H
+#define HPP_FCL_BV_FITTER_H
+
+#include <hpp/fcl/BVH/BVH_internal.h>
+#include <hpp/fcl/BV/kIOS.h>
+#include <hpp/fcl/BV/OBBRSS.h>
+#include <hpp/fcl/BV/AABB.h>
+#include <iostream>
+
+namespace hpp
+{
+namespace fcl
+{
+
+/// @brief Compute a bounding volume that fits a set of n points.
+template<typename BV>
+void fit(Vec3f* ps, int n, BV& bv)
+{
+ for(int i = 0; i < n; ++i)
+ {
+ bv += ps[i];
+ }
+}
+
+template<>
+void fit<OBB>(Vec3f* ps, int n, OBB& bv);
+
+template<>
+void fit<RSS>(Vec3f* ps, int n, RSS& bv);
+
+template<>
+void fit<kIOS>(Vec3f* ps, int n, kIOS& bv);
+
+template<>
+void fit<OBBRSS>(Vec3f* ps, int n, OBBRSS& bv);
+
+template<>
+void fit<AABB>(Vec3f* ps, int n, AABB& bv);
+
+/// @brief The class for the default algorithm fitting a bounding volume to a set of points
+template<typename BV>
+class BVFitterTpl
+{
+public:
+ /// @brief default deconstructor
+ virtual ~BVFitterTpl() {}
+
+ /// @brief Prepare the geometry primitive data for fitting
+ void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
+ {
+ vertices = vertices_;
+ prev_vertices = NULL;
+ tri_indices = tri_indices_;
+ type = type_;
+ }
+
+ /// @brief Prepare the geometry primitive data for fitting, for deformable mesh
+ void set(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* tri_indices_, BVHModelType type_)
+ {
+ vertices = vertices_;
+ prev_vertices = prev_vertices_;
+ tri_indices = tri_indices_;
+ type = type_;
+ }
+
+ /// @brief Compute the fitting BV
+ virtual BV fit(unsigned int* primitive_indices, int num_primitives) = 0;
+
+ /// @brief Clear the geometry primitive data
+ void clear()
+ {
+ vertices = NULL;
+ prev_vertices = NULL;
+ tri_indices = NULL;
+ type = BVH_MODEL_UNKNOWN;
+ }
+
+protected:
+
+ Vec3f* vertices;
+ Vec3f* prev_vertices;
+ Triangle* tri_indices;
+ BVHModelType type;
+};
+
+/// @brief The class for the default algorithm fitting a bounding volume to a set of points
+template<typename BV>
+class BVFitter : public BVFitterTpl<BV>
+{
+public:
+ /// @brief Compute a bounding volume that fits a set of primitives (points or triangles).
+ /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data
+ BV fit(unsigned int* primitive_indices, int num_primitives)
+ {
+ BV bv;
+
+ if(type == BVH_MODEL_TRIANGLES) /// The primitive is triangle
+ {
+ for(int i = 0; i < num_primitives; ++i)
+ {
+ Triangle t = tri_indices[primitive_indices[i]];
+ bv += vertices[t[0]];
+ bv += vertices[t[1]];
+ bv += vertices[t[2]];
+
+ if(prev_vertices) /// can fitting both current and previous frame
+ {
+ bv += prev_vertices[t[0]];
+ bv += prev_vertices[t[1]];
+ bv += prev_vertices[t[2]];
+ }
+ }
+ }
+ else if(type == BVH_MODEL_POINTCLOUD) /// The primitive is point
+ {
+ for(int i = 0; i < num_primitives; ++i)
+ {
+ bv += vertices[primitive_indices[i]];
+
+ if(prev_vertices) /// can fitting both current and previous frame
+ {
+ bv += prev_vertices[primitive_indices[i]];
+ }
+ }
+ }
+
+ return bv;
+ }
+
+protected:
+ using BVFitterTpl<BV>::vertices;
+ using BVFitterTpl<BV>::prev_vertices;
+ using BVFitterTpl<BV>::tri_indices;
+ using BVFitterTpl<BV>::type;
+};
+
+/// @brief Specification of BVFitter for OBB bounding volume
+template<>
+class BVFitter<OBB> : public BVFitterTpl<OBB>
+{
+public:
+ /// @brief Compute a bounding volume that fits a set of primitives (points or triangles).
+ /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data.
+ OBB fit(unsigned int* primitive_indices, int num_primitives);
+};
+
+/// @brief Specification of BVFitter for RSS bounding volume
+template<>
+class BVFitter<RSS> : public BVFitterTpl<RSS>
+{
+public:
+ /// @brief Compute a bounding volume that fits a set of primitives (points or triangles).
+ /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data.
+ RSS fit(unsigned int* primitive_indices, int num_primitives);
+};
+
+/// @brief Specification of BVFitter for kIOS bounding volume
+template<>
+class BVFitter<kIOS> : public BVFitterTpl<kIOS>
+{
+public:
+ /// @brief Compute a bounding volume that fits a set of primitives (points or triangles).
+ /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data.
+ kIOS fit(unsigned int* primitive_indices, int num_primitives);
+};
+
+/// @brief Specification of BVFitter for OBBRSS bounding volume
+template<>
+class BVFitter<OBBRSS> : public BVFitterTpl<OBBRSS>
+{
+public:
+ /// @brief Compute a bounding volume that fits a set of primitives (points or triangles).
+ /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data.
+ OBBRSS fit(unsigned int* primitive_indices, int num_primitives);
+};
+
+/// @brief Specification of BVFitter for AABB bounding volume
+template<>
+class BVFitter<AABB> : public BVFitterTpl<AABB>
+{
+public:
+ /// @brief Compute a bounding volume that fits a set of primitives (points or triangles).
+ /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data.
+ AABB fit(unsigned int* primitive_indices, int num_primitives);
+};
+
+}
+
+} // namespace hpp
+
+#endif
diff --git a/src/BVH/BV_splitter.cpp b/src/BVH/BV_splitter.cpp
index 1674cef4..c709ed63 100644
--- a/src/BVH/BV_splitter.cpp
+++ b/src/BVH/BV_splitter.cpp
@@ -35,7 +35,7 @@
/** \author Jia Pan */
-#include <hpp/fcl/BVH/BV_splitter.h>
+#include "BV_splitter.h"
namespace hpp
{
diff --git a/src/BVH/BV_splitter.h b/src/BVH/BV_splitter.h
new file mode 100644
index 00000000..7be8ca74
--- /dev/null
+++ b/src/BVH/BV_splitter.h
@@ -0,0 +1,274 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * 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.
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "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
+ * COPYRIGHT OWNER OR CONTRIBUTORS 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.
+ */
+
+/** \author Jia Pan */
+
+#ifndef HPP_FCL_BV_SPLITTER_H
+#define HPP_FCL_BV_SPLITTER_H
+
+#include <hpp/fcl/BVH/BVH_internal.h>
+#include <hpp/fcl/BV/kIOS.h>
+#include <hpp/fcl/BV/OBBRSS.h>
+#include <vector>
+#include <iostream>
+
+namespace hpp
+{
+namespace fcl
+{
+
+/// @brief Three types of split algorithms are provided in FCL as default
+enum SplitMethodType {SPLIT_METHOD_MEAN, SPLIT_METHOD_MEDIAN, SPLIT_METHOD_BV_CENTER};
+
+
+/// @brief A class describing the split rule that splits each BV node
+template<typename BV>
+class BVSplitter
+{
+public:
+
+ BVSplitter(SplitMethodType method) : split_vector(0,0,0), split_method(method)
+ {
+ }
+
+ /// @brief Default deconstructor
+ virtual ~BVSplitter() {}
+
+ /// @brief Set the geometry data needed by the split rule
+ void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
+ {
+ vertices = vertices_;
+ tri_indices = tri_indices_;
+ type = type_;
+ }
+
+ /// @brief Compute the split rule according to a subset of geometry and the corresponding BV node
+ void computeRule(const BV& bv, unsigned int* primitive_indices, int num_primitives)
+ {
+ switch(split_method)
+ {
+ case SPLIT_METHOD_MEAN:
+ computeRule_mean(bv, primitive_indices, num_primitives);
+ break;
+ case SPLIT_METHOD_MEDIAN:
+ computeRule_median(bv, primitive_indices, num_primitives);
+ break;
+ case SPLIT_METHOD_BV_CENTER:
+ computeRule_bvcenter(bv, primitive_indices, num_primitives);
+ break;
+ default:
+ std::cerr << "Split method not supported" << std::endl;
+ }
+ }
+
+ /// @brief Apply the split rule on a given point
+ bool apply(const Vec3f& q) const
+ {
+ return q[split_axis] > split_value;
+ }
+
+ /// @brief Clear the geometry data set before
+ void clear()
+ {
+ vertices = NULL;
+ tri_indices = NULL;
+ type = BVH_MODEL_UNKNOWN;
+ }
+
+private:
+
+ /// @brief The axis based on which the split decision is made. For most BV, the axis is aligned with one of the world coordinate, so only split_axis is needed.
+ /// For oriented node, we can use a vector to make a better split decision.
+ int split_axis;
+ Vec3f split_vector;
+
+ /// @brief The split threshold, different primitives are splitted according whether their projection on the split_axis is larger or smaller than the threshold
+ FCL_REAL split_value;
+
+ /// @brief The mesh vertices or points handled by the splitter
+ Vec3f* vertices;
+
+ /// @brief The triangles handled by the splitter
+ Triangle* tri_indices;
+
+ /// @brief Whether the geometry is mesh or point cloud
+ BVHModelType type;
+
+ /// @brief The split algorithm used
+ SplitMethodType split_method;
+
+ /// @brief Split algorithm 1: Split the node from center
+ void computeRule_bvcenter(const BV& bv, unsigned int*, int)
+ {
+ Vec3f center = bv.center();
+ int axis = 2;
+
+ if(bv.width() >= bv.height() && bv.width() >= bv.depth())
+ axis = 0;
+ else if(bv.height() >= bv.width() && bv.height() >= bv.depth())
+ axis = 1;
+
+ split_axis = axis;
+ split_value = center[axis];
+ }
+
+ /// @brief Split algorithm 2: Split the node according to the mean of the data contained
+ void computeRule_mean(const BV& bv, unsigned int* primitive_indices, int num_primitives)
+ {
+ int axis = 2;
+
+ if(bv.width() >= bv.height() && bv.width() >= bv.depth())
+ axis = 0;
+ else if(bv.height() >= bv.width() && bv.height() >= bv.depth())
+ axis = 1;
+
+ split_axis = axis;
+ FCL_REAL sum = 0;
+
+ if(type == BVH_MODEL_TRIANGLES)
+ {
+ for(int i = 0; i < num_primitives; ++i)
+ {
+ const Triangle& t = tri_indices[primitive_indices[i]];
+ sum += (vertices[t[0]][split_axis] + vertices[t[1]][split_axis] + vertices[t[2]][split_axis]);
+ }
+
+ sum /= 3;
+ }
+ else if(type == BVH_MODEL_POINTCLOUD)
+ {
+ for(int i = 0; i < num_primitives; ++i)
+ {
+ sum += vertices[primitive_indices[i]][split_axis];
+ }
+ }
+
+ split_value = sum / num_primitives;
+ }
+
+ /// @brief Split algorithm 3: Split the node according to the median of the data contained
+ void computeRule_median(const BV& bv, unsigned int* primitive_indices, int num_primitives)
+ {
+ int axis = 2;
+
+ if(bv.width() >= bv.height() && bv.width() >= bv.depth())
+ axis = 0;
+ else if(bv.height() >= bv.width() && bv.height() >= bv.depth())
+ axis = 1;
+
+ split_axis = axis;
+ std::vector<FCL_REAL> proj(num_primitives);
+
+ if(type == BVH_MODEL_TRIANGLES)
+ {
+ for(int i = 0; i < num_primitives; ++i)
+ {
+ const Triangle& t = tri_indices[primitive_indices[i]];
+ proj[i] = (vertices[t[0]][split_axis] + vertices[t[1]][split_axis] + vertices[t[2]][split_axis]) / 3;
+ }
+ }
+ else if(type == BVH_MODEL_POINTCLOUD)
+ {
+ for(int i = 0; i < num_primitives; ++i)
+ proj[i] = vertices[primitive_indices[i]][split_axis];
+ }
+
+ std::sort(proj.begin(), proj.end());
+
+ if(num_primitives % 2 == 1)
+ {
+ split_value = proj[(num_primitives - 1) / 2];
+ }
+ else
+ {
+ split_value = (proj[num_primitives / 2] + proj[num_primitives / 2 - 1]) / 2;
+ }
+ }
+};
+
+
+template<>
+bool BVSplitter<OBB>::apply(const Vec3f& q) const;
+
+template<>
+bool BVSplitter<RSS>::apply(const Vec3f& q) const;
+
+template<>
+bool BVSplitter<kIOS>::apply(const Vec3f& q) const;
+
+template<>
+bool BVSplitter<OBBRSS>::apply(const Vec3f& q) const;
+
+template<>
+void BVSplitter<OBB>::computeRule_bvcenter(const OBB& bv, unsigned int* primitive_indices, int num_primitives);
+
+template<>
+void BVSplitter<OBB>::computeRule_mean(const OBB& bv, unsigned int* primitive_indices, int num_primitives);
+
+template<>
+void BVSplitter<OBB>::computeRule_median(const OBB& bv, unsigned int* primitive_indices, int num_primitives);
+
+template<>
+void BVSplitter<RSS>::computeRule_bvcenter(const RSS& bv, unsigned int* primitive_indices, int num_primitives);
+
+template<>
+void BVSplitter<RSS>::computeRule_mean(const RSS& bv, unsigned int* primitive_indices, int num_primitives);
+
+template<>
+void BVSplitter<RSS>::computeRule_median(const RSS& bv, unsigned int* primitive_indices, int num_primitives);
+
+template<>
+void BVSplitter<kIOS>::computeRule_bvcenter(const kIOS& bv, unsigned int* primitive_indices, int num_primitives);
+
+template<>
+void BVSplitter<kIOS>::computeRule_mean(const kIOS& bv, unsigned int* primitive_indices, int num_primitives);
+
+template<>
+void BVSplitter<kIOS>::computeRule_median(const kIOS& bv, unsigned int* primitive_indices, int num_primitives);
+
+template<>
+void BVSplitter<OBBRSS>::computeRule_bvcenter(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives);
+
+template<>
+void BVSplitter<OBBRSS>::computeRule_mean(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives);
+
+template<>
+void BVSplitter<OBBRSS>::computeRule_median(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives);
+
+}
+
+} // namespace hpp
+
+#endif
diff --git a/src/shape/geometric_shapes_utility.cpp b/src/shape/geometric_shapes_utility.cpp
index 70f26152..0a82aeb2 100644
--- a/src/shape/geometric_shapes_utility.cpp
+++ b/src/shape/geometric_shapes_utility.cpp
@@ -37,7 +37,7 @@
#include <hpp/fcl/shape/geometric_shapes_utility.h>
-#include <hpp/fcl/BVH/BV_fitter.h>
+#include "../BVH/BV_fitter.h"
#include "../math/tools.h"
namespace hpp
--
GitLab