Commit b896a309 authored by jpan's avatar jpan
Browse files

git-svn-id: https://kforge.ros.org/fcl/fcl_ros@85 253336fb-580f-4252-a368-f3cef5a2a82b
parent 303f076c
......@@ -37,7 +37,7 @@ link_directories(${CCD_LIBRARY_DIRS})
add_definitions(-DUSE_SVMLIGHT=0)
add_library(${PROJECT_NAME} SHARED src/AABB.cpp src/OBB.cpp src/RSS.cpp src/kIOS.cpp src/vec_3f.cpp src/traversal_node_base.cpp src/traversal_node_bvhs.cpp src/intersect.cpp src/motion.cpp src/BV_fitter.cpp src/BV_splitter.cpp src/BVH_model.cpp src/BVH_utility.cpp src/transform.cpp src/simple_setup.cpp src/geometric_shapes.cpp src/geometric_shapes_utility.cpp src/geometric_shapes_intersect.cpp src/collision_node.cpp src/traversal_recurse.cpp src/broad_phase_collision.cpp src/collision.cpp src/collision_func_matrix.cpp src/interval_tree.cpp src/conservative_advancement.cpp src/matrix_3f.cpp src/interval.cpp src/interval_vector.cpp src/interval_matrix.cpp src/taylor_model.cpp src/taylor_vector.cpp src/taylor_matrix.cpp)
add_library(${PROJECT_NAME} SHARED src/AABB.cpp src/OBB.cpp src/RSS.cpp src/kIOS.cpp src/vec_3f.cpp src/traversal_node_base.cpp src/traversal_node_bvhs.cpp src/intersect.cpp src/motion.cpp src/BV_fitter.cpp src/BV_splitter.cpp src/BVH_model.cpp src/BVH_utility.cpp src/transform.cpp src/simple_setup.cpp src/geometric_shapes.cpp src/geometric_shapes_utility.cpp src/geometric_shapes_intersect.cpp src/collision_node.cpp src/traversal_recurse.cpp src/broad_phase_collision.cpp src/collision.cpp src/collision_func_matrix.cpp src/interval_tree.cpp src/conservative_advancement.cpp src/matrix_3f.cpp src/interval.cpp src/interval_vector.cpp src/interval_matrix.cpp src/taylor_model.cpp src/taylor_vector.cpp src/taylor_matrix.cpp src/simd_intersect.cpp)
target_link_libraries(${PROJECT_NAME} ${FLANN_LIBRARIES} ${CCD_LIBRARIES})
......
......@@ -287,6 +287,9 @@ NODE_TYPE BVHModel<RSS>::getNodeType() const;
template<>
NODE_TYPE BVHModel<kIOS>::getNodeType() const;
template<>
NODE_TYPE BVHModel<OBBRSS>::getNodeType() const;
template<>
NODE_TYPE BVHModel<KDOP<16> >::getNodeType() const;
......
......@@ -45,7 +45,6 @@
namespace fcl
{
/** \brief Expand the BVH bounding boxes according to uncertainty */
template<typename BV>
void BVHExpand(BVHModel<BV>& model, const Uncertainty* ucs, BVH_REAL r)
......@@ -81,31 +80,16 @@ void BVHExpand(BVHModel<RSS>& model, const Uncertainty* ucs, BVH_REAL r);
/** \brief Estimate the uncertainty of point clouds due to sampling procedure */
void estimateSamplingUncertainty(Vec3f* vertices, int num_vertices, Uncertainty* ucs);
/** \brief Compute the covariance matrix for a set or subset of points. */
void getCovariance(Vec3f* ps, Vec3f* ps2, unsigned int* indices, int n, Matrix3f& M);
/** \brief Compute the covariance matrix for a set or subset of triangles. */
/** \brief Compute the covariance matrix for a set or subset of points. if ts = null, then indices refer to points directly; otherwise refer to triangles */
void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Matrix3f& M);
/** \brief Compute the RSS bounding volume parameters: radius, rectangle size and the origin.
* The bounding volume axes are known.
*/
void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, unsigned int* indices, int n, Vec3f axis[3], Vec3f& origin, BVH_REAL l[2], BVH_REAL& r);
/** \brief Compute the RSS bounding volume parameters: radius, rectangle size and the origin.
* The bounding volume axes are known.
*/
void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Vec3f axis[3], Vec3f& origin, BVH_REAL l[2], BVH_REAL& r);
/** \brief Compute the bounding volume extent and center for a set or subset of points.
* The bounding volume axes are known.
*/
void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, unsigned int* indices, int n, Vec3f axis[3], Vec3f& center, Vec3f& extent);
/** \brief Compute the bounding volume extent and center for a set or subset of points.
* The bounding volume axes are known.
*/
......@@ -115,8 +99,6 @@ void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indic
void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c, Vec3f& center, BVH_REAL& radius);
/** \brief Compute the maximum distance from a given center point to a point cloud */
BVH_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, unsigned int* indices, int n, const Vec3f& query);
BVH_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, const Vec3f& query);
......
......@@ -44,6 +44,7 @@
#include "fcl/OBB.h"
#include "fcl/RSS.h"
#include "fcl/kIOS.h"
#include "fcl/OBBRSS.h"
#include <iostream>
/** \brief Main namespace */
......@@ -69,6 +70,9 @@ 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<typename BV>
class BVFitterBase
......@@ -304,6 +308,51 @@ private:
};
/** \brief Specification of BVFitter for OBBRSS bounding volume */
template<>
class BVFitter<OBBRSS> : public BVFitterBase<OBBRSS>
{
public:
/** \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 */
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 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 Clear the geometry primitive data */
void clear()
{
vertices = NULL;
prev_vertices = NULL;
tri_indices = NULL;
type = BVH_MODEL_UNKNOWN;
}
private:
Vec3f* vertices;
Vec3f* prev_vertices;
Triangle* tri_indices;
BVHModelType type;
};
}
#endif
......@@ -44,6 +44,7 @@
#include "fcl/OBB.h"
#include "fcl/RSS.h"
#include "fcl/kIOS.h"
#include "fcl/OBBRSS.h"
#include <vector>
#include <iostream>
......@@ -450,6 +451,76 @@ private:
SplitMethodType split_method;
};
template<>
class BVSplitter<OBBRSS> : public BVSplitterBase<OBBRSS>
{
public:
BVSplitter(SplitMethodType method)
{
split_method = method;
}
/** \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 OBBRSS& 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 split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value;
}
/** \brief Clear the geometry data set before */
void clear()
{
vertices = NULL;
tri_indices = NULL;
type = BVH_MODEL_UNKNOWN;
}
private:
/** \brief Split the node from center */
void computeRule_bvcenter(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives);
/** \brief Split the node according to the mean of the data contained */
void computeRule_mean(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives);
/** \brief Split the node according to the median of the data contained */
void computeRule_median(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives);
BVH_REAL split_value;
Vec3f split_vector;
Vec3f* vertices;
Triangle* tri_indices;
BVHModelType type;
SplitMethodType split_method;
};
}
......
......@@ -72,7 +72,7 @@ public:
}
/** \brief Check whether the OBB contains a point */
inline bool contain(const Vec3f& p) const;
bool contain(const Vec3f& p) const;
/** \brief A simple way to merge the OBB and a point, not compact. */
OBB& operator += (const Vec3f& p);
......
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* 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 Willow Garage, Inc. 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 FCL_OBBRSS_H
#define FCL_OBBRSS_H
#include "fcl/BVH_internal.h"
#include "fcl/vec_3f.h"
#include "fcl/matrix_3f.h"
namespace fcl
{
class OBBRSS
{
public:
OBB obb;
RSS rss;
OBBRSS() {}
bool overlap(const OBBRSS& other) const
{
return obb.overlap(other.obb);
}
bool overlap(const OBBRSS& other, OBBRSS& overlap_part) const
{
return overlap(other);
}
inline bool contain(const Vec3f& p) const
{
return obb.contain(p);
}
OBBRSS& operator += (const Vec3f& p)
{
obb += p;
rss += p;
return *this;
}
OBBRSS& operator += (const OBBRSS& other)
{
*this = *this + other;
return *this;
}
OBBRSS operator + (const OBBRSS& other) const
{
OBBRSS result;
result.obb = obb + other.obb;
result.rss = rss + other.rss;
return result;
}
inline BVH_REAL width() const
{
return obb.width();
}
inline BVH_REAL height() const
{
return obb.height();
}
inline BVH_REAL depth() const
{
return obb.depth();
}
inline BVH_REAL volume() const
{
return obb.volume();
}
inline BVH_REAL size() const
{
return obb.size();
}
inline const Vec3f& center() const
{
return obb.center();
}
BVH_REAL distance(const OBBRSS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const
{
return rss.distance(other.rss, P, Q);
}
};
static bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2)
{
return overlap(R0, T0, b1.obb, b2.obb);
}
static BVH_REAL distance(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2, Vec3f* P = NULL, Vec3f* Q = NULL)
{
return distance(R0, T0, b1.rss, b2.rss, P, Q);
}
}
#endif
......@@ -59,6 +59,8 @@ void distance(MeshDistanceTraversalNodeRSS* node, BVHFrontList* front_list = NUL
void distance(MeshDistanceTraversalNodekIOS* node, BVHFrontList* front_list = NULL, int qsize = 2);
void distance(MeshDistanceTraversalNodeOBBRSS* node, BVHFrontList* front_list = NULL, int qsize = 2);
}
#endif
......@@ -47,7 +47,7 @@ namespace fcl
enum OBJECT_TYPE {OT_UNKNOWN, OT_BVH, OT_GEOM};
enum NODE_TYPE {BV_UNKNOWN, BV_AABB, BV_OBB, BV_RSS, BV_kIOS, BV_KDOP16, BV_KDOP18, BV_KDOP24,
enum NODE_TYPE {BV_UNKNOWN, BV_AABB, BV_OBB, BV_RSS, BV_kIOS, BV_OBBRSS, BV_KDOP16, BV_KDOP18, BV_KDOP24,
GEOM_BOX, GEOM_SPHERE, GEOM_CAPSULE, GEOM_CONE, GEOM_CYLINDER, GEOM_CONVEX, GEOM_PLANE};
class CollisionGeometry
......
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* 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 Willow Garage, Inc. 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 FCL_INPUT_REPRESENT
#define FCL_INPUT_REPRESENT
#include "fcl/primitive.h"
#include "fcl/vec_3f.h"
namespace fcl
{
struct PointFunctor
{
virtual void operator() (const Vec3f& v);
};
struct VarianceStatistics : public PointFunctor
{
VarianceStatistics()
{
for(unsigned int i = 0; i < 3; ++i)
S[i] = Vec3f();
n = 0;
}
void operator() (const Vec3f& v)
{
m += v;
S[0][0] += v[0] * v[0];
S[0][1] += v[0] * v[1];
S[0][2] += v[0] * v[2];
S[1][1] += v[1] * v[1];
S[1][2] += v[1] * v[2];
S[2][2] += v[2] * v[2];
n++;
}
void reduce(Vec3f& mean, Matrix3f& var)
{
mean = m;
var[0][0] = S[0][0] - m[0] * m[0] / n;
var[1][1] = S[1][1] - m[1] * m[1] / n;
var[2][2] = S[2][2] - m[2] * m[2] / n;
var[0][1] = S[0][1] - m[0] * m[1] / n;
var[0][2] = S[0][2] - m[0] * m[2] / n;
var[1][2] = S[1][2] - m[1] * m[2] / n;
}
Vec3f m;
Vec3f S[3];
unsigned int n;
};
class Mesh
{
public:
typedef PrimitiveType Triangle;
Vec3f* vertices;
Vec3f* prev_vertices;
Triangle* triangles;
unsigned int n_vertices;
unsigned int n_tris;
Mesh(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* triangles_)
{
vertices = vertices_;
prev_vertices = prev_vertices_;
triangles = triangles_;
}
void operate(PointFunctor& func)
{
for(unsigned int i = 0; i < n_tris; ++i)
{
const Triangle& t = *(triangles + i);
for(unsigned int j = 0; j < 3; ++j)
{
unsigned int point_id = t[j];
func(vertices[point_id]);
if(prev_vertices) func(prev_vertices[point_id]);
}
}
}
void operate(PointFunctor& func, unsigned int* indices, unsigned int n)
{
for(unsigned int i = 0; i < n; ++i)
{
const Triangle& t = *(triangles + indices[i]);
for(unsigned int j = 0; j < 3; ++j)
{
unsigned int point_id = t[j];
func(vertices[point_id]);
if(prev_vertices) func(prev_vertices[point_id]);
}
}
}
};
class PointCloud
{
public:
typedef PrimitiveType Vec3f;
Vec3f* vertices;
Vec3f* prev_vertices;
unsigned int n_vertices;
PointCloud(Vec3f* vertices_, Vec3f* prev_vertices_)
{
vertices = vertices_;
prev_vertices = prev_vertices_;
}
void operate(PointFunctor& func)
{
for(unsigned int i = 0; i < n_vertices; ++i)
{
func(vertices[i]);
if(prev_vertices) func(prev_vertices[i]);
}
}
void operate(PointFunctor& func, unsigned int* indices, unsigned int n)
{
for(unsigned int i = 0; i < n; ++i)
{
unsigned int point_id = indices[i];
func(vertices[point_id]);
if(prev_vertices) func(prev_vertices[point_id]);
}
}
};
}
#endif
......@@ -40,6 +40,7 @@
#include "fcl/BVH_internal.h"
#include "fcl/vec_3f.h"
#include "fcl/matrix_3f.h"
#include "fcl/OBB.h"
/** \brief Main namespace */
namespace fcl
......@@ -71,20 +72,22 @@ class kIOS
float dist = sqrt(dist2);
kIOS_Sphere s;
s.r = dist + s0.r + s1.r;
s.o = s0.o;
if(dist > 0)
s.o += d * ((s.r - s0.r) / dist);
s.o = s0.o + d * ((s.r - s0.r) / dist);
else
s.o = s0.o;
return s;
}
}
public:
/** \brief The (at most) three spheres for intersection */
/** \brief The (at most) five spheres for intersection */
kIOS_Sphere spheres[5];
unsigned int num_spheres; // num_spheres <= 5
OBB obb_bv;
kIOS() {}