Skip to content
GitLab
Commit 075a5d7c authored by Lucile Remigy's avatar Lucile Remigy
Browse files

tools.h private, delete NarrowPhaseSolver, rename GJKSolver

parent bf4e58dd
Expand all Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
View file @ 075a5d7c
......@@ -126,7 +126,6 @@ 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/BV/AABB.h
View file @ 075a5d7c
......@@ -81,6 +81,30 @@ public:
{
}
//start API in common test
/// @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 +123,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 +152,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 +186,42 @@ 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
/// @}
//End API in common test
/// @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 +269,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
......
include/hpp/fcl/BV/OBB.h
View file @ 075a5d7c
......@@ -60,6 +60,12 @@ 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 +77,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 +93,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 +128,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;
};
......
include/hpp/fcl/BV/OBBRSS.h
View file @ 075a5d7c
......@@ -59,6 +59,16 @@ 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 +84,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 +114,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,25 +149,12 @@ 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
inline bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2)
......
include/hpp/fcl/BV/RSS.h
View file @ 075a5d7c
......@@ -65,6 +65,10 @@ public:
/// @brief Radius of sphere summed with rectangle to form RSS
FCL_REAL r;
/// @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,6 +97,19 @@ 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(l[0] * l[0] + l[1] * l[1]) + 2 * r);
}
/// @brief The RSS center
inline const Vec3f& center() const
{
return Tr;
}
/// @brief Width of the RSS
inline FCL_REAL width() const
{
......@@ -124,21 +134,12 @@ public:
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);
}
/// @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
......
include/hpp/fcl/BV/kDOP.h
View file @ 075a5d7c
......@@ -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;
};
......
include/hpp/fcl/BV/kIOS.h
View file @ 075a5d7c
......@@ -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;
};
......
include/hpp/fcl/traversal/traversal_node_bvh_shape.h
View file @ 075a5d7c
......@@ -746,11 +746,11 @@ public:
};
template<typename S, typename NarrowPhaseSolver>
class MeshShapeDistanceTraversalNodeOBBRSS : public MeshShapeDistanceTraversalNode<OBBRSS, S, NarrowPhaseSolver>
template<typename S, typename GJKSolver>
class MeshShapeDistanceTraversalNodeOBBRSS : public MeshShapeDistanceTraversalNode<OBBRSS, S, GJKSolver>
{
public:
MeshShapeDistanceTraversalNodeOBBRSS() : MeshShapeDistanceTraversalNode<OBBRSS, S, NarrowPhaseSolver>()
MeshShapeDistanceTraversalNodeOBBRSS() : MeshShapeDistanceTraversalNode<OBBRSS, S, GJKSolver>()
{
}
......@@ -780,7 +780,7 @@ public:
};
/// @brief Traversal node for distance between shape and mesh
template<typename S, typename BV, typename NarrowPhaseSolver>
template<typename S, typename BV, typename GJKSolver>
class ShapeMeshDistanceTraversalNode : public ShapeBVHDistanceTraversalNode<S, BV>
{
public:
......@@ -835,14 +835,14 @@ public:
FCL_REAL rel_err;
FCL_REAL abs_err;
const NarrowPhaseSolver* nsolver;
const GJKSolver* nsolver;
};
template<typename S, typename NarrowPhaseSolver>
class ShapeMeshDistanceTraversalNodeRSS : public ShapeMeshDistanceTraversalNode<S, RSS, NarrowPhaseSolver>
template<typename S, typename GJKSolver>
class ShapeMeshDistanceTraversalNodeRSS : public ShapeMeshDistanceTraversalNode<S, RSS, GJKSolver>
{
public:
ShapeMeshDistanceTraversalNodeRSS() : ShapeMeshDistanceTraversalNode<S, RSS, NarrowPhaseSolver>()
ShapeMeshDistanceTraversalNodeRSS() : ShapeMeshDistanceTraversalNode<S, RSS, GJKSolver>()
{
}
......@@ -870,11 +870,11 @@ public:
};
template<typename S, typename NarrowPhaseSolver>
class ShapeMeshDistanceTraversalNodekIOS : public ShapeMeshDistanceTraversalNode<S, kIOS, NarrowPhaseSolver>
template<typename S, typename GJKSolver>
class ShapeMeshDistanceTraversalNodekIOS : public ShapeMeshDistanceTraversalNode<S, kIOS, GJKSolver>
{
public:
ShapeMeshDistanceTraversalNodekIOS() : ShapeMeshDistanceTraversalNode<S, kIOS, NarrowPhaseSolver>()
ShapeMeshDistanceTraversalNodekIOS() : ShapeMeshDistanceTraversalNode<S, kIOS, GJKSolver>()
{
}
......@@ -902,11 +902,11 @@ public:
};
template<typename S, typename NarrowPhaseSolver>
class ShapeMeshDistanceTraversalNodeOBBRSS : public ShapeMeshDistanceTraversalNode<S, OBBRSS, NarrowPhaseSolver>
template<typename S, typename GJKSolver>
class ShapeMeshDistanceTraversalNodeOBBRSS : public ShapeMeshDistanceTraversalNode<S, OBBRSS, GJKSolver>
{
public:
ShapeMeshDistanceTraversalNodeOBBRSS() : ShapeMeshDistanceTraversalNode<S, OBBRSS, NarrowPhaseSolver>()
ShapeMeshDistanceTraversalNodeOBBRSS() : ShapeMeshDistanceTraversalNode<S, OBBRSS, GJKSolver>()
{
}
......
include/hpp/fcl/traversal/traversal_node_octree.h
View file @ 075a5d7c
......@@ -51,11 +51,11 @@ namespace fcl
{
/// @brief Algorithms for collision related with octree
template<typename NarrowPhaseSolver>
template<typename GJKSolver>
class OcTreeSolver
{
private:
const NarrowPhaseSolver* solver;
const GJKSolver* solver;
mutable const CollisionRequest* crequest;
mutable const DistanceRequest* drequest;
......@@ -64,7 +64,7 @@ private:
mutable DistanceResult* dresult;
public:
OcTreeSolver(const NarrowPhaseSolver* solver_) : solver(solver_),
OcTreeSolver(const GJKSolver* solver_) : solver(solver_),
crequest(NULL),
drequest(NULL),
cresult(NULL),
......@@ -890,7 +890,7 @@ private:
/// @brief Traversal node for octree collision
template<typename NarrowPhaseSolver>
template<typename GJKSolver>
class OcTreeCollisionTraversalNode : public CollisionTraversalNodeBase
{
public:
......@@ -923,11 +923,11 @@ public:
Transform3f tf1, tf2;
const OcTreeSolver<NarrowPhaseSolver>* otsolver;
const OcTreeSolver<GJKSolver>* otsolver;
};
/// @brief Traversal node for octree distance
template<typename NarrowPhaseSolver>
template<typename GJKSolver>
class OcTreeDistanceTraversalNode : public DistanceTraversalNodeBase
{
public:
......@@ -958,11 +958,11 @@ public:
const OcTree* model1;
const OcTree* model2;
const OcTreeSolver<NarrowPhaseSolver>* otsolver;
const OcTreeSolver<GJKSolver>* otsolver;
};
/// @brief Traversal node for shape-octree collision
template<typename S, typename NarrowPhaseSolver>