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 2849 additions and 405 deletions
include/coal/BV/BV.h 0 → 100644
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/** \author Jia Pan */
#ifndef COAL_BV_H
#define COAL_BV_H
#include "coal/BV/kDOP.h"
#include "coal/BV/AABB.h"
#include "coal/BV/OBB.h"
#include "coal/BV/RSS.h"
#include "coal/BV/OBBRSS.h"
#include "coal/BV/kIOS.h"
#include "coal/math/transform.h"
/** @brief Main namespace */
namespace coal {
/// @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>
struct Converter {
static void convert(const BV1& bv1, const Transform3s& tf1, BV2& bv2);
static void convert(const BV1& bv1, BV2& bv2);
};
/// @brief Convert from AABB to AABB, not very tight but is fast.
template <>
struct Converter<AABB, AABB> {
static void convert(const AABB& bv1, const Transform3s& tf1, AABB& bv2) {
const Vec3s& center = bv1.center();
CoalScalar r = (bv1.max_ - bv1.min_).norm() * 0.5;
const Vec3s center2 = tf1.transform(center);
bv2.min_ = center2 - Vec3s::Constant(r);
bv2.max_ = center2 + Vec3s::Constant(r);
}
static void convert(const AABB& bv1, AABB& bv2) { bv2 = bv1; }
};
template <>
struct Converter<AABB, OBB> {
static void convert(const AABB& bv1, const Transform3s& tf1, OBB& bv2) {
bv2.To = tf1.transform(bv1.center());
bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5;
bv2.axes = tf1.getRotation();
}
static void convert(const AABB& bv1, OBB& bv2) {
bv2.To = bv1.center();
bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5;
bv2.axes.setIdentity();
}
};
template <>
struct Converter<OBB, OBB> {
static void convert(const OBB& bv1, const Transform3s& tf1, OBB& bv2) {
bv2.extent = bv1.extent;
bv2.To = tf1.transform(bv1.To);
bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
}
static void convert(const OBB& bv1, OBB& bv2) { bv2 = bv1; }
};
template <>
struct Converter<OBBRSS, OBB> {
static void convert(const OBBRSS& bv1, const Transform3s& tf1, OBB& bv2) {
Converter<OBB, OBB>::convert(bv1.obb, tf1, bv2);
}
static void convert(const OBBRSS& bv1, OBB& bv2) {
Converter<OBB, OBB>::convert(bv1.obb, bv2);
}
};
template <>
struct Converter<RSS, OBB> {
static void convert(const RSS& bv1, const Transform3s& tf1, OBB& bv2) {
bv2.extent = Vec3s(bv1.length[0] * 0.5 + bv1.radius,
bv1.length[1] * 0.5 + bv1.radius, bv1.radius);
bv2.To = tf1.transform(bv1.Tr);
bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
}
static void convert(const RSS& bv1, OBB& bv2) {
bv2.extent = Vec3s(bv1.length[0] * 0.5 + bv1.radius,
bv1.length[1] * 0.5 + bv1.radius, bv1.radius);
bv2.To = bv1.Tr;
bv2.axes = bv1.axes;
}
};
template <typename BV1>
struct Converter<BV1, AABB> {
static void convert(const BV1& bv1, const Transform3s& tf1, AABB& bv2) {
const Vec3s& center = bv1.center();
CoalScalar r = Vec3s(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5;
const Vec3s center2 = tf1.transform(center);
bv2.min_ = center2 - Vec3s::Constant(r);
bv2.max_ = center2 + Vec3s::Constant(r);
}
static void convert(const BV1& bv1, AABB& bv2) {
const Vec3s& center = bv1.center();
CoalScalar r = Vec3s(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5;
bv2.min_ = center - Vec3s::Constant(r);
bv2.max_ = center + Vec3s::Constant(r);
}
};
template <typename BV1>
struct Converter<BV1, OBB> {
static void convert(const BV1& bv1, const Transform3s& tf1, OBB& bv2) {
AABB bv;
Converter<BV1, AABB>::convert(bv1, bv);
Converter<AABB, OBB>::convert(bv, tf1, bv2);
}
static void convert(const BV1& bv1, OBB& bv2) {
AABB bv;
Converter<BV1, AABB>::convert(bv1, bv);
Converter<AABB, OBB>::convert(bv, bv2);
}
};
template <>
struct Converter<OBB, RSS> {
static void convert(const OBB& bv1, const Transform3s& tf1, RSS& bv2) {
bv2.Tr = tf1.transform(bv1.To);
bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
bv2.radius = bv1.extent[2];
bv2.length[0] = 2 * (bv1.extent[0] - bv2.radius);
bv2.length[1] = 2 * (bv1.extent[1] - bv2.radius);
}
static void convert(const OBB& bv1, RSS& bv2) {
bv2.Tr = bv1.To;
bv2.axes = bv1.axes;
bv2.radius = bv1.extent[2];
bv2.length[0] = 2 * (bv1.extent[0] - bv2.radius);
bv2.length[1] = 2 * (bv1.extent[1] - bv2.radius);
}
};
template <>
struct Converter<RSS, RSS> {
static void convert(const RSS& bv1, const Transform3s& tf1, RSS& bv2) {
bv2.Tr = tf1.transform(bv1.Tr);
bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
bv2.radius = bv1.radius;
bv2.length[0] = bv1.length[0];
bv2.length[1] = bv1.length[1];
}
static void convert(const RSS& bv1, RSS& bv2) { bv2 = bv1; }
};
template <>
struct Converter<OBBRSS, RSS> {
static void convert(const OBBRSS& bv1, const Transform3s& tf1, RSS& bv2) {
Converter<RSS, RSS>::convert(bv1.rss, tf1, bv2);
}
static void convert(const OBBRSS& bv1, RSS& bv2) {
Converter<RSS, RSS>::convert(bv1.rss, bv2);
}
};
template <>
struct Converter<AABB, RSS> {
static void convert(const AABB& bv1, const Transform3s& tf1, RSS& bv2) {
bv2.Tr = tf1.transform(bv1.center());
/// Sort the AABB edges so that AABB extents are ordered.
CoalScalar d[3] = {bv1.width(), bv1.height(), bv1.depth()};
Eigen::DenseIndex id[3] = {0, 1, 2};
for (Eigen::DenseIndex i = 1; i < 3; ++i) {
for (Eigen::DenseIndex j = i; j > 0; --j) {
if (d[j] > d[j - 1]) {
{
CoalScalar tmp = d[j];
d[j] = d[j - 1];
d[j - 1] = tmp;
}
{
Eigen::DenseIndex tmp = id[j];
id[j] = id[j - 1];
id[j - 1] = tmp;
}
}
}
}
const Vec3s extent = (bv1.max_ - bv1.min_) * 0.5;
bv2.radius = extent[id[2]];
bv2.length[0] = (extent[id[0]] - bv2.radius) * 2;
bv2.length[1] = (extent[id[1]] - bv2.radius) * 2;
const Matrix3s& R = tf1.getRotation();
const bool left_hand = (id[0] == (id[1] + 1) % 3);
if (left_hand)
bv2.axes.col(0) = -R.col(id[0]);
else
bv2.axes.col(0) = R.col(id[0]);
bv2.axes.col(1) = R.col(id[1]);
bv2.axes.col(2) = R.col(id[2]);
}
static void convert(const AABB& bv1, RSS& bv2) {
convert(bv1, Transform3s(), bv2);
}
};
template <>
struct Converter<AABB, OBBRSS> {
static void convert(const AABB& bv1, const Transform3s& tf1, OBBRSS& bv2) {
Converter<AABB, OBB>::convert(bv1, tf1, bv2.obb);
Converter<AABB, RSS>::convert(bv1, tf1, bv2.rss);
}
static void convert(const AABB& bv1, OBBRSS& bv2) {
Converter<AABB, OBB>::convert(bv1, bv2.obb);
Converter<AABB, RSS>::convert(bv1, bv2.rss);
}
};
} // namespace details
/// @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 Transform3s& tf1, BV2& bv2) {
details::Converter<BV1, BV2>::convert(bv1, tf1, bv2);
}
/// @brief Convert a bounding volume of type BV1 to bounding volume of type BV2
/// in identity configuration.
template <typename BV1, typename BV2>
static inline void convertBV(const BV1& bv1, BV2& bv2) {
details::Converter<BV1, BV2>::convert(bv1, bv2);
}
} // namespace coal
#endif
include/fcl/BV/BV_node.h → include/coal/BV/BV_node.h
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
......@@ -14,7 +15,7 @@
* 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
* * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
......@@ -34,99 +35,132 @@
/** \author Jia Pan */
#ifndef COAL_BV_NODE_H
#define COAL_BV_NODE_H
#ifndef FCL_BV_NODE_H
#define FCL_BV_NODE_H
#include "coal/data_types.h"
#include "coal/BV/BV.h"
#include "fcl/math/vec_3f.h"
#include "fcl/math/matrix_3f.h"
namespace coal {
#include "fcl/BV/BV.h"
#include <iostream>
namespace fcl
{
/// @defgroup Construction_Of_BVH Construction of BVHModel
/// Classes which are used to build a BVHModel (Bounding Volume Hierarchy)
/// @{
/// @brief BVNodeBase encodes the tree structure for BVH
struct BVNodeBase
{
struct COAL_DLLAPI BVNodeBase {
/// @brief An index for first child node or primitive
/// If the value is positive, it is the index of the first child bv node
/// If the value is negative, it is -(primitive index + 1)
/// Zero is not used.
int first_child;
/// @brief The start id the primitive belonging to the current node. The index is referred to the primitive_indices in BVHModel and from that
/// we can obtain the primitive's index in original data indirectly.
int first_primitive;
/// @brief The start id the primitive belonging to the current node. The index
/// is referred to the primitive_indices in BVHModel and from that we can
/// obtain the primitive's index in original data indirectly.
unsigned int first_primitive;
/// @brief The number of primitives belonging to the current node
unsigned int num_primitives;
/// @brief Default constructor
BVNodeBase()
: first_child(0),
first_primitive(
(std::numeric_limits<unsigned int>::max)()) // value we should help
// to raise an issue
,
num_primitives(0) {}
/// @brief Equality operator
bool operator==(const BVNodeBase& other) const {
return first_child == other.first_child &&
first_primitive == other.first_primitive &&
num_primitives == other.num_primitives;
}
/// @brief The number of primitives belonging to the current node
int num_primitives;
/// @brief Difference operator
bool operator!=(const BVNodeBase& other) const { return !(*this == other); }
/// @brief Whether current node is a leaf node (i.e. contains a primitive index
/// @brief Whether current node is a leaf node (i.e. contains a primitive
/// index
inline bool isLeaf() const { return first_child < 0; }
/// @brief Return the primitive index. The index is referred to the original data (i.e. vertices or tri_indices) in BVHModel
/// @brief Return the primitive index. The index is referred to the original
/// data (i.e. vertices or tri_indices) in BVHModel
inline int primitiveId() const { return -(first_child + 1); }
/// @brief Return the index of the first child. The index is referred to the bounding volume array (i.e. bvs) in BVHModel
/// @brief Return the index of the first child. The index is referred to the
/// bounding volume array (i.e. bvs) in BVHModel
inline int leftChild() const { return first_child; }
/// @brief Return the index of the second child. The index is referred to the bounding volume array (i.e. bvs) in BVHModel
/// @brief Return the index of the second child. The index is referred to the
/// bounding volume array (i.e. bvs) in BVHModel
inline int rightChild() const { return first_child + 1; }
};
/// @brief A class describing a bounding volume node. It includes the tree structure providing in BVNodeBase and also the geometry data provided in BV template parameter.
template<typename BV>
struct BVNode : public BVNodeBase
{
/// @brief A class describing a bounding volume node. It includes the tree
/// structure providing in BVNodeBase and also the geometry data provided in BV
/// template parameter.
template <typename BV>
struct COAL_DLLAPI BVNode : public BVNodeBase {
typedef BVNodeBase Base;
/// @brief bounding volume storing the geometry
BV bv;
/// @brief Equality operator
bool operator==(const BVNode& other) const {
return Base::operator==(other) && bv == other.bv;
}
/// @brief Difference operator
bool operator!=(const BVNode& other) const { return !(*this == other); }
/// @brief Check whether two BVNode collide
bool overlap(const BVNode& other) const
{
return bv.overlap(other.bv);
bool overlap(const BVNode& other) const { return bv.overlap(other.bv); }
/// @brief Check whether two BVNode collide
bool overlap(const BVNode& other, const CollisionRequest& request,
CoalScalar& sqrDistLowerBound) const {
return bv.overlap(other.bv, request, sqrDistLowerBound);
}
/// @brief Compute the distance between two BVNode. P1 and P2, if not NULL and the underlying BV supports distance, return the nearest points.
FCL_REAL distance(const BVNode& other, Vec3f* P1 = NULL, Vec3f* P2 = NULL) const
{
/// @brief Compute the distance between two BVNode. P1 and P2, if not NULL and
/// the underlying BV supports distance, return the nearest points.
CoalScalar distance(const BVNode& other, Vec3s* P1 = NULL,
Vec3s* P2 = NULL) const {
return bv.distance(other.bv, P1, P2);
}
/// @brief Access the center of the BV
Vec3f getCenter() const { return bv.center(); }
/// @brief Access to the center of the BV
Vec3s getCenter() const { return bv.center(); }
/// @brief Access to the orientation of the BV
const Matrix3s& getOrientation() const {
static const Matrix3s id3 = Matrix3s::Identity();
return id3;
}
/// @brief Access the orientation of the BV
Matrix3f getOrientation() const { return Matrix3f::getIdentity(); }
/// \cond
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// \endcond
};
template<>
inline Matrix3f BVNode<OBB>::getOrientation() const
{
return Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]);
template <>
inline const Matrix3s& BVNode<OBB>::getOrientation() const {
return bv.axes;
}
template<>
inline Matrix3f BVNode<RSS>::getOrientation() const
{
return Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]);
template <>
inline const Matrix3s& BVNode<RSS>::getOrientation() const {
return bv.axes;
}
template<>
inline Matrix3f BVNode<OBBRSS>::getOrientation() const
{
return Matrix3f(bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0],
bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1],
bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]);
template <>
inline const Matrix3s& BVNode<OBBRSS>::getOrientation() const {
return bv.obb.axes;
}
}
} // namespace coal
#endif
include/fcl/BV/OBB.h → include/coal/BV/OBB.h
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
......@@ -14,7 +15,7 @@
* 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
* * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
......@@ -34,108 +35,116 @@
/** \author Jia Pan */
#ifndef FCL_OBB_H
#define FCL_OBB_H
#ifndef COAL_OBB_H
#define COAL_OBB_H
#include "coal/data_types.h"
#include "fcl/math/vec_3f.h"
#include "fcl/math/matrix_3f.h"
namespace coal {
namespace fcl
{
struct CollisionRequest;
/// @addtogroup Bounding_Volume
/// @{
/// @brief Oriented bounding box class
class OBB
{
public:
/// @brief Orientation of OBB. axis[i] is the ith column of the orientation matrix for the box; it is also the i-th principle direction of the box.
/// We assume that axis[0] corresponds to the axis with the longest box edge, axis[1] corresponds to the shorter one and axis[2] corresponds to the shortest one.
Vec3f axis[3];
struct COAL_DLLAPI OBB {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// @brief Orientation of OBB. axis[i] is the ith column of the orientation
/// matrix for the box; it is also the i-th principle direction of the box. We
/// assume that axis[0] corresponds to the axis with the longest box edge,
/// axis[1] corresponds to the shorter one and axis[2] corresponds to the
/// shortest one.
Matrix3s axes;
/// @brief Center of OBB
Vec3f To;
Vec3s To;
/// @brief Half dimensions of OBB
Vec3f extent;
Vec3s extent;
/// @brief Check collision between two OBB, return true if collision happens.
bool overlap(const OBB& other) const;
OBB() : axes(Matrix3s::Zero()), To(Vec3s::Zero()), extent(Vec3s::Zero()) {}
/// @brief Check collision between two OBB and return the overlap part. For OBB, the overlap_part return value is NOT used as the overlap part of two obbs usually is not an obb.
bool overlap(const OBB& other, OBB& overlap_part) const
{
return overlap(other);
/// @brief Equality operator
bool operator==(const OBB& other) const {
return axes == other.axes && To == other.To && extent == other.extent;
}
/// @brief Difference operator
bool operator!=(const OBB& other) const { return !(*this == other); }
/// @brief Check whether the OBB contains a point.
bool contain(const Vec3f& p) const;
bool contain(const Vec3s& p) const;
/// @brief A simple way to merge the OBB and a point (the result is not compact).
OBB& operator += (const Vec3f& p);
/// Check collision between two OBB
/// @return true if collision happens.
bool overlap(const OBB& other) const;
/// @brief Merge the OBB and another OBB (the result is not compact).
OBB& operator += (const OBB& other)
{
*this = *this + other;
return *this;
}
/// Check collision between two OBB
/// @return true if collision happens.
/// @retval sqrDistLowerBound squared lower bound on distance between boxes if
/// they do not overlap.
bool overlap(const OBB& other, const CollisionRequest& request,
CoalScalar& sqrDistLowerBound) const;
/// @brief Return the merged OBB of current OBB and the other one (the result is not compact).
OBB operator + (const OBB& other) const;
/// @brief Distance between two OBBs, not implemented.
CoalScalar distance(const OBB& other, Vec3s* P = NULL, Vec3s* Q = NULL) const;
/// @brief Width of the OBB.
inline FCL_REAL width() const
{
return 2 * extent[0];
}
/// @brief A simple way to merge the OBB and a point (the result is not
/// compact).
OBB& operator+=(const Vec3s& p);
/// @brief Height of the OBB.
inline FCL_REAL height() const
{
return 2 * extent[1];
/// @brief Merge the OBB and another OBB (the result is not compact).
OBB& operator+=(const OBB& other) {
*this = *this + other;
return *this;
}
/// @brief Depth of the OBB
inline FCL_REAL depth() const
{
return 2 * extent[2];
}
/// @brief Volume of the OBB
inline FCL_REAL volume() const
{
return width() * height() * depth();
}
/// @brief Return the merged OBB of current OBB and the other one (the result
/// is not compact).
OBB operator+(const OBB& other) const;
/// @brief Size of the OBB (used in BV_Splitter to order two OBBs)
inline FCL_REAL size() const
{
return extent.sqrLength();
}
inline CoalScalar size() const { return extent.squaredNorm(); }
/// @brief Center of the OBB
inline const Vec3f& center() const
{
return To;
}
/// @brief Distance between two OBBs, not implemented.
FCL_REAL distance(const OBB& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
};
inline const Vec3s& center() const { return To; }
/// @brief Width of the OBB.
inline CoalScalar width() const { return 2 * extent[0]; }
/// @brief Translate the OBB bv
OBB translate(const OBB& bv, const Vec3f& t);
/// @brief Height of the OBB.
inline CoalScalar height() const { return 2 * extent[1]; }
/// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and b2 is in identity.
bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2);
/// @brief Depth of the OBB
inline CoalScalar depth() const { return 2 * extent[2]; }
/// @brief Volume of the OBB
inline CoalScalar volume() const { return width() * height() * depth(); }
};
/// @brief Check collision between two boxes: the first box is in configuration (R, T) and its half dimension is set by a;
/// the second box is in identity configuration and its half dimension is set by b.
bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b);
}
/// @brief Translate the OBB bv
COAL_DLLAPI OBB translate(const OBB& bv, const Vec3s& t);
/// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and
/// b2 is in identity.
COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1,
const OBB& b2);
/// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and
/// b2 is in identity.
COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1,
const OBB& b2, const CollisionRequest& request,
CoalScalar& sqrDistLowerBound);
/// Check collision between two boxes
/// @param B, T orientation and position of first box,
/// @param a half dimensions of first box,
/// @param b half dimensions of second box.
/// The second box is in identity configuration.
COAL_DLLAPI bool obbDisjoint(const Matrix3s& B, const Vec3s& T, const Vec3s& a,
const Vec3s& b);
} // namespace coal
#endif
include/fcl/BV/OBBRSS.h → include/coal/BV/OBBRSS.h
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
......@@ -14,7 +15,7 @@
* 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
* * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
......@@ -34,21 +35,23 @@
/** \author Jia Pan */
#ifndef FCL_OBBRSS_H
#define FCL_OBBRSS_H
#ifndef COAL_OBBRSS_H
#define COAL_OBBRSS_H
#include "coal/BV/OBB.h"
#include "coal/BV/RSS.h"
#include "fcl/BV/OBB.h"
#include "fcl/BV/RSS.h"
namespace coal {
namespace fcl
{
struct CollisionRequest;
/// @addtogroup Bounding_Volume
/// @{
/// @brief Class merging the OBB and RSS, can handle collision and distance simultaneously
class OBBRSS
{
public:
/// @brief Class merging the OBB and RSS, can handle collision and distance
/// simultaneously
struct COAL_DLLAPI OBBRSS {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// @brief OBB member, for rotation
OBB obb;
......@@ -56,101 +59,102 @@ public:
/// @brief RSS member, for distance
RSS rss;
/// @brief Check collision between two OBBRSS
bool overlap(const OBBRSS& other) const
{
return obb.overlap(other.obb);
/// @brief Equality operator
bool operator==(const OBBRSS& other) const {
return obb == other.obb && rss == other.rss;
}
/// @brief Check collision between two OBBRSS and return the overlap part.
bool overlap(const OBBRSS& other, OBBRSS& overlap_part) const
{
return overlap(other);
}
/// @brief Difference operator
bool operator!=(const OBBRSS& other) const { return !(*this == other); }
/// @brief Check whether the OBBRSS contains a point
inline bool contain(const Vec3f& p) const
{
return obb.contain(p);
inline bool contain(const Vec3s& p) const { return obb.contain(p); }
/// @brief Check collision between two OBBRSS
bool overlap(const OBBRSS& other) const { return obb.overlap(other.obb); }
/// Check collision between two OBBRSS
/// @retval sqrDistLowerBound squared lower bound on distance between
/// objects if they do not overlap.
bool overlap(const OBBRSS& other, const CollisionRequest& request,
CoalScalar& sqrDistLowerBound) const {
return obb.overlap(other.obb, request, sqrDistLowerBound);
}
/// @brief Distance between two OBBRSS; P and Q , is not NULL, returns the
/// nearest points
CoalScalar distance(const OBBRSS& other, Vec3s* P = NULL,
Vec3s* Q = NULL) const {
return rss.distance(other.rss, P, Q);
}
/// @brief Merge the OBBRSS and a point
OBBRSS& operator += (const Vec3f& p)
{
OBBRSS& operator+=(const Vec3s& p) {
obb += p;
rss += p;
return *this;
}
/// @brief Merge two OBBRSS
OBBRSS& operator += (const OBBRSS& other)
{
OBBRSS& operator+=(const OBBRSS& other) {
*this = *this + other;
return *this;
}
/// @brief Merge two OBBRSS
OBBRSS operator + (const OBBRSS& other) const
{
OBBRSS operator+(const OBBRSS& other) const {
OBBRSS result;
result.obb = obb + other.obb;
result.rss = rss + other.rss;
return result;
}
/// @brief Size of the OBBRSS (used in BV_Splitter to order two OBBRSS)
inline CoalScalar size() const { return obb.size(); }
/// @brief Center of the OBBRSS
inline const Vec3s& center() const { return obb.center(); }
/// @brief Width of the OBRSS
inline FCL_REAL width() const
{
return obb.width();
}
inline CoalScalar width() const { return obb.width(); }
/// @brief Height of the OBBRSS
inline FCL_REAL height() const
{
return obb.height();
}
inline CoalScalar height() const { return obb.height(); }
/// @brief Depth of the OBBRSS
inline FCL_REAL depth() const
{
return obb.depth();
}
inline CoalScalar depth() const { return obb.depth(); }
/// @brief Volume of the OBBRSS
inline FCL_REAL volume() const
{
return obb.volume();
}
/// @brief Size of the OBBRSS (used in BV_Splitter to order two OBBRSS)
inline FCL_REAL size() const
{
return obb.size();
}
/// @brief Center of the OBBRSS
inline const Vec3f& center() const
{
return obb.center();
}
/// @brief Distance between two OBBRSS; P and Q , is not NULL, returns the nearest points
FCL_REAL distance(const OBBRSS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const
{
return rss.distance(other.rss, P, Q);
}
inline CoalScalar volume() const { return obb.volume(); }
};
/// @brief Translate the OBBRSS bv
OBBRSS translate(const OBBRSS& bv, const Vec3f& t);
/// @brief Check collision between two OBBRSS, b1 is in configuration (R0, T0) and b2 is in indentity
bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2);
/// @brief Check collision between two OBBRSS, b1 is in configuration (R0, T0)
/// and b2 is in indentity
inline bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1,
const OBBRSS& b2) {
return overlap(R0, T0, b1.obb, b2.obb);
}
/// @brief Computate distance between two OBBRSS, b1 is in configuation (R0, T0) and b2 is in indentity; P and Q, is not NULL, returns the nearest points
FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2, Vec3f* P = NULL, Vec3f* Q = NULL);
/// Check collision between two OBBRSS
/// @param R0, T0 configuration of b1
/// @param b1 first OBBRSS in configuration (R0, T0)
/// @param b2 second OBBRSS in identity position
/// @retval sqrDistLowerBound squared lower bound on the distance if OBBRSS do
/// not overlap.
inline bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1,
const OBBRSS& b2, const CollisionRequest& request,
CoalScalar& sqrDistLowerBound) {
return overlap(R0, T0, b1.obb, b2.obb, request, sqrDistLowerBound);
}
/// @brief Computate distance between two OBBRSS, b1 is in configuation (R0, T0)
/// and b2 is in indentity; P and Q, is not NULL, returns the nearest points
inline CoalScalar distance(const Matrix3s& R0, const Vec3s& T0,
const OBBRSS& b1, const OBBRSS& b2, Vec3s* P = NULL,
Vec3s* Q = NULL) {
return distance(R0, T0, b1.rss, b2.rss, P, Q);
}
} // namespace coal
#endif
include/fcl/BV/RSS.h → include/coal/BV/RSS.h
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
......@@ -14,7 +15,7 @@
* 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
* * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
......@@ -34,117 +35,139 @@
/** \author Jia Pan */
#ifndef FCL_RSS_H
#define FCL_RSS_H
#ifndef COAL_RSS_H
#define COAL_RSS_H
#include "coal/data_types.h"
#include "fcl/math/vec_3f.h"
#include "fcl/math/matrix_3f.h"
#include <boost/math/constants/constants.hpp>
namespace fcl
{
namespace coal {
struct CollisionRequest;
/// @addtogroup Bounding_Volume
/// @{
/// @brief A class for rectangle sphere-swept bounding volume
class RSS
{
public:
/// @brief Orientation of RSS. axis[i] is the ith column of the orientation matrix for the RSS; it is also the i-th principle direction of the RSS.
/// We assume that axis[0] corresponds to the axis with the longest length, axis[1] corresponds to the shorter one and axis[2] corresponds to the shortest one.
Vec3f axis[3];
struct COAL_DLLAPI RSS {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// @brief Orientation of RSS. axis[i] is the ith column of the orientation
/// matrix for the RSS; it is also the i-th principle direction of the RSS. We
/// assume that axis[0] corresponds to the axis with the longest length,
/// axis[1] corresponds to the shorter one and axis[2] corresponds to the
/// shortest one.
Matrix3s axes;
/// @brief Origin of the rectangle in RSS
Vec3f Tr;
Vec3s Tr;
/// @brief Side lengths of rectangle
FCL_REAL l[2];
CoalScalar length[2];
/// @brief Radius of sphere summed with rectangle to form RSS
FCL_REAL r;
CoalScalar radius;
/// @brief Default constructor with default values
RSS() : axes(Matrix3s::Zero()), Tr(Vec3s::Zero()), radius(-1) {
length[0] = 0;
length[1] = 0;
}
/// @brief Equality operator
bool operator==(const RSS& other) const {
return axes == other.axes && Tr == other.Tr &&
length[0] == other.length[0] && length[1] == other.length[1] &&
radius == other.radius;
}
/// @brief Difference operator
bool operator!=(const RSS& other) const { return !(*this == other); }
/// @brief Check whether the RSS contains a point
bool contain(const Vec3s& p) const;
/// @brief Check collision between two RSS
bool overlap(const RSS& other) const;
/// @brief Check collision between two RSS and return the overlap part.
/// For RSS, we return nothing, as the overlap part of two RSSs usually is not a RSS.
bool overlap(const RSS& other, RSS& overlap_part) const
{
/// Not implemented
bool overlap(const RSS& other, const CollisionRequest&,
CoalScalar& sqrDistLowerBound) const {
sqrDistLowerBound = sqrt(-1);
return overlap(other);
}
/// @brief Check whether the RSS contains a point
inline bool contain(const Vec3f& p) const;
/// @brief the distance between two RSS; P and Q, if not NULL, return the
/// nearest points
CoalScalar distance(const RSS& other, Vec3s* P = NULL, Vec3s* Q = NULL) const;
/// @brief A simple way to merge the RSS and a point, not compact.
/// @todo This function may have some bug.
RSS& operator += (const Vec3f& p);
RSS& operator+=(const Vec3s& p);
/// @brief Merge the RSS and another RSS
inline RSS& operator += (const RSS& other)
{
inline RSS& operator+=(const RSS& other) {
*this = *this + other;
return *this;
}
/// @brief Return the merged RSS of current RSS and the other one
RSS operator + (const RSS& other) const;
RSS operator+(const RSS& other) const;
/// @brief Width of the RSS
inline FCL_REAL width() const
{
return l[0] + 2 * r;
/// @brief Size of the RSS (used in BV_Splitter to order two RSSs)
inline CoalScalar size() const {
return (std::sqrt(length[0] * length[0] + length[1] * length[1]) +
2 * radius);
}
/// @brief The RSS center
inline const Vec3s& center() const { return Tr; }
/// @brief Width of the RSS
inline CoalScalar width() const { return length[0] + 2 * radius; }
/// @brief Height of the RSS
inline FCL_REAL height() const
{
return l[1] + 2 * r;
}
inline CoalScalar height() const { return length[1] + 2 * radius; }
/// @brief Depth of the RSS
inline FCL_REAL depth() const
{
return 2 * r;
}
inline CoalScalar depth() const { return 2 * radius; }
/// @brief Volume of the RSS
inline FCL_REAL volume() const
{
return (l[0] * l[1] * 2 * r + 4 * boost::math::constants::pi<FCL_REAL>() * r * r * r);
}
/// @brief Size of the RSS (used in BV_Splitter to order two RSSs)
inline FCL_REAL size() const
{
return (std::sqrt(l[0] * l[0] + l[1] * l[1]) + 2 * r);
inline CoalScalar volume() const {
return (length[0] * length[1] * 2 * radius +
4 * boost::math::constants::pi<CoalScalar>() * radius * radius *
radius);
}
/// @brief The RSS center
inline const Vec3f& center() const
{
return Tr;
/// @brief Check collision between two RSS and return the overlap part.
/// For RSS, we return nothing, as the overlap part of two RSSs usually is not
/// a RSS.
bool overlap(const RSS& other, RSS& /*overlap_part*/) const {
return overlap(other);
}
/// @brief the distance between two RSS; P and Q, if not NULL, return the nearest points
FCL_REAL distance(const RSS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
};
/// @brief Translate the RSS bv
RSS translate(const RSS& bv, const Vec3f& t);
/// @brief distance between two RSS bounding volumes
/// P and Q (optional return values) are the closest points in the rectangles, not the RSS. But the direction P - Q is the correct direction for cloest points
/// Notice that P and Q are both in the local frame of the first RSS (not global frame and not even the local frame of object 1)
FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2, Vec3f* P = NULL, Vec3f* Q = NULL);
/// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and b2 is in identity.
bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2);
}
/// P and Q (optional return values) are the closest points in the rectangles,
/// not the RSS. But the direction P - Q is the correct direction for cloest
/// points Notice that P and Q are both in the local frame of the first RSS (not
/// global frame and not even the local frame of object 1)
COAL_DLLAPI CoalScalar distance(const Matrix3s& R0, const Vec3s& T0,
const RSS& b1, const RSS& b2, Vec3s* P = NULL,
Vec3s* Q = NULL);
/// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and
/// b2 is in identity.
COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const RSS& b1,
const RSS& b2);
/// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and
/// b2 is in identity.
COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const RSS& b1,
const RSS& b2, const CollisionRequest& request,
CoalScalar& sqrDistLowerBound);
} // namespace coal
#endif
include/fcl/BV/kDOP.h → include/coal/BV/kDOP.h
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
......@@ -14,7 +15,7 @@
* 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
* * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
......@@ -34,19 +35,25 @@
/** \author Jia Pan */
#ifndef FCL_KDOP_H
#define FCL_KDOP_H
#ifndef COAL_KDOP_H
#define COAL_KDOP_H
#include "coal/fwd.hh"
#include "coal/data_types.h"
#include "fcl/math/vec_3f.h"
namespace coal {
namespace fcl
{
struct CollisionRequest;
/// @addtogroup Bounding_Volume
/// @{
/// @brief KDOP class describes the KDOP collision structures. K is set as the template parameter, which should be 16, 18, or 24
/// The KDOP structure is defined by some pairs of parallel planes defined by some axes.
/// For K = 16, the planes are 6 AABB planes and 10 diagonal planes that cut off some space of the edges:
/// @brief KDOP class describes the KDOP collision structures. K is set as the
/// template parameter, which should be 16, 18, or 24
/// The KDOP structure is defined by some pairs of parallel planes defined by
/// some axes.
/// For K = 16, the planes are 6 AABB planes and 10 diagonal planes that cut off
/// some space of the edges:
/// (-1,0,0) and (1,0,0) -> indices 0 and 8
/// (0,-1,0) and (0,1,0) -> indices 1 and 9
/// (0,0,-1) and (0,0,1) -> indices 2 and 10
......@@ -55,7 +62,8 @@ namespace fcl
/// (0,-1,-1) and (0,1,1) -> indices 5 and 13
/// (-1,1,0) and (1,-1,0) -> indices 6 and 14
/// (-1,0,1) and (1,0,-1) -> indices 7 and 15
/// For K = 18, the planes are 6 AABB planes and 12 diagonal planes that cut off some space of the edges:
/// For K = 18, the planes are 6 AABB planes and 12 diagonal planes that cut off
/// some space of the edges:
/// (-1,0,0) and (1,0,0) -> indices 0 and 9
/// (0,-1,0) and (0,1,0) -> indices 1 and 10
/// (0,0,-1) and (0,0,1) -> indices 2 and 11
......@@ -65,7 +73,8 @@ namespace fcl
/// (-1,1,0) and (1,-1,0) -> indices 6 and 15
/// (-1,0,1) and (1,0,-1) -> indices 7 and 16
/// (0,-1,1) and (0,1,-1) -> indices 8 and 17
/// For K = 18, the planes are 6 AABB planes and 18 diagonal planes that cut off some space of the edges:
/// For K = 18, the planes are 6 AABB planes and 18 diagonal planes that cut off
/// some space of the edges:
/// (-1,0,0) and (1,0,0) -> indices 0 and 12
/// (0,-1,0) and (0,1,0) -> indices 1 and 13
/// (0,0,-1) and (0,0,1) -> indices 2 and 14
......@@ -78,97 +87,104 @@ namespace fcl
/// (-1, -1, 1) and (1, 1, -1) --> indices 9 and 21
/// (-1, 1, -1) and (1, -1, 1) --> indices 10 and 22
/// (1, -1, -1) and (-1, 1, 1) --> indices 11 and 23
template<size_t N>
class KDOP
{
public:
template <short N>
class COAL_DLLAPI KDOP {
protected:
/// @brief Origin's distances to N KDOP planes
Eigen::Array<CoalScalar, N, 1> dist_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// @brief Creating kDOP containing nothing
KDOP();
/// @brief Creating kDOP containing only one point
KDOP(const Vec3f& v);
KDOP(const Vec3s& v);
/// @brief Creating kDOP containing two points
KDOP(const Vec3f& a, const Vec3f& b);
/// @brief Check whether two KDOPs are overlapped
bool overlap(const KDOP<N>& other) const;
KDOP(const Vec3s& a, const Vec3s& b);
//// @brief Check whether one point is inside the KDOP
bool inside(const Vec3f& p) const;
/// @brief Equality operator
bool operator==(const KDOP& other) const {
return (dist_ == other.dist_).all();
}
/// @brief Merge the point and the KDOP
KDOP<N>& operator += (const Vec3f& p);
/// @brief Difference operator
bool operator!=(const KDOP& other) const {
return (dist_ != other.dist_).any();
}
/// @brief Merge two KDOPs
KDOP<N>& operator += (const KDOP<N>& other);
/// @brief Check whether two KDOPs overlap.
bool overlap(const KDOP<N>& other) const;
/// @brief Create a KDOP by mergin two KDOPs
KDOP<N> operator + (const KDOP<N>& other) const;
/// @brief Check whether two KDOPs overlap.
/// @return true if collision happens.
/// @retval sqrDistLowerBound squared lower bound on distance between boxes if
/// they do not overlap.
bool overlap(const KDOP<N>& other, const CollisionRequest& request,
CoalScalar& sqrDistLowerBound) const;
/// @brief The (AABB) width
inline FCL_REAL width() const
{
return dist_[N / 2] - dist_[0];
}
/// @brief The distance between two KDOP<N>. Not implemented.
CoalScalar distance(const KDOP<N>& other, Vec3s* P = NULL,
Vec3s* Q = NULL) const;
/// @brief The (AABB) height
inline FCL_REAL height() const
{
return dist_[N / 2 + 1] - dist_[1];
}
/// @brief Merge the point and the KDOP
KDOP<N>& operator+=(const Vec3s& p);
/// @brief The (AABB) depth
inline FCL_REAL depth() const
{
return dist_[N / 2 + 2] - dist_[2];
}
/// @brief Merge two KDOPs
KDOP<N>& operator+=(const KDOP<N>& other);
/// @brief The (AABB) volume
inline FCL_REAL volume() const
{
return width() * height() * depth();
}
/// @brief Create a KDOP by mergin two KDOPs
KDOP<N> operator+(const KDOP<N>& other) const;
/// @brief Size of the kDOP (used in BV_Splitter to order two kDOPs)
inline FCL_REAL size() const
{
inline CoalScalar size() const {
return width() * width() + height() * height() + depth() * depth();
}
/// @brief The (AABB) center
inline Vec3f center() const
{
return Vec3f(dist_[0] + dist_[N / 2], dist_[1] + dist_[N / 2 + 1], dist_[2] + dist_[N / 2 + 2]) * 0.5;
inline Vec3s center() const {
return (dist_.template head<3>() + dist_.template segment<3>(N / 2)) / 2;
}
/// @brief The distance between two KDOP<N>. Not implemented.
FCL_REAL distance(const KDOP<N>& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
/// @brief The (AABB) width
inline CoalScalar width() const { return dist_[N / 2] - dist_[0]; }
private:
/// @brief Origin's distances to N KDOP planes
FCL_REAL dist_[N];
/// @brief The (AABB) height
inline CoalScalar height() const { return dist_[N / 2 + 1] - dist_[1]; }
public:
inline FCL_REAL dist(std::size_t i) const
{
return dist_[i];
}
/// @brief The (AABB) depth
inline CoalScalar depth() const { return dist_[N / 2 + 2] - dist_[2]; }
inline FCL_REAL& dist(std::size_t i)
{
return dist_[i];
}
/// @brief The (AABB) volume
inline CoalScalar volume() const { return width() * height() * depth(); }
inline CoalScalar dist(short i) const { return dist_[i]; }
inline CoalScalar& dist(short i) { return dist_[i]; }
//// @brief Check whether one point is inside the KDOP
bool inside(const Vec3s& p) const;
};
template <short N>
bool overlap(const Matrix3s& /*R0*/, const Vec3s& /*T0*/, const KDOP<N>& /*b1*/,
const KDOP<N>& /*b2*/) {
COAL_THROW_PRETTY("not implemented", std::logic_error);
}
template <short N>
bool overlap(const Matrix3s& /*R0*/, const Vec3s& /*T0*/, const KDOP<N>& /*b1*/,
const KDOP<N>& /*b2*/, const CollisionRequest& /*request*/,
CoalScalar& /*sqrDistLowerBound*/) {
COAL_THROW_PRETTY("not implemented", std::logic_error);
}
/// @brief translate the KDOP BV
template<size_t N>
KDOP<N> translate(const KDOP<N>& bv, const Vec3f& t);
template <short N>
COAL_DLLAPI KDOP<N> translate(const KDOP<N>& bv, const Vec3s& t);
}
} // namespace coal
#endif
include/fcl/BV/kIOS.h → include/coal/BV/kIOS.h
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
......@@ -14,7 +15,7 @@
* 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
* * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
......@@ -34,54 +35,85 @@
/** \author Jia Pan */
#ifndef FCL_KIOS_H
#define FCL_KIOS_H
#ifndef COAL_KIOS_H
#define COAL_KIOS_H
#include "fcl/BV/OBB.h"
#include "coal/BV/OBB.h"
namespace coal {
namespace fcl
{
/// @brief A class describing the kIOS collision structure, which is a set of spheres.
class kIOS
{
struct CollisionRequest;
/// @addtogroup Bounding_Volume
/// @{
/// @brief A class describing the kIOS collision structure, which is a set of
/// spheres.
class COAL_DLLAPI kIOS {
/// @brief One sphere in kIOS
struct kIOS_Sphere
{
Vec3f o;
FCL_REAL r;
struct COAL_DLLAPI kIOS_Sphere {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Vec3s o;
CoalScalar r;
bool operator==(const kIOS_Sphere& other) const {
return o == other.o && r == other.r;
}
bool operator!=(const kIOS_Sphere& other) const {
return !(*this == other);
}
};
/// @brief generate one sphere enclosing two spheres
static kIOS_Sphere encloseSphere(const kIOS_Sphere& s0, const kIOS_Sphere& s1)
{
Vec3f d = s1.o - s0.o;
FCL_REAL dist2 = d.sqrLength();
FCL_REAL diff_r = s1.r - s0.r;
static kIOS_Sphere encloseSphere(const kIOS_Sphere& s0,
const kIOS_Sphere& s1) {
Vec3s d = s1.o - s0.o;
CoalScalar dist2 = d.squaredNorm();
CoalScalar diff_r = s1.r - s0.r;
/** The sphere with the larger radius encloses the other */
if(diff_r * diff_r >= dist2)
{
if(s1.r > s0.r) return s1;
else return s0;
}
else /** spheres partially overlapping or disjoint */
if (diff_r * diff_r >= dist2) {
if (s1.r > s0.r)
return s1;
else
return s0;
} else /** spheres partially overlapping or disjoint */
{
float dist = std::sqrt(dist2);
float dist = (float)std::sqrt(dist2);
kIOS_Sphere s;
s.r = dist + s0.r + s1.r;
if(dist > 0)
if (dist > 0)
s.o = s0.o + d * ((s.r - s0.r) / dist);
else
s.o = s0.o;
return s;
}
}
public:
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// @brief Equality operator
bool operator==(const kIOS& other) const {
bool res = obb == other.obb && num_spheres == other.num_spheres;
if (!res) return false;
for (size_t k = 0; k < num_spheres; ++k) {
if (spheres[k] != other.spheres[k]) return false;
}
return true;
}
/// @brief Difference operator
bool operator!=(const kIOS& other) const { return !(*this == other); }
static constexpr size_t max_num_spheres = 5;
/// @brief The (at most) five spheres for intersection
kIOS_Sphere spheres[5];
kIOS_Sphere spheres[max_num_spheres];
/// @brief The number of spheres, no larger than 5
unsigned int num_spheres;
......@@ -89,70 +121,73 @@ public:
/// @ OBB related with kIOS
OBB obb;
/// @brief Check whether the kIOS contains a point
bool contain(const Vec3s& p) const;
/// @brief Check collision between two kIOS
bool overlap(const kIOS& other) const;
/// @brief Check collision between two kIOS and return the overlap part.
/// For kIOS, we return nothing, as the overlappart of two kIOS usually is not an kIOS
/// @todo Not efficient. It first checks the sphere collisions and then use OBB for further culling.
bool overlap(const kIOS& other, kIOS& overlap_part) const
{
return overlap(other);
}
/// @brief Check collision between two kIOS
bool overlap(const kIOS& other, const CollisionRequest&,
CoalScalar& sqrDistLowerBound) const;
/// @brief Check whether the kIOS contains a point
inline bool contain(const Vec3f& p) const;
/// @brief The distance between two kIOS
CoalScalar distance(const kIOS& other, Vec3s* P = NULL,
Vec3s* Q = NULL) const;
/// @brief A simple way to merge the kIOS and a point
kIOS& operator += (const Vec3f& p);
kIOS& operator+=(const Vec3s& p);
/// @brief Merge the kIOS and another kIOS
kIOS& operator += (const kIOS& other)
{
kIOS& operator+=(const kIOS& other) {
*this = *this + other;
return *this;
}
/// @brief Return the merged kIOS of current kIOS and the other one
kIOS operator + (const kIOS& other) const;
kIOS operator+(const kIOS& other) const;
/// @brief size of the kIOS (used in BV_Splitter to order two kIOSs)
CoalScalar size() const;
/// @brief Center of the kIOS
const Vec3f& center() const
{
return spheres[0].o;
}
const Vec3s& center() const { return spheres[0].o; }
/// @brief Width of the kIOS
FCL_REAL width() const;
CoalScalar width() const;
/// @brief Height of the kIOS
FCL_REAL height() const;
CoalScalar height() const;
/// @brief Depth of the kIOS
FCL_REAL depth() const;
CoalScalar depth() const;
/// @brief Volume of the kIOS
FCL_REAL volume() const;
/// @brief size of the kIOS (used in BV_Splitter to order two kIOSs)
FCL_REAL size() const;
/// @brief The distance between two kIOS
FCL_REAL distance(const kIOS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
CoalScalar volume() const;
};
/// @brief Translate the kIOS BV
kIOS translate(const kIOS& bv, const Vec3f& t);
COAL_DLLAPI kIOS translate(const kIOS& bv, const Vec3s& t);
/// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0)
/// and b2 is in identity.
/// @todo Not efficient
COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1,
const kIOS& b2);
/// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0) and b2 is in identity.
/// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0)
/// and b2 is in identity.
/// @todo Not efficient
bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2);
COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1,
const kIOS& b2, const CollisionRequest& request,
CoalScalar& sqrDistLowerBound);
/// @brief Approximate distance between two kIOS bounding volumes
/// @todo P and Q is not returned, need implementation
FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2, Vec3f* P = NULL, Vec3f* Q = NULL);
COAL_DLLAPI CoalScalar distance(const Matrix3s& R0, const Vec3s& T0,
const kIOS& b1, const kIOS& b2, Vec3s* P = NULL,
Vec3s* Q = NULL);
}
} // namespace coal
#endif
include/fcl/BVH/BVH_front.h → include/coal/BVH/BVH_front.h
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
......@@ -14,7 +15,7 @@
* 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
* * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
......@@ -34,44 +35,42 @@
/** \author Jia Pan */
#ifndef FCL_BVH_FRONT_H
#define FCL_BVH_FRONT_H
#ifndef COAL_BVH_FRONT_H
#define COAL_BVH_FRONT_H
#include <list>
namespace fcl
{
#include "coal/config.hh"
namespace coal {
/// @brief Front list acceleration for collision
/// Front list is a set of internal and leaf nodes in the BVTT hierarchy, where
/// the traversal terminates while performing a query during a given time instance. The front list reflects the subset of a
/// BVTT that is traversed for that particular proximity query.
struct BVHFrontNode
{
/// @brief The nodes to start in the future, i.e. the wave front of the traversal tree.
int left, right;
/// the traversal terminates while performing a query during a given time
/// instance. The front list reflects the subset of a BVTT that is traversed for
/// that particular proximity query.
struct COAL_DLLAPI BVHFrontNode {
/// @brief The nodes to start in the future, i.e. the wave front of the
/// traversal tree.
unsigned int left, right;
/// @brief The front node is not valid when collision is detected on the front node.
/// @brief The front node is not valid when collision is detected on the front
/// node.
bool valid;
BVHFrontNode(int left_, int right_) : left(left_),
right(right_),
valid(true)
{
}
BVHFrontNode(unsigned int left_, unsigned int right_)
: left(left_), right(right_), valid(true) {}
};
/// @brief BVH front list is a list of front nodes.
typedef std::list<BVHFrontNode> BVHFrontList;
/// @brief Add new front node into the front list
inline void updateFrontList(BVHFrontList* front_list, int b1, int b2)
{
if(front_list) front_list->push_back(BVHFrontNode(b1, b2));
inline void updateFrontList(BVHFrontList* front_list, unsigned int b1,
unsigned int b2) {
if (front_list) front_list->push_back(BVHFrontNode(b1, b2));
}
}
} // namespace coal
#endif
include/fcl/BVH/BVH_internal.h → include/coal/BVH/BVH_internal.h
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
......@@ -14,7 +15,7 @@
* 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
* * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
......@@ -34,51 +35,53 @@
/** \author Jia Pan */
#ifndef FCL_BVH_INTERNAL_H
#define FCL_BVH_INTERNAL_H
#ifndef COAL_BVH_INTERNAL_H
#define COAL_BVH_INTERNAL_H
#include "fcl/data_types.h"
#include "coal/data_types.h"
namespace fcl
{
namespace coal {
/// @brief States for BVH construction
/// empty->begun->processed ->replace_begun->processed -> ......
/// |
/// |-> update_begun -> updated -> .....
enum BVHBuildState
{
BVH_BUILD_STATE_EMPTY, /// empty state, immediately after constructor
BVH_BUILD_STATE_BEGUN, /// after beginModel(), state for adding geometry primitives
BVH_BUILD_STATE_PROCESSED, /// after tree has been build, ready for cd use
BVH_BUILD_STATE_UPDATE_BEGUN, /// after beginUpdateModel(), state for updating geometry primitives
BVH_BUILD_STATE_UPDATED, /// after tree has been build for updated geometry, ready for ccd use
BVH_BUILD_STATE_REPLACE_BEGUN, /// after beginReplaceModel(), state for replacing geometry primitives
};
enum BVHBuildState {
BVH_BUILD_STATE_EMPTY, /// empty state, immediately after constructor
BVH_BUILD_STATE_BEGUN, /// after beginModel(), state for adding geometry
/// primitives
BVH_BUILD_STATE_PROCESSED, /// after tree has been build, ready for cd use
BVH_BUILD_STATE_UPDATE_BEGUN, /// after beginUpdateModel(), state for
/// updating geometry primitives
BVH_BUILD_STATE_UPDATED, /// after tree has been build for updated geometry,
/// ready for ccd use
BVH_BUILD_STATE_REPLACE_BEGUN /// after beginReplaceModel(), state for
/// replacing geometry primitives
};
/// @brief Error code for BVH
enum BVHReturnCode
{
BVH_OK = 0, /// BVH is valid
BVH_ERR_MODEL_OUT_OF_MEMORY = -1, /// Cannot allocate memory for vertices and triangles
BVH_ERR_BUILD_OUT_OF_SEQUENCE = -2, /// BVH construction does not follow correct sequence
BVH_ERR_BUILD_EMPTY_MODEL = -3, /// BVH geometry is not prepared
BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME = -4, /// BVH geometry in previous frame is not prepared
BVH_ERR_UNSUPPORTED_FUNCTION = -5, /// BVH funtion is not supported
BVH_ERR_UNUPDATED_MODEL = -6, /// BVH model update failed
BVH_ERR_INCORRECT_DATA = -7, /// BVH data is not valid
BVH_ERR_UNKNOWN = -8 /// Unknown failure
};
/// @brief Error code for BVH
enum BVHReturnCode {
BVH_OK = 0, /// BVH is valid
BVH_ERR_MODEL_OUT_OF_MEMORY =
-1, /// Cannot allocate memory for vertices and triangles
BVH_ERR_BUILD_OUT_OF_SEQUENCE =
-2, /// BVH construction does not follow correct sequence
BVH_ERR_BUILD_EMPTY_MODEL = -3, /// BVH geometry is not prepared
BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME =
-4, /// BVH geometry in previous frame is not prepared
BVH_ERR_UNSUPPORTED_FUNCTION = -5, /// BVH funtion is not supported
BVH_ERR_UNUPDATED_MODEL = -6, /// BVH model update failed
BVH_ERR_INCORRECT_DATA = -7, /// BVH data is not valid
BVH_ERR_UNKNOWN = -8 /// Unknown failure
};
/// @brief BVH model type
enum BVHModelType
{
BVH_MODEL_UNKNOWN, /// @brief unknown model type
BVH_MODEL_TRIANGLES, /// @brief triangle model
BVH_MODEL_POINTCLOUD /// @brief point cloud model
};
enum BVHModelType {
BVH_MODEL_UNKNOWN, /// @brief unknown model type
BVH_MODEL_TRIANGLES, /// @brief triangle model
BVH_MODEL_POINTCLOUD /// @brief point cloud model
};
}
} // namespace coal
#endif
include/coal/BVH/BVH_model.h 0 → 100644
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2015, Open Source Robotics Foundation
* Copyright (c) 2020-2022, INRIA
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/** \author Jia Pan */
#ifndef COAL_BVH_MODEL_H
#define COAL_BVH_MODEL_H
#include "coal/fwd.hh"
#include "coal/collision_object.h"
#include "coal/BVH/BVH_internal.h"
#include "coal/BV/BV_node.h"
#include <vector>
#include <memory>
#include <iostream>
namespace coal {
/// @addtogroup Construction_Of_BVH
/// @{
class ConvexBase;
template <typename BV>
class BVFitter;
template <typename BV>
class BVSplitter;
/// @brief A base class describing the bounding hierarchy of a mesh model or a
/// point cloud model (which is viewed as a degraded version of mesh)
class COAL_DLLAPI BVHModelBase : public CollisionGeometry {
public:
/// @brief Geometry point data
std::shared_ptr<std::vector<Vec3s>> vertices;
/// @brief Geometry triangle index data, will be NULL for point clouds
std::shared_ptr<std::vector<Triangle>> tri_indices;
/// @brief Geometry point data in previous frame
std::shared_ptr<std::vector<Vec3s>> prev_vertices;
/// @brief Number of triangles
unsigned int num_tris;
/// @brief Number of points
unsigned int num_vertices;
/// @brief The state of BVH building process
BVHBuildState build_state;
/// @brief Convex<Triangle> representation of this object
shared_ptr<ConvexBase> convex;
/// @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
BVHModelBase();
/// @brief copy from another BVH
BVHModelBase(const BVHModelBase& other);
/// @brief deconstruction, delete mesh data related.
virtual ~BVHModelBase() {}
/// @brief Get the object type: it is a BVH
OBJECT_TYPE getObjectType() const { return OT_BVH; }
/// @brief Compute the AABB for the BVH, used for broad-phase collision
void computeLocalAABB();
/// @brief Begin a new BVH model
int beginModel(unsigned int num_tris = 0, unsigned int num_vertices = 0);
/// @brief Add one point in the new BVH model
int addVertex(const Vec3s& p);
/// @brief Add points in the new BVH model
int addVertices(const MatrixX3s& points);
/// @brief Add triangles in the new BVH model
int addTriangles(const Matrixx3i& triangles);
/// @brief Add one triangle in the new BVH model
int addTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3);
/// @brief Add a set of triangles in the new BVH model
int addSubModel(const std::vector<Vec3s>& ps,
const std::vector<Triangle>& ts);
/// @brief Add a set of points in the new BVH model
int addSubModel(const std::vector<Vec3s>& 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 Vec3s& p);
/// @brief Replace one triangle in the old BVH model
int replaceTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3);
/// @brief Replace a set of points in the old BVH model
int replaceSubModel(const std::vector<Vec3s>& 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 Vec3s& p);
/// @brief Update one triangle in the old BVH model
int updateTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3);
/// @brief Update a set of points in the old BVH model
int updateSubModel(const std::vector<Vec3s>& ps);
/// @brief End BVH model update, will also refit or rebuild the bounding
/// volume hierarchy
int endUpdateModel(bool refit = true, bool bottomup = true);
/// @brief Build this \ref Convex "Convex<Triangle>" representation of this
/// model. The result is stored in attribute \ref convex. \note this only
/// takes the points of this model. It does not check that the
/// object is convex. It does not compute a convex hull.
void buildConvexRepresentation(bool share_memory);
/// @brief Build a convex hull
/// and store it in attribute \ref convex.
/// \param keepTriangle whether the convex should be triangulated.
/// \param qhullCommand see \ref ConvexBase::convexHull.
/// \return \c true if this object is convex, hence the convex hull represents
/// the same object.
/// \sa ConvexBase::convexHull
/// \warning At the moment, the return value only checks whether there are as
/// many points in the convex hull as in the original object. This is
/// neither necessary (duplicated vertices get merged) nor sufficient
/// (think of a U with 4 vertices and 3 edges).
bool buildConvexHull(bool keepTriangle, const char* qhullCommand = NULL);
virtual int memUsage(const bool msg = false) const = 0;
/// @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.
virtual void makeParentRelative() = 0;
Vec3s computeCOM() const {
CoalScalar vol = 0;
Vec3s com(0, 0, 0);
if (!(vertices.get())) {
std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain "
"vertices."
<< std::endl;
return com;
}
const std::vector<Vec3s>& vertices_ = *vertices;
if (!(tri_indices.get())) {
std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain "
"triangles."
<< std::endl;
return com;
}
const std::vector<Triangle>& tri_indices_ = *tri_indices;
for (unsigned int i = 0; i < num_tris; ++i) {
const Triangle& tri = tri_indices_[i];
CoalScalar 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);
}
CoalScalar computeVolume() const {
CoalScalar vol = 0;
if (!(vertices.get())) {
std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain "
"vertices."
<< std::endl;
return vol;
}
const std::vector<Vec3s>& vertices_ = *vertices;
if (!(tri_indices.get())) {
std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain "
"triangles."
<< std::endl;
return vol;
}
const std::vector<Triangle>& tri_indices_ = *tri_indices;
for (unsigned int i = 0; i < num_tris; ++i) {
const Triangle& tri = tri_indices_[i];
CoalScalar d_six_vol =
(vertices_[tri[0]].cross(vertices_[tri[1]])).dot(vertices_[tri[2]]);
vol += d_six_vol;
}
return vol / 6;
}
Matrix3s computeMomentofInertia() const {
Matrix3s C = Matrix3s::Zero();
Matrix3s C_canonical;
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;
if (!(vertices.get())) {
std::cerr << "BVH Error in `computeMomentofInertia`! The BVHModel does "
"not contain vertices."
<< std::endl;
return C;
}
const std::vector<Vec3s>& vertices_ = *vertices;
if (!(vertices.get())) {
std::cerr << "BVH Error in `computeMomentofInertia`! The BVHModel does "
"not contain vertices."
<< std::endl;
return C;
}
const std::vector<Triangle>& tri_indices_ = *tri_indices;
for (unsigned int i = 0; i < num_tris; ++i) {
const Triangle& tri = tri_indices_[i];
const Vec3s& v1 = vertices_[tri[0]];
const Vec3s& v2 = vertices_[tri[1]];
const Vec3s& v3 = vertices_[tri[2]];
Matrix3s A;
A << v1.transpose(), v2.transpose(), v3.transpose();
C += A.derived().transpose() * C_canonical * A * (v1.cross(v2)).dot(v3);
}
return C.trace() * Matrix3s::Identity() - C;
}
protected:
virtual void deleteBVs() = 0;
virtual bool allocateBVs() = 0;
/// @brief Build the bounding volume hierarchy
virtual int buildTree() = 0;
/// @brief Refit the bounding volume hierarchy
virtual int refitTree(bool bottomup) = 0;
unsigned int num_tris_allocated;
unsigned int num_vertices_allocated;
unsigned int num_vertex_updated; /// for ccd vertex update
protected:
/// \brief Comparison operators
virtual bool isEqual(const CollisionGeometry& other) const;
};
/// @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) \tparam BV one
/// of the bounding volume class in \ref Bounding_Volume.
template <typename BV>
class COAL_DLLAPI BVHModel : public BVHModelBase {
typedef BVHModelBase Base;
public:
using bv_node_vector_t =
std::vector<BVNode<BV>, Eigen::aligned_allocator<BVNode<BV>>>;
/// @brief Split rule to split one BV node into two children
shared_ptr<BVSplitter<BV>> bv_splitter;
/// @brief Fitting rule to fit a BV node to a set of geometry primitives
shared_ptr<BVFitter<BV>> bv_fitter;
/// @brief Default constructor to build an empty BVH
BVHModel();
/// @brief Copy constructor from another BVH
///
/// \param[in] other BVHModel to copy.
///
BVHModel(const BVHModel& other);
/// @brief Clone *this into a new BVHModel
virtual BVHModel<BV>* clone() const { return new BVHModel(*this); }
/// @brief deconstruction, delete mesh data related.
~BVHModel() {}
/// @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(unsigned int i) const {
assert(i < num_bvs);
return (*bvs)[i];
}
/// @brief Access the bv giving the its index
BVNode<BV>& getBV(unsigned int i) {
assert(i < num_bvs);
return (*bvs)[i];
}
/// @brief Get the number of bv in the BVH
unsigned int getNumBVs() const { return num_bvs; }
/// @brief Get the BV type: default is unknown
NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
/// @brief Check the number of memory used
int memUsage(const bool 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() {
Matrix3s I(Matrix3s::Identity());
makeParentRelativeRecurse(0, I, Vec3s::Zero());
}
protected:
void deleteBVs();
bool allocateBVs();
unsigned int num_bvs_allocated;
std::shared_ptr<std::vector<unsigned int>> primitive_indices;
/// @brief Bounding volume hierarchy
std::shared_ptr<bv_node_vector_t> bvs;
/// @brief Number of BV nodes in bounding volume hierarchy
unsigned 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, unsigned int first_primitive,
unsigned 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, Matrix3s& parent_axes,
const Vec3s& parent_c) {
bv_node_vector_t& bvs_ = *bvs;
if (!bvs_[static_cast<size_t>(bv_id)].isLeaf()) {
makeParentRelativeRecurse(bvs_[static_cast<size_t>(bv_id)].first_child,
parent_axes,
bvs_[static_cast<size_t>(bv_id)].getCenter());
makeParentRelativeRecurse(
bvs_[static_cast<size_t>(bv_id)].first_child + 1, parent_axes,
bvs_[static_cast<size_t>(bv_id)].getCenter());
}
bvs_[static_cast<size_t>(bv_id)].bv =
translate(bvs_[static_cast<size_t>(bv_id)].bv, -parent_c);
}
private:
virtual bool isEqual(const CollisionGeometry& _other) const {
const BVHModel* other_ptr = dynamic_cast<const BVHModel*>(&_other);
if (other_ptr == nullptr) return false;
const BVHModel& other = *other_ptr;
bool res = Base::isEqual(other);
if (!res) return false;
// unsigned int other_num_primitives = 0;
// if(other.primitive_indices)
// {
// switch(other.getModelType())
// {
// case BVH_MODEL_TRIANGLES:
// other_num_primitives = num_tris;
// break;
// case BVH_MODEL_POINTCLOUD:
// other_num_primitives = num_vertices;
// break;
// default:
// ;
// }
// }
// unsigned int num_primitives = 0;
// if(primitive_indices)
// {
//
// switch(other.getModelType())
// {
// case BVH_MODEL_TRIANGLES:
// num_primitives = num_tris;
// break;
// case BVH_MODEL_POINTCLOUD:
// num_primitives = num_vertices;
// break;
// default:
// ;
// }
// }
//
// if(num_primitives != other_num_primitives)
// return false;
//
// for(int k = 0; k < num_primitives; ++k)
// {
// if(primitive_indices[k] != other.primitive_indices[k])
// return false;
// }
if (num_bvs != other.num_bvs) return false;
if ((!(bvs.get()) && other.bvs.get()) || (bvs.get() && !(other.bvs.get())))
return false;
if (bvs.get() && other.bvs.get()) {
const bv_node_vector_t& bvs_ = *bvs;
const bv_node_vector_t& other_bvs_ = *(other.bvs);
for (unsigned int k = 0; k < num_bvs; ++k) {
if (bvs_[k] != other_bvs_[k]) return false;
}
}
return true;
}
};
/// @}
template <>
void BVHModel<OBB>::makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes,
const Vec3s& parent_c);
template <>
void BVHModel<RSS>::makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes,
const Vec3s& parent_c);
template <>
void BVHModel<OBBRSS>::makeParentRelativeRecurse(int bv_id,
Matrix3s& parent_axes,
const Vec3s& 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;
} // namespace coal
#endif
include/coal/BVH/BVH_utility.h 0 → 100644
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/** \author Jia Pan */
#ifndef COAL_BVH_UTILITY_H
#define COAL_BVH_UTILITY_H
#include "coal/BVH/BVH_model.h"
namespace coal {
/// @brief Extract the part of the BVHModel that is inside an AABB.
/// A triangle in collision with the AABB is considered inside.
template <typename BV>
COAL_DLLAPI BVHModel<BV>* BVHExtract(const BVHModel<BV>& model,
const Transform3s& pose, const AABB& aabb);
template <>
COAL_DLLAPI BVHModel<OBB>* BVHExtract(const BVHModel<OBB>& model,
const Transform3s& pose,
const AABB& aabb);
template <>
COAL_DLLAPI BVHModel<AABB>* BVHExtract(const BVHModel<AABB>& model,
const Transform3s& pose,
const AABB& aabb);
template <>
COAL_DLLAPI BVHModel<RSS>* BVHExtract(const BVHModel<RSS>& model,
const Transform3s& pose,
const AABB& aabb);
template <>
COAL_DLLAPI BVHModel<kIOS>* BVHExtract(const BVHModel<kIOS>& model,
const Transform3s& pose,
const AABB& aabb);
template <>
COAL_DLLAPI BVHModel<OBBRSS>* BVHExtract(const BVHModel<OBBRSS>& model,
const Transform3s& pose,
const AABB& aabb);
template <>
COAL_DLLAPI BVHModel<KDOP<16> >* BVHExtract(const BVHModel<KDOP<16> >& model,
const Transform3s& pose,
const AABB& aabb);
template <>
COAL_DLLAPI BVHModel<KDOP<18> >* BVHExtract(const BVHModel<KDOP<18> >& model,
const Transform3s& pose,
const AABB& aabb);
template <>
COAL_DLLAPI BVHModel<KDOP<24> >* BVHExtract(const BVHModel<KDOP<24> >& model,
const Transform3s& pose,
const AABB& aabb);
/// @brief Compute the covariance matrix for a set or subset of points. if ts =
/// null, then indices refer to points directly; otherwise refer to triangles
COAL_DLLAPI void getCovariance(Vec3s* ps, Vec3s* ps2, Triangle* ts,
unsigned int* indices, unsigned int n,
Matrix3s& M);
/// @brief Compute the RSS bounding volume parameters: radius, rectangle size
/// and the origin, given the BV axises.
COAL_DLLAPI void getRadiusAndOriginAndRectangleSize(
Vec3s* ps, Vec3s* ps2, Triangle* ts, unsigned int* indices, unsigned int n,
const Matrix3s& axes, Vec3s& origin, CoalScalar l[2], CoalScalar& r);
/// @brief Compute the bounding volume extent and center for a set or subset of
/// points, given the BV axises.
COAL_DLLAPI void getExtentAndCenter(Vec3s* ps, Vec3s* ps2, Triangle* ts,
unsigned int* indices, unsigned int n,
Matrix3s& axes, Vec3s& center,
Vec3s& extent);
/// @brief Compute the center and radius for a triangle's circumcircle
COAL_DLLAPI void circumCircleComputation(const Vec3s& a, const Vec3s& b,
const Vec3s& c, Vec3s& center,
CoalScalar& radius);
/// @brief Compute the maximum distance from a given center point to a point
/// cloud
COAL_DLLAPI CoalScalar maximumDistance(Vec3s* ps, Vec3s* ps2, Triangle* ts,
unsigned int* indices, unsigned int n,
const Vec3s& query);
} // namespace coal
#endif
include/coal/broadphase/broadphase.h 0 → 100644
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2022, Inria
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef COAL_BROADPHASE_BROADPHASE_H
#define COAL_BROADPHASE_BROADPHASE_H
#include "coal/broadphase/broadphase_dynamic_AABB_tree.h"
#include "coal/broadphase/broadphase_dynamic_AABB_tree_array.h"
#include "coal/broadphase/broadphase_bruteforce.h"
#include "coal/broadphase/broadphase_SaP.h"
#include "coal/broadphase/broadphase_SSaP.h"
#include "coal/broadphase/broadphase_interval_tree.h"
#include "coal/broadphase/broadphase_spatialhash.h"
#include "coal/broadphase/default_broadphase_callbacks.h"
#endif // ifndef COAL_BROADPHASE_BROADPHASE_H
include/fcl/broadphase/broadphase_SSaP.h → include/coal/broadphase/broadphase_SSaP.h
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2016, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
......@@ -14,7 +15,7 @@
* 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
* * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
......@@ -30,25 +31,25 @@
* 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 */
/** @author Jia Pan */
#ifndef COAL_BROAD_PHASE_SSAP_H
#define COAL_BROAD_PHASE_SSAP_H
#ifndef FCL_BROAD_PHASE_SSAP_H
#define FCL_BROAD_PHASE_SSAP_H
#include <vector>
#include "coal/broadphase/broadphase_collision_manager.h"
#include "fcl/broadphase/broadphase.h"
namespace coal {
namespace fcl
{
/// @brief Simple SAP collision manager
class COAL_DLLAPI SSaPCollisionManager : public BroadPhaseCollisionManager {
public:
typedef BroadPhaseCollisionManager Base;
using Base::getObjects;
/// @brief Simple SAP collision manager
class SSaPCollisionManager : public BroadPhaseCollisionManager
{
public:
SSaPCollisionManager() : setup_(false)
{}
SSaPCollisionManager();
/// @brief remove one object from the manager
void registerObject(CollisionObject* obj);
......@@ -60,7 +61,7 @@ public:
void setup();
/// @brief update the condition of manager
void update();
virtual void update();
/// @brief clear the manager
void clear();
......@@ -68,75 +69,63 @@ public:
/// @brief return the objects managed by the manager
void getObjects(std::vector<CollisionObject*>& objs) const;
/// @brief perform collision test between one object and all the objects belonging to the manager
void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
/// @brief perform collision test between one object and all the objects
/// belonging to the manager
void collide(CollisionObject* obj, CollisionCallBackBase* callback) const;
/// @brief perform distance computation between one object and all the objects belonging to the manager
void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const;
/// @brief perform distance computation between one object and all the objects
/// belonging to the manager
void distance(CollisionObject* obj, DistanceCallBackBase* callback) const;
/// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision)
void collide(void* cdata, CollisionCallBack callback) const;
/// @brief perform collision test for the objects belonging to the manager
/// (i.e., N^2 self collision)
void collide(CollisionCallBackBase* callback) const;
/// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance)
void distance(void* cdata, DistanceCallBack callback) const;
/// @brief perform distance test for the objects belonging to the manager
/// (i.e., N^2 self distance)
void distance(DistanceCallBackBase* callback) const;
/// @brief perform collision test with objects belonging to another manager
void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const;
void collide(BroadPhaseCollisionManager* other_manager,
CollisionCallBackBase* callback) const;
/// @brief perform distance test with objects belonging to another manager
void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const;
void distance(BroadPhaseCollisionManager* other_manager,
DistanceCallBackBase* callback) const;
/// @brief whether the manager is empty
bool empty() const;
/// @brief the number of objects managed by the manager
inline size_t size() const { return objs_x.size(); }
protected:
/// @brief check collision between one object and a list of objects, return value is whether stop is possible
bool checkColl(std::vector<CollisionObject*>::const_iterator pos_start, std::vector<CollisionObject*>::const_iterator pos_end,
CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
/// @brief check distance between one object and a list of objects, return value is whether stop is possible
bool checkDis(std::vector<CollisionObject*>::const_iterator pos_start, std::vector<CollisionObject*>::const_iterator pos_end,
CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
bool collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
static inline size_t selectOptimalAxis(const std::vector<CollisionObject*>& objs_x, const std::vector<CollisionObject*>& objs_y, const std::vector<CollisionObject*>& objs_z, std::vector<CollisionObject*>::const_iterator& it_beg, std::vector<CollisionObject*>::const_iterator& it_end)
{
/// simple sweep and prune method
double delta_x = (objs_x[objs_x.size() - 1])->getAABB().min_[0] - (objs_x[0])->getAABB().min_[0];
double delta_y = (objs_x[objs_y.size() - 1])->getAABB().min_[1] - (objs_y[0])->getAABB().min_[1];
double delta_z = (objs_z[objs_z.size() - 1])->getAABB().min_[2] - (objs_z[0])->getAABB().min_[2];
int axis = 0;
if(delta_y > delta_x && delta_y > delta_z)
axis = 1;
else if(delta_z > delta_y && delta_z > delta_x)
axis = 2;
switch(axis)
{
case 0:
it_beg = objs_x.begin();
it_end = objs_x.end();
break;
case 1:
it_beg = objs_y.begin();
it_end = objs_y.end();
break;
case 2:
it_beg = objs_z.begin();
it_end = objs_z.end();
break;
}
return axis;
}
/// @brief the number of objects managed by the manager
size_t size() const;
protected:
/// @brief check collision between one object and a list of objects, return
/// value is whether stop is possible
bool checkColl(
typename std::vector<CollisionObject*>::const_iterator pos_start,
typename std::vector<CollisionObject*>::const_iterator pos_end,
CollisionObject* obj, CollisionCallBackBase* callback) const;
/// @brief check distance between one object and a list of objects, return
/// value is whether stop is possible
bool checkDis(
typename std::vector<CollisionObject*>::const_iterator pos_start,
typename std::vector<CollisionObject*>::const_iterator pos_end,
CollisionObject* obj, DistanceCallBackBase* callback,
CoalScalar& min_dist) const;
bool collide_(CollisionObject* obj, CollisionCallBackBase* callback) const;
bool distance_(CollisionObject* obj, DistanceCallBackBase* callback,
CoalScalar& min_dist) const;
static int selectOptimalAxis(
const std::vector<CollisionObject*>& objs_x,
const std::vector<CollisionObject*>& objs_y,
const std::vector<CollisionObject*>& objs_z,
typename std::vector<CollisionObject*>::const_iterator& it_beg,
typename std::vector<CollisionObject*>::const_iterator& it_end);
/// @brief Objects sorted according to lower x value
std::vector<CollisionObject*> objs_x;
......@@ -147,11 +136,11 @@ protected:
/// @brief Objects sorted according to lower z value
std::vector<CollisionObject*> objs_z;
/// @brief tag about whether the environment is maintained suitably (i.e., the objs_x, objs_y, objs_z are sorted correctly
/// @brief tag about whether the environment is maintained suitably (i.e., the
/// objs_x, objs_y, objs_z are sorted correctly
bool setup_;
};
}
} // namespace coal
#endif
include/fcl/broadphase/broadphase_SaP.h → include/coal/broadphase/broadphase_SaP.h
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2016, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
......@@ -14,7 +15,7 @@
* 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
* * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
......@@ -30,39 +31,29 @@
* 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 */
/** @author Jia Pan */
#ifndef FCL_BROAD_PHASE_SAP_H
#define FCL_BROAD_PHASE_SAP_H
#include "fcl/broadphase/broadphase.h"
#ifndef COAL_BROAD_PHASE_SAP_H
#define COAL_BROAD_PHASE_SAP_H
#include <map>
#include <list>
namespace fcl
{
#include "coal/broadphase/broadphase_collision_manager.h"
/// @brief Rigorous SAP collision manager
class SaPCollisionManager : public BroadPhaseCollisionManager
{
public:
namespace coal {
SaPCollisionManager()
{
elist[0] = NULL;
elist[1] = NULL;
elist[2] = NULL;
/// @brief Rigorous SAP collision manager
class COAL_DLLAPI SaPCollisionManager : public BroadPhaseCollisionManager {
public:
typedef BroadPhaseCollisionManager Base;
using Base::getObjects;
optimal_axis = 0;
}
SaPCollisionManager();
~SaPCollisionManager()
{
clear();
}
~SaPCollisionManager();
/// @brief add objects to the manager
void registerObjects(const std::vector<CollisionObject*>& other_objs);
......@@ -77,7 +68,7 @@ public:
void setup();
/// @brief update the condition of manager
void update();
virtual void update();
/// @brief update the manager by explicitly given the object updated
void update(CollisionObject* updated_obj);
......@@ -91,37 +82,41 @@ public:
/// @brief return the objects managed by the manager
void getObjects(std::vector<CollisionObject*>& objs) const;
/// @brief perform collision test between one object and all the objects belonging to the manager
void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
/// @brief perform collision test between one object and all the objects
/// belonging to the manager
void collide(CollisionObject* obj, CollisionCallBackBase* callback) const;
/// @brief perform distance computation between one object and all the objects belonging to the manager
void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const;
/// @brief perform distance computation between one object and all the objects
/// belonging to the manager
void distance(CollisionObject* obj, DistanceCallBackBase* callback) const;
/// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision)
void collide(void* cdata, CollisionCallBack callback) const;
/// @brief perform collision test for the objects belonging to the manager
/// (i.e., N^2 self collision)
void collide(CollisionCallBackBase* callback) const;
/// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance)
void distance(void* cdata, DistanceCallBack callback) const;
/// @brief perform distance test for the objects belonging to the manager
/// (i.e., N^2 self distance)
void distance(DistanceCallBackBase* callback) const;
/// @brief perform collision test with objects belonging to another manager
void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const;
void collide(BroadPhaseCollisionManager* other_manager,
CollisionCallBackBase* callback) const;
/// @brief perform distance test with objects belonging to another manager
void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const;
void distance(BroadPhaseCollisionManager* other_manager,
DistanceCallBackBase* callback) const;
/// @brief whether the manager is empty
bool empty() const;
/// @brief the number of objects managed by the manager
inline size_t size() const { return AABB_arr.size(); }
protected:
/// @brief the number of objects managed by the manager
size_t size() const;
protected:
struct EndPoint;
/// @brief SAP interval for one object
struct SaPAABB
{
struct SaPAABB {
/// @brief object
CollisionObject* obj;
......@@ -136,9 +131,9 @@ protected:
};
/// @brief End point for an interval
struct EndPoint
{
/// @brief tag for whether it is a lower bound or higher bound of an interval, 0 for lo, and 1 for hi
struct EndPoint {
/// @brief tag for whether it is a lower bound or higher bound of an
/// interval, 0 for lo, and 1 for hi
char minmax;
/// @brief back pointer to SAP interval
......@@ -146,116 +141,61 @@ protected:
/// @brief the previous end point in the end point list
EndPoint* prev[3];
/// @brief the next end point in the end point list
EndPoint* next[3];
/// @brief get the value of the end point
inline const Vec3f& getVal() const
{
if(minmax) return aabb->cached.max_;
else return aabb->cached.min_;
}
const Vec3s& getVal() const;
/// @brief set the value of the end point
inline Vec3f& getVal()
{
if(minmax) return aabb->cached.max_;
else return aabb->cached.min_;
}
inline Vec3f::U getVal(size_t i) const
{
if(minmax) return aabb->cached.max_[i];
else return aabb->cached.min_[i];
}
inline Vec3f::U& getVal(size_t i)
{
if(minmax) return aabb->cached.max_[i];
else return aabb->cached.min_[i];
}
Vec3s& getVal();
CoalScalar getVal(int i) const;
CoalScalar& getVal(int i);
};
/// @brief A pair of objects that are not culling away and should further check collision
struct SaPPair
{
SaPPair(CollisionObject* a, CollisionObject* b)
{
if(a < b)
{
obj1 = a;
obj2 = b;
}
else
{
obj1 = b;
obj2 = a;
}
}
/// @brief A pair of objects that are not culling away and should further
/// check collision
struct SaPPair {
SaPPair(CollisionObject* a, CollisionObject* b);
CollisionObject* obj1;
CollisionObject* obj2;
bool operator == (const SaPPair& other) const
{
return ((obj1 == other.obj1) && (obj2 == other.obj2));
}
bool operator==(const SaPPair& other) const;
};
/// @brief Functor to help unregister one object
class isUnregistered
{
class COAL_DLLAPI isUnregistered {
CollisionObject* obj;
public:
isUnregistered(CollisionObject* obj_) : obj(obj_)
{}
public:
isUnregistered(CollisionObject* obj_);
bool operator() (const SaPPair& pair) const
{
return (pair.obj1 == obj) || (pair.obj2 == obj);
}
bool operator()(const SaPPair& pair) const;
};
/// @brief Functor to help remove collision pairs no longer valid (i.e., should be culled away)
class isNotValidPair
{
/// @brief Functor to help remove collision pairs no longer valid (i.e.,
/// should be culled away)
class COAL_DLLAPI isNotValidPair {
CollisionObject* obj1;
CollisionObject* obj2;
public:
isNotValidPair(CollisionObject* obj1_, CollisionObject* obj2_) : obj1(obj1_),
obj2(obj2_)
{}
public:
isNotValidPair(CollisionObject* obj1_, CollisionObject* obj2_);
bool operator() (const SaPPair& pair)
{
return (pair.obj1 == obj1) && (pair.obj2 == obj2);
}
bool operator()(const SaPPair& pair);
};
void update_(SaPAABB* updated_aabb);
void updateVelist()
{
for(int coord = 0; coord < 3; ++coord)
{
velist[coord].resize(size() * 2);
EndPoint* current = elist[coord];
size_t id = 0;
while(current)
{
velist[coord][id] = current;
current = current->next[coord];
id++;
}
}
}
void updateVelist();
/// @brief End point list for x, y, z coordinates
EndPoint* elist[3];
/// @brief vector version of elist, for acceleration
std::vector<EndPoint*> velist[3];
......@@ -265,52 +205,20 @@ protected:
/// @brief The pair of objects that should further check for collision
std::list<SaPPair> overlap_pairs;
size_t optimal_axis;
int optimal_axis;
std::map<CollisionObject*, SaPAABB*> obj_aabb_map;
bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
bool collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
void addToOverlapPairs(const SaPPair& p)
{
bool repeated = false;
for(std::list<SaPPair>::iterator it = overlap_pairs.begin(), end = overlap_pairs.end();
it != end;
++it)
{
if(*it == p)
{
repeated = true;
break;
}
}
if(!repeated)
overlap_pairs.push_back(p);
}
void removeFromOverlapPairs(const SaPPair& p)
{
for(std::list<SaPPair>::iterator it = overlap_pairs.begin(), end = overlap_pairs.end();
it != end;
++it)
{
if(*it == p)
{
overlap_pairs.erase(it);
break;
}
}
// or overlap_pairs.remove_if(isNotValidPair(p));
}
};
bool distance_(CollisionObject* obj, DistanceCallBackBase* callback,
CoalScalar& min_dist) const;
bool collide_(CollisionObject* obj, CollisionCallBackBase* callback) const;
void addToOverlapPairs(const SaPPair& p);
}
void removeFromOverlapPairs(const SaPPair& p);
};
} // namespace coal
#endif
include/fcl/broadphase/broadphase_bruteforce.h → include/coal/broadphase/broadphase_bruteforce.h
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2016, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
......@@ -14,7 +15,7 @@
* 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
* * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
......@@ -30,25 +31,25 @@
* 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 */
/** @author Jia Pan */
#ifndef FCL_BROAD_PHASE_BRUTE_FORCE_H
#define FCL_BROAD_PHASE_BRUTE_FORCE_H
#ifndef COAL_BROAD_PHASE_BRUTE_FORCE_H
#define COAL_BROAD_PHASE_BRUTE_FORCE_H
#include "fcl/broadphase/broadphase.h"
#include <list>
#include "coal/broadphase/broadphase_collision_manager.h"
namespace fcl
{
namespace coal {
/// @brief Brute force N-body collision manager
class NaiveCollisionManager : public BroadPhaseCollisionManager
{
public:
NaiveCollisionManager() {}
class COAL_DLLAPI NaiveCollisionManager : public BroadPhaseCollisionManager {
public:
typedef BroadPhaseCollisionManager Base;
using Base::getObjects;
NaiveCollisionManager();
/// @brief add objects to the manager
void registerObjects(const std::vector<CollisionObject*>& other_objs);
......@@ -63,7 +64,7 @@ public:
void setup();
/// @brief update the condition of manager
void update();
virtual void update();
/// @brief clear the manager
void clear();
......@@ -71,37 +72,41 @@ public:
/// @brief return the objects managed by the manager
void getObjects(std::vector<CollisionObject*>& objs) const;
/// @brief perform collision test between one object and all the objects belonging to the manager
void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
/// @brief perform collision test between one object and all the objects
/// belonging to the manager
void collide(CollisionObject* obj, CollisionCallBackBase* callback) const;
/// @brief perform distance computation between one object and all the objects belonging to the manager
void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const;
/// @brief perform distance computation between one object and all the objects
/// belonging to the manager
void distance(CollisionObject* obj, DistanceCallBackBase* callback) const;
/// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision)
void collide(void* cdata, CollisionCallBack callback) const;
/// @brief perform collision test for the objects belonging to the manager
/// (i.e., N^2 self collision)
void collide(CollisionCallBackBase* callback) const;
/// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance)
void distance(void* cdata, DistanceCallBack callback) const;
/// @brief perform distance test for the objects belonging to the manager
/// (i.e., N^2 self distance)
void distance(DistanceCallBackBase* callback) const;
/// @brief perform collision test with objects belonging to another manager
void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const;
void collide(BroadPhaseCollisionManager* other_manager,
CollisionCallBackBase* callback) const;
/// @brief perform distance test with objects belonging to another manager
void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const;
void distance(BroadPhaseCollisionManager* other_manager,
DistanceCallBackBase* callback) const;
/// @brief whether the manager is empty
bool empty() const;
/// @brief the number of objects managed by the manager
inline size_t size() const { return objs.size(); }
protected:
/// @brief the number of objects managed by the manager
size_t size() const;
protected:
/// @brief objects belonging to the manager are stored in a list structure
std::list<CollisionObject*> objs;
};
}
} // namespace coal
#endif
include/coal/broadphase/broadphase_callbacks.h 0 → 100644
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2022, INRIA
* 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 copyright holder 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 Justin Carpentier (justin.carpentier@inria.fr) */
#ifndef COAL_BROADPHASE_BROAD_PHASE_CALLBACKS_H
#define COAL_BROADPHASE_BROAD_PHASE_CALLBACKS_H
#include "coal/fwd.hh"
#include "coal/data_types.h"
namespace coal {
/// @brief Base callback class for collision queries.
/// This class can be supersed by child classes to provide desired behaviors
/// according to the application (e.g, only listing the potential
/// CollisionObjects in collision).
struct COAL_DLLAPI CollisionCallBackBase {
/// @brief Initialization of the callback before running the collision
/// broadphase manager.
virtual void init() {};
/// @brief Collision evaluation between two objects in collision.
/// This callback will cause the broadphase evaluation to stop if it
/// returns true.
///
/// @param[in] o1 Collision object #1.
/// @param[in] o2 Collision object #2.
virtual bool collide(CollisionObject* o1, CollisionObject* o2) = 0;
/// @brief Functor call associated to the collide operation.
virtual bool operator()(CollisionObject* o1, CollisionObject* o2) {
return collide(o1, o2);
}
};
/// @brief Base callback class for distance queries.
/// This class can be supersed by child classes to provide desired behaviors
/// according to the application (e.g, only listing the potential
/// CollisionObjects in collision).
struct COAL_DLLAPI DistanceCallBackBase {
/// @brief Initialization of the callback before running the collision
/// broadphase manager.
virtual void init() {};
/// @brief Distance evaluation between two objects in collision.
/// This callback will cause the broadphase evaluation to stop if it
/// returns true.
///
/// @param[in] o1 Collision object #1.
/// @param[in] o2 Collision object #2.
/// @param[out] dist Distance between the two collision geometries.
virtual bool distance(CollisionObject* o1, CollisionObject* o2,
CoalScalar& dist) = 0;
/// @brief Functor call associated to the distance operation.
virtual bool operator()(CollisionObject* o1, CollisionObject* o2,
CoalScalar& dist) {
return distance(o1, o2, dist);
}
};
} // namespace coal
#endif // COAL_BROADPHASE_BROAD_PHASE_CALLBACKS_H
include/coal/broadphase/broadphase_collision_manager.h 0 → 100644
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2016, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/** @author Jia Pan */
#ifndef COAL_BROADPHASE_BROADPHASECOLLISIONMANAGER_H
#define COAL_BROADPHASE_BROADPHASECOLLISIONMANAGER_H
#include <set>
#include <vector>
#include <boost/function.hpp>
#include "coal/collision_object.h"
#include "coal/broadphase/broadphase_callbacks.h"
namespace coal {
/// @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 COAL_DLLAPI BroadPhaseCollisionManager {
public:
BroadPhaseCollisionManager();
virtual ~BroadPhaseCollisionManager();
/// @brief add objects to the manager
virtual void registerObjects(const std::vector<CollisionObject*>& other_objs);
/// @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);
/// @brief update the manager by explicitly given the set of objects update
virtual void update(const std::vector<CollisionObject*>& updated_objs);
/// @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 return the objects managed by the manager
virtual std::vector<CollisionObject*> getObjects() const {
std::vector<CollisionObject*> res(size());
getObjects(res);
return res;
};
/// @brief perform collision test between one object and all the objects
/// belonging to the manager
virtual void collide(CollisionObject* obj,
CollisionCallBackBase* callback) const = 0;
/// @brief perform distance computation between one object and all the objects
/// belonging to the manager
virtual void distance(CollisionObject* obj,
DistanceCallBackBase* callback) const = 0;
/// @brief perform collision test for the objects belonging to the manager
/// (i.e., N^2 self collision)
virtual void collide(CollisionCallBackBase* callback) const = 0;
/// @brief perform distance test for the objects belonging to the manager
/// (i.e., N^2 self distance)
virtual void distance(DistanceCallBackBase* callback) const = 0;
/// @brief perform collision test with objects belonging to another manager
virtual void collide(BroadPhaseCollisionManager* other_manager,
CollisionCallBackBase* callback) const = 0;
/// @brief perform distance test with objects belonging to another manager
virtual void distance(BroadPhaseCollisionManager* other_manager,
DistanceCallBackBase* 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;
void insertTestedSet(CollisionObject* a, CollisionObject* b) const;
};
} // namespace coal
#endif
src/articulated_model/model_config.cpp → include/coal/broadphase/broadphase_continuous_collision_manager-inl.h
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2016, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
......@@ -14,7 +15,7 @@
* 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
* * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
......@@ -32,58 +33,52 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
/** \author Dalibor Matura, Jia Pan */
/** @author Jia Pan */
#include "fcl/articulated_model/model_config.h"
#include "fcl/articulated_model/joint.h"
#include <algorithm>
#ifndef COAL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_INL_H
#define COAL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_INL_H
// Define for boost version < 1.47
#ifndef BOOST_ASSERT_MSG
#define BOOST_ASSERT_MSG(expr, msg) ((void)0)
#endif
#include "coal/broadphase/broadphase_continuous_collision_manager.h"
#include <algorithm>
namespace fcl
{
namespace coal {
ModelConfig::ModelConfig() {}
//==============================================================================
BroadPhaseContinuousCollisionManager::BroadPhaseContinuousCollisionManager() {
// Do nothing
}
ModelConfig::ModelConfig(const ModelConfig& model_cfg) :
joint_cfgs_map_(model_cfg.joint_cfgs_map_)
{}
//==============================================================================
ModelConfig::ModelConfig(std::map<std::string, boost::shared_ptr<Joint> > joints_map)
{
std::map<std::string, boost::shared_ptr<Joint> >::iterator it;
for(it = joints_map.begin(); it != joints_map.end(); ++it)
joint_cfgs_map_[it->first] = JointConfig(it->second);
BroadPhaseContinuousCollisionManager::~BroadPhaseContinuousCollisionManager() {
// Do nothing
}
JointConfig ModelConfig::getJointConfigByJointName(const std::string& joint_name) const
{
std::map<std::string, JointConfig>::const_iterator it = joint_cfgs_map_.find(joint_name);
BOOST_ASSERT_MSG((it != joint_cfgs_map_.end()), "Joint name not valid");
//==============================================================================
return it->second;
void BroadPhaseContinuousCollisionManager::registerObjects(
const std::vector<ContinuousCollisionObject*>& other_objs) {
for (size_t i = 0; i < other_objs.size(); ++i) registerObject(other_objs[i]);
}
JointConfig& ModelConfig::getJointConfigByJointName(const std::string& joint_name)
{
std::map<std::string, JointConfig>::iterator it = joint_cfgs_map_.find(joint_name);
BOOST_ASSERT_MSG((it != joint_cfgs_map_.end()), "Joint name not valid");
//==============================================================================
return it->second;
}
void BroadPhaseContinuousCollisionManager::update(
ContinuousCollisionObject* updated_obj) {
COAL_UNUSED_VARIABLE(updated_obj);
JointConfig ModelConfig::getJointConfigByJoint(boost::shared_ptr<Joint> joint) const
{
return getJointConfigByJointName(joint->getName());
update();
}
JointConfig& ModelConfig::getJointConfigByJoint(boost::shared_ptr<Joint> joint)
{
return getJointConfigByJointName(joint->getName());
}
//==============================================================================
void BroadPhaseContinuousCollisionManager::update(
const std::vector<ContinuousCollisionObject*>& updated_objs) {
COAL_UNUSED_VARIABLE(updated_objs);
update();
}
} // namespace coal
#endif
include/coal/broadphase/broadphase_continuous_collision_manager.h 0 → 100644
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2016, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/** @author Jia Pan */
#ifndef COAL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_H
#define COAL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_H
#include "coal/broadphase/broadphase_collision_manager.h"
#include "coal/collision_object.h"
#include "coal/narrowphase/continuous_collision_object.h"
namespace coal {
/// @brief Callback for continuous collision between two objects. Return value
/// is whether can stop now.
template <typename S>
using ContinuousCollisionCallBack = bool (*)(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.
template <typename S>
using ContinuousDistanceCallBack = bool (*)(ContinuousCollisionObject* o1,
ContinuousCollisionObject* o2,
S& 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.
template <typename S>
class COAL_DLLAPI BroadPhaseContinuousCollisionManager {
public:
BroadPhaseContinuousCollisionManager();
virtual ~BroadPhaseContinuousCollisionManager();
/// @brief add objects to the manager
virtual void registerObjects(
const std::vector<ContinuousCollisionObject*>& other_objs);
/// @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);
/// @brief update the manager by explicitly given the set of objects update
virtual void update(
const std::vector<ContinuousCollisionObject*>& updated_objs);
/// @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,
CollisionCallBackBase* callback) const = 0;
/// @brief perform distance computation between one object and all the objects
/// belonging to the manager
virtual void distance(ContinuousCollisionObject* obj,
DistanceCallBackBase* callback) const = 0;
/// @brief perform collision test for the objects belonging to the manager
/// (i.e., N^2 self collision)
virtual void collide(CollisionCallBackBase* callback) const = 0;
/// @brief perform distance test for the objects belonging to the manager
/// (i.e., N^2 self distance)
virtual void distance(DistanceCallBackBase* callback) const = 0;
/// @brief perform collision test with objects belonging to another manager
virtual void collide(BroadPhaseContinuousCollisionManager* other_manager,
CollisionCallBackBase* callback) const = 0;
/// @brief perform distance test with objects belonging to another manager
virtual void distance(BroadPhaseContinuousCollisionManager* other_manager,
DistanceCallBackBase* 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;
};
using BroadPhaseContinuousCollisionManagerf =
BroadPhaseContinuousCollisionManager<float>;
using BroadPhaseContinuousCollisionManagerd =
BroadPhaseContinuousCollisionManager<CoalScalar>;
} // namespace coal
#include "coal/broadphase/broadphase_continuous_collision_manager-inl.h"
#endif
include/coal/broadphase/broadphase_dynamic_AABB_tree-inl.h 0 → 100644
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2016, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/** @author Jia Pan */
#ifndef COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_INL_H
#define COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_INL_H
#include "coal/broadphase/broadphase_dynamic_AABB_tree.h"
#include <limits>
#if COAL_HAVE_OCTOMAP
#include "coal/octree.h"
#endif
#include "coal/BV/BV.h"
#include "coal/shape/geometric_shapes_utility.h"
namespace coal {
namespace detail {
namespace dynamic_AABB_tree {
#if COAL_HAVE_OCTOMAP
//==============================================================================
template <typename Derived>
bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
const OcTree* tree2, const OcTree::OcTreeNode* root2,
const AABB& root2_bv,
const Eigen::MatrixBase<Derived>& translation2,
CollisionCallBackBase* callback) {
if (!root2) {
if (root1->isLeaf()) {
CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
if (!obj1->collisionGeometry()->isFree()) {
const AABB& root2_bv_t = translate(root2_bv, translation2);
if (root1->bv.overlap(root2_bv_t)) {
Box* box = new Box();
Transform3s box_tf;
Transform3s tf2 = Transform3s::Identity();
tf2.translation() = translation2;
constructBox(root2_bv, tf2, *box, box_tf);
box->cost_density =
tree2->getOccupancyThres(); // thresholds are 0, 1, so uncertain
CollisionObject obj2(shared_ptr<CollisionGeometry>(box), box_tf);
return (*callback)(obj1, &obj2);
}
}
} else {
if (collisionRecurse_(root1->children[0], tree2, nullptr, root2_bv,
translation2, callback))
return true;
if (collisionRecurse_(root1->children[1], tree2, nullptr, root2_bv,
translation2, callback))
return true;
}
return false;
} else if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) {
const AABB& root2_bv_t = translate(root2_bv, translation2);
if (root1->bv.overlap(root2_bv_t)) {
Box* box = new Box();
Transform3s box_tf;
Transform3s tf2 = Transform3s::Identity();
tf2.translation() = translation2;
constructBox(root2_bv, tf2, *box, box_tf);
box->cost_density = root2->getOccupancy();
box->threshold_occupied = tree2->getOccupancyThres();
CollisionObject obj2(shared_ptr<CollisionGeometry>(box), box_tf);
return (*callback)(obj1, &obj2);
} else
return false;
} else
return false;
}
const AABB& root2_bv_t = translate(root2_bv, translation2);
if (tree2->isNodeFree(root2) || !root1->bv.overlap(root2_bv_t)) return false;
if (!tree2->nodeHasChildren(root2) ||
(!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) {
if (collisionRecurse_(root1->children[0], tree2, root2, root2_bv,
translation2, callback))
return true;
if (collisionRecurse_(root1->children[1], tree2, root2, root2_bv,
translation2, callback))
return true;
} else {
for (unsigned int i = 0; i < 8; ++i) {
if (tree2->nodeChildExists(root2, i)) {
const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
AABB child_bv;
computeChildBV(root2_bv, i, child_bv);
if (collisionRecurse_(root1, tree2, child, child_bv, translation2,
callback))
return true;
} else {
AABB child_bv;
computeChildBV(root2_bv, i, child_bv);
if (collisionRecurse_(root1, tree2, nullptr, child_bv, translation2,
callback))
return true;
}
}
}
return false;
}
//==============================================================================
template <typename Derived>
bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
const OcTree* tree2, const OcTree::OcTreeNode* root2,
const AABB& root2_bv,
const Eigen::MatrixBase<Derived>& translation2,
DistanceCallBackBase* callback, CoalScalar& min_dist) {
if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
if (tree2->isNodeOccupied(root2)) {
Box* box = new Box();
Transform3s box_tf;
Transform3s tf2 = Transform3s::Identity();
tf2.translation() = translation2;
constructBox(root2_bv, tf2, *box, box_tf);
CollisionObject obj(shared_ptr<CollisionGeometry>(box), box_tf);
return (*callback)(static_cast<CollisionObject*>(root1->data), &obj,
min_dist);
} else
return false;
}
if (!tree2->isNodeOccupied(root2)) return false;
if (!tree2->nodeHasChildren(root2) ||
(!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) {
const AABB& aabb2 = translate(root2_bv, translation2);
CoalScalar d1 = aabb2.distance(root1->children[0]->bv);
CoalScalar d2 = aabb2.distance(root1->children[1]->bv);
if (d2 < d1) {
if (d2 < min_dist) {
if (distanceRecurse_(root1->children[1], tree2, root2, root2_bv,
translation2, callback, min_dist))
return true;
}
if (d1 < min_dist) {
if (distanceRecurse_(root1->children[0], tree2, root2, root2_bv,
translation2, callback, min_dist))
return true;
}
} else {
if (d1 < min_dist) {
if (distanceRecurse_(root1->children[0], tree2, root2, root2_bv,
translation2, callback, min_dist))
return true;
}
if (d2 < min_dist) {
if (distanceRecurse_(root1->children[1], tree2, root2, root2_bv,
translation2, callback, min_dist))
return true;
}
}
} else {
for (unsigned int i = 0; i < 8; ++i) {
if (tree2->nodeChildExists(root2, i)) {
const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
AABB child_bv;
computeChildBV(root2_bv, i, child_bv);
const AABB& aabb2 = translate(child_bv, translation2);
CoalScalar d = root1->bv.distance(aabb2);
if (d < min_dist) {
if (distanceRecurse_(root1, tree2, child, child_bv, translation2,
callback, min_dist))
return true;
}
}
}
}
return false;
}
#endif
} // namespace dynamic_AABB_tree
} // namespace detail
} // namespace coal
#endif