Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • gsaurel/hpp-fcl
  • coal-library/coal
2 results
Show changes
Hide whitespace changes
Inline Side-by-side
Showing
with 0 additions and 6667 deletions
include/fcl/BV/BV.h deleted 100644 → 0
View file @ 12194e71
/*
* 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_BV_H
#define FCL_BV_H
#include "fcl/BV/kDOP.h"
#include "fcl/BV/AABB.h"
#include "fcl/BV/OBB.h"
#include "fcl/BV/RSS.h"
#include "fcl/BV/OBBRSS.h"
#include "fcl/BV/kIOS.h"
#include "fcl/math/transform.h"
/** \brief Main namespace */
namespace fcl
{
/// @cond IGNORE
namespace details
{
/// @brief Convert a bounding volume of type BV1 in configuration tf1 to a bounding volume of type BV2 in I configuration.
template<typename BV1, typename BV2>
class Converter
{
private:
static void convert(const BV1& bv1, const Transform3f& tf1, BV2& bv2)
{
// should only use the specialized version, so it is private.
}
};
/// @brief Convert from AABB to AABB, not very tight but is fast.
template<>
class Converter<AABB, AABB>
{
public:
static void convert(const AABB& bv1, const Transform3f& tf1, AABB& bv2)
{
const Vec3f& center = bv1.center();
FCL_REAL r = (bv1.max_ - bv1.min_).length() * 0.5;
Vec3f center2 = tf1.transform(center);
Vec3f delta(r, r, r);
bv2.min_ = center2 - delta;
bv2.max_ = center2 + delta;
}
};
template<>
class Converter<AABB, OBB>
{
public:
static void convert(const AABB& bv1, const Transform3f& tf1, OBB& bv2)
{
/*
bv2.To = tf1.transform(bv1.center());
/// Sort the AABB edges so that AABB extents are ordered.
FCL_REAL d[3] = {bv1.width(), bv1.height(), bv1.depth() };
std::size_t id[3] = {0, 1, 2};
for(std::size_t i = 1; i < 3; ++i)
{
for(std::size_t j = i; j > 0; --j)
{
if(d[j] > d[j-1])
{
{
FCL_REAL tmp = d[j];
d[j] = d[j-1];
d[j-1] = tmp;
}
{
std::size_t tmp = id[j];
id[j] = id[j-1];
id[j-1] = tmp;
}
}
}
}
Vec3f extent = (bv1.max_ - bv1.min_) * 0.5;
bv2.extent = Vec3f(extent[id[0]], extent[id[1]], extent[id[2]]);
const Matrix3f& R = tf1.getRotation();
bool left_hand = (id[0] == (id[1] + 1) % 3);
bv2.axis[0] = left_hand ? -R.getColumn(id[0]) : R.getColumn(id[0]);
bv2.axis[1] = R.getColumn(id[1]);
bv2.axis[2] = R.getColumn(id[2]);
*/
bv2.To = tf1.transform(bv1.center());
bv2.extent = (bv1.max_ - bv1.min_) * 0.5;
const Matrix3f& R = tf1.getRotation();
bv2.axis[0] = R.getColumn(0);
bv2.axis[1] = R.getColumn(1);
bv2.axis[2] = R.getColumn(2);
}
};
template<>
class Converter<OBB, OBB>
{
public:
static void convert(const OBB& bv1, const Transform3f& tf1, OBB& bv2)
{
bv2.extent = bv1.extent;
bv2.To = tf1.transform(bv1.To);
bv2.axis[0] = tf1.getQuatRotation().transform(bv1.axis[0]);
bv2.axis[1] = tf1.getQuatRotation().transform(bv1.axis[1]);
bv2.axis[2] = tf1.getQuatRotation().transform(bv1.axis[2]);
}
};
template<>
class Converter<OBBRSS, OBB>
{
public:
static void convert(const OBBRSS& bv1, const Transform3f& tf1, OBB& bv2)
{
Converter<OBB, OBB>::convert(bv1.obb, tf1, bv2);
}
};
template<>
class Converter<RSS, OBB>
{
public:
static void convert(const RSS& bv1, const Transform3f& tf1, OBB& bv2)
{
bv2.extent = Vec3f(bv1.l[0] * 0.5 + bv1.r, bv1.l[1] * 0.5 + bv1.r, bv1.r);
bv2.To = tf1.transform(bv1.Tr);
bv2.axis[0] = tf1.getQuatRotation().transform(bv1.axis[0]);
bv2.axis[1] = tf1.getQuatRotation().transform(bv1.axis[1]);
bv2.axis[2] = tf1.getQuatRotation().transform(bv1.axis[2]);
}
};
template<typename BV1>
class Converter<BV1, AABB>
{
public:
static void convert(const BV1& bv1, const Transform3f& tf1, AABB& bv2)
{
const Vec3f& center = bv1.center();
FCL_REAL r = Vec3f(bv1.width(), bv1.height(), bv1.depth()).length() * 0.5;
Vec3f delta(r, r, r);
Vec3f center2 = tf1.transform(center);
bv2.min_ = center2 - delta;
bv2.max_ = center2 + delta;
}
};
template<typename BV1>
class Converter<BV1, OBB>
{
public:
static void convert(const BV1& bv1, const Transform3f& tf1, OBB& bv2)
{
AABB bv;
Converter<BV1, AABB>::convert(bv1, Transform3f(), bv);
Converter<AABB, OBB>::convert(bv, tf1, bv2);
}
};
template<>
class Converter<OBB, RSS>
{
public:
static void convert(const OBB& bv1, const Transform3f& tf1, RSS& bv2)
{
bv2.Tr = tf1.transform(bv1.To);
bv2.axis[0] = tf1.getQuatRotation().transform(bv1.axis[0]);
bv2.axis[1] = tf1.getQuatRotation().transform(bv1.axis[1]);
bv2.axis[2] = tf1.getQuatRotation().transform(bv1.axis[2]);
bv2.r = bv1.extent[2];
bv2.l[0] = 2 * (bv1.extent[0] - bv2.r);
bv2.l[1] = 2 * (bv1.extent[1] - bv2.r);
}
};
template<>
class Converter<RSS, RSS>
{
public:
static void convert(const RSS& bv1, const Transform3f& tf1, RSS& bv2)
{
bv2.Tr = tf1.transform(bv1.Tr);
bv2.axis[0] = tf1.getQuatRotation().transform(bv1.axis[0]);
bv2.axis[1] = tf1.getQuatRotation().transform(bv1.axis[1]);
bv2.axis[2] = tf1.getQuatRotation().transform(bv1.axis[2]);
bv2.r = bv1.r;
bv2.l[0] = bv1.l[0];
bv2.l[1] = bv1.l[1];
}
};
template<>
class Converter<OBBRSS, RSS>
{
public:
static void convert(const OBBRSS& bv1, const Transform3f& tf1, RSS& bv2)
{
Converter<RSS, RSS>::convert(bv1.rss, tf1, bv2);
}
};
template<>
class Converter<AABB, RSS>
{
public:
static void convert(const AABB& bv1, const Transform3f& tf1, RSS& bv2)
{
bv2.Tr = tf1.transform(bv1.center());
/// Sort the AABB edges so that AABB extents are ordered.
FCL_REAL d[3] = {bv1.width(), bv1.height(), bv1.depth() };
std::size_t id[3] = {0, 1, 2};
for(std::size_t i = 1; i < 3; ++i)
{
for(std::size_t j = i; j > 0; --j)
{
if(d[j] > d[j-1])
{
{
FCL_REAL tmp = d[j];
d[j] = d[j-1];
d[j-1] = tmp;
}
{
std::size_t tmp = id[j];
id[j] = id[j-1];
id[j-1] = tmp;
}
}
}
}
Vec3f extent = (bv1.max_ - bv1.min_) * 0.5;
bv2.r = extent[id[2]];
bv2.l[0] = (extent[id[0]] - bv2.r) * 2;
bv2.l[1] = (extent[id[1]] - bv2.r) * 2;
const Matrix3f& R = tf1.getRotation();
bool left_hand = (id[0] == (id[1] + 1) % 3);
bv2.axis[0] = left_hand ? -R.getColumn(id[0]) : R.getColumn(id[0]);
bv2.axis[1] = R.getColumn(id[1]);
bv2.axis[2] = R.getColumn(id[2]);
}
};
}
/// @endcond
/// @brief Convert a bounding volume of type BV1 in configuration tf1 to bounding volume of type BV2 in identity configuration.
template<typename BV1, typename BV2>
static inline void convertBV(const BV1& bv1, const Transform3f& tf1, BV2& bv2)
{
details::Converter<BV1, BV2>::convert(bv1, tf1, bv2);
}
}
#endif
include/fcl/BVH/BVH_model.h deleted 100644 → 0
View file @ 12194e71
/*
* 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_BVH_MODEL_H
#define FCL_BVH_MODEL_H
#include "fcl/collision_object.h"
#include "fcl/BVH/BVH_internal.h"
#include "fcl/BV/BV_node.h"
#include "fcl/BVH/BV_splitter.h"
#include "fcl/BVH/BV_fitter.h"
#include <vector>
#include <boost/shared_ptr.hpp>
#include <boost/noncopyable.hpp>
namespace fcl
{
/// @brief A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as a degraded version of mesh)
template<typename BV>
class BVHModel : public CollisionGeometry,
private boost::noncopyable
{
public:
/// @brief Model type described by the instance
BVHModelType getModelType() const
{
if(num_tris && num_vertices)
return BVH_MODEL_TRIANGLES;
else if(num_vertices)
return BVH_MODEL_POINTCLOUD;
else
return BVH_MODEL_UNKNOWN;
}
/// @brief Constructing an empty BVH
BVHModel() : vertices(NULL),
tri_indices(NULL),
prev_vertices(NULL),
num_tris(0),
num_vertices(0),
build_state(BVH_BUILD_STATE_EMPTY),
bv_splitter(new BVSplitter<BV>(SPLIT_METHOD_MEAN)),
bv_fitter(new BVFitter<BV>()),
num_tris_allocated(0),
num_vertices_allocated(0),
num_bvs_allocated(0),
num_vertex_updated(0),
primitive_indices(NULL),
bvs(NULL),
num_bvs(0)
{
}
/// @brief copy from another BVH
BVHModel(const BVHModel& other);
/// @brief deconstruction, delete mesh data related.
~BVHModel()
{
delete [] vertices;
delete [] tri_indices;
delete [] bvs;
delete [] prev_vertices;
delete [] primitive_indices;
}
/// @brief We provide getBV() and getNumBVs() because BVH may be compressed (in future), so we must provide some flexibility here
/// @brief Access the bv giving the its index
const BVNode<BV>& getBV(int id) const
{
return bvs[id];
}
/// @brief Access the bv giving the its index
BVNode<BV>& getBV(int id)
{
return bvs[id];
}
/// @brief Get the number of bv in the BVH
int getNumBVs() const
{
return num_bvs;
}
/// @brief Get the object type: it is a BVH
OBJECT_TYPE getObjectType() const { return OT_BVH; }
/// @brief Get the BV type: default is unknown
NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
/// @brief Compute the AABB for the BVH, used for broad-phase collision
void computeLocalAABB();
/// @brief Begin a new BVH model
int beginModel(int num_tris = 0, int num_vertices = 0);
/// @brief Add one point in the new BVH model
int addVertex(const Vec3f& p);
/// @brief Add one triangle in the new BVH model
int addTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3);
/// @brief Add a set of triangles in the new BVH model
int addSubModel(const std::vector<Vec3f>& ps, const std::vector<Triangle>& ts);
/// @brief Add a set of points in the new BVH model
int addSubModel(const std::vector<Vec3f>& ps);
/// @brief End BVH model construction, will build the bounding volume hierarchy
int endModel();
/// @brief Replace the geometry information of current frame (i.e. should have the same mesh topology with the previous frame)
int beginReplaceModel();
/// @brief Replace one point in the old BVH model
int replaceVertex(const Vec3f& p);
/// @brief Replace one triangle in the old BVH model
int replaceTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3);
/// @brief Replace a set of points in the old BVH model
int replaceSubModel(const std::vector<Vec3f>& ps);
/// @brief End BVH model replacement, will also refit or rebuild the bounding volume hierarchy
int endReplaceModel(bool refit = true, bool bottomup = true);
/// @brief Replace the geometry information of current frame (i.e. should have the same mesh topology with the previous frame).
/// The current frame will be saved as the previous frame in prev_vertices.
int beginUpdateModel();
/// @brief Update one point in the old BVH model
int updateVertex(const Vec3f& p);
/// @brief Update one triangle in the old BVH model
int updateTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3);
/// @brief Update a set of points in the old BVH model
int updateSubModel(const std::vector<Vec3f>& ps);
/// @brief End BVH model update, will also refit or rebuild the bounding volume hierarchy
int endUpdateModel(bool refit = true, bool bottomup = true);
/// @brief Check the number of memory used
int memUsage(int msg) const;
/// @brief This is a special acceleration: BVH_model default stores the BV's transform in world coordinate. However, we can also store each BV's transform related to its parent
/// BV node. When traversing the BVH, this can save one matrix transformation.
void makeParentRelative()
{
Vec3f I[3] = {Vec3f(1, 0, 0), Vec3f(0, 1, 0), Vec3f(0, 0, 1)};
makeParentRelativeRecurse(0, I, Vec3f());
}
Vec3f computeCOM() const
{
FCL_REAL vol = 0;
Vec3f com;
for(int i = 0; i < num_tris; ++i)
{
const Triangle& tri = tri_indices[i];
FCL_REAL d_six_vol = (vertices[tri[0]].cross(vertices[tri[1]])).dot(vertices[tri[2]]);
vol += d_six_vol;
com += (vertices[tri[0]] + vertices[tri[1]] + vertices[tri[2]]) * d_six_vol;
}
return com / (vol * 4);
}
FCL_REAL computeVolume() const
{
FCL_REAL vol = 0;
for(int i = 0; i < num_tris; ++i)
{
const Triangle& tri = tri_indices[i];
FCL_REAL d_six_vol = (vertices[tri[0]].cross(vertices[tri[1]])).dot(vertices[tri[2]]);
vol += d_six_vol;
}
return vol / 6;
}
Matrix3f computeMomentofInertia() const
{
Matrix3f C(0, 0, 0,
0, 0, 0,
0, 0, 0);
Matrix3f C_canonical(1/60.0, 1/120.0, 1/120.0,
1/120.0, 1/60.0, 1/120.0,
1/120.0, 1/120.0, 1/60.0);
for(int i = 0; i < num_tris; ++i)
{
const Triangle& tri = tri_indices[i];
const Vec3f& v1 = vertices[tri[0]];
const Vec3f& v2 = vertices[tri[1]];
const Vec3f& v3 = vertices[tri[2]];
FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3);
Matrix3f A(v1, v2, v3);
C += transpose(A) * C_canonical * A * d_six_vol;
}
FCL_REAL trace_C = C(0, 0) + C(1, 1) + C(2, 2);
return Matrix3f(trace_C - C(0, 0), -C(0, 1), -C(0, 2),
-C(1, 0), trace_C - C(1, 1), -C(1, 2),
-C(2, 0), -C(2, 1), trace_C - C(2, 2));
}
public:
/// @brief Geometry point data
Vec3f* vertices;
/// @brief Geometry triangle index data, will be NULL for point clouds
Triangle* tri_indices;
/// @brief Geometry point data in previous frame
Vec3f* prev_vertices;
/// @brief Number of triangles
int num_tris;
/// @brief Number of points
int num_vertices;
/// @brief The state of BVH building process
BVHBuildState build_state;
/// @brief Split rule to split one BV node into two children
boost::shared_ptr<BVSplitterBase<BV> > bv_splitter;
/// @brief Fitting rule to fit a BV node to a set of geometry primitives
boost::shared_ptr<BVFitterBase<BV> > bv_fitter;
private:
int num_tris_allocated;
int num_vertices_allocated;
int num_bvs_allocated;
int num_vertex_updated; /// for ccd vertex update
unsigned int* primitive_indices;
/// @brief Bounding volume hierarchy
BVNode<BV>* bvs;
/// @brief Number of BV nodes in bounding volume hierarchy
int num_bvs;
/// @brief Build the bounding volume hierarchy
int buildTree();
/// @brief Refit the bounding volume hierarchy
int refitTree(bool bottomup);
/// @brief Refit the bounding volume hierarchy in a top-down way (slow but more compact)
int refitTree_topdown();
/// @brief Refit the bounding volume hierarchy in a bottom-up way (fast but less compact)
int refitTree_bottomup();
/// @brief Recursive kernel for hierarchy construction
int recursiveBuildTree(int bv_id, int first_primitive, int num_primitives);
/// @brief Recursive kernel for bottomup refitting
int recursiveRefitTree_bottomup(int bv_id);
/// @recursively compute each bv's transform related to its parent. For default BV, only the translation works.
/// For oriented BV (OBB, RSS, OBBRSS), special implementation is provided.
void makeParentRelativeRecurse(int bv_id, Vec3f parent_axis[], const Vec3f& parent_c)
{
if(!bvs[bv_id].isLeaf())
{
makeParentRelativeRecurse(bvs[bv_id].first_child, parent_axis, bvs[bv_id].getCenter());
makeParentRelativeRecurse(bvs[bv_id].first_child + 1, parent_axis, bvs[bv_id].getCenter());
}
bvs[bv_id].bv = translate(bvs[bv_id].bv, -parent_c);
}
};
template<>
void BVHModel<OBB>::makeParentRelativeRecurse(int bv_id, Vec3f parent_axis[], const Vec3f& parent_c);
template<>
void BVHModel<RSS>::makeParentRelativeRecurse(int bv_id, Vec3f parent_axis[], const Vec3f& parent_c);
template<>
void BVHModel<OBBRSS>::makeParentRelativeRecurse(int bv_id, Vec3f parent_axis[], const Vec3f& parent_c);
/// @brief Specialization of getNodeType() for BVHModel with different BV types
template<>
NODE_TYPE BVHModel<AABB>::getNodeType() const;
template<>
NODE_TYPE BVHModel<OBB>::getNodeType() const;
template<>
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;
template<>
NODE_TYPE BVHModel<KDOP<18> >::getNodeType() const;
template<>
NODE_TYPE BVHModel<KDOP<24> >::getNodeType() const;
}
#endif
include/fcl/CMakeLists.txt deleted 100644 → 0
View file @ 12194e71
file(TO_NATIVE_PATH "${CMAKE_CURRENT_SOURCE_DIR}" VERSION_DIR)
configure_file("${VERSION_DIR}/config.h.in" "${VERSION_DIR}/config.h")
include/fcl/articulated_model/joint.h deleted 100644 → 0
View file @ 12194e71
/*
* 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 Dalibor Matura, Jia Pan */
#ifndef FCL_ARTICULATED_MODEL_JOINT_H
#define FCL_ARTICULATED_MODEL_JOINT_H
#include "fcl/math/transform.h"
#include "fcl/data_types.h"
#include <string>
#include <vector>
#include <map>
#include <limits>
#include <boost/shared_ptr.hpp>
#include <boost/weak_ptr.hpp>
namespace fcl
{
class JointConfig;
class Link;
enum JointType {JT_UNKNOWN, JT_PRISMATIC, JT_REVOLUTE, JT_BALLEULER};
/// @brief Base Joint
class Joint
{
public:
Joint(const boost::shared_ptr<Link>& link_parent, const boost::shared_ptr<Link>& link_child,
const Transform3f& transform_to_parent,
const std::string& name);
Joint(const std::string& name);
virtual ~Joint() {}
const std::string& getName() const;
void setName(const std::string& name);
virtual Transform3f getLocalTransform() const = 0;
virtual std::size_t getNumDofs() const = 0;
boost::shared_ptr<JointConfig> getJointConfig() const;
void setJointConfig(const boost::shared_ptr<JointConfig>& joint_cfg);
boost::shared_ptr<Link> getParentLink() const;
boost::shared_ptr<Link> getChildLink() const;
void setParentLink(const boost::shared_ptr<Link>& link);
void setChildLink(const boost::shared_ptr<Link>& link);
JointType getJointType() const;
const Transform3f& getTransformToParent() const;
void setTransformToParent(const Transform3f& t);
protected:
/// links to parent and child are only for connection, so weak_ptr to avoid cyclic dependency
boost::weak_ptr<Link> link_parent_, link_child_;
JointType type_;
std::string name_;
boost::shared_ptr<JointConfig> joint_cfg_;
Transform3f transform_to_parent_;
};
class PrismaticJoint : public Joint
{
public:
PrismaticJoint(const boost::shared_ptr<Link>& link_parent, const boost::shared_ptr<Link>& link_child,
const Transform3f& transform_to_parent,
const std::string& name,
const Vec3f& axis);
virtual ~PrismaticJoint() {}
Transform3f getLocalTransform() const;
std::size_t getNumDofs() const;
const Vec3f& getAxis() const;
protected:
Vec3f axis_;
};
class RevoluteJoint : public Joint
{
public:
RevoluteJoint(const boost::shared_ptr<Link>& link_parent, const boost::shared_ptr<Link>& link_child,
const Transform3f& transform_to_parent,
const std::string& name,
const Vec3f& axis);
virtual ~RevoluteJoint() {}
Transform3f getLocalTransform() const;
std::size_t getNumDofs() const;
const Vec3f& getAxis() const;
protected:
Vec3f axis_;
};
class BallEulerJoint : public Joint
{
public:
BallEulerJoint(const boost::shared_ptr<Link>& link_parent, const boost::shared_ptr<Link>& link_child,
const Transform3f& transform_to_parent,
const std::string& name);
virtual ~BallEulerJoint() {}
std::size_t getNumDofs() const;
Transform3f getLocalTransform() const;
};
}
#endif
include/fcl/broadphase/broadphase.h deleted 100644 → 0
View file @ 12194e71
/*
* 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_BROAD_PHASE_H
#define FCL_BROAD_PHASE_H
#include "fcl/collision_object.h"
#include <set>
#include <vector>
namespace fcl
{
/// @brief Callback for collision between two objects. Return value is whether can stop now.
typedef bool (*CollisionCallBack)(CollisionObject* o1, CollisionObject* o2, void* cdata);
/// @brief Callback for distance between two objects, Return value is whether can stop now, also return the minimum distance till now.
typedef bool (*DistanceCallBack)(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& dist);
/// @brief Base class for broad phase collision. It helps to accelerate the collision/distance between N objects. Also support self collision, self distance and collision/distance with another M objects.
class BroadPhaseCollisionManager
{
public:
BroadPhaseCollisionManager() : enable_tested_set_(false)
{
}
virtual ~BroadPhaseCollisionManager() {}
/// @brief add objects to the manager
virtual void registerObjects(const std::vector<CollisionObject*>& other_objs)
{
for(size_t i = 0; i < other_objs.size(); ++i)
registerObject(other_objs[i]);
}
/// @brief add one object to the manager
virtual void registerObject(CollisionObject* obj) = 0;
/// @brief remove one object from the manager
virtual void unregisterObject(CollisionObject* obj) = 0;
/// @brief initialize the manager, related with the specific type of manager
virtual void setup() = 0;
/// @brief update the condition of manager
virtual void update() = 0;
/// @brief update the manager by explicitly given the object updated
virtual void update(CollisionObject* updated_obj)
{
update();
}
/// @brief update the manager by explicitly given the set of objects update
virtual void update(const std::vector<CollisionObject*>& updated_objs)
{
update();
}
/// @brief clear the manager
virtual void clear() = 0;
/// @brief return the objects managed by the manager
virtual void getObjects(std::vector<CollisionObject*>& objs) const = 0;
/// @brief perform collision test between one object and all the objects belonging to the manager
virtual void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const = 0;
/// @brief perform distance computation between one object and all the objects belonging to the manager
virtual void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const = 0;
/// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision)
virtual void collide(void* cdata, CollisionCallBack callback) const = 0;
/// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance)
virtual void distance(void* cdata, DistanceCallBack callback) const = 0;
/// @brief perform collision test with objects belonging to another manager
virtual void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const = 0;
/// @brief perform distance test with objects belonging to another manager
virtual void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const = 0;
/// @brief whether the manager is empty
virtual bool empty() const = 0;
/// @brief the number of objects managed by the manager
virtual size_t size() const = 0;
protected:
/// @brief tools help to avoid repeating collision or distance callback for the pairs of objects tested before. It can be useful for some of the broadphase algorithms.
mutable std::set<std::pair<CollisionObject*, CollisionObject*> > tested_set;
mutable bool enable_tested_set_;
bool inTestedSet(CollisionObject* a, CollisionObject* b) const
{
if(a < b) return tested_set.find(std::make_pair(a, b)) != tested_set.end();
else return tested_set.find(std::make_pair(b, a)) != tested_set.end();
}
void insertTestedSet(CollisionObject* a, CollisionObject* b) const
{
if(a < b) tested_set.insert(std::make_pair(a, b));
else tested_set.insert(std::make_pair(b, a));
}
};
/// @brief Callback for continuous collision between two objects. Return value is whether can stop now.
typedef bool (*ContinuousCollisionCallBack)(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, void* cdata);
/// @brief Callback for continuous distance between two objects, Return value is whether can stop now, also return the minimum distance till now.
typedef bool (*ContinuousDistanceCallBack)(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, void* cdata, FCL_REAL& dist);
/// @brief Base class for broad phase continuous collision. It helps to accelerate the continuous collision/distance between N objects. Also support self collision, self distance and collision/distance with another M objects.
class BroadPhaseContinuousCollisionManager
{
public:
BroadPhaseContinuousCollisionManager()
{
}
virtual ~BroadPhaseContinuousCollisionManager() {}
/// @brief add objects to the manager
virtual void registerObjects(const std::vector<ContinuousCollisionObject*>& other_objs)
{
for(size_t i = 0; i < other_objs.size(); ++i)
registerObject(other_objs[i]);
}
/// @brief add one object to the manager
virtual void registerObject(ContinuousCollisionObject* obj) = 0;
/// @brief remove one object from the manager
virtual void unregisterObject(ContinuousCollisionObject* obj) = 0;
/// @brief initialize the manager, related with the specific type of manager
virtual void setup() = 0;
/// @brief update the condition of manager
virtual void update() = 0;
/// @brief update the manager by explicitly given the object updated
virtual void update(ContinuousCollisionObject* updated_obj)
{
update();
}
/// @brief update the manager by explicitly given the set of objects update
virtual void update(const std::vector<ContinuousCollisionObject*>& updated_objs)
{
update();
}
/// @brief clear the manager
virtual void clear() = 0;
/// @brief return the objects managed by the manager
virtual void getObjects(std::vector<ContinuousCollisionObject*>& objs) const = 0;
/// @brief perform collision test between one object and all the objects belonging to the manager
virtual void collide(ContinuousCollisionObject* obj, void* cdata, CollisionCallBack callback) const = 0;
/// @brief perform distance computation between one object and all the objects belonging to the manager
virtual void distance(ContinuousCollisionObject* obj, void* cdata, DistanceCallBack callback) const = 0;
/// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision)
virtual void collide(void* cdata, CollisionCallBack callback) const = 0;
/// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance)
virtual void distance(void* cdata, DistanceCallBack callback) const = 0;
/// @brief perform collision test with objects belonging to another manager
virtual void collide(BroadPhaseContinuousCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const = 0;
/// @brief perform distance test with objects belonging to another manager
virtual void distance(BroadPhaseContinuousCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const = 0;
/// @brief whether the manager is empty
virtual bool empty() const = 0;
/// @brief the number of objects managed by the manager
virtual size_t size() const = 0;
};
}
#include "fcl/broadphase/broadphase_bruteforce.h"
#include "fcl/broadphase/broadphase_spatialhash.h"
#include "fcl/broadphase/broadphase_SaP.h"
#include "fcl/broadphase/broadphase_SSaP.h"
#include "fcl/broadphase/broadphase_interval_tree.h"
#include "fcl/broadphase/broadphase_dynamic_AABB_tree.h"
#include "fcl/broadphase/broadphase_dynamic_AABB_tree_array.h"
#endif
include/fcl/broadphase/broadphase_spatialhash.hxx deleted 100644 → 0
View file @ 12194e71
/*
* 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 */
namespace fcl
{
template<typename HashTable>
void SpatialHashingCollisionManager<HashTable>::registerObject(CollisionObject* obj)
{
objs.push_back(obj);
const AABB& obj_aabb = obj->getAABB();
AABB overlap_aabb;
if(scene_limit.overlap(obj_aabb, overlap_aabb))
{
if(!scene_limit.contain(obj_aabb))
objs_outside_scene_limit.push_back(obj);
hash_table->insert(overlap_aabb, obj);
}
else
objs_outside_scene_limit.push_back(obj);
obj_aabb_map[obj] = obj_aabb;
}
template<typename HashTable>
void SpatialHashingCollisionManager<HashTable>::unregisterObject(CollisionObject* obj)
{
objs.remove(obj);
const AABB& obj_aabb = obj->getAABB();
AABB overlap_aabb;
if(scene_limit.overlap(obj_aabb, overlap_aabb))
{
if(!scene_limit.contain(obj_aabb))
objs_outside_scene_limit.remove(obj);
hash_table->remove(overlap_aabb, obj);
}
else
objs_outside_scene_limit.remove(obj);
obj_aabb_map.erase(obj);
}
template<typename HashTable>
void SpatialHashingCollisionManager<HashTable>::setup()
{}
template<typename HashTable>
void SpatialHashingCollisionManager<HashTable>::update()
{
hash_table->clear();
objs_outside_scene_limit.clear();
for(std::list<CollisionObject*>::const_iterator it = objs.begin(), end = objs.end();
it != end; ++it)
{
CollisionObject* obj = *it;
const AABB& obj_aabb = obj->getAABB();
AABB overlap_aabb;
if(scene_limit.overlap(obj_aabb, overlap_aabb))
{
if(!scene_limit.contain(obj_aabb))
objs_outside_scene_limit.push_back(obj);
hash_table->insert(overlap_aabb, obj);
}
else
objs_outside_scene_limit.push_back(obj);
obj_aabb_map[obj] = obj_aabb;
}
}
template<typename HashTable>
void SpatialHashingCollisionManager<HashTable>::update(CollisionObject* updated_obj)
{
const AABB& new_aabb = updated_obj->getAABB();
const AABB& old_aabb = obj_aabb_map[updated_obj];
if(!scene_limit.contain(old_aabb)) // previously not completely in scene limit
{
if(scene_limit.contain(new_aabb))
{
std::list<CollisionObject*>::iterator find_it = std::find(objs_outside_scene_limit.begin(),
objs_outside_scene_limit.end(),
updated_obj);
objs_outside_scene_limit.erase(find_it);
}
}
else if(!scene_limit.contain(new_aabb)) // previous completely in scenelimit, now not
objs_outside_scene_limit.push_back(updated_obj);
AABB old_overlap_aabb;
if(scene_limit.overlap(old_aabb, old_overlap_aabb))
hash_table->remove(old_overlap_aabb, updated_obj);
AABB new_overlap_aabb;
if(scene_limit.overlap(new_aabb, new_overlap_aabb))
hash_table->insert(new_overlap_aabb, updated_obj);
obj_aabb_map[updated_obj] = new_aabb;
}
template<typename HashTable>
void SpatialHashingCollisionManager<HashTable>::update(const std::vector<CollisionObject*>& updated_objs)
{
for(size_t i = 0; i < updated_objs.size(); ++i)
update(updated_objs[i]);
}
template<typename HashTable>
void SpatialHashingCollisionManager<HashTable>::clear()
{
objs.clear();
hash_table->clear();
objs_outside_scene_limit.clear();
obj_aabb_map.clear();
}
template<typename HashTable>
void SpatialHashingCollisionManager<HashTable>::getObjects(std::vector<CollisionObject*>& objs_) const
{
objs_.resize(objs.size());
std::copy(objs.begin(), objs.end(), objs_.begin());
}
template<typename HashTable>
void SpatialHashingCollisionManager<HashTable>::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
{
if(size() == 0) return;
collide_(obj, cdata, callback);
}
template<typename HashTable>
void SpatialHashingCollisionManager<HashTable>::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
{
if(size() == 0) return;
FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
distance_(obj, cdata, callback, min_dist);
}
template<typename HashTable>
bool SpatialHashingCollisionManager<HashTable>::collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
{
const AABB& obj_aabb = obj->getAABB();
AABB overlap_aabb;
if(scene_limit.overlap(obj_aabb, overlap_aabb))
{
if(!scene_limit.contain(obj_aabb))
{
for(std::list<CollisionObject*>::const_iterator it = objs_outside_scene_limit.begin(), end = objs_outside_scene_limit.end();
it != end; ++it)
{
if(obj == *it) continue;
if(callback(obj, *it, cdata)) return true;
}
}
std::vector<CollisionObject*> query_result = hash_table->query(overlap_aabb);
for(unsigned int i = 0; i < query_result.size(); ++i)
{
if(obj == query_result[i]) continue;
if(callback(obj, query_result[i], cdata)) return true;
}
}
else
{
;
for(std::list<CollisionObject*>::const_iterator it = objs_outside_scene_limit.begin(), end = objs_outside_scene_limit.end();
it != end; ++it)
{
if(obj == *it) continue;
if(callback(obj, *it, cdata)) return true;
}
}
return false;
}
template<typename HashTable>
bool SpatialHashingCollisionManager<HashTable>::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
{
Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
AABB aabb = obj->getAABB();
if(min_dist < std::numeric_limits<FCL_REAL>::max())
{
Vec3f min_dist_delta(min_dist, min_dist, min_dist);
aabb.expand(min_dist_delta);
}
AABB overlap_aabb;
int status = 1;
FCL_REAL old_min_distance;
while(1)
{
old_min_distance = min_dist;
if(scene_limit.overlap(aabb, overlap_aabb))
{
if(!scene_limit.contain(aabb))
{
for(std::list<CollisionObject*>::const_iterator it = objs_outside_scene_limit.begin(), end = objs_outside_scene_limit.end();
it != end; ++it)
{
if(obj == *it) continue;
if(!enable_tested_set_)
{
if(obj->getAABB().distance((*it)->getAABB()) < min_dist)
if(callback(obj, *it, cdata, min_dist)) return true;
}
else
{
if(!inTestedSet(obj, *it))
{
if(obj->getAABB().distance((*it)->getAABB()) < min_dist)
if(callback(obj, *it, cdata, min_dist)) return true;
insertTestedSet(obj, *it);
}
}
}
}
std::vector<CollisionObject*> query_result = hash_table->query(overlap_aabb);
for(unsigned int i = 0; i < query_result.size(); ++i)
{
if(obj == query_result[i]) continue;
if(!enable_tested_set_)
{
if(obj->getAABB().distance(query_result[i]->getAABB()) < min_dist)
if(callback(obj, query_result[i], cdata, min_dist)) return true;
}
else
{
if(!inTestedSet(obj, query_result[i]))
{
if(obj->getAABB().distance(query_result[i]->getAABB()) < min_dist)
if(callback(obj, query_result[i], cdata, min_dist)) return true;
insertTestedSet(obj, query_result[i]);
}
}
}
}
else
{
for(std::list<CollisionObject*>::const_iterator it = objs_outside_scene_limit.begin(), end = objs_outside_scene_limit.end();
it != end; ++it)
{
if(obj == *it) continue;
if(!enable_tested_set_)
{
if(obj->getAABB().distance((*it)->getAABB()) < min_dist)
if(callback(obj, *it, cdata, min_dist)) return true;
}
else
{
if(!inTestedSet(obj, *it))
{
if(obj->getAABB().distance((*it)->getAABB()) < min_dist)
if(callback(obj, *it, cdata, min_dist)) return true;
insertTestedSet(obj, *it);
}
}
}
}
if(status == 1)
{
if(old_min_distance < std::numeric_limits<FCL_REAL>::max())
break;
else
{
if(min_dist < old_min_distance)
{
Vec3f min_dist_delta(min_dist, min_dist, min_dist);
aabb = AABB(obj->getAABB(), min_dist_delta);
status = 0;
}
else
{
if(aabb.equal(obj->getAABB()))
aabb.expand(delta);
else
aabb.expand(obj->getAABB(), 2.0);
}
}
}
else if(status == 0)
break;
}
return false;
}
template<typename HashTable>
void SpatialHashingCollisionManager<HashTable>::collide(void* cdata, CollisionCallBack callback) const
{
if(size() == 0) return;
for(std::list<CollisionObject*>::const_iterator it1 = objs.begin(), end1 = objs.end();
it1 != end1; ++it1)
{
const AABB& obj_aabb = (*it1)->getAABB();
AABB overlap_aabb;
if(scene_limit.overlap(obj_aabb, overlap_aabb))
{
if(!scene_limit.contain(obj_aabb))
{
for(std::list<CollisionObject*>::const_iterator it2 = objs_outside_scene_limit.begin(), end2 = objs_outside_scene_limit.end();
it2 != end2; ++it2)
{
if(*it1 < *it2) { if(callback(*it1, *it2, cdata)) return; }
}
}
std::vector<CollisionObject*> query_result = hash_table->query(overlap_aabb);
for(unsigned int i = 0; i < query_result.size(); ++i)
{
if(*it1 < query_result[i]) { if(callback(*it1, query_result[i], cdata)) return; }
}
}
else
{
for(std::list<CollisionObject*>::const_iterator it2 = objs_outside_scene_limit.begin(), end2 = objs_outside_scene_limit.end();
it2 != end2; ++it2)
{
if(*it1 < *it2) { if(callback(*it1, *it2, cdata)) return; }
}
}
}
}
template<typename HashTable>
void SpatialHashingCollisionManager<HashTable>::distance(void* cdata, DistanceCallBack callback) const
{
if(size() == 0) return;
enable_tested_set_ = true;
tested_set.clear();
FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
for(std::list<CollisionObject*>::const_iterator it = objs.begin(), end = objs.end(); it != end; ++it)
if(distance_(*it, cdata, callback, min_dist)) break;
enable_tested_set_ = false;
tested_set.clear();
}
template<typename HashTable>
void SpatialHashingCollisionManager<HashTable>::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const
{
SpatialHashingCollisionManager<HashTable>* other_manager = static_cast<SpatialHashingCollisionManager<HashTable>* >(other_manager_);
if((size() == 0) || (other_manager->size() == 0)) return;
if(this == other_manager)
{
collide(cdata, callback);
return;
}
if(this->size() < other_manager->size())
{
for(std::list<CollisionObject*>::const_iterator it = objs.begin(), end = objs.end(); it != end; ++it)
if(other_manager->collide_(*it, cdata, callback)) return;
}
else
{
for(std::list<CollisionObject*>::const_iterator it = other_manager->objs.begin(), end = other_manager->objs.end(); it != end; ++it)
if(collide_(*it, cdata, callback)) return;
}
}
template<typename HashTable>
void SpatialHashingCollisionManager<HashTable>::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const
{
SpatialHashingCollisionManager<HashTable>* other_manager = static_cast<SpatialHashingCollisionManager<HashTable>* >(other_manager_);
if((size() == 0) || (other_manager->size() == 0)) return;
if(this == other_manager)
{
distance(cdata, callback);
return;
}
FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
if(this->size() < other_manager->size())
{
for(std::list<CollisionObject*>::const_iterator it = objs.begin(), end = objs.end(); it != end; ++it)
if(other_manager->distance_(*it, cdata, callback, min_dist)) return;
}
else
{
for(std::list<CollisionObject*>::const_iterator it = other_manager->objs.begin(), end = other_manager->objs.end(); it != end; ++it)
if(distance_(*it, cdata, callback, min_dist)) return;
}
}
template<typename HashTable>
bool SpatialHashingCollisionManager<HashTable>::empty() const
{
return objs.empty();
}
template<typename HashTable>
size_t SpatialHashingCollisionManager<HashTable>::size() const
{
return objs.size();
}
}
include/fcl/broadphase/hash.h deleted 100644 → 0
View file @ 12194e71
/*
* 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_HASH_H
#define FCL_HASH_H
#include <stdexcept>
#include <set>
#include <vector>
#include <list>
#include <boost/unordered_map.hpp>
namespace fcl
{
/// @brief A simple hash table implemented as multiple buckets. HashFnc is any extended hash function: HashFnc(key) = {index1, index2, ..., }
template<typename Key, typename Data, typename HashFnc>
class SimpleHashTable
{
protected:
typedef std::list<Data> Bin;
std::vector<Bin> table_;
HashFnc h_;
size_t table_size_;
public:
SimpleHashTable(const HashFnc& h) : h_(h)
{
}
///@ brief Init the number of bins in the hash table
void init(size_t size)
{
if(size == 0)
{
throw std::logic_error("SimpleHashTable must have non-zero size.");
}
table_.resize(size);
table_size_ = size;
}
//// @brief Insert a key-value pair into the table
void insert(Key key, Data value)
{
std::vector<unsigned int> indices = h_(key);
size_t range = table_.size();
for(size_t i = 0; i < indices.size(); ++i)
table_[indices[i] % range].push_back(value);
}
/// @brief Find the elements in the hash table whose key is the same as query key.
std::vector<Data> query(Key key) const
{
size_t range = table_.size();
std::vector<unsigned int> indices = h_(key);
std::set<Data> result;
for(size_t i = 0; i < indices.size(); ++i)
{
unsigned int index = indices[i] % range;
std::copy(table_[index].begin(), table_[index].end(), std::inserter(result, result.end()));
}
return std::vector<Data>(result.begin(), result.end());
}
/// @brief remove the key-value pair from the table
void remove(Key key, Data value)
{
size_t range = table_.size();
std::vector<unsigned int> indices = h_(key);
for(size_t i = 0; i < indices.size(); ++i)
{
unsigned int index = indices[i] % range;
table_[index].remove(value);
}
}
/// @brief clear the hash table
void clear()
{
table_.clear();
table_.resize(table_size_);
}
};
template<typename U, typename V>
class unordered_map_hash_table : public boost::unordered_map<U, V> {};
/// @brief A hash table implemented using unordered_map
template<typename Key, typename Data, typename HashFnc, template<typename, typename> class TableT = unordered_map_hash_table>
class SparseHashTable
{
protected:
HashFnc h_;
typedef std::list<Data> Bin;
typedef TableT<size_t, Bin> Table;
Table table_;
public:
SparseHashTable(const HashFnc& h) : h_(h) {}
/// @brief Init the hash table. The bucket size is dynamically decided.
void init(size_t) { table_.clear(); }
/// @brief insert one key-value pair into the hash table
void insert(Key key, Data value)
{
std::vector<unsigned int> indices = h_(key);
for(size_t i = 0; i < indices.size(); ++i)
table_[indices[i]].push_back(value);
}
/// @brief find the elements whose key is the same as the query
std::vector<Data> query(Key key) const
{
std::vector<unsigned int> indices = h_(key);
std::set<Data> result;
for(size_t i = 0; i < indices.size(); ++i)
{
unsigned int index = indices[i];
typename Table::const_iterator p = table_.find(index);
if(p != table_.end())
std::copy((*p).second.begin(), (*p).second.end(), std::inserter(result, result.end()));
}
return std::vector<Data>(result.begin(), result.end());
}
/// @brief remove one key-value pair from the hash table
void remove(Key key, Data value)
{
std::vector<unsigned int> indices = h_(key);
for(size_t i = 0; i < indices.size(); ++i)
{
unsigned int index = indices[i];
table_[index].remove(value);
}
}
/// @brief clear the hash table
void clear()
{
table_.clear();
}
};
}
#endif
include/fcl/broadphase/hierarchy_tree.h deleted 100644 → 0
View file @ 12194e71
/*
* 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_HIERARCHY_TREE_H
#define FCL_HIERARCHY_TREE_H
#include <vector>
#include <map>
#include "fcl/BV/AABB.h"
#include "fcl/broadphase/morton.h"
#include <boost/bind.hpp>
#include <boost/iterator/zip_iterator.hpp>
namespace fcl
{
/// @brief dynamic AABB tree node
template<typename BV>
struct NodeBase
{
/// @brief the bounding volume for the node
BV bv;
/// @brief pointer to parent node
NodeBase<BV>* parent;
/// @brief whether is a leaf
bool isLeaf() const { return (children[1] == NULL); }
/// @brief whether is internal node
bool isInternal() const { return !isLeaf(); }
union
{
/// @brief for leaf node, children nodes
NodeBase<BV>* children[2];
void* data;
};
/// @brief morton code for current BV
FCL_UINT32 code;
NodeBase()
{
parent = NULL;
children[0] = NULL;
children[1] = NULL;
}
};
/// @brief Compare two nodes accoording to the d-th dimension of node center
template<typename BV>
bool nodeBaseLess(NodeBase<BV>* a, NodeBase<BV>* b, int d)
{
if(a->bv.center()[d] < b->bv.center()[d]) return true;
return false;
}
/// @brief select from node1 and node2 which is close to a given query. 0 for node1 and 1 for node2
template<typename BV>
size_t select(const NodeBase<BV>& query, const NodeBase<BV>& node1, const NodeBase<BV>& node2)
{
return 0;
}
template<>
size_t select(const NodeBase<AABB>& node, const NodeBase<AABB>& node1, const NodeBase<AABB>& node2);
/// @brief select from node1 and node2 which is close to a given query bounding volume. 0 for node1 and 1 for node2
template<typename BV>
size_t select(const BV& query, const NodeBase<BV>& node1, const NodeBase<BV>& node2)
{
return 0;
}
template<>
size_t select(const AABB& query, const NodeBase<AABB>& node1, const NodeBase<AABB>& node2);
/// @brief Class for hierarchy tree structure
template<typename BV>
class HierarchyTree
{
typedef NodeBase<BV> NodeType;
typedef typename std::vector<NodeBase<BV>* >::iterator NodeVecIterator;
typedef typename std::vector<NodeBase<BV>* >::const_iterator NodeVecConstIterator;
struct SortByMorton
{
bool operator() (const NodeType* a, const NodeType* b) const
{
return a->code < b->code;
}
};
public:
/// @brief Create hierarchy tree with suitable setting.
/// bu_threshold decides the height of tree node to start bottom-up construction / optimization;
/// topdown_level decides different methods to construct tree in topdown manner.
/// lower level method constructs tree with better quality but is slower.
HierarchyTree(int bu_threshold_ = 16, int topdown_level_ = 0);
~HierarchyTree();
/// @brief Initialize the tree by a set of leaves using algorithm with a given level.
void init(std::vector<NodeType*>& leaves, int level = 0);
/// @brief Insest a node
NodeType* insert(const BV& bv, void* data);
/// @brief Remove a leaf node
void remove(NodeType* leaf);
/// @brief Clear the tree
void clear();
/// @brief Whether the tree is empty
bool empty() const;
/// @brief update one leaf node
void update(NodeType* leaf, int lookahead_level = -1);
/// @brief update the tree when the bounding volume of a given leaf has changed
bool update(NodeType* leaf, const BV& bv);
/// @brief update one leaf's bounding volume, with prediction
bool update(NodeType* leaf, const BV& bv, const Vec3f& vel, FCL_REAL margin);
/// @brief update one leaf's bounding volume, with prediction
bool update(NodeType* leaf, const BV& bv, const Vec3f& vel);
/// @brief get the max height of the tree
size_t getMaxHeight() const;
/// @brief get the max depth of the tree
size_t getMaxDepth() const;
/// @brief balance the tree from bottom
void balanceBottomup();
/// @brief balance the tree from top
void balanceTopdown();
/// @brief balance the tree in an incremental way
void balanceIncremental(int iterations);
/// @brief refit the tree, i.e., when the leaf nodes' bounding volumes change, update the entire tree in a bottom-up manner
void refit();
/// @brief extract all the leaves of the tree
void extractLeaves(const NodeType* root, std::vector<NodeType*>& leaves) const;
/// @brief number of leaves in the tree
size_t size() const;
/// @brief get the root of the tree
NodeType* getRoot() const;
NodeType*& getRoot();
/// @brief print the tree in a recursive way
void print(NodeType* root, int depth);
private:
/// @brief construct a tree for a set of leaves from bottom -- very heavy way
void bottomup(const NodeVecIterator lbeg, const NodeVecIterator lend);
/// @brief construct a tree for a set of leaves from top
NodeType* topdown(const NodeVecIterator lbeg, const NodeVecIterator lend);
/// @brief compute the maximum height of a subtree rooted from a given node
size_t getMaxHeight(NodeType* node) const;
/// @brief compute the maximum depth of a subtree rooted from a given node
void getMaxDepth(NodeType* node, size_t depth, size_t& max_depth) const;
/// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a topdown manner.
/// During construction, first compute the best split axis as the axis along with the longest AABB edge.
/// Then compute the median of all nodes' center projection onto the axis and using it as the split threshold.
NodeType* topdown_0(const NodeVecIterator lbeg, const NodeVecIterator lend);
/// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a topdown manner.
/// During construction, first compute the best split thresholds for different axes as the average of all nodes' center.
/// Then choose the split axis as the axis whose threshold can divide the nodes into two parts with almost similar size.
/// This construction is more expensive then topdown_0, but also can provide tree with better quality.
NodeType* topdown_1(const NodeVecIterator lbeg, const NodeVecIterator lend);
/// @brief init tree from leaves in the topdown manner (topdown_0 or topdown_1)
void init_0(std::vector<NodeType*>& leaves);
/// @brief init tree from leaves using morton code. It uses morton_0, i.e., for nodes which is of depth more than the maximum bits of the morton code,
/// we use bottomup method to construct the subtree, which is slow but can construct tree with high quality.
void init_1(std::vector<NodeType*>& leaves);
/// @brief init tree from leaves using morton code. It uses morton_0, i.e., for nodes which is of depth more than the maximum bits of the morton code,
/// we split the leaves into two parts with the same size simply using the node index.
void init_2(std::vector<NodeType*>& leaves);
/// @brief init tree from leaves using morton code. It uses morton_2, i.e., for all nodes, we simply divide the leaves into parts with the same size simply using the node index.
void init_3(std::vector<NodeType*>& leaves);
NodeType* mortonRecurse_0(const NodeVecIterator lbeg, const NodeVecIterator lend, const FCL_UINT32& split, int bits);
NodeType* mortonRecurse_1(const NodeVecIterator lbeg, const NodeVecIterator lend, const FCL_UINT32& split, int bits);
NodeType* mortonRecurse_2(const NodeVecIterator lbeg, const NodeVecIterator lend);
/// @brief update one leaf node's bounding volume
void update_(NodeType* leaf, const BV& bv);
/// @brief sort node n and its parent according to their memory position
NodeType* sort(NodeType* n, NodeType*& r);
/// @brief Insert a leaf node and also update its ancestors
void insertLeaf(NodeType* root, NodeType* leaf);
/// @brief Remove a leaf. The leaf node itself is not deleted yet, but all the unnecessary internal nodes are deleted.
/// return the node with the smallest depth and is influenced by the remove operation
NodeType* removeLeaf(NodeType* leaf);
/// @brief Delete all internal nodes and return all leaves nodes with given depth from root
void fetchLeaves(NodeType* root, std::vector<NodeType*>& leaves, int depth = -1);
static size_t indexOf(NodeType* node);
/// @brief create one node (leaf or internal)
NodeType* createNode(NodeType* parent,
const BV& bv,
void* data);
NodeType* createNode(NodeType* parent,
const BV& bv1,
const BV& bv2,
void* data);
NodeType* createNode(NodeType* parent,
void* data);
void deleteNode(NodeType* node);
void recurseDeleteNode(NodeType* node);
void recurseRefit(NodeType* node);
static BV bounds(const std::vector<NodeType*>& leaves);
static BV bounds(const NodeVecIterator lbeg, const NodeVecIterator lend);
protected:
NodeType* root_node;
size_t n_leaves;
unsigned int opath;
/// This is a one NodeType cache, the reason is that we need to remove a node and then add it again frequently.
NodeType* free_node;
int max_lookahead_level;
public:
/// @brief decide which topdown algorithm to use
int topdown_level;
/// @brief decide the depth to use expensive bottom-up algorithm
int bu_threshold;
};
template<>
bool HierarchyTree<AABB>::update(NodeBase<AABB>* leaf, const AABB& bv_, const Vec3f& vel, FCL_REAL margin);
template<>
bool HierarchyTree<AABB>::update(NodeBase<AABB>* leaf, const AABB& bv_, const Vec3f& vel);
namespace implementation_array
{
template<typename BV>
struct NodeBase
{
BV bv;
union
{
size_t parent;
size_t next;
};
union
{
size_t children[2];
void* data;
};
FCL_UINT32 code;
bool isLeaf() const { return (children[1] == (size_t)(-1)); }
bool isInternal() const { return !isLeaf(); }
};
/// @brief Functor comparing two nodes
template<typename BV>
struct nodeBaseLess
{
nodeBaseLess(const NodeBase<BV>* nodes_, size_t d_) : nodes(nodes_),
d(d_)
{}
bool operator() (size_t i, size_t j) const
{
if(nodes[i].bv.center()[d] < nodes[j].bv.center()[d])
return true;
return false;
}
private:
/// @brief the nodes array
const NodeBase<BV>* nodes;
/// @brief the dimension to compare
size_t d;
};
/// @brief select the node from node1 and node2 which is close to the query-th node in the nodes. 0 for node1 and 1 for node2.
template<typename BV>
size_t select(size_t query, size_t node1, size_t node2, NodeBase<BV>* nodes)
{
return 0;
}
template<>
size_t select(size_t query, size_t node1, size_t node2, NodeBase<AABB>* nodes);
/// @brief select the node from node1 and node2 which is close to the query AABB. 0 for node1 and 1 for node2.
template<typename BV>
size_t select(const BV& query, size_t node1, size_t node2, NodeBase<BV>* nodes)
{
return 0;
}
template<>
size_t select(const AABB& query, size_t node1, size_t node2, NodeBase<AABB>* nodes);
/// @brief Class for hierarchy tree structure
template<typename BV>
class HierarchyTree
{
typedef NodeBase<BV> NodeType;
struct SortByMorton
{
bool operator() (size_t a, size_t b) const
{
if((a != NULL_NODE) && (b != NULL_NODE))
return nodes[a].code < nodes[b].code;
else if(a == NULL_NODE)
return split < nodes[b].code;
else if(b == NULL_NODE)
return nodes[a].code < split;
return false;
}
NodeType* nodes;
FCL_UINT32 split;
};
public:
/// @brief Create hierarchy tree with suitable setting.
/// bu_threshold decides the height of tree node to start bottom-up construction / optimization;
/// topdown_level decides different methods to construct tree in topdown manner.
/// lower level method constructs tree with better quality but is slower.
HierarchyTree(int bu_threshold_ = 16, int topdown_level_ = 0);
~HierarchyTree();
/// @brief Initialize the tree by a set of leaves using algorithm with a given level.
void init(NodeType* leaves, int n_leaves_, int level = 0);
/// @brief Initialize the tree by a set of leaves using algorithm with a given level.
size_t insert(const BV& bv, void* data);
/// @brief Remove a leaf node
void remove(size_t leaf);
/// @brief Clear the tree
void clear();
/// @brief Whether the tree is empty
bool empty() const;
/// @brief update one leaf node
void update(size_t leaf, int lookahead_level = -1);
/// @brief update the tree when the bounding volume of a given leaf has changed
bool update(size_t leaf, const BV& bv);
/// @brief update one leaf's bounding volume, with prediction
bool update(size_t leaf, const BV& bv, const Vec3f& vel, FCL_REAL margin);
/// @brief update one leaf's bounding volume, with prediction
bool update(size_t leaf, const BV& bv, const Vec3f& vel);
/// @brief get the max height of the tree
size_t getMaxHeight() const;
/// @brief get the max depth of the tree
size_t getMaxDepth() const;
/// @brief balance the tree from bottom
void balanceBottomup();
/// @brief balance the tree from top
void balanceTopdown();
/// @brief balance the tree in an incremental way
void balanceIncremental(int iterations);
/// @brief refit the tree, i.e., when the leaf nodes' bounding volumes change, update the entire tree in a bottom-up manner
void refit();
/// @brief extract all the leaves of the tree
void extractLeaves(size_t root, NodeType*& leaves) const;
/// @brief number of leaves in the tree
size_t size() const;
/// @brief get the root of the tree
size_t getRoot() const;
/// @brief get the pointer to the nodes array
NodeType* getNodes() const;
/// @brief print the tree in a recursive way
void print(size_t root, int depth);
private:
/// @brief construct a tree for a set of leaves from bottom -- very heavy way
void bottomup(size_t* lbeg, size_t* lend);
/// @brief construct a tree for a set of leaves from top
size_t topdown(size_t* lbeg, size_t* lend);
/// @brief compute the maximum height of a subtree rooted from a given node
size_t getMaxHeight(size_t node) const;
/// @brief compute the maximum depth of a subtree rooted from a given node
void getMaxDepth(size_t node, size_t depth, size_t& max_depth) const;
/// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a topdown manner.
/// During construction, first compute the best split axis as the axis along with the longest AABB edge.
/// Then compute the median of all nodes' center projection onto the axis and using it as the split threshold.
size_t topdown_0(size_t* lbeg, size_t* lend);
/// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a topdown manner.
/// During construction, first compute the best split thresholds for different axes as the average of all nodes' center.
/// Then choose the split axis as the axis whose threshold can divide the nodes into two parts with almost similar size.
/// This construction is more expensive then topdown_0, but also can provide tree with better quality.
size_t topdown_1(size_t* lbeg, size_t* lend);
/// @brief init tree from leaves in the topdown manner (topdown_0 or topdown_1)
void init_0(NodeType* leaves, int n_leaves_);
/// @brief init tree from leaves using morton code. It uses morton_0, i.e., for nodes which is of depth more than the maximum bits of the morton code,
/// we use bottomup method to construct the subtree, which is slow but can construct tree with high quality.
void init_1(NodeType* leaves, int n_leaves_);
/// @brief init tree from leaves using morton code. It uses morton_0, i.e., for nodes which is of depth more than the maximum bits of the morton code,
/// we split the leaves into two parts with the same size simply using the node index.
void init_2(NodeType* leaves, int n_leaves_);
/// @brief init tree from leaves using morton code. It uses morton_2, i.e., for all nodes, we simply divide the leaves into parts with the same size simply using the node index.
void init_3(NodeType* leaves, int n_leaves_);
size_t mortonRecurse_0(size_t* lbeg, size_t* lend, const FCL_UINT32& split, int bits);
size_t mortonRecurse_1(size_t* lbeg, size_t* lend, const FCL_UINT32& split, int bits);
size_t mortonRecurse_2(size_t* lbeg, size_t* lend);
/// @brief update one leaf node's bounding volume
void update_(size_t leaf, const BV& bv);
/// @brief Insert a leaf node and also update its ancestors
void insertLeaf(size_t root, size_t leaf);
/// @brief Remove a leaf. The leaf node itself is not deleted yet, but all the unnecessary internal nodes are deleted.
/// return the node with the smallest depth and is influenced by the remove operation
size_t removeLeaf(size_t leaf);
/// @brief Delete all internal nodes and return all leaves nodes with given depth from root
void fetchLeaves(size_t root, NodeType*& leaves, int depth = -1);
size_t indexOf(size_t node);
size_t allocateNode();
/// @brief create one node (leaf or internal)
size_t createNode(size_t parent,
const BV& bv1,
const BV& bv2,
void* data);
size_t createNode(size_t parent,
const BV& bv,
void* data);
size_t createNode(size_t parent,
void* data);
void deleteNode(size_t node);
void recurseRefit(size_t node);
protected:
size_t root_node;
NodeType* nodes;
size_t n_nodes;
size_t n_nodes_alloc;
size_t n_leaves;
size_t freelist;
unsigned int opath;
int max_lookahead_level;
public:
/// @brief decide which topdown algorithm to use
int topdown_level;
/// @brief decide the depth to use expensive bottom-up algorithm
int bu_threshold;
public:
static const size_t NULL_NODE = -1;
};
template<typename BV>
const size_t HierarchyTree<BV>::NULL_NODE;
}
}
#include "fcl/broadphase/hierarchy_tree.hxx"
#endif
include/fcl/broadphase/hierarchy_tree.hxx deleted 100644 → 0
View file @ 12194e71
/*
* 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 */
namespace fcl
{
template<typename BV>
HierarchyTree<BV>::HierarchyTree(int bu_threshold_, int topdown_level_)
{
root_node = NULL;
n_leaves = 0;
free_node = NULL;
max_lookahead_level = -1;
opath = 0;
bu_threshold = bu_threshold_;
topdown_level = topdown_level_;
}
template<typename BV>
HierarchyTree<BV>::~HierarchyTree()
{
clear();
}
template<typename BV>
void HierarchyTree<BV>::init(std::vector<NodeType*>& leaves, int level)
{
switch(level)
{
case 0:
init_0(leaves);
break;
case 1:
init_1(leaves);
break;
case 2:
init_2(leaves);
break;
case 3:
init_3(leaves);
break;
default:
init_0(leaves);
}
}
template<typename BV>
typename HierarchyTree<BV>::NodeType* HierarchyTree<BV>::insert(const BV& bv, void* data)
{
NodeType* leaf = createNode(NULL, bv, data);
insertLeaf(root_node, leaf);
++n_leaves;
return leaf;
}
template<typename BV>
void HierarchyTree<BV>::remove(NodeType* leaf)
{
removeLeaf(leaf);
deleteNode(leaf);
--n_leaves;
}
template<typename BV>
void HierarchyTree<BV>::clear()
{
if(root_node)
recurseDeleteNode(root_node);
n_leaves = 0;
delete free_node;
free_node = NULL;
max_lookahead_level = -1;
opath = 0;
}
template<typename BV>
bool HierarchyTree<BV>::empty() const
{
return (NULL == root_node);
}
template<typename BV>
void HierarchyTree<BV>::update(NodeType* leaf, int lookahead_level)
{
NodeType* root = removeLeaf(leaf);
if(root)
{
if(lookahead_level > 0)
{
for(int i = 0; (i < lookahead_level) && root->parent; ++i)
root = root->parent;
}
else
root = root_node;
}
insertLeaf(root, leaf);
}
template<typename BV>
bool HierarchyTree<BV>::update(NodeType* leaf, const BV& bv)
{
if(leaf->bv.contain(bv)) return false;
update_(leaf, bv);
return true;
}
template<typename BV>
bool HierarchyTree<BV>::update(NodeType* leaf, const BV& bv, const Vec3f& vel, FCL_REAL margin)
{
if(leaf->bv.contain(bv)) return false;
update_(leaf, bv);
return true;
}
template<typename BV>
bool HierarchyTree<BV>::update(NodeType* leaf, const BV& bv, const Vec3f& vel)
{
if(leaf->bv.contain(bv)) return false;
update_(leaf, bv);
return true;
}
template<typename BV>
size_t HierarchyTree<BV>::getMaxHeight() const
{
if(!root_node)
return 0;
return getMaxHeight(root_node);
}
template<typename BV>
size_t HierarchyTree<BV>::getMaxDepth() const
{
if(!root_node) return 0;
size_t max_depth;
getMaxDepth(root_node, 0, max_depth);
return max_depth;
}
template<typename BV>
void HierarchyTree<BV>::balanceBottomup()
{
if(root_node)
{
std::vector<NodeType*> leaves;
leaves.reserve(n_leaves);
fetchLeaves(root_node, leaves);
bottomup(leaves.begin(), leaves.end());
root_node = leaves[0];
}
}
template<typename BV>
void HierarchyTree<BV>::balanceTopdown()
{
if(root_node)
{
std::vector<NodeType*> leaves;
leaves.reserve(n_leaves);
fetchLeaves(root_node, leaves);
root_node = topdown(leaves.begin(), leaves.end());
}
}
template<typename BV>
void HierarchyTree<BV>::balanceIncremental(int iterations)
{
if(iterations < 0) iterations = n_leaves;
if(root_node && (iterations > 0))
{
for(int i = 0; i < iterations; ++i)
{
NodeType* node = root_node;
unsigned int bit = 0;
while(!node->isLeaf())
{
node = sort(node, root_node)->children[(opath>>bit)&1];
bit = (bit+1)&(sizeof(unsigned int) * 8-1);
}
update(node);
++opath;
}
}
}
template<typename BV>
void HierarchyTree<BV>::refit()
{
if(root_node)
recurseRefit(root_node);
}
template<typename BV>
void HierarchyTree<BV>::extractLeaves(const NodeType* root, std::vector<NodeType*>& leaves) const
{
if(!root->isLeaf())
{
extractLeaves(root->children[0], leaves);
extractLeaves(root->children[1], leaves);
}
else
leaves.push_back(root);
}
template<typename BV>
size_t HierarchyTree<BV>::size() const
{
return n_leaves;
}
template<typename BV>
typename HierarchyTree<BV>::NodeType* HierarchyTree<BV>::getRoot() const
{
return root_node;
}
template<typename BV>
typename HierarchyTree<BV>::NodeType*& HierarchyTree<BV>::getRoot()
{
return root_node;
}
template<typename BV>
void HierarchyTree<BV>::print(NodeType* root, int depth)
{
for(int i = 0; i < depth; ++i)
std::cout << " ";
std::cout << " (" << root->bv.min_[0] << ", " << root->bv.min_[1] << ", " << root->bv.min_[2] << "; " << root->bv.max_[0] << ", " << root->bv.max_[1] << ", " << root->bv.max_[2] << ")" << std::endl;
if(root->isLeaf())
{
}
else
{
print(root->children[0], depth+1);
print(root->children[1], depth+1);
}
}
template<typename BV>
void HierarchyTree<BV>::bottomup(const NodeVecIterator lbeg, const NodeVecIterator lend)
{
NodeVecIterator lcur_end = lend;
while(lbeg < lcur_end - 1)
{
NodeVecIterator min_it1, min_it2;
FCL_REAL min_size = std::numeric_limits<FCL_REAL>::max();
for(NodeVecIterator it1 = lbeg; it1 < lcur_end; ++it1)
{
for(NodeVecIterator it2 = it1 + 1; it2 < lcur_end; ++it2)
{
FCL_REAL cur_size = ((*it1)->bv + (*it2)->bv).size();
if(cur_size < min_size)
{
min_size = cur_size;
min_it1 = it1;
min_it2 = it2;
}
}
}
NodeType* n[2] = {*min_it1, *min_it2};
NodeType* p = createNode(NULL, n[0]->bv, n[1]->bv, NULL);
p->children[0] = n[0];
p->children[1] = n[1];
n[0]->parent = p;
n[1]->parent = p;
*min_it1 = p;
NodeType* tmp = *min_it2;
lcur_end--;
*min_it2 = *lcur_end;
*lcur_end = tmp;
}
}
template<typename BV>
typename HierarchyTree<BV>::NodeType* HierarchyTree<BV>::topdown(const NodeVecIterator lbeg, const NodeVecIterator lend)
{
switch(topdown_level)
{
case 0:
return topdown_0(lbeg, lend);
break;
case 1:
return topdown_1(lbeg, lend);
break;
default:
return topdown_0(lbeg, lend);
}
}
template<typename BV>
size_t HierarchyTree<BV>::getMaxHeight(NodeType* node) const
{
if(!node->isLeaf())
{
size_t height1 = getMaxHeight(node->children[0]);
size_t height2 = getMaxHeight(node->children[1]);
return std::max(height1, height2) + 1;
}
else
return 0;
}
template<typename BV>
void HierarchyTree<BV>::getMaxDepth(NodeType* node, size_t depth, size_t& max_depth) const
{
if(!node->isLeaf())
{
getMaxDepth(node->children[0], depth+1, max_depth);
getMaxDepth(node->children[1], depth+1, max_depth);
}
else
max_depth = std::max(max_depth, depth);
}
template<typename BV>
typename HierarchyTree<BV>::NodeType* HierarchyTree<BV>::topdown_0(const NodeVecIterator lbeg, const NodeVecIterator lend)
{
int num_leaves = lend - lbeg;
if(num_leaves > 1)
{
if(num_leaves > bu_threshold)
{
BV vol = (*lbeg)->bv;
for(NodeVecIterator it = lbeg + 1; it < lend; ++it)
vol += (*it)->bv;
int best_axis = 0;
FCL_REAL extent[3] = {vol.width(), vol.height(), vol.depth()};
if(extent[1] > extent[0]) best_axis = 1;
if(extent[2] > extent[best_axis]) best_axis = 2;
// compute median
NodeVecIterator lcenter = lbeg + num_leaves / 2;
std::nth_element(lbeg, lcenter, lend, boost::bind(&nodeBaseLess<BV>, _1, _2, boost::ref(best_axis)));
NodeType* node = createNode(NULL, vol, NULL);
node->children[0] = topdown_0(lbeg, lcenter);
node->children[1] = topdown_0(lcenter, lend);
node->children[0]->parent = node;
node->children[1]->parent = node;
return node;
}
else
{
bottomup(lbeg, lend);
return *lbeg;
}
}
return *lbeg;
}
template<typename BV>
typename HierarchyTree<BV>::NodeType* HierarchyTree<BV>::topdown_1(const NodeVecIterator lbeg, const NodeVecIterator lend)
{
int num_leaves = lend - lbeg;
if(num_leaves > 1)
{
if(num_leaves > bu_threshold)
{
Vec3f split_p = (*lbeg)->bv.center();
BV vol = (*lbeg)->bv;
NodeVecIterator it;
for(it = lbeg + 1; it < lend; ++it)
{
split_p += (*it)->bv.center();
vol += (*it)->bv;
}
split_p /= (FCL_REAL)(num_leaves);
int best_axis = -1;
int bestmidp = num_leaves;
int splitcount[3][2] = {{0,0}, {0,0}, {0,0}};
for(it = lbeg; it < lend; ++it)
{
Vec3f x = (*it)->bv.center() - split_p;
for(size_t j = 0; j < 3; ++j)
++splitcount[j][x[j] > 0 ? 1 : 0];
}
for(size_t i = 0; i < 3; ++i)
{
if((splitcount[i][0] > 0) && (splitcount[i][1] > 0))
{
int midp = std::abs(splitcount[i][0] - splitcount[i][1]);
if(midp < bestmidp)
{
best_axis = i;
bestmidp = midp;
}
}
}
if(best_axis < 0) best_axis = 0;
FCL_REAL split_value = split_p[best_axis];
NodeVecIterator lcenter = lbeg;
for(it = lbeg; it < lend; ++it)
{
if((*it)->bv.center()[best_axis] < split_value)
{
NodeType* temp = *it;
*it = *lcenter;
*lcenter = temp;
++lcenter;
}
}
NodeType* node = createNode(NULL, vol, NULL);
node->children[0] = topdown_1(lbeg, lcenter);
node->children[1] = topdown_1(lcenter, lend);
node->children[0]->parent = node;
node->children[1]->parent = node;
return node;
}
else
{
bottomup(lbeg, lend);
return *lbeg;
}
}
return *lbeg;
}
template<typename BV>
void HierarchyTree<BV>::init_0(std::vector<NodeType*>& leaves)
{
clear();
root_node = topdown(leaves.begin(), leaves.end());
n_leaves = leaves.size();
max_lookahead_level = -1;
opath = 0;
}
template<typename BV>
void HierarchyTree<BV>::init_1(std::vector<NodeType*>& leaves)
{
clear();
BV bound_bv;
if(leaves.size() > 0)
bound_bv = leaves[0]->bv;
for(size_t i = 1; i < leaves.size(); ++i)
bound_bv += leaves[i]->bv;
morton_functor<FCL_UINT32> coder(bound_bv);
for(size_t i = 0; i < leaves.size(); ++i)
leaves[i]->code = coder(leaves[i]->bv.center());
std::sort(leaves.begin(), leaves.end(), SortByMorton());
root_node = mortonRecurse_0(leaves.begin(), leaves.end(), (1 << (coder.bits()-1)), coder.bits()-1);
refit();
n_leaves = leaves.size();
max_lookahead_level = -1;
opath = 0;
}
template<typename BV>
void HierarchyTree<BV>::init_2(std::vector<NodeType*>& leaves)
{
clear();
BV bound_bv;
if(leaves.size() > 0)
bound_bv = leaves[0]->bv;
for(size_t i = 1; i < leaves.size(); ++i)
bound_bv += leaves[i]->bv;
morton_functor<FCL_UINT32> coder(bound_bv);
for(size_t i = 0; i < leaves.size(); ++i)
leaves[i]->code = coder(leaves[i]->bv.center());
std::sort(leaves.begin(), leaves.end(), SortByMorton());
root_node = mortonRecurse_1(leaves.begin(), leaves.end(), (1 << (coder.bits()-1)), coder.bits()-1);
refit();
n_leaves = leaves.size();
max_lookahead_level = -1;
opath = 0;
}
template<typename BV>
void HierarchyTree<BV>::init_3(std::vector<NodeType*>& leaves)
{
clear();
BV bound_bv;
if(leaves.size() > 0)
bound_bv = leaves[0]->bv;
for(size_t i = 1; i < leaves.size(); ++i)
bound_bv += leaves[i]->bv;
morton_functor<FCL_UINT32> coder(bound_bv);
for(size_t i = 0; i < leaves.size(); ++i)
leaves[i]->code = coder(leaves[i]->bv.center());
std::sort(leaves.begin(), leaves.end(), SortByMorton());
root_node = mortonRecurse_2(leaves.begin(), leaves.end());
refit();
n_leaves = leaves.size();
max_lookahead_level = -1;
opath = 0;
}
template<typename BV>
typename HierarchyTree<BV>::NodeType* HierarchyTree<BV>::mortonRecurse_0(const NodeVecIterator lbeg, const NodeVecIterator lend, const FCL_UINT32& split, int bits)
{
int num_leaves = lend - lbeg;
if(num_leaves > 1)
{
if(bits > 0)
{
NodeType dummy;
dummy.code = split;
NodeVecIterator lcenter = std::lower_bound(lbeg, lend, &dummy, SortByMorton());
if(lcenter == lbeg)
{
FCL_UINT32 split2 = split | (1 << (bits - 1));
return mortonRecurse_0(lbeg, lend, split2, bits - 1);
}
else if(lcenter == lend)
{
FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
return mortonRecurse_0(lbeg, lend, split1, bits - 1);
}
else
{
FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
FCL_UINT32 split2 = split | (1 << (bits - 1));
NodeType* child1 = mortonRecurse_0(lbeg, lcenter, split1, bits - 1);
NodeType* child2 = mortonRecurse_0(lcenter, lend, split2, bits - 1);
NodeType* node = createNode(NULL, NULL);
node->children[0] = child1;
node->children[1] = child2;
child1->parent = node;
child2->parent = node;
return node;
}
}
else
{
NodeType* node = topdown(lbeg, lend);
return node;
}
}
else
return *lbeg;
}
template<typename BV>
typename HierarchyTree<BV>::NodeType* HierarchyTree<BV>::mortonRecurse_1(const NodeVecIterator lbeg, const NodeVecIterator lend, const FCL_UINT32& split, int bits)
{
int num_leaves = lend - lbeg;
if(num_leaves > 1)
{
if(bits > 0)
{
NodeType dummy;
dummy.code = split;
NodeVecIterator lcenter = std::lower_bound(lbeg, lend, &dummy, SortByMorton());
if(lcenter == lbeg)
{
FCL_UINT32 split2 = split | (1 << (bits - 1));
return mortonRecurse_1(lbeg, lend, split2, bits - 1);
}
else if(lcenter == lend)
{
FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
return mortonRecurse_1(lbeg, lend, split1, bits - 1);
}
else
{
FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
FCL_UINT32 split2 = split | (1 << (bits - 1));
NodeType* child1 = mortonRecurse_1(lbeg, lcenter, split1, bits - 1);
NodeType* child2 = mortonRecurse_1(lcenter, lend, split2, bits - 1);
NodeType* node = createNode(NULL, NULL);
node->children[0] = child1;
node->children[1] = child2;
child1->parent = node;
child2->parent = node;
return node;
}
}
else
{
NodeType* child1 = mortonRecurse_1(lbeg, lbeg + num_leaves / 2, 0, bits - 1);
NodeType* child2 = mortonRecurse_1(lbeg + num_leaves / 2, lend, 0, bits - 1);
NodeType* node = createNode(NULL, NULL);
node->children[0] = child1;
node->children[1] = child2;
child1->parent = node;
child2->parent = node;
return node;
}
}
else
return *lbeg;
}
template<typename BV>
typename HierarchyTree<BV>::NodeType* HierarchyTree<BV>::mortonRecurse_2(const NodeVecIterator lbeg, const NodeVecIterator lend)
{
int num_leaves = lend - lbeg;
if(num_leaves > 1)
{
NodeType* child1 = mortonRecurse_2(lbeg, lbeg + num_leaves / 2);
NodeType* child2 = mortonRecurse_2(lbeg + num_leaves / 2, lend);
NodeType* node = createNode(NULL, NULL);
node->children[0] = child1;
node->children[1] = child2;
child1->parent = node;
child2->parent = node;
return node;
}
else
return *lbeg;
}
template<typename BV>
void HierarchyTree<BV>::update_(NodeType* leaf, const BV& bv)
{
NodeType* root = removeLeaf(leaf);
if(root)
{
if(max_lookahead_level >= 0)
{
for(int i = 0; (i < max_lookahead_level) && root->parent; ++i)
root = root->parent;
}
else
root = root_node;
}
leaf->bv = bv;
insertLeaf(root, leaf);
}
template<typename BV>
typename HierarchyTree<BV>::NodeType* HierarchyTree<BV>::sort(NodeType* n, NodeType*& r)
{
NodeType* p = n->parent;
if(p > n)
{
int i = indexOf(n);
int j = 1 - i;
NodeType* s = p->children[j];
NodeType* q = p->parent;
if(q) q->children[indexOf(p)] = n; else r = n;
s->parent = n;
p->parent = n;
n->parent = q;
p->children[0] = n->children[0];
p->children[1] = n->children[1];
n->children[0]->parent = p;
n->children[1]->parent = p;
n->children[i] = p;
n->children[j] = s;
std::swap(p->bv, n->bv);
return p;
}
return n;
}
template<typename BV>
void HierarchyTree<BV>::insertLeaf(NodeType* root, NodeType* leaf)
{
if(!root_node)
{
root_node = leaf;
leaf->parent = NULL;
}
else
{
if(!root->isLeaf())
{
do
{
root = root->children[select(*leaf, *(root->children[0]), *(root->children[1]))];
}
while(!root->isLeaf());
}
NodeType* prev = root->parent;
NodeType* node = createNode(prev, leaf->bv, root->bv, NULL);
if(prev)
{
prev->children[indexOf(root)] = node;
node->children[0] = root; root->parent = node;
node->children[1] = leaf; leaf->parent = node;
do
{
if(!prev->bv.contain(node->bv))
prev->bv = prev->children[0]->bv + prev->children[1]->bv;
else
break;
node = prev;
} while (NULL != (prev = node->parent));
}
else
{
node->children[0] = root; root->parent = node;
node->children[1] = leaf; leaf->parent = node;
root_node = node;
}
}
}
template<typename BV>
typename HierarchyTree<BV>::NodeType* HierarchyTree<BV>::removeLeaf(NodeType* leaf)
{
if(leaf == root_node)
{
root_node = NULL;
return NULL;
}
else
{
NodeType* parent = leaf->parent;
NodeType* prev = parent->parent;
NodeType* sibling = parent->children[1-indexOf(leaf)];
if(prev)
{
prev->children[indexOf(parent)] = sibling;
sibling->parent = prev;
deleteNode(parent);
while(prev)
{
BV new_bv = prev->children[0]->bv + prev->children[1]->bv;
if(!new_bv.equal(prev->bv))
{
prev->bv = new_bv;
prev = prev->parent;
}
else break;
}
return prev ? prev : root_node;
}
else
{
root_node = sibling;
sibling->parent = NULL;
deleteNode(parent);
return root_node;
}
}
}
template<typename BV>
void HierarchyTree<BV>::fetchLeaves(NodeType* root, std::vector<NodeType*>& leaves, int depth)
{
if((!root->isLeaf()) && depth)
{
fetchLeaves(root->children[0], leaves, depth-1);
fetchLeaves(root->children[1], leaves, depth-1);
deleteNode(root);
}
else
{
leaves.push_back(root);
}
}
template<typename BV>
size_t HierarchyTree<BV>::indexOf(NodeType* node)
{
// node cannot be NULL
return (node->parent->children[1] == node);
}
template<typename BV>
typename HierarchyTree<BV>::NodeType* HierarchyTree<BV>::createNode(NodeType* parent,
const BV& bv,
void* data)
{
NodeType* node = createNode(parent, data);
node->bv = bv;
return node;
}
template<typename BV>
typename HierarchyTree<BV>::NodeType* HierarchyTree<BV>::createNode(NodeType* parent,
const BV& bv1,
const BV& bv2,
void* data)
{
NodeType* node = createNode(parent, data);
node->bv = bv1 + bv2;
return node;
}
template<typename BV>
typename HierarchyTree<BV>::NodeType* HierarchyTree<BV>::createNode(NodeType* parent, void* data)
{
NodeType* node = NULL;
if(free_node)
{
node = free_node;
free_node = NULL;
}
else
node = new NodeType();
node->parent = parent;
node->data = data;
node->children[1] = 0;
return node;
}
template<typename BV>
void HierarchyTree<BV>::deleteNode(NodeType* node)
{
if(free_node != node)
{
delete free_node;
free_node = node;
}
}
template<typename BV>
void HierarchyTree<BV>::recurseDeleteNode(NodeType* node)
{
if(!node->isLeaf())
{
recurseDeleteNode(node->children[0]);
recurseDeleteNode(node->children[1]);
}
if(node == root_node) root_node = NULL;
deleteNode(node);
}
template<typename BV>
void HierarchyTree<BV>::recurseRefit(NodeType* node)
{
if(!node->isLeaf())
{
recurseRefit(node->children[0]);
recurseRefit(node->children[1]);
node->bv = node->children[0]->bv + node->children[1]->bv;
}
else
return;
}
template<typename BV>
BV HierarchyTree<BV>::bounds(const std::vector<NodeType*>& leaves)
{
if(leaves.size() == 0) return BV();
BV bv = leaves[0]->bv;
for(size_t i = 1; i < leaves.size(); ++i)
{
bv += leaves[i]->bv;
}
return bv;
}
template<typename BV>
BV HierarchyTree<BV>::bounds(const NodeVecIterator lbeg, const NodeVecIterator lend)
{
if(lbeg == lend) return BV();
BV bv = *lbeg;
for(NodeVecIterator it = lbeg + 1; it < lend; ++it)
{
bv += (*it)->bv;
}
return bv;
}
}
namespace fcl
{
namespace implementation_array
{
template<typename BV>
HierarchyTree<BV>::HierarchyTree(int bu_threshold_, int topdown_level_)
{
root_node = NULL_NODE;
n_nodes = 0;
n_nodes_alloc = 16;
nodes = new NodeType[n_nodes_alloc];
for(size_t i = 0; i < n_nodes_alloc - 1; ++i)
nodes[i].next = i + 1;
nodes[n_nodes_alloc - 1].next = NULL_NODE;
n_leaves = 0;
freelist = 0;
opath = 0;
max_lookahead_level = -1;
bu_threshold = bu_threshold_;
topdown_level = topdown_level_;
}
template<typename BV>
HierarchyTree<BV>::~HierarchyTree()
{
delete [] nodes;
}
template<typename BV>
void HierarchyTree<BV>::init(NodeType* leaves, int n_leaves_, int level)
{
switch(level)
{
case 0:
init_0(leaves, n_leaves_);
break;
case 1:
init_1(leaves, n_leaves_);
break;
case 2:
init_2(leaves, n_leaves_);
break;
case 3:
init_3(leaves, n_leaves_);
break;
default:
init_0(leaves, n_leaves_);
}
}
template<typename BV>
void HierarchyTree<BV>::init_0(NodeType* leaves, int n_leaves_)
{
clear();
n_leaves = n_leaves_;
root_node = NULL_NODE;
nodes = new NodeType[n_leaves * 2];
memcpy(nodes, leaves, sizeof(NodeType) * n_leaves);
freelist = n_leaves;
n_nodes = n_leaves;
n_nodes_alloc = 2 * n_leaves;
for(size_t i = n_leaves; i < n_nodes_alloc; ++i)
nodes[i].next = i + 1;
nodes[n_nodes_alloc - 1].next = NULL_NODE;
size_t* ids = new size_t[n_leaves];
for(size_t i = 0; i < n_leaves; ++i)
ids[i] = i;
root_node = topdown(ids, ids + n_leaves);
delete [] ids;
opath = 0;
max_lookahead_level = -1;
}
template<typename BV>
void HierarchyTree<BV>::init_1(NodeType* leaves, int n_leaves_)
{
clear();
n_leaves = n_leaves_;
root_node = NULL_NODE;
nodes = new NodeType[n_leaves * 2];
memcpy(nodes, leaves, sizeof(NodeType) * n_leaves);
freelist = n_leaves;
n_nodes = n_leaves;
n_nodes_alloc = 2 * n_leaves;
for(size_t i = n_leaves; i < n_nodes_alloc; ++i)
nodes[i].next = i + 1;
nodes[n_nodes_alloc - 1].next = NULL_NODE;
BV bound_bv;
if(n_leaves > 0)
bound_bv = nodes[0].bv;
for(size_t i = 1; i < n_leaves; ++i)
bound_bv += nodes[i].bv;
morton_functor<FCL_UINT32> coder(bound_bv);
for(size_t i = 0; i < n_leaves; ++i)
nodes[i].code = coder(nodes[i].bv.center());
size_t* ids = new size_t[n_leaves];
for(size_t i = 0; i < n_leaves; ++i)
ids[i] = i;
SortByMorton comp;
comp.nodes = nodes;
std::sort(ids, ids + n_leaves, comp);
root_node = mortonRecurse_0(ids, ids + n_leaves, (1 << (coder.bits()-1)), coder.bits()-1);
delete [] ids;
refit();
opath = 0;
max_lookahead_level = -1;
}
template<typename BV>
void HierarchyTree<BV>::init_2(NodeType* leaves, int n_leaves_)
{
clear();
n_leaves = n_leaves_;
root_node = NULL_NODE;
nodes = new NodeType[n_leaves * 2];
memcpy(nodes, leaves, sizeof(NodeType) * n_leaves);
freelist = n_leaves;
n_nodes = n_leaves;
n_nodes_alloc = 2 * n_leaves;
for(size_t i = n_leaves; i < n_nodes_alloc; ++i)
nodes[i].next = i + 1;
nodes[n_nodes_alloc - 1].next = NULL_NODE;
BV bound_bv;
if(n_leaves > 0)
bound_bv = nodes[0].bv;
for(size_t i = 1; i < n_leaves; ++i)
bound_bv += nodes[i].bv;
morton_functor<FCL_UINT32> coder(bound_bv);
for(size_t i = 0; i < n_leaves; ++i)
nodes[i].code = coder(nodes[i].bv.center());
size_t* ids = new size_t[n_leaves];
for(size_t i = 0; i < n_leaves; ++i)
ids[i] = i;
SortByMorton comp;
comp.nodes = nodes;
std::sort(ids, ids + n_leaves, comp);
root_node = mortonRecurse_1(ids, ids + n_leaves, (1 << (coder.bits()-1)), coder.bits()-1);
delete [] ids;
refit();
opath = 0;
max_lookahead_level = -1;
}
template<typename BV>
void HierarchyTree<BV>::init_3(NodeType* leaves, int n_leaves_)
{
clear();
n_leaves = n_leaves_;
root_node = NULL_NODE;
nodes = new NodeType[n_leaves * 2];
memcpy(nodes, leaves, sizeof(NodeType) * n_leaves);
freelist = n_leaves;
n_nodes = n_leaves;
n_nodes_alloc = 2 * n_leaves;
for(size_t i = n_leaves; i < n_nodes_alloc; ++i)
nodes[i].next = i + 1;
nodes[n_nodes_alloc - 1].next = NULL_NODE;
BV bound_bv;
if(n_leaves > 0)
bound_bv = nodes[0].bv;
for(size_t i = 1; i < n_leaves; ++i)
bound_bv += nodes[i].bv;
morton_functor<FCL_UINT32> coder(bound_bv);
for(size_t i = 0; i < n_leaves; ++i)
nodes[i].code = coder(nodes[i].bv.center());
size_t* ids = new size_t[n_leaves];
for(size_t i = 0; i < n_leaves; ++i)
ids[i] = i;
SortByMorton comp;
comp.nodes = nodes;
std::sort(ids, ids + n_leaves, comp);
root_node = mortonRecurse_2(ids, ids + n_leaves);
delete [] ids;
refit();
opath = 0;
max_lookahead_level = -1;
}
template<typename BV>
size_t HierarchyTree<BV>::insert(const BV& bv, void* data)
{
size_t node = createNode(NULL_NODE, bv, data);
insertLeaf(root_node, node);
++n_leaves;
return node;
}
template<typename BV>
void HierarchyTree<BV>::remove(size_t leaf)
{
removeLeaf(leaf);
deleteNode(leaf);
--n_leaves;
}
template<typename BV>
void HierarchyTree<BV>::clear()
{
delete [] nodes;
root_node = NULL_NODE;
n_nodes = 0;
n_nodes_alloc = 16;
nodes = new NodeType[n_nodes_alloc];
for(size_t i = 0; i < n_nodes_alloc; ++i)
nodes[i].next = i + 1;
nodes[n_nodes_alloc - 1].next = NULL_NODE;
n_leaves = 0;
freelist = 0;
opath = 0;
max_lookahead_level = -1;
}
template<typename BV>
bool HierarchyTree<BV>::empty() const
{
return (n_nodes == 0);
}
template<typename BV>
void HierarchyTree<BV>::update(size_t leaf, int lookahead_level)
{
size_t root = removeLeaf(leaf);
if(root != NULL_NODE)
{
if(lookahead_level > 0)
{
for(int i = 0; (i < lookahead_level) && (nodes[root].parent != NULL_NODE); ++i)
root = nodes[root].parent;
}
else
root = root_node;
}
insertLeaf(root, leaf);
}
template<typename BV>
bool HierarchyTree<BV>::update(size_t leaf, const BV& bv)
{
if(nodes[leaf].bv.contain(bv)) return false;
update_(leaf, bv);
return true;
}
template<typename BV>
bool HierarchyTree<BV>::update(size_t leaf, const BV& bv, const Vec3f& vel, FCL_REAL margin)
{
if(nodes[leaf].bv.contain(bv)) return false;
update_(leaf, bv);
return true;
}
template<typename BV>
bool HierarchyTree<BV>::update(size_t leaf, const BV& bv, const Vec3f& vel)
{
if(nodes[leaf].bv.contain(bv)) return false;
update_(leaf, bv);
return true;
}
template<typename BV>
size_t HierarchyTree<BV>::getMaxHeight() const
{
if(root_node == NULL_NODE) return 0;
return getMaxHeight(root_node);
}
template<typename BV>
size_t HierarchyTree<BV>::getMaxDepth() const
{
if(root_node == NULL_NODE) return 0;
size_t max_depth;
getMaxDepth(root_node, 0, max_depth);
return max_depth;
}
template<typename BV>
void HierarchyTree<BV>::balanceBottomup()
{
if(root_node != NULL_NODE)
{
NodeType* leaves = new NodeType[n_leaves];
NodeType* leaves_ = leaves;
extractLeaves(root_node, leaves_);
root_node = NULL_NODE;
memcpy(nodes, leaves, sizeof(NodeType) * n_leaves);
freelist = n_leaves;
n_nodes = n_leaves;
for(size_t i = n_leaves; i < n_nodes_alloc; ++i)
nodes[i].next = i + 1;
nodes[n_nodes_alloc - 1].next = NULL_NODE;
size_t* ids = new size_t[n_leaves];
for(size_t i = 0; i < n_leaves; ++i)
ids[i] = i;
bottomup(ids, ids + n_leaves);
root_node = *ids;
delete [] ids;
}
}
template<typename BV>
void HierarchyTree<BV>::balanceTopdown()
{
if(root_node != NULL_NODE)
{
NodeType* leaves = new NodeType[n_leaves];
NodeType* leaves_ = leaves;
extractLeaves(root_node, leaves_);
root_node = NULL_NODE;
memcpy(nodes, leaves, sizeof(NodeType) * n_leaves);
freelist = n_leaves;
n_nodes = n_leaves;
for(size_t i = n_leaves; i < n_nodes_alloc; ++i)
nodes[i].next = i + 1;
nodes[n_nodes_alloc - 1].next = NULL_NODE;
size_t* ids = new size_t[n_leaves];
for(size_t i = 0; i < n_leaves; ++i)
ids[i] = i;
root_node = topdown(ids, ids + n_leaves);
delete [] ids;
}
}
template<typename BV>
void HierarchyTree<BV>::balanceIncremental(int iterations)
{
if(iterations < 0) iterations = n_leaves;
if((root_node != NULL_NODE) && (iterations > 0))
{
for(int i = 0; i < iterations; ++i)
{
size_t node = root_node;
unsigned int bit = 0;
while(!nodes[node].isLeaf())
{
node = nodes[node].children[(opath>>bit)&1];
bit = (bit+1)&(sizeof(unsigned int) * 8-1);
}
update(node);
++opath;
}
}
}
template<typename BV>
void HierarchyTree<BV>::refit()
{
if(root_node != NULL_NODE)
recurseRefit(root_node);
}
template<typename BV>
void HierarchyTree<BV>::extractLeaves(size_t root, NodeType*& leaves) const
{
if(!nodes[root].isLeaf())
{
extractLeaves(nodes[root].children[0], leaves);
extractLeaves(nodes[root].children[1], leaves);
}
else
{
*leaves = nodes[root];
leaves++;
}
}
template<typename BV>
size_t HierarchyTree<BV>::size() const
{
return n_leaves;
}
template<typename BV>
size_t HierarchyTree<BV>::getRoot() const
{
return root_node;
}
template<typename BV>
typename HierarchyTree<BV>::NodeType* HierarchyTree<BV>::getNodes() const
{
return nodes;
}
template<typename BV>
void HierarchyTree<BV>::print(size_t root, int depth)
{
for(int i = 0; i < depth; ++i)
std::cout << " ";
NodeType* n = nodes + root;
std::cout << " (" << n->bv.min_[0] << ", " << n->bv.min_[1] << ", " << n->bv.min_[2] << "; " << n->bv.max_[0] << ", " << n->bv.max_[1] << ", " << n->bv.max_[2] << ")" << std::endl;
if(n->isLeaf())
{
}
else
{
print(n->children[0], depth+1);
print(n->children[1], depth+1);
}
}
template<typename BV>
size_t HierarchyTree<BV>::getMaxHeight(size_t node) const
{
if(!nodes[node].isLeaf())
{
size_t height1 = getMaxHeight(nodes[node].children[0]);
size_t height2 = getMaxHeight(nodes[node].children[1]);
return std::max(height1, height2) + 1;
}
else
return 0;
}
template<typename BV>
void HierarchyTree<BV>::getMaxDepth(size_t node, size_t depth, size_t& max_depth) const
{
if(!nodes[node].isLeaf())
{
getMaxDepth(nodes[node].children[0], depth+1, max_depth);
getmaxDepth(nodes[node].children[1], depth+1, max_depth);
}
else
max_depth = std::max(max_depth, depth);
}
template<typename BV>
void HierarchyTree<BV>::bottomup(size_t* lbeg, size_t* lend)
{
size_t* lcur_end = lend;
while(lbeg < lcur_end - 1)
{
size_t* min_it1 = NULL, *min_it2 = NULL;
FCL_REAL min_size = std::numeric_limits<FCL_REAL>::max();
for(size_t* it1 = lbeg; it1 < lcur_end; ++it1)
{
for(size_t* it2 = it1 + 1; it2 < lcur_end; ++it2)
{
FCL_REAL cur_size = (nodes[*it1].bv + nodes[*it2].bv).size();
if(cur_size < min_size)
{
min_size = cur_size;
min_it1 = it1;
min_it2 = it2;
}
}
}
size_t p = createNode(NULL_NODE, nodes[*min_it1].bv, nodes[*min_it2].bv, NULL);
nodes[p].children[0] = *min_it1;
nodes[p].children[1] = *min_it2;
nodes[*min_it1].parent = p;
nodes[*min_it2].parent = p;
*min_it1 = p;
size_t tmp = *min_it2;
lcur_end--;
*min_it2 = *lcur_end;
*lcur_end = tmp;
}
}
template<typename BV>
size_t HierarchyTree<BV>::topdown(size_t* lbeg, size_t* lend)
{
switch(topdown_level)
{
case 0:
return topdown_0(lbeg, lend);
break;
case 1:
return topdown_1(lbeg, lend);
break;
default:
return topdown_0(lbeg, lend);
}
}
template<typename BV>
size_t HierarchyTree<BV>::topdown_0(size_t* lbeg, size_t* lend)
{
int num_leaves = lend - lbeg;
if(num_leaves > 1)
{
if(num_leaves > bu_threshold)
{
BV vol = nodes[*lbeg].bv;
for(size_t* i = lbeg + 1; i < lend; ++i)
vol += nodes[*i].bv;
int best_axis = 0;
FCL_REAL extent[3] = {vol.width(), vol.height(), vol.depth()};
if(extent[1] > extent[0]) best_axis = 1;
if(extent[2] > extent[best_axis]) best_axis = 2;
nodeBaseLess<BV> comp(nodes, best_axis);
size_t* lcenter = lbeg + num_leaves / 2;
std::nth_element(lbeg, lcenter, lend, comp);
size_t node = createNode(NULL_NODE, vol, NULL);
nodes[node].children[0] = topdown_0(lbeg, lcenter);
nodes[node].children[1] = topdown_0(lcenter, lend);
nodes[nodes[node].children[0]].parent = node;
nodes[nodes[node].children[1]].parent = node;
return node;
}
else
{
bottomup(lbeg, lend);
return *lbeg;
}
}
return *lbeg;
}
template<typename BV>
size_t HierarchyTree<BV>::topdown_1(size_t* lbeg, size_t* lend)
{
int num_leaves = lend - lbeg;
if(num_leaves > 1)
{
if(num_leaves > bu_threshold)
{
Vec3f split_p = nodes[*lbeg].bv.center();
BV vol = nodes[*lbeg].bv;
for(size_t* i = lbeg + 1; i < lend; ++i)
{
split_p += nodes[*i].bv.center();
vol += nodes[*i].bv;
}
split_p /= (FCL_REAL)(num_leaves);
int best_axis = -1;
int bestmidp = num_leaves;
int splitcount[3][2] = {{0,0}, {0,0}, {0,0}};
for(size_t* i = lbeg; i < lend; ++i)
{
Vec3f x = nodes[*i].bv.center() - split_p;
for(size_t j = 0; j < 3; ++j)
++splitcount[j][x[j] > 0 ? 1 : 0];
}
for(size_t i = 0; i < 3; ++i)
{
if((splitcount[i][0] > 0) && (splitcount[i][1] > 0))
{
int midp = std::abs(splitcount[i][0] - splitcount[i][1]);
if(midp < bestmidp)
{
best_axis = i;
bestmidp = midp;
}
}
}
if(best_axis < 0) best_axis = 0;
FCL_REAL split_value = split_p[best_axis];
size_t* lcenter = lbeg;
for(size_t* i = lbeg; i < lend; ++i)
{
if(nodes[*i].bv.center()[best_axis] < split_value)
{
size_t temp = *i;
*i = *lcenter;
*lcenter = temp;
++lcenter;
}
}
size_t node = createNode(NULL_NODE, vol, NULL);
nodes[node].children[0] = topdown_1(lbeg, lcenter);
nodes[node].children[1] = topdown_1(lcenter, lend);
nodes[nodes[node].children[0]].parent = node;
nodes[nodes[node].children[1]].parent = node;
return node;
}
else
{
bottomup(lbeg, lend);
return *lbeg;
}
}
return *lbeg;
}
template<typename BV>
size_t HierarchyTree<BV>::mortonRecurse_0(size_t* lbeg, size_t* lend, const FCL_UINT32& split, int bits)
{
int num_leaves = lend - lbeg;
if(num_leaves > 1)
{
if(bits > 0)
{
SortByMorton comp;
comp.nodes = nodes;
comp.split = split;
size_t* lcenter = std::lower_bound(lbeg, lend, NULL_NODE, comp);
if(lcenter == lbeg)
{
FCL_UINT32 split2 = split | (1 << (bits - 1));
return mortonRecurse_0(lbeg, lend, split2, bits - 1);
}
else if(lcenter == lend)
{
FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
return mortonRecurse_0(lbeg, lend, split1, bits - 1);
}
else
{
FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
FCL_UINT32 split2 = split | (1 << (bits - 1));
size_t child1 = mortonRecurse_0(lbeg, lcenter, split1, bits - 1);
size_t child2 = mortonRecurse_0(lcenter, lend, split2, bits - 1);
size_t node = createNode(NULL_NODE, NULL);
nodes[node].children[0] = child1;
nodes[node].children[1] = child2;
nodes[child1].parent = node;
nodes[child2].parent = node;
return node;
}
}
else
{
size_t node = topdown(lbeg, lend);
return node;
}
}
else
return *lbeg;
}
template<typename BV>
size_t HierarchyTree<BV>::mortonRecurse_1(size_t* lbeg, size_t* lend, const FCL_UINT32& split, int bits)
{
int num_leaves = lend - lbeg;
if(num_leaves > 1)
{
if(bits > 0)
{
SortByMorton comp;
comp.nodes = nodes;
comp.split = split;
size_t* lcenter = std::lower_bound(lbeg, lend, NULL_NODE, comp);
if(lcenter == lbeg)
{
FCL_UINT32 split2 = split | (1 << (bits - 1));
return mortonRecurse_1(lbeg, lend, split2, bits - 1);
}
else if(lcenter == lend)
{
FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
return mortonRecurse_1(lbeg, lend, split1, bits - 1);
}
else
{
FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
FCL_UINT32 split2 = split | (1 << (bits - 1));
size_t child1 = mortonRecurse_1(lbeg, lcenter, split1, bits - 1);
size_t child2 = mortonRecurse_1(lcenter, lend, split2, bits - 1);
size_t node = createNode(NULL_NODE, NULL);
nodes[node].children[0] = child1;
nodes[node].children[1] = child2;
nodes[child1].parent = node;
nodes[child2].parent = node;
return node;
}
}
else
{
size_t child1 = mortonRecurse_1(lbeg, lbeg + num_leaves / 2, 0, bits - 1);
size_t child2 = mortonRecurse_1(lbeg + num_leaves / 2, lend, 0, bits - 1);
size_t node = createNode(NULL_NODE, NULL);
nodes[node].children[0] = child1;
nodes[node].children[1] = child2;
nodes[child1].parent = node;
nodes[child2].parent = node;
return node;
}
}
else
return *lbeg;
}
template<typename BV>
size_t HierarchyTree<BV>::mortonRecurse_2(size_t* lbeg, size_t* lend)
{
int num_leaves = lend - lbeg;
if(num_leaves > 1)
{
size_t child1 = mortonRecurse_2(lbeg, lbeg + num_leaves / 2);
size_t child2 = mortonRecurse_2(lbeg + num_leaves / 2, lend);
size_t node = createNode(NULL_NODE, NULL);
nodes[node].children[0] = child1;
nodes[node].children[1] = child2;
nodes[child1].parent = node;
nodes[child2].parent = node;
return node;
}
else
return *lbeg;
}
template<typename BV>
void HierarchyTree<BV>::insertLeaf(size_t root, size_t leaf)
{
if(root_node == NULL_NODE)
{
root_node = leaf;
nodes[leaf].parent = NULL_NODE;
}
else
{
if(!nodes[root].isLeaf())
{
do
{
root = nodes[root].children[select(leaf, nodes[root].children[0], nodes[root].children[1], nodes)];
}
while(!nodes[root].isLeaf());
}
size_t prev = nodes[root].parent;
size_t node = createNode(prev, nodes[leaf].bv, nodes[root].bv, NULL);
if(prev != NULL_NODE)
{
nodes[prev].children[indexOf(root)] = node;
nodes[node].children[0] = root; nodes[root].parent = node;
nodes[node].children[1] = leaf; nodes[leaf].parent = node;
do
{
if(!nodes[prev].bv.contain(nodes[node].bv))
nodes[prev].bv = nodes[nodes[prev].children[0]].bv + nodes[nodes[prev].children[1]].bv;
else
break;
node = prev;
} while (NULL_NODE != (prev = nodes[node].parent));
}
else
{
nodes[node].children[0] = root; nodes[root].parent = node;
nodes[node].children[1] = leaf; nodes[leaf].parent = node;
root_node = node;
}
}
}
template<typename BV>
size_t HierarchyTree<BV>::removeLeaf(size_t leaf)
{
if(leaf == root_node)
{
root_node = NULL_NODE;
return NULL_NODE;
}
else
{
size_t parent = nodes[leaf].parent;
size_t prev = nodes[parent].parent;
size_t sibling = nodes[parent].children[1-indexOf(leaf)];
if(prev != NULL_NODE)
{
nodes[prev].children[indexOf(parent)] = sibling;
nodes[sibling].parent = prev;
deleteNode(parent);
while(prev != NULL_NODE)
{
BV new_bv = nodes[nodes[prev].children[0]].bv + nodes[nodes[prev].children[1]].bv;
if(!new_bv.equal(nodes[prev].bv))
{
nodes[prev].bv = new_bv;
prev = nodes[prev].parent;
}
else break;
}
return (prev != NULL_NODE) ? prev : root_node;
}
else
{
root_node = sibling;
nodes[sibling].parent = NULL_NODE;
deleteNode(parent);
return root_node;
}
}
}
template<typename BV>
void HierarchyTree<BV>::update_(size_t leaf, const BV& bv)
{
size_t root = removeLeaf(leaf);
if(root != NULL_NODE)
{
if(max_lookahead_level >= 0)
{
for(int i = 0; (i < max_lookahead_level) && (nodes[root].parent != NULL_NODE); ++i)
root = nodes[root].parent;
}
nodes[leaf].bv = bv;
insertLeaf(root, leaf);
}
}
template<typename BV>
size_t HierarchyTree<BV>::indexOf(size_t node)
{
return (nodes[nodes[node].parent].children[1] == node);
}
template<typename BV>
size_t HierarchyTree<BV>::allocateNode()
{
if(freelist == NULL_NODE)
{
NodeType* old_nodes = nodes;
n_nodes_alloc *= 2;
nodes = new NodeType[n_nodes_alloc];
memcpy(nodes, old_nodes, n_nodes * sizeof(NodeType));
delete [] old_nodes;
for(size_t i = n_nodes; i < n_nodes_alloc - 1; ++i)
nodes[i].next = i + 1;
nodes[n_nodes_alloc - 1].next = NULL_NODE;
freelist = n_nodes;
}
size_t node_id = freelist;
freelist = nodes[node_id].next;
nodes[node_id].parent = NULL_NODE;
nodes[node_id].children[0] = NULL_NODE;
nodes[node_id].children[1] = NULL_NODE;
++n_nodes;
return node_id;
}
template<typename BV>
size_t HierarchyTree<BV>::createNode(size_t parent,
const BV& bv1,
const BV& bv2,
void* data)
{
size_t node = allocateNode();
nodes[node].parent = parent;
nodes[node].data = data;
nodes[node].bv = bv1 + bv2;
return node;
}
template<typename BV>
size_t HierarchyTree<BV>::createNode(size_t parent,
const BV& bv,
void* data)
{
size_t node = allocateNode();
nodes[node].parent = parent;
nodes[node].data = data;
nodes[node].bv = bv;
return node;
}
template<typename BV>
size_t HierarchyTree<BV>::createNode(size_t parent,
void* data)
{
size_t node = allocateNode();
nodes[node].parent = parent;
nodes[node].data = data;
return node;
}
template<typename BV>
void HierarchyTree<BV>::deleteNode(size_t node)
{
nodes[node].next = freelist;
freelist = node;
--n_nodes;
}
template<typename BV>
void HierarchyTree<BV>::recurseRefit(size_t node)
{
if(!nodes[node].isLeaf())
{
recurseRefit(nodes[node].children[0]);
recurseRefit(nodes[node].children[1]);
nodes[node].bv = nodes[nodes[node].children[0]].bv + nodes[nodes[node].children[1]].bv;
}
else
return;
}
template<typename BV>
void HierarchyTree<BV>::fetchLeaves(size_t root, NodeType*& leaves, int depth)
{
if((!nodes[root].isLeaf()) && depth)
{
fetchLeaves(nodes[root].children[0], leaves, depth-1);
fetchLeaves(nodes[root].children[1], leaves, depth-1);
deleteNode(root);
}
else
{
*leaves = nodes[root];
leaves++;
}
}
}
}
include/fcl/broadphase/morton.h deleted 100644 → 0
View file @ 12194e71
/*
* 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_MORTON_H
#define FCL_MORTON_H
#include <boost/dynamic_bitset.hpp>
#include "fcl/data_types.h"
#include "fcl/BV/AABB.h"
namespace fcl
{
/// @cond IGNORE
namespace details
{
static inline FCL_UINT32 quantize(FCL_REAL x, FCL_UINT32 n)
{
return std::max(std::min((FCL_UINT32)(x * (FCL_REAL)n), FCL_UINT32(n-1)), FCL_UINT32(0));
}
/// @brief compute 30 bit morton code
static inline FCL_UINT32 morton_code(FCL_UINT32 x, FCL_UINT32 y, FCL_UINT32 z)
{
x = (x | (x << 16)) & 0x030000FF;
x = (x | (x << 8)) & 0x0300F00F;
x = (x | (x << 4)) & 0x030C30C3;
x = (x | (x << 2)) & 0x09249249;
y = (y | (y << 16)) & 0x030000FF;
y = (y | (y << 8)) & 0x0300F00F;
y = (y | (y << 4)) & 0x030C30C3;
y = (y | (y << 2)) & 0x09249249;
z = (z | (z << 16)) & 0x030000FF;
z = (z | (z << 8)) & 0x0300F00F;
z = (z | (z << 4)) & 0x030C30C3;
z = (z | (z << 2)) & 0x09249249;
return x | (y << 1) | (z << 2);
}
/// @brief compute 60 bit morton code
static inline FCL_UINT64 morton_code60(FCL_UINT32 x, FCL_UINT32 y, FCL_UINT32 z)
{
FCL_UINT32 lo_x = x & 1023u;
FCL_UINT32 lo_y = y & 1023u;
FCL_UINT32 lo_z = z & 1023u;
FCL_UINT32 hi_x = x >> 10u;
FCL_UINT32 hi_y = y >> 10u;
FCL_UINT32 hi_z = z >> 10u;
return (FCL_UINT64(morton_code(hi_x, hi_y, hi_z)) << 30) | FCL_UINT64(morton_code(lo_x, lo_y, lo_z));
}
}
/// @endcond
/// @brief Functor to compute the morton code for a given AABB
template<typename T>
struct morton_functor {};
/// @brief Functor to compute 30 bit morton code for a given AABB
template<>
struct morton_functor<FCL_UINT32>
{
morton_functor(const AABB& bbox) : base(bbox.min_),
inv(1.0 / (bbox.max_[0] - bbox.min_[0]),
1.0 / (bbox.max_[1] - bbox.min_[1]),
1.0 / (bbox.max_[2] - bbox.min_[2]))
{}
FCL_UINT32 operator() (const Vec3f& point) const
{
FCL_UINT32 x = details::quantize((point[0] - base[0]) * inv[0], 1024u);
FCL_UINT32 y = details::quantize((point[1] - base[1]) * inv[1], 1024u);
FCL_UINT32 z = details::quantize((point[2] - base[2]) * inv[2], 1024u);
return details::morton_code(x, y, z);
}
const Vec3f base;
const Vec3f inv;
size_t bits() const { return 30; }
};
/// @brief Functor to compute 60 bit morton code for a given AABB
template<>
struct morton_functor<FCL_UINT64>
{
morton_functor(const AABB& bbox) : base(bbox.min_),
inv(1.0 / (bbox.max_[0] - bbox.min_[0]),
1.0 / (bbox.max_[1] - bbox.min_[1]),
1.0 / (bbox.max_[2] - bbox.min_[2]))
{}
FCL_UINT64 operator() (const Vec3f& point) const
{
FCL_UINT32 x = details::quantize((point[0] - base[0]) * inv[0], 1u << 20);
FCL_UINT32 y = details::quantize((point[1] - base[1]) * inv[1], 1u << 20);
FCL_UINT32 z = details::quantize((point[2] - base[2]) * inv[2], 1u << 20);
return details::morton_code60(x, y, z);
}
const Vec3f base;
const Vec3f inv;
size_t bits() const { return 60; }
};
/// @brief Functor to compute n bit morton code for a given AABB
template<>
struct morton_functor<boost::dynamic_bitset<> >
{
morton_functor(const AABB& bbox, size_t bit_num_) : base(bbox.min_),
inv(1.0 / (bbox.max_[0] - bbox.min_[0]),
1.0 / (bbox.max_[1] - bbox.min_[1]),
1.0 / (bbox.max_[2] - bbox.min_[2])),
bit_num(bit_num_)
{}
boost::dynamic_bitset<> operator() (const Vec3f& point) const
{
FCL_REAL x = (point[0] - base[0]) * inv[0];
FCL_REAL y = (point[1] - base[1]) * inv[1];
FCL_REAL z = (point[2] - base[2]) * inv[2];
int start_bit = bit_num * 3 - 1;
boost::dynamic_bitset<> bits(bit_num * 3);
x *= 2;
y *= 2;
z *= 2;
for(size_t i = 0; i < bit_num; ++i)
{
bits[start_bit--] = ((z < 1) ? 0 : 1);
bits[start_bit--] = ((y < 1) ? 0 : 1);
bits[start_bit--] = ((x < 1) ? 0 : 1);
x = ((x >= 1) ? 2*(x-1) : 2*x);
y = ((y >= 1) ? 2*(y-1) : 2*y);
z = ((z >= 1) ? 2*(z-1) : 2*z);
}
return bits;
}
const Vec3f base;
const Vec3f inv;
const size_t bit_num;
size_t bits() const { return bit_num * 3; }
};
}
#endif
include/fcl/ccd/interpolation/interpolation_linear.h deleted 100644 → 0
View file @ 12194e71
/*
* 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 Dalibor Matura, Jia Pan */
#ifndef FCL_CCD_INTERPOLATION_INTERPOLATION_LINEAR_H
#define FCL_CCD_INTERPOLATION_INTERPOLATION_LINEAR_H
#include "fcl/data_types.h"
#include "fcl/ccd/interpolation/interpolation.h"
#include <boost/shared_ptr.hpp>
namespace fcl
{
class InterpolationFactory;
class InterpolationLinear : public Interpolation
{
public:
InterpolationLinear();
InterpolationLinear(FCL_REAL start_value, FCL_REAL end_value);
virtual FCL_REAL getValue(FCL_REAL time) const;
virtual FCL_REAL getValueLowerBound() const;
virtual FCL_REAL getValueUpperBound() const;
virtual InterpolationType getType() const;
virtual FCL_REAL getMovementLengthBound(FCL_REAL time) const;
virtual FCL_REAL getVelocityBound(FCL_REAL time) const;
public:
static boost::shared_ptr<Interpolation> create(FCL_REAL start_value, FCL_REAL end_value);
static void registerToFactory();
};
}
#endif
include/fcl/ccd/interval.h deleted 100644 → 0
View file @ 12194e71
/*
* 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_CCD_INTERVAL_H
#define FCL_CCD_INTERVAL_H
#include "fcl/data_types.h"
namespace fcl
{
/// @brief Interval class for [a, b]
struct Interval
{
FCL_REAL i_[2];
Interval() { i_[0] = i_[1] = 0; }
explicit Interval(FCL_REAL v)
{
i_[0] = i_[1] = v;
}
/// @brief construct interval [left, right]
Interval(FCL_REAL left, FCL_REAL right)
{
i_[0] = left; i_[1] = right;
}
/// @brief construct interval [left, right]
inline void setValue(FCL_REAL a, FCL_REAL b)
{
i_[0] = a; i_[1] = b;
}
/// @brief construct zero interval [x, x]
inline void setValue(FCL_REAL x)
{
i_[0] = i_[1] = x;
}
/// @brief access the interval endpoints: 0 for left, 1 for right end
inline FCL_REAL operator [] (size_t i) const
{
return i_[i];
}
/// @brief access the interval endpoints: 0 for left, 1 for right end
inline FCL_REAL& operator [] (size_t i)
{
return i_[i];
}
/// @brief whether two intervals are the same
inline bool operator == (const Interval& other) const
{
if(i_[0] != other.i_[0]) return false;
if(i_[1] != other.i_[1]) return false;
return true;
}
/// @brief add two intervals
inline Interval operator + (const Interval& other) const
{
return Interval(i_[0] + other.i_[0], i_[1] + other.i_[1]);
}
/// @brief minus another interval
inline Interval operator - (const Interval& other) const
{
return Interval(i_[0] - other.i_[1], i_[1] - other.i_[0]);
}
inline Interval& operator += (const Interval& other)
{
i_[0] += other.i_[0];
i_[1] += other.i_[1];
return *this;
}
inline Interval& operator -= (const Interval& other)
{
i_[0] -= other.i_[1];
i_[1] -= other.i_[0];
return *this;
}
Interval operator * (const Interval& other) const;
Interval& operator *= (const Interval& other);
inline Interval operator * (FCL_REAL d) const
{
if(d >= 0) return Interval(i_[0] * d, i_[1] * d);
return Interval(i_[1] * d, i_[0] * d);
}
inline Interval& operator *= (FCL_REAL d)
{
if(d >= 0)
{
i_[0] *= d;
i_[1] *= d;
}
else
{
FCL_REAL tmp = i_[0];
i_[0] = i_[1] * d;
i_[1] = tmp * d;
}
return *this;
}
/// @brief other must not contain 0
Interval operator / (const Interval& other) const;
Interval& operator /= (const Interval& other);
/// @brief determine whether the intersection between intervals is empty
inline bool overlap(const Interval& other) const
{
if(i_[1] < other.i_[0]) return false;
if(i_[0] > other.i_[1]) return false;
return true;
}
inline bool intersect(const Interval& other)
{
if(i_[1] < other.i_[0]) return false;
if(i_[0] > other.i_[1]) return false;
if(i_[1] > other.i_[1]) i_[1] = other.i_[1];
if(i_[0] < other.i_[0]) i_[0] = other.i_[0];
return true;
}
inline Interval operator - () const
{
return Interval(-i_[1], -i_[0]);
}
/// @brief Return the nearest distance for points within the interval to zero
inline FCL_REAL getAbsLower() const
{
if(i_[0] >= 0) return i_[0];
if(i_[1] >= 0) return 0;
return -i_[1];
}
/// @brief Return the farthest distance for points within the interval to zero
inline FCL_REAL getAbsUpper() const
{
if(i_[0] + i_[1] >= 0) return i_[1];
return i_[0];
}
inline bool contains(FCL_REAL v) const
{
if(v < i_[0]) return false;
if(v > i_[1]) return false;
return true;
}
/// @brief Compute the minimum interval contains v and original interval
inline Interval& bound(FCL_REAL v)
{
if(v < i_[0]) i_[0] = v;
if(v > i_[1]) i_[1] = v;
return *this;
}
/// @brief Compute the minimum interval contains other and original interval
inline Interval& bound(const Interval& other)
{
if(other.i_[0] < i_[0]) i_[0] = other.i_[0];
if(other.i_[1] > i_[1]) i_[1] = other.i_[1];
return *this;
}
void print() const;
inline FCL_REAL center() const { return 0.5 * (i_[0] + i_[1]); }
inline FCL_REAL diameter() const { return i_[1] -i_[0]; }
};
Interval bound(const Interval& i, FCL_REAL v);
Interval bound(const Interval& i, const Interval& other);
}
#endif
include/fcl/ccd/motion.h deleted 100644 → 0
View file @ 12194e71
/*
* 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_CCD_MOTION_H
#define FCL_CCD_MOTION_H
#include "fcl/ccd/motion_base.h"
#include "fcl/intersect.h"
#include <iostream>
#include <vector>
namespace fcl
{
class TranslationMotion : public MotionBase
{
public:
/// @brief Construct motion from intial and goal transform
TranslationMotion(const Transform3f& tf1,
const Transform3f& tf2) : MotionBase(),
rot(tf1.getQuatRotation()),
trans_start(tf1.getTranslation()),
trans_range(tf2.getTranslation() - tf1.getTranslation())
{
}
TranslationMotion(const Matrix3f& R, const Vec3f& T1, const Vec3f& T2) : MotionBase()
{
rot.fromRotation(R);
trans_start = T1;
trans_range = T2 - T1;
}
bool integrate(FCL_REAL dt) const
{
if(dt > 1) dt = 1;
tf = Transform3f(rot, trans_start + trans_range * dt);
return true;
}
FCL_REAL computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const
{
return mb_visitor.visit(*this);
}
FCL_REAL computeMotionBound(const TriangleMotionBoundVisitor& mb_visitor) const
{
return mb_visitor.visit(*this);
}
void getCurrentTransform(Transform3f& tf_) const
{
tf_ = tf;
}
void getTaylorModel(TMatrix3& tm, TVector3& tv) const
{
}
Vec3f getVelocity() const
{
return trans_range;
}
private:
/// @brief initial and goal transforms
Quaternion3f rot;
Vec3f trans_start, trans_range;
mutable Transform3f tf;
};
class SplineMotion : public MotionBase
{
public:
/// @brief Construct motion from 4 deBoor points
SplineMotion(const Vec3f& Td0, const Vec3f& Td1, const Vec3f& Td2, const Vec3f& Td3,
const Vec3f& Rd0, const Vec3f& Rd1, const Vec3f& Rd2, const Vec3f& Rd3);
// @brief Construct motion from initial and goal transform
SplineMotion(const Matrix3f& R1, const Vec3f& T1,
const Matrix3f& R2, const Vec3f& T2) : MotionBase()
{
// TODO
}
SplineMotion(const Transform3f& tf1,
const Transform3f& tf2) : MotionBase()
{
// TODO
}
/// @brief Integrate the motion from 0 to dt
/// We compute the current transformation from zero point instead of from last integrate time, for precision.
bool integrate(double dt) const;
/// @brief Compute the motion bound for a bounding volume along a given direction n, which is defined in the visitor
FCL_REAL computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const
{
return mb_visitor.visit(*this);
}
/// @brief Compute the motion bound for a triangle along a given direction n, which is defined in the visitor
FCL_REAL computeMotionBound(const TriangleMotionBoundVisitor& mb_visitor) const
{
return mb_visitor.visit(*this);
}
/// @brief Get the rotation and translation in current step
void getCurrentTransform(Transform3f& tf_) const
{
tf_ = tf;
}
void getTaylorModel(TMatrix3& tm, TVector3& tv) const
{
// set tv
Vec3f c[4];
c[0] = (Td[0] + Td[1] * 4 + Td[2] + Td[3]) * (1/6.0);
c[1] = (-Td[0] + Td[2]) * (1/2.0);
c[2] = (Td[0] - Td[1] * 2 + Td[2]) * (1/2.0);
c[3] = (-Td[0] + Td[1] * 3 - Td[2] * 3 + Td[3]) * (1/6.0);
tv.setTimeInterval(getTimeInterval());
for(std::size_t i = 0; i < 3; ++i)
{
for(std::size_t j = 0; j < 4; ++j)
{
tv[i].coeff(j) = c[j][i];
}
}
// set tm
Matrix3f I(1, 0, 0, 0, 1, 0, 0, 0, 1);
// R(t) = R(t0) + R'(t0) (t-t0) + 1/2 R''(t0)(t-t0)^2 + 1 / 6 R'''(t0) (t-t0)^3 + 1 / 24 R''''(l)(t-t0)^4; t0 = 0.5
/// 1. compute M(1/2)
Vec3f Rt0 = (Rd[0] + Rd[1] * 23 + Rd[2] * 23 + Rd[3]) * (1 / 48.0);
FCL_REAL Rt0_len = Rt0.length();
FCL_REAL inv_Rt0_len = 1.0 / Rt0_len;
FCL_REAL inv_Rt0_len_3 = inv_Rt0_len * inv_Rt0_len * inv_Rt0_len;
FCL_REAL inv_Rt0_len_5 = inv_Rt0_len_3 * inv_Rt0_len * inv_Rt0_len;
FCL_REAL theta0 = Rt0_len;
FCL_REAL costheta0 = cos(theta0);
FCL_REAL sintheta0 = sin(theta0);
Vec3f Wt0 = Rt0 * inv_Rt0_len;
Matrix3f hatWt0;
hat(hatWt0, Wt0);
Matrix3f hatWt0_sqr = hatWt0 * hatWt0;
Matrix3f Mt0 = I + hatWt0 * sintheta0 + hatWt0_sqr * (1 - costheta0);
/// 2. compute M'(1/2)
Vec3f dRt0 = (-Rd[0] - Rd[1] * 5 + Rd[2] * 5 + Rd[3]) * (1 / 8.0);
FCL_REAL Rt0_dot_dRt0 = Rt0.dot(dRt0);
FCL_REAL dtheta0 = Rt0_dot_dRt0 * inv_Rt0_len;
Vec3f dWt0 = dRt0 * inv_Rt0_len - Rt0 * (Rt0_dot_dRt0 * inv_Rt0_len_3);
Matrix3f hatdWt0;
hat(hatdWt0, dWt0);
Matrix3f dMt0 = hatdWt0 * sintheta0 + hatWt0 * (costheta0 * dtheta0) + hatWt0_sqr * (sintheta0 * dtheta0) + (hatWt0 * hatdWt0 + hatdWt0 * hatWt0) * (1 - costheta0);
/// 3.1. compute M''(1/2)
Vec3f ddRt0 = (Rd[0] - Rd[1] - Rd[2] + Rd[3]) * 0.5;
FCL_REAL Rt0_dot_ddRt0 = Rt0.dot(ddRt0);
FCL_REAL dRt0_dot_dRt0 = dRt0.sqrLength();
FCL_REAL ddtheta0 = (Rt0_dot_ddRt0 + dRt0_dot_dRt0) * inv_Rt0_len - Rt0_dot_dRt0 * Rt0_dot_dRt0 * inv_Rt0_len_3;
Vec3f ddWt0 = ddRt0 * inv_Rt0_len - (dRt0 * (2 * Rt0_dot_dRt0) + Rt0 * (Rt0_dot_ddRt0 + dRt0_dot_dRt0)) * inv_Rt0_len_3 + (Rt0 * (3 * Rt0_dot_dRt0 * Rt0_dot_dRt0)) * inv_Rt0_len_5;
Matrix3f hatddWt0;
hat(hatddWt0, ddWt0);
Matrix3f ddMt0 =
hatddWt0 * sintheta0 +
hatWt0 * (costheta0 * dtheta0 - sintheta0 * dtheta0 * dtheta0 + costheta0 * ddtheta0) +
hatdWt0 * (costheta0 * dtheta0) +
(hatWt0 * hatdWt0 + hatdWt0 * hatWt0) * (sintheta0 * dtheta0 * 2) +
hatdWt0 * hatdWt0 * (2 * (1 - costheta0)) +
hatWt0 * hatWt0 * (costheta0 * dtheta0 * dtheta0 + sintheta0 * ddtheta0) +
(hatWt0 * hatddWt0 + hatddWt0 + hatWt0) * (1 - costheta0);
tm.setTimeInterval(getTimeInterval());
for(std::size_t i = 0; i < 3; ++i)
{
for(std::size_t j = 0; j < 3; ++j)
{
tm(i, j).coeff(0) = Mt0(i, j) - dMt0(i, j) * 0.5 + ddMt0(i, j) * 0.25 * 0.5;
tm(i, j).coeff(1) = dMt0(i, j) - ddMt0(i, j) * 0.5;
tm(i, j).coeff(2) = ddMt0(i, j) * 0.5;
tm(i, j).coeff(3) = 0;
tm(i, j).remainder() = Interval(-1/48.0, 1/48.0); /// not correct, should fix
}
}
}
protected:
void computeSplineParameter()
{
}
FCL_REAL getWeight0(FCL_REAL t) const;
FCL_REAL getWeight1(FCL_REAL t) const;
FCL_REAL getWeight2(FCL_REAL t) const;
FCL_REAL getWeight3(FCL_REAL t) const;
Vec3f Td[4];
Vec3f Rd[4];
Vec3f TA, TB, TC;
Vec3f RA, RB, RC;
FCL_REAL Rd0Rd0, Rd0Rd1, Rd0Rd2, Rd0Rd3, Rd1Rd1, Rd1Rd2, Rd1Rd3, Rd2Rd2, Rd2Rd3, Rd3Rd3;
//// @brief The transformation at current time t
mutable Transform3f tf;
/// @brief The time related with tf
mutable FCL_REAL tf_t;
public:
FCL_REAL computeTBound(const Vec3f& n) const;
FCL_REAL computeDWMax() const;
FCL_REAL getCurrentTime() const
{
return tf_t;
}
};
class ScrewMotion : public MotionBase
{
public:
/// @brief Default transformations are all identities
ScrewMotion() : MotionBase()
{
// Default angular velocity is zero
axis.setValue(1, 0, 0);
angular_vel = 0;
// Default reference point is local zero point
// Default linear velocity is zero
linear_vel = 0;
}
/// @brief Construct motion from the initial rotation/translation and goal rotation/translation
ScrewMotion(const Matrix3f& R1, const Vec3f& T1,
const Matrix3f& R2, const Vec3f& T2) : MotionBase(),
tf1(R1, T1),
tf2(R2, T2),
tf(tf1)
{
computeScrewParameter();
}
/// @brief Construct motion from the initial transform and goal transform
ScrewMotion(const Transform3f& tf1_,
const Transform3f& tf2_) : tf1(tf1_),
tf2(tf2_),
tf(tf1)
{
computeScrewParameter();
}
/// @brief Integrate the motion from 0 to dt
/// We compute the current transformation from zero point instead of from last integrate time, for precision.
bool integrate(double dt) const
{
if(dt > 1) dt = 1;
tf.setQuatRotation(absoluteRotation(dt));
Quaternion3f delta_rot = deltaRotation(dt);
tf.setTranslation(p + axis * (dt * linear_vel) + delta_rot.transform(tf1.getTranslation() - p));
return true;
}
/// @brief Compute the motion bound for a bounding volume along a given direction n, which is defined in the visitor
FCL_REAL computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const
{
return mb_visitor.visit(*this);
}
/// @brief Compute the motion bound for a triangle along a given direction n, which is defined in the visitor
FCL_REAL computeMotionBound(const TriangleMotionBoundVisitor& mb_visitor) const
{
return mb_visitor.visit(*this);
}
/// @brief Get the rotation and translation in current step
void getCurrentTransform(Transform3f& tf_) const
{
tf_ = tf;
}
void getTaylorModel(TMatrix3& tm, TVector3& tv) const
{
Matrix3f hat_axis;
hat(hat_axis, axis);
TaylorModel cos_model(getTimeInterval());
generateTaylorModelForCosFunc(cos_model, angular_vel, 0);
TaylorModel sin_model(getTimeInterval());
generateTaylorModelForSinFunc(sin_model, angular_vel, 0);
TMatrix3 delta_R = hat_axis * sin_model - hat_axis * hat_axis * (cos_model - 1) + Matrix3f(1, 0, 0, 0, 1, 0, 0, 0, 1);
TaylorModel a(getTimeInterval()), b(getTimeInterval()), c(getTimeInterval());
generateTaylorModelForLinearFunc(a, 0, linear_vel * axis[0]);
generateTaylorModelForLinearFunc(b, 0, linear_vel * axis[1]);
generateTaylorModelForLinearFunc(c, 0, linear_vel * axis[2]);
TVector3 delta_T = p - delta_R * p + TVector3(a, b, c);
tm = delta_R * tf1.getRotation();
tv = delta_R * tf1.getTranslation() + delta_T;
}
protected:
void computeScrewParameter()
{
Quaternion3f deltaq = tf2.getQuatRotation() * inverse(tf1.getQuatRotation());
deltaq.toAxisAngle(axis, angular_vel);
if(angular_vel < 0)
{
angular_vel = -angular_vel;
axis = -axis;
}
if(angular_vel < 1e-10)
{
angular_vel = 0;
axis = tf2.getTranslation() - tf1.getTranslation();
linear_vel = axis.length();
p = tf1.getTranslation();
}
else
{
Vec3f o = tf2.getTranslation() - tf1.getTranslation();
p = (tf1.getTranslation() + tf2.getTranslation() + axis.cross(o) * (1.0 / tan(angular_vel / 2.0))) * 0.5;
linear_vel = o.dot(axis);
}
}
Quaternion3f deltaRotation(FCL_REAL dt) const
{
Quaternion3f res;
res.fromAxisAngle(axis, (FCL_REAL)(dt * angular_vel));
return res;
}
Quaternion3f absoluteRotation(FCL_REAL dt) const
{
Quaternion3f delta_t = deltaRotation(dt);
return delta_t * tf1.getQuatRotation();
}
/// @brief The transformation at time 0
Transform3f tf1;
/// @brief The transformation at time 1
Transform3f tf2;
/// @brief The transformation at current time t
mutable Transform3f tf;
/// @brief screw axis
Vec3f axis;
/// @brief A point on the axis S
Vec3f p;
/// @brief linear velocity along the axis
FCL_REAL linear_vel;
/// @brief angular velocity
FCL_REAL angular_vel;
public:
inline FCL_REAL getLinearVelocity() const
{
return linear_vel;
}
inline FCL_REAL getAngularVelocity() const
{
return angular_vel;
}
inline const Vec3f& getAxis() const
{
return axis;
}
inline const Vec3f& getAxisOrigin() const
{
return p;
}
};
/// @brief Linear interpolation motion
/// Each Motion is assumed to have constant linear velocity and angular velocity
/// The motion is R(t)(p - p_ref) + p_ref + T(t)
/// Therefore, R(0) = R0, R(1) = R1
/// T(0) = T0 + R0 p_ref - p_ref
/// T(1) = T1 + R1 p_ref - p_ref
class InterpMotion : public MotionBase
{
public:
/// @brief Default transformations are all identities
InterpMotion();
/// @brief Construct motion from the initial rotation/translation and goal rotation/translation
InterpMotion(const Matrix3f& R1, const Vec3f& T1,
const Matrix3f& R2, const Vec3f& T2);
InterpMotion(const Transform3f& tf1_, const Transform3f& tf2_);
/// @brief Construct motion from the initial rotation/translation and goal rotation/translation related to some rotation center
InterpMotion(const Matrix3f& R1, const Vec3f& T1,
const Matrix3f& R2, const Vec3f& T2,
const Vec3f& O);
InterpMotion(const Transform3f& tf1_, const Transform3f& tf2_, const Vec3f& O);
/// @brief Integrate the motion from 0 to dt
/// We compute the current transformation from zero point instead of from last integrate time, for precision.
bool integrate(double dt) const;
/// @brief Compute the motion bound for a bounding volume along a given direction n, which is defined in the visitor
FCL_REAL computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const
{
return mb_visitor.visit(*this);
}
/// @brief Compute the motion bound for a triangle along a given direction n, which is defined in the visitor
FCL_REAL computeMotionBound(const TriangleMotionBoundVisitor& mb_visitor) const
{
return mb_visitor.visit(*this);
}
/// @brief Get the rotation and translation in current step
void getCurrentTransform(Transform3f& tf_) const
{
tf_ = tf;
}
void getTaylorModel(TMatrix3& tm, TVector3& tv) const
{
Matrix3f hat_angular_axis;
hat(hat_angular_axis, angular_axis);
TaylorModel cos_model(getTimeInterval());
generateTaylorModelForCosFunc(cos_model, angular_vel, 0);
TaylorModel sin_model(getTimeInterval());
generateTaylorModelForSinFunc(sin_model, angular_vel, 0);
TMatrix3 delta_R = hat_angular_axis * sin_model - hat_angular_axis * hat_angular_axis * (cos_model - 1) + Matrix3f(1, 0, 0, 0, 1, 0, 0, 0, 1);
TaylorModel a(getTimeInterval()), b(getTimeInterval()), c(getTimeInterval());
generateTaylorModelForLinearFunc(a, 0, linear_vel[0]);
generateTaylorModelForLinearFunc(b, 0, linear_vel[1]);
generateTaylorModelForLinearFunc(c, 0, linear_vel[2]);
TVector3 delta_T(a, b, c);
tm = delta_R * tf1.getRotation();
tv = tf1.transform(reference_p) + delta_T - delta_R * tf1.getQuatRotation().transform(reference_p);
}
protected:
void computeVelocity();
Quaternion3f deltaRotation(FCL_REAL dt) const;
Quaternion3f absoluteRotation(FCL_REAL dt) const;
/// @brief The transformation at time 0
Transform3f tf1;
/// @brief The transformation at time 1
Transform3f tf2;
/// @brief The transformation at current time t
mutable Transform3f tf;
/// @brief Linear velocity
Vec3f linear_vel;
/// @brief Angular speed
FCL_REAL angular_vel;
/// @brief Angular velocity axis
Vec3f angular_axis;
/// @brief Reference point for the motion (in the object's local frame)
Vec3f reference_p;
public:
const Vec3f& getReferencePoint() const
{
return reference_p;
}
const Vec3f& getAngularAxis() const
{
return angular_axis;
}
FCL_REAL getAngularVelocity() const
{
return angular_vel;
}
const Vec3f& getLinearVelocity() const
{
return linear_vel;
}
};
}
#endif
include/fcl/ccd/motion_base.h deleted 100644 → 0
View file @ 12194e71
/*
* 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_CCD_MOTION_BASE_H
#define FCL_CCD_MOTION_BASE_H
#include "fcl/math/transform.h"
#include "fcl/ccd/taylor_matrix.h"
#include "fcl/ccd/taylor_vector.h"
#include "fcl/BV/RSS.h"
namespace fcl
{
class MotionBase;
class SplineMotion;
class ScrewMotion;
class InterpMotion;
class TranslationMotion;
/// @brief Compute the motion bound for a bounding volume, given the closest direction n between two query objects
class BVMotionBoundVisitor
{
public:
virtual FCL_REAL visit(const MotionBase& motion) const = 0;
virtual FCL_REAL visit(const SplineMotion& motion) const = 0;
virtual FCL_REAL visit(const ScrewMotion& motion) const = 0;
virtual FCL_REAL visit(const InterpMotion& motion) const = 0;
virtual FCL_REAL visit(const TranslationMotion& motion) const = 0;
};
template<typename BV>
class TBVMotionBoundVisitor : public BVMotionBoundVisitor
{
public:
TBVMotionBoundVisitor(const BV& bv_, const Vec3f& n_) : bv(bv_), n(n_) {}
virtual FCL_REAL visit(const MotionBase& motion) const { return 0; }
virtual FCL_REAL visit(const SplineMotion& motion) const { return 0; }
virtual FCL_REAL visit(const ScrewMotion& motion) const { return 0; }
virtual FCL_REAL visit(const InterpMotion& motion) const { return 0; }
virtual FCL_REAL visit(const TranslationMotion& motion) const { return 0; }
protected:
BV bv;
Vec3f n;
};
template<>
FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const SplineMotion& motion) const;
template<>
FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const ScrewMotion& motion) const;
template<>
FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const InterpMotion& motion) const;
template<>
FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const TranslationMotion& motion) const;
class TriangleMotionBoundVisitor
{
public:
TriangleMotionBoundVisitor(const Vec3f& a_, const Vec3f& b_, const Vec3f& c_, const Vec3f& n_) :
a(a_), b(b_), c(c_), n(n_) {}
virtual FCL_REAL visit(const MotionBase& motion) const { return 0; }
virtual FCL_REAL visit(const SplineMotion& motion) const;
virtual FCL_REAL visit(const ScrewMotion& motion) const;
virtual FCL_REAL visit(const InterpMotion& motion) const;
virtual FCL_REAL visit(const TranslationMotion& motion) const;
protected:
Vec3f a, b, c, n;
};
class MotionBase
{
public:
MotionBase() : time_interval_(boost::shared_ptr<TimeInterval>(new TimeInterval(0, 1)))
{
}
virtual ~MotionBase() {}
/** \brief Integrate the motion from 0 to dt */
virtual bool integrate(double dt) const = 0;
/** \brief Compute the motion bound for a bounding volume, given the closest direction n between two query objects */
virtual FCL_REAL computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const = 0;
/** \brief Compute the motion bound for a triangle, given the closest direction n between two query objects */
virtual FCL_REAL computeMotionBound(const TriangleMotionBoundVisitor& mb_visitor) const = 0;
/** \brief Get the rotation and translation in current step */
void getCurrentTransform(Matrix3f& R, Vec3f& T) const
{
Transform3f tf;
getCurrentTransform(tf);
R = tf.getRotation();
T = tf.getTranslation();
}
void getCurrentTransform(Quaternion3f& Q, Vec3f& T) const
{
Transform3f tf;
getCurrentTransform(tf);
Q = tf.getQuatRotation();
T = tf.getTranslation();
}
void getCurrentRotation(Matrix3f& R) const
{
Transform3f tf;
getCurrentTransform(tf);
R = tf.getRotation();
}
void getCurrentRotation(Quaternion3f& Q) const
{
Transform3f tf;
getCurrentTransform(tf);
Q = tf.getQuatRotation();
}
void getCurrentTranslation(Vec3f& T) const
{
Transform3f tf;
getCurrentTransform(tf);
T = tf.getTranslation();
}
virtual void getCurrentTransform(Transform3f& tf) const = 0;
virtual void getTaylorModel(TMatrix3& tm, TVector3& tv) const = 0;
const boost::shared_ptr<TimeInterval>& getTimeInterval() const
{
return time_interval_;
}
protected:
boost::shared_ptr<TimeInterval> time_interval_;
};
}
#endif
include/fcl/ccd/taylor_model.h deleted 100644 → 0
View file @ 12194e71
/*
* 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_CCD_TAYLOR_MODEL_H
#define FCL_CCD_TAYLOR_MODEL_H
#include "fcl/ccd/interval.h"
#include <boost/shared_ptr.hpp>
namespace fcl
{
struct TimeInterval
{
/// @brief time interval and different powers
Interval t_; // [t1, t2]
Interval t2_; // [t1, t2]^2
Interval t3_; // [t1, t2]^3
Interval t4_; // [t1, t2]^4
Interval t5_; // [t1, t2]^5
Interval t6_; // [t1, t2]^6
TimeInterval() {}
TimeInterval(FCL_REAL l, FCL_REAL r)
{
setValue(l, r);
}
void setValue(FCL_REAL l, FCL_REAL r)
{
t_.setValue(l, r);
t2_.setValue(l * t_[0], r * t_[1]);
t3_.setValue(l * t2_[0], r * t2_[1]);
t4_.setValue(l * t3_[0], r * t3_[1]);
t5_.setValue(l * t4_[0], r * t4_[1]);
t6_.setValue(l * t5_[0], r * t5_[1]);
}
};
/// @brief TaylorModel implements a third order Taylor model, i.e., a cubic approximation of a function
/// over a time interval, with an interval remainder.
/// All the operations on two Taylor models assume their time intervals are the same.
class TaylorModel
{
/// @brief time interval
boost::shared_ptr<TimeInterval> time_interval_;
/// @brief Coefficients of the cubic polynomial approximation
FCL_REAL coeffs_[4];
/// @brief interval remainder
Interval r_;
public:
void setTimeInterval(FCL_REAL l, FCL_REAL r)
{
time_interval_->setValue(l, r);
}
void setTimeInterval(const boost::shared_ptr<TimeInterval>& time_interval)
{
time_interval_ = time_interval;
}
const boost::shared_ptr<TimeInterval>& getTimeInterval() const
{
return time_interval_;
}
FCL_REAL coeff(std::size_t i) const { return coeffs_[i]; }
FCL_REAL& coeff(std::size_t i) { return coeffs_[i]; }
const Interval& remainder() const { return r_; }
Interval& remainder() { return r_; }
TaylorModel();
TaylorModel(const boost::shared_ptr<TimeInterval>& time_interval);
TaylorModel(FCL_REAL coeff, const boost::shared_ptr<TimeInterval>& time_interval);
TaylorModel(FCL_REAL coeffs[3], const Interval& r, const boost::shared_ptr<TimeInterval>& time_interval);
TaylorModel(FCL_REAL c0, FCL_REAL c1, FCL_REAL c2, FCL_REAL c3, const Interval& r, const boost::shared_ptr<TimeInterval>& time_interval);
TaylorModel operator + (const TaylorModel& other) const;
TaylorModel& operator += (const TaylorModel& other);
TaylorModel operator - (const TaylorModel& other) const;
TaylorModel& operator -= (const TaylorModel& other);
TaylorModel operator + (FCL_REAL d) const;
TaylorModel& operator += (FCL_REAL d);
TaylorModel operator - (FCL_REAL d) const;
TaylorModel& operator -= (FCL_REAL d);
TaylorModel operator * (const TaylorModel& other) const;
TaylorModel operator * (FCL_REAL d) const;
TaylorModel& operator *= (const TaylorModel& other);
TaylorModel& operator *= (FCL_REAL d);
TaylorModel operator - () const;
void print() const;
Interval getBound() const;
Interval getBound(FCL_REAL l, FCL_REAL r) const;
Interval getTightBound() const;
Interval getTightBound(FCL_REAL l, FCL_REAL r) const;
Interval getBound(FCL_REAL t) const;
void setZero();
};
TaylorModel operator * (FCL_REAL d, const TaylorModel& a);
TaylorModel operator + (FCL_REAL d, const TaylorModel& a);
TaylorModel operator - (FCL_REAL d, const TaylorModel& a);
/// @brief Generate Taylor model for cos(w t + q0)
void generateTaylorModelForCosFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0);
/// @brief Generate Taylor model for sin(w t + q0)
void generateTaylorModelForSinFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0);
/// @brief Generate Taylor model for p + v t
void generateTaylorModelForLinearFunc(TaylorModel& tm, FCL_REAL p, FCL_REAL v);
}
#endif
include/fcl/collision_data.h deleted 100644 → 0
View file @ 12194e71
/*
* 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_COLLISION_DATA_H
#define FCL_COLLISION_DATA_H
#include "fcl/collision_object.h"
#include "fcl/learning/classifier.h"
#include "fcl/knn/nearest_neighbors.h"
#include "fcl/math/vec_3f.h"
#include <vector>
#include <set>
#include <limits>
namespace fcl
{
/// @brief Type of narrow phase GJK solver
enum GJKSolverType {GST_LIBCCD, GST_INDEP};
/// @brief Contact information returned by collision
struct Contact
{
/// @brief collision object 1
const CollisionGeometry* o1;
/// @brief collision object 2
const CollisionGeometry* o2;
/// @brief contact primitive in object 1
/// if object 1 is mesh or point cloud, it is the triangle or point id
/// if object 1 is geometry shape, it is NONE (-1),
/// if object 1 is octree, it is the id of the cell
int b1;
/// @brief contact primitive in object 2
/// if object 2 is mesh or point cloud, it is the triangle or point id
/// if object 2 is geometry shape, it is NONE (-1),
/// if object 2 is octree, it is the id of the cell
int b2;
/// @brief contact normal, pointing from o1 to o2
Vec3f normal;
/// @brief contact position, in world space
Vec3f pos;
/// @brief penetration depth
FCL_REAL penetration_depth;
/// @brief invalid contact primitive information
static const int NONE = -1;
Contact() : o1(NULL),
o2(NULL),
b1(NONE),
b2(NONE)
{}
Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_) : o1(o1_),
o2(o2_),
b1(b1_),
b2(b2_)
{}
Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_,
const Vec3f& pos_, const Vec3f& normal_, FCL_REAL depth_) : o1(o1_),
o2(o2_),
b1(b1_),
b2(b2_),
normal(normal_),
pos(pos_),
penetration_depth(depth_)
{}
bool operator < (const Contact& other) const
{
if(b1 == other.b1)
return b2 < other.b2;
return b1 < other.b1;
}
};
/// @brief Cost source describes an area with a cost. The area is described by an AABB region.
struct CostSource
{
/// @brief aabb lower bound
Vec3f aabb_min;
/// @brief aabb upper bound
Vec3f aabb_max;
/// @brief cost density in the AABB region
FCL_REAL cost_density;
FCL_REAL total_cost;
CostSource(const Vec3f& aabb_min_, const Vec3f& aabb_max_, FCL_REAL cost_density_) : aabb_min(aabb_min_),
aabb_max(aabb_max_),
cost_density(cost_density_)
{
total_cost = cost_density * (aabb_max[0] - aabb_min[0]) * (aabb_max[1] - aabb_min[1]) * (aabb_max[2] - aabb_min[2]);
}
CostSource(const AABB& aabb, FCL_REAL cost_density_) : aabb_min(aabb.min_),
aabb_max(aabb.max_),
cost_density(cost_density_)
{
total_cost = cost_density * (aabb_max[0] - aabb_min[0]) * (aabb_max[1] - aabb_min[1]) * (aabb_max[2] - aabb_min[2]);
}
CostSource() {}
bool operator < (const CostSource& other) const
{
if(total_cost < other.total_cost)
return false;
if(total_cost > other.total_cost)
return true;
if(cost_density < other.cost_density)
return false;
if(cost_density > other.cost_density)
return true;
for(size_t i = 0; i < 3; ++i)
if(aabb_min[i] != other.aabb_min[i])
return aabb_min[i] < other.aabb_min[i];
return false;
}
};
struct CollisionResult;
/// @brief request to the collision algorithm
struct CollisionRequest
{
/// @brief The maximum number of contacts will return
size_t num_max_contacts;
/// @brief whether the contact information (normal, penetration depth and contact position) will return
bool enable_contact;
/// @brief The maximum number of cost sources will return
size_t num_max_cost_sources;
/// @brief whether the cost sources will be computed
bool enable_cost;
/// @brief whether the cost computation is approximated
bool use_approximate_cost;
/// @brief narrow phase solver
GJKSolverType gjk_solver_type;
/// @brief whether enable gjk intial guess
bool enable_cached_gjk_guess;
/// @brief the gjk intial guess set by user
Vec3f cached_gjk_guess;
CollisionRequest(size_t num_max_contacts_ = 1,
bool enable_contact_ = false,
size_t num_max_cost_sources_ = 1,
bool enable_cost_ = false,
bool use_approximate_cost_ = true,
GJKSolverType gjk_solver_type_ = GST_LIBCCD) : num_max_contacts(num_max_contacts_),
enable_contact(enable_contact_),
num_max_cost_sources(num_max_cost_sources_),
enable_cost(enable_cost_),
use_approximate_cost(use_approximate_cost_),
gjk_solver_type(gjk_solver_type_)
{
enable_cached_gjk_guess = false;
cached_gjk_guess = Vec3f(1, 0, 0);
}
bool isSatisfied(const CollisionResult& result) const;
};
/// @brief collision result
struct CollisionResult
{
private:
/// @brief contact information
std::vector<Contact> contacts;
/// @brief cost sources
std::set<CostSource> cost_sources;
public:
Vec3f cached_gjk_guess;
public:
CollisionResult()
{
}
/// @brief add one contact into result structure
inline void addContact(const Contact& c)
{
contacts.push_back(c);
}
/// @brief add one cost source into result structure
inline void addCostSource(const CostSource& c, std::size_t num_max_cost_sources)
{
cost_sources.insert(c);
while (cost_sources.size() > num_max_cost_sources)
cost_sources.erase(--cost_sources.end());
}
/// @brief return binary collision result
bool isCollision() const
{
return contacts.size() > 0;
}
/// @brief number of contacts found
size_t numContacts() const
{
return contacts.size();
}
/// @brief number of cost sources found
size_t numCostSources() const
{
return cost_sources.size();
}
/// @brief get the i-th contact calculated
const Contact& getContact(size_t i) const
{
if(i < contacts.size())
return contacts[i];
else
return contacts.back();
}
/// @brief get all the contacts
void getContacts(std::vector<Contact>& contacts_)
{
contacts_.resize(contacts.size());
std::copy(contacts.begin(), contacts.end(), contacts_.begin());
}
/// @brief get all the cost sources
void getCostSources(std::vector<CostSource>& cost_sources_)
{
cost_sources_.resize(cost_sources.size());
std::copy(cost_sources.begin(), cost_sources.end(), cost_sources_.begin());
}
/// @brief clear the results obtained
void clear()
{
contacts.clear();
cost_sources.clear();
}
};
struct DistanceResult;
/// @brief request to the distance computation
struct DistanceRequest
{
/// @brief whether to return the nearest points
bool enable_nearest_points;
/// @brief error threshold for approximate distance
FCL_REAL rel_err; // relative error, between 0 and 1
FCL_REAL abs_err; // absoluate error
/// @brief narrow phase solver type
GJKSolverType gjk_solver_type;
DistanceRequest(bool enable_nearest_points_ = false,
FCL_REAL rel_err_ = 0.0,
FCL_REAL abs_err_ = 0.0,
GJKSolverType gjk_solver_type_ = GST_LIBCCD) : enable_nearest_points(enable_nearest_points_),
rel_err(rel_err_),
abs_err(abs_err_),
gjk_solver_type(gjk_solver_type_)
{
}
bool isSatisfied(const DistanceResult& result) const;
};
/// @brief distance result
struct DistanceResult
{
public:
/// @brief minimum distance between two objects. if two objects are in collision, min_distance <= 0.
FCL_REAL min_distance;
/// @brief nearest points
Vec3f nearest_points[2];
/// @brief collision object 1
const CollisionGeometry* o1;
/// @brief collision object 2
const CollisionGeometry* o2;
/// @brief information about the nearest point in object 1
/// if object 1 is mesh or point cloud, it is the triangle or point id
/// if object 1 is geometry shape, it is NONE (-1),
/// if object 1 is octree, it is the id of the cell
int b1;
/// @brief information about the nearest point in object 2
/// if object 2 is mesh or point cloud, it is the triangle or point id
/// if object 2 is geometry shape, it is NONE (-1),
/// if object 2 is octree, it is the id of the cell
int b2;
/// @brief invalid contact primitive information
static const int NONE = -1;
DistanceResult(FCL_REAL min_distance_ = std::numeric_limits<FCL_REAL>::max()) : min_distance(min_distance_),
o1(NULL),
o2(NULL),
b1(NONE),
b2(NONE)
{
}
/// @brief add distance information into the result
void update(FCL_REAL distance, const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_)
{
if(min_distance > distance)
{
min_distance = distance;
o1 = o1_;
o2 = o2_;
b1 = b1_;
b2 = b2_;
}
}
/// @brief add distance information into the result
void update(FCL_REAL distance, const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_, const Vec3f& p1, const Vec3f& p2)
{
if(min_distance > distance)
{
min_distance = distance;
o1 = o1_;
o2 = o2_;
b1 = b1_;
b2 = b2_;
nearest_points[0] = p1;
nearest_points[1] = p2;
}
}
/// @brief add distance information into the result
void update(const DistanceResult& other_result)
{
if(min_distance > other_result.min_distance)
{
min_distance = other_result.min_distance;
o1 = other_result.o1;
o2 = other_result.o2;
b1 = other_result.b1;
b2 = other_result.b2;
nearest_points[0] = other_result.nearest_points[0];
nearest_points[1] = other_result.nearest_points[1];
}
}
/// @brief clear the result
void clear()
{
min_distance = std::numeric_limits<FCL_REAL>::max();
o1 = NULL;
o2 = NULL;
b1 = NONE;
b2 = NONE;
}
};
enum CCDMotionType {CCDM_TRANS, CCDM_LINEAR, CCDM_SCREW, CCDM_SPLINE};
enum CCDSolverType {CCDC_NAIVE, CCDC_CONSERVATIVE_ADVANCEMENT, CCDC_RAY_SHOOTING, CCDC_POLYNOMIAL_SOLVER};
struct ContinuousCollisionRequest
{
/// @brief maximum num of iterations
std::size_t num_max_iterations;
/// @brief error in first contact time
FCL_REAL toc_err;
/// @brief ccd motion type
CCDMotionType ccd_motion_type;
/// @brief gjk solver type
GJKSolverType gjk_solver_type;
/// @brief ccd solver type
CCDSolverType ccd_solver_type;
ContinuousCollisionRequest(std::size_t num_max_iterations_ = 10,
FCL_REAL toc_err_ = 0.0001,
CCDMotionType ccd_motion_type_ = CCDM_TRANS,
GJKSolverType gjk_solver_type_ = GST_LIBCCD,
CCDSolverType ccd_solver_type_ = CCDC_NAIVE) : num_max_iterations(num_max_iterations_),
toc_err(toc_err_),
ccd_motion_type(ccd_motion_type_),
gjk_solver_type(gjk_solver_type_),
ccd_solver_type(ccd_solver_type_)
{
}
};
/// @brief continuous collision result
struct ContinuousCollisionResult
{
/// @brief collision or not
bool is_collide;
/// @brief time of contact in [0, 1]
FCL_REAL time_of_contact;
Transform3f contact_tf1, contact_tf2;
ContinuousCollisionResult() : is_collide(false), time_of_contact(1.0)
{
}
};
enum PenetrationDepthType {PDT_TRANSLATIONAL, PDT_GENERAL_EULER, PDT_GENERAL_QUAT, PDT_GENERAL_EULER_BALL, PDT_GENERAL_QUAT_BALL};
enum KNNSolverType {KNN_LINEAR, KNN_GNAT, KNN_SQRTAPPROX};
struct PenetrationDepthRequest
{
void* classifier;
NearestNeighbors<Transform3f>::DistanceFunction distance_func;
/// @brief KNN solver type
KNNSolverType knn_solver_type;
/// @brief PD algorithm type
PenetrationDepthType pd_type;
/// @brief gjk solver type
GJKSolverType gjk_solver_type;
std::vector<Transform3f> contact_vectors;
PenetrationDepthRequest(void* classifier_,
NearestNeighbors<Transform3f>::DistanceFunction distance_func_,
KNNSolverType knn_solver_type_ = KNN_LINEAR,
PenetrationDepthType pd_type_ = PDT_TRANSLATIONAL,
GJKSolverType gjk_solver_type_ = GST_LIBCCD) : classifier(classifier_),
distance_func(distance_func_),
knn_solver_type(knn_solver_type_),
pd_type(pd_type_),
gjk_solver_type(gjk_solver_type_)
{
}
};
struct PenetrationDepthResult
{
/// @brief penetration depth value
FCL_REAL pd_value;
/// @brief the transform where the collision is resolved
Transform3f resolved_tf;
};
}
#endif
include/fcl/continuous_collision.h deleted 100644 → 0
View file @ 12194e71
#ifndef FCL_CONTINUOUS_COLLISION_H
#define FCL_CONTINUOUS_COLLISION_H
#include "fcl/math/vec_3f.h"
#include "fcl/collision_object.h"
#include "fcl/collision_data.h"
namespace fcl
{
/// @brief continous collision checking between two objects
FCL_REAL continuousCollide(const CollisionGeometry* o1, const Transform3f& tf1_beg, const Transform3f& tf1_end,
const CollisionGeometry* o2, const Transform3f& tf2_beg, const Transform3f& tf2_end,
const ContinuousCollisionRequest& request,
ContinuousCollisionResult& result);
FCL_REAL continuousCollide(const CollisionObject* o1, const Transform3f& tf1_end,
const CollisionObject* o2, const Transform3f& tf2_end,
const ContinuousCollisionRequest& request,
ContinuousCollisionResult& result);
FCL_REAL collide(const ContinuousCollisionObject* o1, const ContinuousCollisionObject* o2,
const ContinuousCollisionRequest& request,
ContinuousCollisionResult& result);
}
#endif
include/fcl/exception.h deleted 100644 → 0
View file @ 12194e71
#ifndef FCL_EXCEPTION_H
#define FCL_EXCEPTION_H
#include <stdexcept>
#include <string>
namespace fcl
{
class Exception : public std::runtime_error
{
public:
/** \brief This is just a wrapper on std::runtime_error */
explicit
Exception(const std::string& what) : std::runtime_error(what)
{
}
/** \brief This is just a wrapper on std::runtime_error with a
prefix added */
Exception(const std::string &prefix, const std::string& what) : std::runtime_error(prefix + ": " + what)
{
}
virtual ~Exception(void) throw()
{
}
};
}
#endif
include/fcl/intersect.h deleted 100644 → 0
View file @ 12194e71
/*
* 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_INTERSECT_H
#define FCL_INTERSECT_H
#include "fcl/math/transform.h"
#include <boost/math/special_functions/erf.hpp>
namespace fcl
{
/// @brief A class solves polynomial degree (1,2,3) equations
class PolySolver
{
public:
/// @brief Solve a linear equation with coefficients c, return roots s and number of roots
static int solveLinear(FCL_REAL c[2], FCL_REAL s[1]);
/// @brief Solve a quadratic function with coefficients c, return roots s and number of roots
static int solveQuadric(FCL_REAL c[3], FCL_REAL s[2]);
/// @brief Solve a cubic function with coefficients c, return roots s and number of roots
static int solveCubic(FCL_REAL c[4], FCL_REAL s[3]);
private:
/// @brief Check whether v is zero
static inline bool isZero(FCL_REAL v);
/// @brief Compute v^{1/3}
static inline bool cbrt(FCL_REAL v);
static const FCL_REAL NEAR_ZERO_THRESHOLD;
};
/// @brief CCD intersect kernel among primitives
class Intersect
{
public:
/// @brief CCD intersect between one vertex and one face
/// [a0, b0, c0] and [a1, b1, c1] are points for the triangle face in time t0 and t1
/// p0 and p1 are points for vertex in time t0 and t1
/// p_i returns the coordinate of the collision point
static bool intersect_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1,
FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
/// @brief CCD intersect between two edges
/// [a0, b0] and [a1, b1] are points for one edge in time t0 and t1
/// [c0, d0] and [c1, d1] are points for the other edge in time t0 and t1
/// p_i returns the coordinate of the collision point
static bool intersect_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1,
FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
/// @brief CCD intersect between one vertex and one face, using additional filter
static bool intersect_VF_filtered(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1,
FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
/// @brief CCD intersect between two edges, using additional filter
static bool intersect_EE_filtered(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1,
FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
/// @brief CCD intersect between one vertex and and one edge
static bool intersect_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
const Vec3f& a1, const Vec3f& b1, const Vec3f& p1,
const Vec3f& L);
/// @brief CD intersect between two triangles [P1, P2, P3] and [Q1, Q2, Q3]
static bool intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
Vec3f* contact_points = NULL,
unsigned int* num_contact_points = NULL,
FCL_REAL* penetration_depth = NULL,
Vec3f* normal = NULL);
static bool intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
const Matrix3f& R, const Vec3f& T,
Vec3f* contact_points = NULL,
unsigned int* num_contact_points = NULL,
FCL_REAL* penetration_depth = NULL,
Vec3f* normal = NULL);
static bool intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
const Transform3f& tf,
Vec3f* contact_points = NULL,
unsigned int* num_contact_points = NULL,
FCL_REAL* penetration_depth = NULL,
Vec3f* normal = NULL);
private:
/// @brief Project function used in intersect_Triangle()
static int project6(const Vec3f& ax,
const Vec3f& p1, const Vec3f& p2, const Vec3f& p3,
const Vec3f& q1, const Vec3f& q2, const Vec3f& q3);
/// @brief Check whether one value is zero
static inline bool isZero(FCL_REAL v);
/// @brief Solve the cubic function using Newton method, also satisfies the interval restriction
static bool solveCubicWithIntervalNewton(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
FCL_REAL& l, FCL_REAL& r, bool bVF, FCL_REAL coeffs[], Vec3f* data = NULL);
/// @brief Check whether one point p is within triangle [a, b, c]
static bool insideTriangle(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f&p);
/// @brief Check whether one point p is within a line segment [a, b]
static bool insideLineSegment(const Vec3f& a, const Vec3f& b, const Vec3f& p);
/// @brief Calculate the line segment papb that is the shortest route between
/// two lines p1p2 and p3p4. Calculate also the values of mua and mub where
/// pa = p1 + mua (p2 - p1)
/// pb = p3 + mub (p4 - p3)
/// return FALSE if no solution exists.
static bool linelineIntersect(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3, const Vec3f& p4,
Vec3f* pa, Vec3f* pb, FCL_REAL* mua, FCL_REAL* mub);
/// @brief Check whether a root for VF intersection is valid (i.e. within the triangle at intersection t
static bool checkRootValidity_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp,
FCL_REAL t);
/// @brief Check whether a root for EE intersection is valid (i.e. within the two edges intersected at the given time
static bool checkRootValidity_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
FCL_REAL t, Vec3f* q_i = NULL);
/// @brief Check whether a root for VE intersection is valid
static bool checkRootValidity_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
const Vec3f& va, const Vec3f& vb, const Vec3f& vp,
FCL_REAL t);
/// @brief Solve a square function for EE intersection (with interval restriction)
static bool solveSquare(FCL_REAL a, FCL_REAL b, FCL_REAL c,
const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
bool bVF,
FCL_REAL* ret);
/// @brief Solve a square function for VE intersection (with interval restriction)
static bool solveSquare(FCL_REAL a, FCL_REAL b, FCL_REAL c,
const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
const Vec3f& va, const Vec3f& vb, const Vec3f& vp);
/// @brief Compute the cubic coefficients for VF intersection
/// See Paper "Interactive Continuous Collision Detection between Deformable Models using Connectivity-Based Culling", Equation 1.
static void computeCubicCoeff_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp,
FCL_REAL* a, FCL_REAL* b, FCL_REAL* c, FCL_REAL* d);
/// @brief Compute the cubic coefficients for EE intersection
static void computeCubicCoeff_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
FCL_REAL* a, FCL_REAL* b, FCL_REAL* c, FCL_REAL* d);
/// @brief Compute the cubic coefficients for VE intersection
static void computeCubicCoeff_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
const Vec3f& va, const Vec3f& vb, const Vec3f& vp,
const Vec3f& L,
FCL_REAL* a, FCL_REAL* b, FCL_REAL* c);
/// @brief filter for intersection, works for both VF and EE
static bool intersectPreFiltering(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1);
/// @brief distance of point v to a plane n * x - t = 0
static FCL_REAL distanceToPlane(const Vec3f& n, FCL_REAL t, const Vec3f& v);
/// @brief check wether points v1, v2, v2 are on the same side of plane n * x - t = 0
static bool sameSideOfPlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, const Vec3f& n, FCL_REAL t);
/// @brief clip triangle v1, v2, v3 by the prism made by t1, t2 and t3. The normal of the prism is tn and is cutted up by to
static void clipTriangleByTriangleAndEdgePlanes(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3,
const Vec3f& t1, const Vec3f& t2, const Vec3f& t3,
const Vec3f& tn, FCL_REAL to,
Vec3f clipped_points[], unsigned int* num_clipped_points, bool clip_triangle = false);
/// @brief build a plane passed through triangle v1 v2 v3
static bool buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, Vec3f* n, FCL_REAL* t);
/// @brief build a plane pass through edge v1 and v2, normal is tn
static bool buildEdgePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& tn, Vec3f* n, FCL_REAL* t);
/// @brief compute the points which has deepest penetration depth
static void computeDeepestPoints(Vec3f* clipped_points, unsigned int num_clipped_points, const Vec3f& n, FCL_REAL t, FCL_REAL* penetration_depth, Vec3f* deepest_points, unsigned int* num_deepest_points);
/// @brief clip polygon by plane
static void clipPolygonByPlane(Vec3f* polygon_points, unsigned int num_polygon_points, const Vec3f& n, FCL_REAL t, Vec3f clipped_points[], unsigned int* num_clipped_points);
/// @brief clip a line segment by plane
static void clipSegmentByPlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& n, FCL_REAL t, Vec3f* clipped_point);
/// @brief compute the cdf(x)
static FCL_REAL gaussianCDF(FCL_REAL x)
{
return 0.5 * boost::math::erfc(-x / sqrt(2.0));
}
static const FCL_REAL EPSILON;
static const FCL_REAL NEAR_ZERO_THRESHOLD;
static const FCL_REAL CCD_RESOLUTION;
static const unsigned int MAX_TRIANGLE_CLIPS = 8;
};
/// @brief Project functions
class Project
{
public:
struct ProjectResult
{
/// @brief Parameterization of the projected point (based on the simplex to be projected, use 2 or 3 or 4 of the array)
FCL_REAL parameterization[4];
/// @brief square distance from the query point to the projected simplex
FCL_REAL sqr_distance;
/// @brief the code of the projection type
unsigned int encode;
ProjectResult() : sqr_distance(-1), encode(0)
{
}
};
/// @brief Project point p onto line a-b
static ProjectResult projectLine(const Vec3f& a, const Vec3f& b, const Vec3f& p);
/// @brief Project point p onto triangle a-b-c
static ProjectResult projectTriangle(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& p);
/// @brief Project point p onto tetrahedra a-b-c-d
static ProjectResult projectTetrahedra(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& d, const Vec3f& p);
/// @brief Project origin (0) onto line a-b
static ProjectResult projectLineOrigin(const Vec3f& a, const Vec3f& b);
/// @brief Project origin (0) onto triangle a-b-c
static ProjectResult projectTriangleOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c);
/// @brief Project origin (0) onto tetrahedran a-b-c-d
static ProjectResult projectTetrahedraOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& d);
};
/// @brief Triangle distance functions
class TriangleDistance
{
public:
/// @brief Returns closest points between an segment pair.
/// The first segment is P + t * A
/// The second segment is Q + t * B
/// X, Y are the closest points on the two segments
/// VEC is the vector between X and Y
static void segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, const Vec3f& B,
Vec3f& VEC, Vec3f& X, Vec3f& Y);
/// @brief Compute the closest points on two triangles given their absolute coordinate, and returns the distance between them
/// S and T are two triangles
/// If the triangles are disjoint, P and Q give the closet points of S and T respectively. However,
/// if the triangles overlap, P and Q are basically a random pair of points from the triangles, not
/// coincident points on the intersection of the triangles, as might be expected.
static FCL_REAL triDistance(const Vec3f S[3], const Vec3f T[3], Vec3f& P, Vec3f& Q);
static FCL_REAL triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
const Vec3f& T1, const Vec3f& T2, const Vec3f& T3,
Vec3f& P, Vec3f& Q);
/// @brief Compute the closest points on two triangles given the relative transform between them, and returns the distance between them
/// S and T are two triangles
/// If the triangles are disjoint, P and Q give the closet points of S and T respectively. However,
/// if the triangles overlap, P and Q are basically a random pair of points from the triangles, not
/// coincident points on the intersection of the triangles, as might be expected.
/// The returned P and Q are both in the coordinate of the first triangle's coordinate
static FCL_REAL triDistance(const Vec3f S[3], const Vec3f T[3],
const Matrix3f& R, const Vec3f& Tl,
Vec3f& P, Vec3f& Q);
static FCL_REAL triDistance(const Vec3f S[3], const Vec3f T[3],
const Transform3f& tf,
Vec3f& P, Vec3f& Q);
static FCL_REAL triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
const Vec3f& T1, const Vec3f& T2, const Vec3f& T3,
const Matrix3f& R, const Vec3f& Tl,
Vec3f& P, Vec3f& Q);
static FCL_REAL triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
const Vec3f& T1, const Vec3f& T2, const Vec3f& T3,
const Transform3f& tf,
Vec3f& P, Vec3f& Q);
};
}
#endif
include/fcl/knn/greedy_kcenters.h deleted 100644 → 0
View file @ 12194e71
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Rice University
* 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 the Rice University 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: Mark Moll */
#ifndef FCL_KNN_GREEDY_KCENTERS_H
#define FCL_KNN_GREEDY_KCENTERS_H
#include "fcl/math/sampling.h"
namespace fcl
{
/// @brief An instance of this class can be used to greedily select a given
/// number of representatives from a set of data points that are all far
/// apart from each other.
template<typename _T>
class GreedyKCenters
{
public:
/// @brief The definition of a distance function
typedef boost::function<double(const _T&, const _T&)> DistanceFunction;
GreedyKCenters(void)
{
}
virtual ~GreedyKCenters(void)
{
}
/// @brief Set the distance function to use
void setDistanceFunction(const DistanceFunction &distFun)
{
distFun_ = distFun;
}
/// @brief Get the distance function used
const DistanceFunction& getDistanceFunction(void) const
{
return distFun_;
}
/// @brief Greedy algorithm for selecting k centers
/// @param data a vector of data points
/// @param k the desired number of centers
/// @param centers a vector of length k containing the indices into data of the k centers
/// @param dists a 2-dimensional array such that dists[i][j] is the distance between data[i] and data[center[j]]
void kcenters(const std::vector<_T>& data, unsigned int k,
std::vector<unsigned int>& centers, std::vector<std::vector<double> >& dists)
{
// array containing the minimum distance between each data point
// and the centers computed so far
std::vector<double> minDist(data.size(), std::numeric_limits<double>::infinity());
centers.clear();
centers.reserve(k);
dists.resize(data.size(), std::vector<double>(k));
// first center is picked randomly
centers.push_back(rng_.uniformInt(0, data.size() - 1));
for (unsigned i=1; i<k; ++i)
{
unsigned ind;
const _T& center = data[centers[i - 1]];
double maxDist = -std::numeric_limits<double>::infinity();
for (unsigned j=0; j<data.size(); ++j)
{
if ((dists[j][i-1] = distFun_(data[j], center)) < minDist[j])
minDist[j] = dists[j][i - 1];
// the j-th center is the one furthest away from center 0,..,j-1
if (minDist[j] > maxDist)
{
ind = j;
maxDist = minDist[j];
}
}
// no more centers available
if (maxDist < std::numeric_limits<double>::epsilon()) break;
centers.push_back(ind);
}
const _T& center = data[centers.back()];
unsigned i = centers.size() - 1;
for (unsigned j = 0; j < data.size(); ++j)
dists[j][i] = distFun_(data[j], center);
}
protected:
/// @brief The used distance function
DistanceFunction distFun_;
/// Random number generator used to select first center
RNG rng_;
};
}
#endif