Commit 63d0670e authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Merge remote-tracking branch 'lucile/devel' into refactoring

parents 8b49fc63 2c2ff317
......@@ -132,24 +132,12 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/fcl/math/matrix_3f.h
include/hpp/fcl/math/vec_3f.h
include/hpp/fcl/math/types.h
include/hpp/fcl/math/tools.h
include/hpp/fcl/math/transform.h
include/hpp/fcl/traversal/details/traversal.h
include/hpp/fcl/traversal/traversal_node_shapes.h
include/hpp/fcl/traversal/traversal_node_setup.h
include/hpp/fcl/traversal/traversal_recurse.h
include/hpp/fcl/traversal/traversal_node_octree.h
include/hpp/fcl/traversal/traversal_node_bvhs.h
include/hpp/fcl/traversal/traversal_node_bvh_shape.h
include/hpp/fcl/traversal/traversal_node_base.h
include/hpp/fcl/data_types.h
include/hpp/fcl/BVH/BV_splitter.h
include/hpp/fcl/BVH/BVH_internal.h
include/hpp/fcl/BVH/BVH_model.h
include/hpp/fcl/BVH/BV_fitter.h
include/hpp/fcl/BVH/BVH_front.h
include/hpp/fcl/BVH/BVH_utility.h
include/hpp/fcl/intersect.h
include/hpp/fcl/collision_object.h
include/hpp/fcl/collision_utility.h
include/hpp/fcl/octree.h
......
......@@ -39,7 +39,7 @@
#define HPP_FCL_AABB_H
#include <stdexcept>
#include <hpp/fcl/math/types.h>
#include <hpp/fcl/data_types.h>
namespace hpp
{
......@@ -81,6 +81,20 @@ public:
{
}
/// @name Bounding volume API
/// Common API to BVs.
/// @{
/// @brief Check whether the AABB contains a point
inline bool contain(const Vec3f& p) const
{
if(p[0] < min_[0] || p[0] > max_[0]) return false;
if(p[1] < min_[1] || p[1] > max_[1]) return false;
if(p[2] < min_[2] || p[2] > max_[2]) return false;
return true;
}
/// @brief Check whether two AABB are overlap
inline bool overlap(const AABB& other) const
{
......@@ -99,35 +113,11 @@ public:
bool overlap(const AABB& other, const CollisionRequest& request,
FCL_REAL& sqrDistLowerBound) const;
/// @brief Check whether the AABB contains another AABB
inline bool contain(const AABB& other) const
{
return (other.min_[0] >= min_[0]) && (other.max_[0] <= max_[0]) && (other.min_[1] >= min_[1]) && (other.max_[1] <= max_[1]) && (other.min_[2] >= min_[2]) && (other.max_[2] <= max_[2]);
}
/// @brief Check whether two AABB are overlap and return the overlap part
inline bool overlap(const AABB& other, AABB& overlap_part) const
{
if(!overlap(other))
{
return false;
}
overlap_part.min_ = min_.cwiseMax(other.min_);
overlap_part.max_ = max_.cwiseMin(other.max_);
return true;
}
/// @brief Check whether the AABB contains a point
inline bool contain(const Vec3f& p) const
{
if(p[0] < min_[0] || p[0] > max_[0]) return false;
if(p[1] < min_[1] || p[1] > max_[1]) return false;
if(p[2] < min_[2] || p[2] > max_[2]) return false;
/// @brief Distance between two AABBs
FCL_REAL distance(const AABB& other) const;
return true;
}
/// @brief Distance between two AABBs; P and Q, should not be NULL, return the nearest points
FCL_REAL distance(const AABB& other, Vec3f* P, Vec3f* Q) const;
/// @brief Merge the AABB and a point
inline AABB& operator += (const Vec3f& p)
......@@ -152,6 +142,18 @@ public:
return res += other;
}
/// @brief Size of the AABB (used in BV_Splitter to order two AABBs)
inline FCL_REAL size() const
{
return (max_ - min_).squaredNorm();
}
/// @brief Center of the AABB
inline Vec3f center() const
{
return (min_ + max_) * 0.5;
}
/// @brief Width of the AABB
inline FCL_REAL width() const
{
......@@ -174,26 +176,29 @@ public:
inline FCL_REAL volume() const
{
return width() * height() * depth();
}
}
/// @brief Size of the AABB (used in BV_Splitter to order two AABBs)
inline FCL_REAL size() const
/// @}
/// @brief Check whether the AABB contains another AABB
inline bool contain(const AABB& other) const
{
return (max_ - min_).squaredNorm();
return (other.min_[0] >= min_[0]) && (other.max_[0] <= max_[0]) && (other.min_[1] >= min_[1]) && (other.max_[1] <= max_[1]) && (other.min_[2] >= min_[2]) && (other.max_[2] <= max_[2]);
}
/// @brief Center of the AABB
inline Vec3f center() const
/// @brief Check whether two AABB are overlap and return the overlap part
inline bool overlap(const AABB& other, AABB& overlap_part) const
{
return (min_ + max_) * 0.5;
if(!overlap(other))
{
return false;
}
overlap_part.min_ = min_.cwiseMax(other.min_);
overlap_part.max_ = max_.cwiseMin(other.max_);
return true;
}
/// @brief Distance between two AABBs; P and Q, should not be NULL, return the nearest points
FCL_REAL distance(const AABB& other, Vec3f* P, Vec3f* Q) const;
/// @brief Distance between two AABBs
FCL_REAL distance(const AABB& other) const;
/// @brief expand the half size of the AABB by delta, and keep the center unchanged.
inline AABB& expand(const Vec3f& delta)
{
......@@ -241,7 +246,6 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, const AABB& b2
bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1,
const AABB& b2, const CollisionRequest& request,
FCL_REAL& sqrDistLowerBound);
}
} // namespace hpp
......
......@@ -125,7 +125,7 @@ class Converter<RSS, OBB>
public:
static void convert(const RSS& bv1, const Transform3f& tf1, OBB& bv2)
{
bv2.extent.noalias() = Vec3f(bv1.l[0] * 0.5 + bv1.r, bv1.l[1] * 0.5 + bv1.r, bv1.r);
bv2.extent.noalias() = Vec3f(bv1.length[0] * 0.5 + bv1.radius, bv1.length[1] * 0.5 + bv1.radius, bv1.radius);
bv2.To.noalias() = tf1.transform(bv1.Tr);
bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
}
......@@ -168,9 +168,9 @@ public:
bv2.Tr = tf1.transform(bv1.To);
bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
bv2.r = bv1.extent[2];
bv2.l[0] = 2 * (bv1.extent[0] - bv2.r);
bv2.l[1] = 2 * (bv1.extent[1] - bv2.r);
bv2.radius = bv1.extent[2];
bv2.length[0] = 2 * (bv1.extent[0] - bv2.radius);
bv2.length[1] = 2 * (bv1.extent[1] - bv2.radius);
}
};
......@@ -183,9 +183,9 @@ public:
bv2.Tr.noalias() = tf1.transform(bv1.Tr);
bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
bv2.r = bv1.r;
bv2.l[0] = bv1.l[0];
bv2.l[1] = bv1.l[1];
bv2.radius = bv1.radius;
bv2.length[0] = bv1.length[0];
bv2.length[1] = bv1.length[1];
}
};
......@@ -232,9 +232,9 @@ public:
}
Vec3f extent = (bv1.max_ - bv1.min_) * 0.5;
bv2.r = extent[id[2]];
bv2.l[0] = (extent[id[0]] - bv2.r) * 2;
bv2.l[1] = (extent[id[1]] - bv2.r) * 2;
bv2.radius = extent[id[2]];
bv2.length[0] = (extent[id[0]] - bv2.radius) * 2;
bv2.length[1] = (extent[id[1]] - bv2.radius) * 2;
const Matrix3f& R = tf1.getRotation();
bool left_hand = (id[0] == (id[1] + 1) % 3);
......
......@@ -39,7 +39,7 @@
#ifndef HPP_FCL_BV_NODE_H
#define HPP_FCL_BV_NODE_H
#include <hpp/fcl/math/types.h>
#include <hpp/fcl/data_types.h>
#include <hpp/fcl/BV/BV.h>
#include <iostream>
......
......@@ -38,7 +38,7 @@
#ifndef HPP_FCL_OBB_H
#define HPP_FCL_OBB_H
#include <hpp/fcl/math/types.h>
#include <hpp/fcl/data_types.h>
namespace hpp
{
......@@ -60,6 +60,9 @@ public:
/// @brief Half dimensions of OBB
Vec3f extent;
/// @brief Check whether the OBB contains a point.
bool contain(const Vec3f& p) const;
/// Check collision between two OBB
/// \return true if collision happens.
bool overlap(const OBB& other) const;
......@@ -71,8 +74,8 @@ public:
bool overlap(const OBB& other, const CollisionRequest& request,
FCL_REAL& sqrDistLowerBound) const;
/// @brief Check whether the OBB contains a point.
bool contain(const Vec3f& p) const;
/// @brief Distance between two OBBs, not implemented.
FCL_REAL distance(const OBB& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
/// @brief A simple way to merge the OBB and a point (the result is not compact).
OBB& operator += (const Vec3f& p);
......@@ -87,6 +90,18 @@ public:
/// @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.squaredNorm();
}
/// @brief Center of the OBB
inline const Vec3f& center() const
{
return To;
}
/// @brief Width of the OBB.
inline FCL_REAL width() const
{
......@@ -110,22 +125,6 @@ public:
{
return width() * height() * depth();
}
/// @brief Size of the OBB (used in BV_Splitter to order two OBBs)
inline FCL_REAL 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;
};
......
......@@ -59,6 +59,12 @@ public:
/// @brief RSS member, for distance
RSS rss;
/// @brief Check whether the OBBRSS contains a point
inline bool contain(const Vec3f& p) const
{
return obb.contain(p);
}
/// @brief Check collision between two OBBRSS
bool overlap(const OBBRSS& other) const
{
......@@ -74,10 +80,10 @@ public:
return obb.overlap(other.obb, request, sqrDistLowerBound);
}
/// @brief Check whether the OBBRSS contains a point
inline bool contain(const Vec3f& p) const
/// @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 obb.contain(p);
return rss.distance(other.rss, P, Q);
}
/// @brief Merge the OBBRSS and a point
......@@ -104,6 +110,18 @@ public:
return result;
}
/// @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 Width of the OBRSS
inline FCL_REAL width() const
{
......@@ -127,24 +145,6 @@ public:
{
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);
}
};
/// @brief Check collision between two OBBRSS, b1 is in configuration (R0, T0) and b2 is in indentity
......
......@@ -39,7 +39,7 @@
#define HPP_FCL_RSS_H
#include <stdexcept>
#include <hpp/fcl/math/types.h>
#include <hpp/fcl/data_types.h>
#include <boost/math/constants/constants.hpp>
namespace hpp
......@@ -60,10 +60,14 @@ public:
Vec3f Tr;
/// @brief Side lengths of rectangle
FCL_REAL l[2];
FCL_REAL length[2];
/// @brief Radius of sphere summed with rectangle to form RSS
FCL_REAL r;
FCL_REAL radius;
/// @brief Check whether the RSS contains a point
inline bool contain(const Vec3f& p) const;
/// @brief Check collision between two RSS
bool overlap(const RSS& other) const;
......@@ -76,15 +80,8 @@ public:
return overlap (other);
}
/// @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 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
FCL_REAL distance(const RSS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
/// @brief A simple way to merge the RSS and a point, not compact.
/// @todo This function may have some bug.
......@@ -100,45 +97,49 @@ public:
/// @brief Return the merged RSS of current RSS and the other one
RSS operator + (const RSS& other) const;
/// @brief Size of the RSS (used in BV_Splitter to order two RSSs)
inline FCL_REAL size() const
{
return (std::sqrt(length[0] * length[0] + length[1] * length[1]) + 2 * radius);
}
/// @brief The RSS center
inline const Vec3f& center() const
{
return Tr;
}
/// @brief Width of the RSS
inline FCL_REAL width() const
{
return l[0] + 2 * r;
return length[0] + 2 * radius;
}
/// @brief Height of the RSS
inline FCL_REAL height() const
{
return l[1] + 2 * r;
return length[1] + 2 * radius;
}
/// @brief Depth of the RSS
inline FCL_REAL depth() const
{
return 2 * r;
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);
return (length[0] * length[1] * 2 * radius + 4 * boost::math::constants::pi<FCL_REAL>() * radius * radius * radius);
}
/// @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);
}
/// @brief The RSS center
inline const Vec3f& center() 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
{
return Tr;
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 distance between two RSS bounding volumes
......@@ -160,4 +161,4 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2,
} // namespace hpp
#endif
#endif
\ No newline at end of file
......@@ -39,7 +39,7 @@
#define HPP_FCL_KDOP_H
#include <stdexcept>
#include <hpp/fcl/math/types.h>
#include <hpp/fcl/data_types.h>
namespace hpp
{
......@@ -111,8 +111,8 @@ public:
return overlap (other);
}
//// @brief Check whether one point is inside the KDOP
bool inside(const Vec3f& p) const;
/// @brief The distance between two KDOP<N>. Not implemented.
FCL_REAL distance(const KDOP<N>& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
/// @brief Merge the point and the KDOP
KDOP<N>& operator += (const Vec3f& p);
......@@ -123,6 +123,19 @@ public:
/// @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
{
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;
}
/// @brief The (AABB) width
inline FCL_REAL width() const
{
......@@ -147,21 +160,6 @@ public:
return width() * height() * depth();
}
/// @brief Size of the kDOP (used in BV_Splitter to order two kDOPs)
inline FCL_REAL 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;
}
/// @brief The distance between two KDOP<N>. Not implemented.
FCL_REAL distance(const KDOP<N>& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
inline FCL_REAL dist(std::size_t i) const
{
return dist_[i];
......@@ -172,6 +170,8 @@ public:
return dist_[i];
}
//// @brief Check whether one point is inside the KDOP
bool inside(const Vec3f& p) const;
};
......
......@@ -93,6 +93,9 @@ public:
/// @ OBB related with kIOS
OBB obb;
/// @brief Check whether the kIOS contains a point
inline bool contain(const Vec3f& p) const;
/// @brief Check collision between two kIOS
bool overlap(const kIOS& other) const;
......@@ -100,8 +103,8 @@ public:
bool overlap(const kIOS& other, const CollisionRequest&,
FCL_REAL& sqrDistLowerBound) const;
/// @brief Check whether the kIOS contains a point
inline bool contain(const Vec3f& p) const;
/// @brief The distance between two kIOS
FCL_REAL distance(const kIOS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
/// @brief A simple way to merge the kIOS and a point
kIOS& operator += (const Vec3f& p);
......@@ -116,6 +119,9 @@ public:
/// @brief Return the merged kIOS of current kIOS and the other one
kIOS operator + (const kIOS& other) const;
/// @brief size of the kIOS (used in BV_Splitter to order two kIOSs)
FCL_REAL size() const;
/// @brief Center of the kIOS
const Vec3f& center() const
{
......@@ -133,12 +139,6 @@ public:
/// @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;
};
......
......@@ -41,8 +41,8 @@
#include <hpp/fcl/collision_object.h>
#include <hpp/fcl/BVH/BVH_internal.h>
#include <hpp/fcl/BV/BV_node.h>
#include <hpp/fcl/BVH/BV_splitter.h>
#include <hpp/fcl/BVH/BV_fitter.h>
#include "../../src/BVH/BV_splitter.h"
#include "../../src/BVH/BV_fitter.h"
#include <vector>
#include <boost/shared_ptr.hpp>
#include <boost/noncopyable.hpp>
......@@ -252,10 +252,10 @@ class BVHModel : public BVHModelBase
public:
/// @brief Split rule to split one BV node into two children
boost::shared_ptr<BVSplitterBase<BV> > bv_splitter;
boost::shared_ptr<BVSplitter<BV> > bv_splitter;
/// @brief Fitting rule to fit a BV node to a set of geometry primitives
boost::shared_ptr<BVFitterBase<BV> > bv_fitter;