Commit c92151a6 authored by Lucile Remigy's avatar Lucile Remigy
Browse files

File BV_splitter.h and BV_fitter.h private

parent cfdb3136
......@@ -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>
......
......@@ -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"
......
/*
* 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
......@@ -35,7 +35,7 @@
/** \author Jia Pan */
#include <hpp/fcl/BVH/BV_splitter.h>
#include "BV_splitter.h"
namespace hpp
{
......
/*
* 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
......@@ -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
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment