Unverified Commit f68ff486 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by GitHub
Browse files

Merge pull request #70 from jmirabel/cleaning

[Cleaning]  Remove classes MeshCollisionTraversalNode[bvtype]
parents 88d08616 8e75a145
......@@ -130,6 +130,7 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/fcl/math/vec_3f.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
......
......@@ -262,6 +262,14 @@ static inline AABB rotate(const AABB& aabb, const Matrix3f& t)
return res;
}
/// @brief Check collision between two aabbs, b1 is in configuration (R0, T0) and b2 is in identity.
bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, const AABB& b2);
/// @brief Check collision between two aabbs, b1 is in configuration (R0, T0) and b2 is in identity.
bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1,
const AABB& b2, const CollisionRequest& request,
FCL_REAL& sqrDistLowerBound);
}
} // namespace hpp
......
......@@ -155,6 +155,10 @@ FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS&
/// @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);
/// @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,
const CollisionRequest& request,
FCL_REAL& sqrDistLowerBound);
}
......
......@@ -39,7 +39,7 @@
#define HPP_FCL_KDOP_H
#include <stdexcept>
#include <hpp/fcl/math/vec_3f.h>
#include <hpp/fcl/math/matrix_3f.h>
namespace hpp
{
......@@ -176,6 +176,20 @@ public:
};
template<size_t N>
bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/,
const KDOP<N>& /*b1*/, const KDOP<N>& /*b2*/)
{
throw std::logic_error ("not implemented");
}
template<size_t N>
bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/,
const KDOP<N>& /*b1*/, const KDOP<N>& /*b2*/,
const CollisionRequest& /*request*/, FCL_REAL& /*sqrDistLowerBound*/)
{
throw std::logic_error ("not implemented");
}
/// @brief translate the KDOP BV
template<size_t N>
......
......@@ -157,6 +157,12 @@ kIOS translate(const kIOS& bv, const Vec3f& t);
/// @todo Not efficient
bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2);
/// @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,
const CollisionRequest& request,
FCL_REAL& 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);
......
......@@ -101,6 +101,7 @@ namespace fcl
}
//// @brief intersection checking between one shape and a triangle with transformation
/// \return true if the shape are colliding.
template<typename S>
bool shapeTriangleInteraction
(const S& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2,
......
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2019, LAAS CNRS
* 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 Joseph Mirabel */
#ifndef HPP_FCL_TRAVERSAL_DETAILS_TRAVERSAL_H
#define HPP_FCL_TRAVERSAL_DETAILS_TRAVERSAL_H
namespace hpp
{
namespace fcl
{
enum {
RelativeTransformationIsIdentity = 1
};
namespace details
{
template <bool enabled>
struct RelativeTransformation
{
RelativeTransformation () : R (Matrix3f::Identity()) {}
const Matrix3f& _R () const { return R; }
const Vec3f & _T () const { return T; }
Matrix3f R;
Vec3f T;
};
template <>
struct RelativeTransformation <false>
{
static const Matrix3f& _R () { throw std::logic_error ("should never reach this point"); }
static const Vec3f & _T () { throw std::logic_error ("should never reach this point"); }
};
} // namespace details
}
} // namespace hpp
#endif
......@@ -137,6 +137,8 @@ public:
virtual ~DistanceTraversalNodeBase();
/// @brief BV test between b1 and b2
/// \return a lower bound of the distance between the two BV.
/// \note except for OBB, this method returns the distance.
virtual FCL_REAL BVTesting(int b1, int b2) const;
/// @brief Leaf test between node b1 and b2, if they are both leafs
......
......@@ -47,6 +47,7 @@
#include <hpp/fcl/intersect.h>
#include <hpp/fcl/shape/geometric_shapes.h>
#include <hpp/fcl/narrowphase/narrowphase.h>
#include <hpp/fcl/traversal/details/traversal.h>
#include <boost/shared_array.hpp>
#include <boost/shared_ptr.hpp>
......@@ -124,24 +125,6 @@ public:
{
return model2->getBV(b).rightChild();
}
/// @brief BV culling test in one BVTT node
bool BVTesting(int b1, int b2) const
{
if(enable_statistics) num_bv_tests++;
return !model1->getBV(b1).overlap(model2->getBV(b2));
}
/// BV test between b1 and b2
/// \param b1, b2 Bounding volumes to test,
/// \retval sqrDistLowerBound square of a lower bound of the minimal
/// distance between bounding volumes.
bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
{
if(enable_statistics) num_bv_tests++;
return !model1->getBV(b1).overlap(model2->getBV(b2), request,
sqrDistLowerBound);
}
/// @brief The first BVH model
const BVHModel<BV>* model1;
......@@ -154,12 +137,16 @@ public:
mutable FCL_REAL query_time_seconds;
};
/// @brief Traversal node for collision between two meshes
template<typename BV>
template<typename BV, int _Options = RelativeTransformationIsIdentity>
class MeshCollisionTraversalNode : public BVHCollisionTraversalNode<BV>
{
public:
enum {
Options = _Options,
RTIsIdentity = _Options & RelativeTransformationIsIdentity
};
MeshCollisionTraversalNode(const CollisionRequest& request) :
BVHCollisionTraversalNode<BV> (request)
{
......@@ -169,6 +156,36 @@ public:
tri_indices2 = NULL;
}
/// @brief BV culling test in one BVTT node
bool BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
if (RTIsIdentity)
return !this->model1->getBV(b1).overlap(this->model2->getBV(b2));
else
return !overlap(RT._R(), RT._T(),
this->model1->getBV(b1).bv, this->model2->getBV(b2).bv);
}
/// BV test between b1 and b2
/// \param b1, b2 Bounding volumes to test,
/// \retval sqrDistLowerBound square of a lower bound of the minimal
/// distance between bounding volumes.
bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
{
if(this->enable_statistics) this->num_bv_tests++;
if (RTIsIdentity)
return !this->model1->getBV(b1).overlap(this->model2->getBV(b2),
this->request, sqrDistLowerBound);
else {
bool res = !overlap(RT._R(), RT._T(),
this->model1->getBV(b1).bv, this->model2->getBV(b2).bv,
this->request, sqrDistLowerBound);
assert (!res || sqrDistLowerBound > 0);
return res;
}
}
/// Intersection testing between leaves (two triangles)
///
/// \param b1, b2 id of primitive in bounding volume hierarchy
......@@ -242,61 +259,41 @@ public:
Triangle* tri_indices1;
Triangle* tri_indices2;
};
/// @brief Traversal node for collision between two meshes if their underlying BVH node is oriented node (OBB, RSS, OBBRSS, kIOS)
class MeshCollisionTraversalNodeOBB : public MeshCollisionTraversalNode<OBB>
{
public:
MeshCollisionTraversalNodeOBB (const CollisionRequest& request);
bool BVTesting(int b1, int b2) const;
bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const;
Matrix3f R;
Vec3f T;
};
class MeshCollisionTraversalNodeRSS : public MeshCollisionTraversalNode<RSS>
{
public:
MeshCollisionTraversalNodeRSS (const CollisionRequest& request);
bool BVTesting(int b1, int b2) const;
bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const;
Matrix3f R;
Vec3f T;
details::RelativeTransformation<!bool(RTIsIdentity)> RT;
};
class MeshCollisionTraversalNodekIOS : public MeshCollisionTraversalNode<kIOS>
{
public:
MeshCollisionTraversalNodekIOS (const CollisionRequest& request);
bool BVTesting(int b1, int b2) const;
bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const;
Matrix3f R;
Vec3f T;
};
/// @brief Traversal node for collision between two meshes if their underlying BVH node is oriented node (OBB, RSS, OBBRSS, kIOS)
typedef MeshCollisionTraversalNode<OBB , 0> MeshCollisionTraversalNodeOBB ;
typedef MeshCollisionTraversalNode<RSS , 0> MeshCollisionTraversalNodeRSS ;
typedef MeshCollisionTraversalNode<kIOS , 0> MeshCollisionTraversalNodekIOS ;
typedef MeshCollisionTraversalNode<OBBRSS, 0> MeshCollisionTraversalNodeOBBRSS;
class MeshCollisionTraversalNodeOBBRSS : public MeshCollisionTraversalNode<OBBRSS>
namespace details
{
public:
MeshCollisionTraversalNodeOBBRSS (const CollisionRequest& request);
bool BVTesting(int b1, int b2) const;
bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const;
template<typename BV> struct DistanceTraversalBVTesting_impl
{
static FCL_REAL run(const BVNode<BV>& b1, const BVNode<BV>& b2)
{
return b1.distance(b2);
}
};
Matrix3f R;
Vec3f T;
};
template<> struct DistanceTraversalBVTesting_impl<OBB>
{
static FCL_REAL run(const BVNode<OBB>& b1, const BVNode<OBB>& b2)
{
FCL_REAL sqrDistLowerBound;
CollisionRequest request (DISTANCE_LOWER_BOUND, 0);
// request.break_distance = ?
if (b1.overlap(b2, request, sqrDistLowerBound)) {
// TODO A penetration upper bound should be computed.
return -1;
}
return sqrt (sqrDistLowerBound);
}
};
} // namespace details
/// @brief Traversal node for distance computation between BVH models
template<typename BV>
......@@ -367,7 +364,8 @@ public:
FCL_REAL BVTesting(int b1, int b2) const
{
if(enable_statistics) num_bv_tests++;
return model1->getBV(b1).distance(model2->getBV(b2));
return details::DistanceTraversalBVTesting_impl<BV>
::run (model1->getBV(b1), model2->getBV(b2));
}
/// @brief The first BVH model
......
......@@ -128,6 +128,20 @@ FCL_REAL AABB::distance(const AABB& other) const
return std::sqrt(result);
}
bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, const AABB& b2)
{
AABB bb1 (translate (rotate (b1, R0), T0));
return bb1.overlap (b2);
}
bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1,
const AABB& b2, const CollisionRequest& request,
FCL_REAL& sqrDistLowerBound)
{
AABB bb1 (translate (rotate (b1, R0), T0));
return bb1.overlap (b2, request, sqrDistLowerBound);
}
}
} // namespace hpp
......@@ -862,6 +862,25 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2)
return (dist <= (b1.r + b2.r));
}
bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2,
const CollisionRequest& /*request*/,
FCL_REAL& sqrDistLowerBound)
{
// ROb2 = R0 . b2
// where b2 = [ b2.axis [0] | b2.axis [1] | b2.axis [2] ]
// (1 0 0)^T R0b2^T axis [0] = (1 0 0)^T b2^T R0^T axis [0]
// R = b2^T RO^T b1
Vec3f Ttemp (R0 * b2.Tr + T0 - b1.Tr);
Vec3f T(b1.axes.transpose() * Ttemp);
Matrix3f R(b1.axes.transpose() * R0 * b2.axes);
FCL_REAL dist = rectDistance(R, T, b1.l, b2.l) - b1.r - b2.r;
if (dist <= 0) return true;
sqrDistLowerBound = dist * dist;
return false;
}
bool RSS::contain(const Vec3f& p) const
{
Vec3f local_p = p - Tr;
......
......@@ -61,17 +61,27 @@ bool kIOS::overlap(const kIOS& other) const
}
return obb.overlap(other.obb);
return true;
}
bool kIOS::overlap(const kIOS& other, const CollisionRequest&,
FCL_REAL& sqrDistLowerBound) const
bool kIOS::overlap(const kIOS& other, const CollisionRequest& request,
FCL_REAL& sqrDistLowerBound) const
{
for(unsigned int i = 0; i < num_spheres; ++i)
{
sqrDistLowerBound = sqrt (-1);
return overlap (other);
for(unsigned int j = 0; j < other.num_spheres; ++j)
{
FCL_REAL o_dist = (spheres[i].o - other.spheres[j].o).squaredNorm();
FCL_REAL sum_r = spheres[i].r + other.spheres[j].r;
if(o_dist > sum_r * sum_r) {
o_dist = sqrt(o_dist) - sum_r;
sqrDistLowerBound = o_dist*o_dist;
return false;
}
}
}
return obb.overlap(other.obb, request, sqrDistLowerBound);
}
bool kIOS::contain(const Vec3f& p) const
{
......@@ -192,6 +202,23 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2
return b1.overlap(b2_temp);
}
bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2,
const CollisionRequest& request,
FCL_REAL& sqrDistLowerBound)
{
kIOS b2_temp = b2;
for(unsigned int i = 0; i < b2_temp.num_spheres; ++i)
{
b2_temp.spheres[i].o = R0 * b2_temp.spheres[i].o + T0;
}
b2_temp.obb.To = R0 * b2_temp.obb.To + T0;
b2_temp.obb.axes.applyOnTheLeft(R0);
return b1.overlap(b2_temp, request, sqrDistLowerBound);
}
FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2, Vec3f* P, Vec3f* Q)
{
kIOS b2_temp = b2;
......
......@@ -379,6 +379,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
distance_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeDistancer<AABB, Convex, NarrowPhaseSolver>::distance;
distance_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeDistancer<AABB, Plane, NarrowPhaseSolver>::distance;
distance_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeDistancer<AABB, Halfspace, NarrowPhaseSolver>::distance;
*/
distance_matrix[BV_OBB][GEOM_BOX] = &BVHShapeDistancer<OBB, Box, NarrowPhaseSolver>::distance;
distance_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeDistancer<OBB, Sphere, NarrowPhaseSolver>::distance;
......@@ -388,7 +389,6 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
distance_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeDistancer<OBB, Convex, NarrowPhaseSolver>::distance;
distance_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeDistancer<OBB, Plane, NarrowPhaseSolver>::distance;
distance_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeDistancer<OBB, Halfspace, NarrowPhaseSolver>::distance;
*/
distance_matrix[BV_RSS][GEOM_BOX] = &BVHShapeDistancer<RSS, Box, NarrowPhaseSolver>::distance;
distance_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeDistancer<RSS, Sphere, NarrowPhaseSolver>::distance;
......@@ -448,6 +448,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
distance_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeDistancer<OBBRSS, Halfspace, NarrowPhaseSolver>::distance;
distance_matrix[BV_AABB][BV_AABB] = &BVHDistance<AABB, NarrowPhaseSolver>;
distance_matrix[BV_OBB][BV_OBB] = &BVHDistance<OBB, NarrowPhaseSolver>;
distance_matrix[BV_RSS][BV_RSS] = &BVHDistance<RSS, NarrowPhaseSolver>;
distance_matrix[BV_kIOS][BV_kIOS] = &BVHDistance<kIOS, NarrowPhaseSolver>;
distance_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHDistance<OBBRSS, NarrowPhaseSolver>;
......
......@@ -86,92 +86,6 @@ static inline void meshDistanceOrientedNodeLeafTesting(int b1, int b2,
}
} // namespace details
MeshCollisionTraversalNodeOBB::MeshCollisionTraversalNodeOBB
(const CollisionRequest& request) :
MeshCollisionTraversalNode<OBB> (request)
{
R.setIdentity();
}
bool MeshCollisionTraversalNodeOBB::BVTesting(int b1, int b2) const
{
if(enable_statistics) num_bv_tests++;
return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
}
bool MeshCollisionTraversalNodeOBB::BVTesting
(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
{
if(enable_statistics) num_bv_tests++;
return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv,
request, sqrDistLowerBound);
}
MeshCollisionTraversalNodeRSS::MeshCollisionTraversalNodeRSS
(const CollisionRequest& request) :
MeshCollisionTraversalNode<RSS> (request)
{
R.setIdentity();
}
bool MeshCollisionTraversalNodeRSS::BVTesting(int b1, int b2) const
{
if(enable_statistics) num_bv_tests++;
return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
}
bool MeshCollisionTraversalNodeRSS::BVTesting(int b1, int b2,
FCL_REAL& sqrDistLowerBound) const
{
if(enable_statistics) num_bv_tests++;
sqrDistLowerBound = 0;
return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
}
MeshCollisionTraversalNodekIOS::MeshCollisionTraversalNodekIOS
(const CollisionRequest& request) :
MeshCollisionTraversalNode<kIOS>(request)
{
R.setIdentity();
}
bool MeshCollisionTraversalNodekIOS::BVTesting(int b1, int b2) const
{
if(enable_statistics) num_bv_tests++;
return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
}
bool MeshCollisionTraversalNodekIOS::BVTesting
(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
{
if(enable_statistics) num_bv_tests++;
sqrDistLowerBound = 0;
return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
}
MeshCollisionTraversalNodeOBBRSS::MeshCollisionTraversalNodeOBBRSS
(const CollisionRequest& request) :
MeshCollisionTraversalNode<OBBRSS> (request)
{
R.setIdentity();
}
bool MeshCollisionTraversalNodeOBBRSS::BVTesting(int b1, int b2) const
{
if(enable_statistics) num_bv_tests++;
return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
}
bool MeshCollisionTraversalNodeOBBRSS::BVTesting
(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
{
if(enable_statistics) num_bv_tests++;
bool res (!overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv,
request, sqrDistLowerBound));
assert (!res || sqrDistLowerBound > 0);
return res;
}
namespace details
{
......
......@@ -68,7 +68,7 @@ static inline bool setupMeshCollisionOrientedNode(OrientedNode& node,
node.result = &result;
relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), node.R, node.T);
relativeTransform(tf1.getRotation(), tf1