Program Listing for File BV_splitter.h
↰ Return to documentation for file (/tmp/ws/src/hpp-fcl/include/hpp/fcl/internal/BV_splitter.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.
*/
#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 {
enum SplitMethodType {
SPLIT_METHOD_MEAN,
SPLIT_METHOD_MEDIAN,
SPLIT_METHOD_BV_CENTER
};
template <typename BV>
class BVSplitter {
public:
BVSplitter(SplitMethodType method)
: split_vector(0, 0, 0), split_method(method) {}
virtual ~BVSplitter() {}
void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_) {
vertices = vertices_;
tri_indices = tri_indices_;
type = type_;
}
void computeRule(const BV& bv, unsigned int* primitive_indices,
unsigned 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;
}
}
bool apply(const Vec3f& q) const { return q[split_axis] > split_value; }
void clear() {
vertices = NULL;
tri_indices = NULL;
type = BVH_MODEL_UNKNOWN;
}
protected:
int split_axis;
Vec3f split_vector;
FCL_REAL split_value;
Vec3f* vertices;
Triangle* tri_indices;
BVHModelType type;
SplitMethodType split_method;
void computeRule_bvcenter(const BV& bv, unsigned int*, unsigned 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];
}
void computeRule_mean(const BV& bv, unsigned int* primitive_indices,
unsigned 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 (unsigned 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 (unsigned int i = 0; i < num_primitives; ++i) {
sum += vertices[primitive_indices[i]][split_axis];
}
}
split_value = sum / num_primitives;
}
void computeRule_median(const BV& bv, unsigned int* primitive_indices,
unsigned 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((size_t)num_primitives);
if (type == BVH_MODEL_TRIANGLES) {
for (unsigned 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 (unsigned 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 HPP_FCL_DLLAPI BVSplitter<OBB>::apply(const Vec3f& q) const;
template <>
bool HPP_FCL_DLLAPI BVSplitter<RSS>::apply(const Vec3f& q) const;
template <>
bool HPP_FCL_DLLAPI BVSplitter<kIOS>::apply(const Vec3f& q) const;
template <>
bool HPP_FCL_DLLAPI BVSplitter<OBBRSS>::apply(const Vec3f& q) const;
template <>
void HPP_FCL_DLLAPI BVSplitter<OBB>::computeRule_bvcenter(
const OBB& bv, unsigned int* primitive_indices,
unsigned int num_primitives);
template <>
void HPP_FCL_DLLAPI BVSplitter<OBB>::computeRule_mean(
const OBB& bv, unsigned int* primitive_indices,
unsigned int num_primitives);
template <>
void HPP_FCL_DLLAPI BVSplitter<OBB>::computeRule_median(
const OBB& bv, unsigned int* primitive_indices,
unsigned int num_primitives);
template <>
void HPP_FCL_DLLAPI BVSplitter<RSS>::computeRule_bvcenter(
const RSS& bv, unsigned int* primitive_indices,
unsigned int num_primitives);
template <>
void HPP_FCL_DLLAPI BVSplitter<RSS>::computeRule_mean(
const RSS& bv, unsigned int* primitive_indices,
unsigned int num_primitives);
template <>
void HPP_FCL_DLLAPI BVSplitter<RSS>::computeRule_median(
const RSS& bv, unsigned int* primitive_indices,
unsigned int num_primitives);
template <>
void HPP_FCL_DLLAPI BVSplitter<kIOS>::computeRule_bvcenter(
const kIOS& bv, unsigned int* primitive_indices,
unsigned int num_primitives);
template <>
void HPP_FCL_DLLAPI BVSplitter<kIOS>::computeRule_mean(
const kIOS& bv, unsigned int* primitive_indices,
unsigned int num_primitives);
template <>
void HPP_FCL_DLLAPI BVSplitter<kIOS>::computeRule_median(
const kIOS& bv, unsigned int* primitive_indices,
unsigned int num_primitives);
template <>
void HPP_FCL_DLLAPI BVSplitter<OBBRSS>::computeRule_bvcenter(
const OBBRSS& bv, unsigned int* primitive_indices,
unsigned int num_primitives);
template <>
void HPP_FCL_DLLAPI BVSplitter<OBBRSS>::computeRule_mean(
const OBBRSS& bv, unsigned int* primitive_indices,
unsigned int num_primitives);
template <>
void HPP_FCL_DLLAPI BVSplitter<OBBRSS>::computeRule_median(
const OBBRSS& bv, unsigned int* primitive_indices,
unsigned int num_primitives);
} // namespace fcl
} // namespace hpp
#endif