Commit fc5c10aa authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Merge classes MeshDistanceTraversalNode[bvtype] into MeshDistanceTraversalNode

parent fa07cf5f
......@@ -283,6 +283,10 @@ namespace details
{
return b1.distance(b2);
}
static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<BV>& b1, const BVNode<BV>& b2)
{
return distance(R, T, b1.bv, b2.bv);
}
};
template<> struct DistanceTraversalBVDistanceLowerBound_impl<OBB>
......@@ -298,6 +302,43 @@ namespace details
}
return sqrt (sqrDistLowerBound);
}
static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<OBB>& b1, const BVNode<OBB>& b2)
{
FCL_REAL sqrDistLowerBound;
CollisionRequest request (DISTANCE_LOWER_BOUND, 0);
// request.break_distance = ?
if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) {
// TODO A penetration upper bound should be computed.
return -1;
}
return sqrt (sqrDistLowerBound);
}
};
template<> struct DistanceTraversalBVDistanceLowerBound_impl<AABB>
{
static FCL_REAL run(const BVNode<AABB>& b1, const BVNode<AABB>& 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);
}
static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<AABB>& b1, const BVNode<AABB>& b2)
{
FCL_REAL sqrDistLowerBound;
CollisionRequest request (DISTANCE_LOWER_BOUND, 0);
// request.break_distance = ?
if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) {
// TODO A penetration upper bound should be computed.
return -1;
}
return sqrt (sqrDistLowerBound);
}
};
} // namespace details
......@@ -369,14 +410,6 @@ public:
return model2->getBV(b).rightChild();
}
/// @brief BV culling test in one BVTT node
FCL_REAL BVDistanceLowerBound(int b1, int b2) const
{
if(enable_statistics) num_bv_tests++;
return details::DistanceTraversalBVDistanceLowerBound_impl<BV>
::run (model1->getBV(b1), model2->getBV(b2));
}
/// @brief The first BVH model
const BVHModel<BV>* model1;
/// @brief The second BVH model
......@@ -390,10 +423,24 @@ public:
/// @brief Traversal node for distance computation between two meshes
template<typename BV>
template<typename BV, int _Options = RelativeTransformationIsIdentity>
class MeshDistanceTraversalNode : public BVHDistanceTraversalNode<BV>
{
public:
enum {
Options = _Options,
RTIsIdentity = _Options & RelativeTransformationIsIdentity
};
using BVHDistanceTraversalNode<BV>::enable_statistics;
using BVHDistanceTraversalNode<BV>::request;
using BVHDistanceTraversalNode<BV>::result;
using BVHDistanceTraversalNode<BV>::tf1;
using BVHDistanceTraversalNode<BV>::model1;
using BVHDistanceTraversalNode<BV>::model2;
using BVHDistanceTraversalNode<BV>::num_bv_tests;
using BVHDistanceTraversalNode<BV>::num_leaf_tests;
MeshDistanceTraversalNode() : BVHDistanceTraversalNode<BV>()
{
vertices1 = NULL;
......@@ -405,6 +452,28 @@ public:
abs_err = this->request.abs_err;
}
void preprocess()
{
if(!RTIsIdentity) preprocessOrientedNode();
}
void postprocess()
{
if(!RTIsIdentity) postprocessOrientedNode();
}
/// @brief BV culling test in one BVTT node
FCL_REAL BVDistanceLowerBound(int b1, int b2) const
{
if(enable_statistics) num_bv_tests++;
if (RTIsIdentity)
return details::DistanceTraversalBVDistanceLowerBound_impl<BV>
::run (model1->getBV(b1), model2->getBV(b2));
else
return details::DistanceTraversalBVDistanceLowerBound_impl<BV>
::run (RT._R(), RT._T(), model1->getBV(b1), model2->getBV(b2));
}
/// @brief Distance testing between leaves (two triangles)
void leafComputeDistance(int b1, int b2) const
{
......@@ -430,8 +499,15 @@ public:
// nearest point pair
Vec3f P1, P2, normal;
FCL_REAL d = sqrt (TriangleDistance::sqrTriDistance
(t11, t12, t13, t21, t22, t23, P1, P2));
FCL_REAL d2;
if (RTIsIdentity)
d2 = TriangleDistance::sqrTriDistance (t11, t12, t13, t21, t22, t23,
P1, P2);
else
d2 = TriangleDistance::sqrTriDistance (t11, t12, t13, t21, t22, t23,
RT._R(), RT._T(),
P1, P2);
FCL_REAL d = sqrt(d2);
this->result->update(d, this->model1, this->model2, primitive_id1,
primitive_id2, P1, P2, normal);
......@@ -454,62 +530,52 @@ public:
/// @brief relative and absolute error, default value is 0.01 for both terms
FCL_REAL rel_err;
FCL_REAL abs_err;
};
/// @brief Traversal node for distance computation between two meshes if their underlying BVH node is oriented node (RSS, OBBRSS, kIOS)
class MeshDistanceTraversalNodeRSS : public MeshDistanceTraversalNode<RSS>
{
public:
MeshDistanceTraversalNodeRSS();
void preprocess();
void postprocess();
FCL_REAL BVDistanceLowerBound(int b1, int b2) const;
void leafComputeDistance(int b1, int b2) const;
Matrix3f R;
Vec3f T;
};
details::RelativeTransformation<!bool(RTIsIdentity)> RT;
class MeshDistanceTraversalNodekIOS : public MeshDistanceTraversalNode<kIOS>
{
public:
MeshDistanceTraversalNodekIOS();
void preprocess();
void postprocess();
FCL_REAL BVDistanceLowerBound(int b1, int b2) const;
void leafComputeDistance(int b1, int b2) const;
Matrix3f R;
Vec3f T;
private:
void preprocessOrientedNode()
{
const int init_tri_id1 = 0, init_tri_id2 = 0;
const Triangle& init_tri1 = tri_indices1[init_tri_id1];
const Triangle& init_tri2 = tri_indices2[init_tri_id2];
Vec3f init_tri1_points[3];
Vec3f init_tri2_points[3];
init_tri1_points[0] = vertices1[init_tri1[0]];
init_tri1_points[1] = vertices1[init_tri1[1]];
init_tri1_points[2] = vertices1[init_tri1[2]];
init_tri2_points[0] = vertices2[init_tri2[0]];
init_tri2_points[1] = vertices2[init_tri2[1]];
init_tri2_points[2] = vertices2[init_tri2[2]];
Vec3f p1, p2, normal;
FCL_REAL distance = sqrt (TriangleDistance::sqrTriDistance
(init_tri1_points[0], init_tri1_points[1],
init_tri1_points[2], init_tri2_points[0],
init_tri2_points[1], init_tri2_points[2],
RT._R(), RT._T(), p1, p2));
result->update(distance, model1, model2, init_tri_id1, init_tri_id2, p1, p2,
normal);
}
void postprocessOrientedNode()
{
/// the points obtained by triDistance are not in world space: both are in object1's local coordinate system, so we need to convert them into the world space.
if(request.enable_nearest_points && (result->o1 == model1) && (result->o2 == model2))
{
result->nearest_points[0] = tf1.transform(result->nearest_points[0]);
result->nearest_points[1] = tf1.transform(result->nearest_points[1]);
}
}
};
class MeshDistanceTraversalNodeOBBRSS : public MeshDistanceTraversalNode<OBBRSS>
{
public:
MeshDistanceTraversalNodeOBBRSS();
void preprocess();
void postprocess();
FCL_REAL BVDistanceLowerBound(int b1, int b2) const;
FCL_REAL BVDistanceLowerBound(int b1, int b2, FCL_REAL& sqrDistLowerBound) const;
void leafComputeDistance(int b1, int b2) const;
Matrix3f R;
Vec3f T;
};
/// @brief Traversal node for distance computation between two meshes if their underlying BVH node is oriented node (RSS, OBBRSS, kIOS)
typedef MeshDistanceTraversalNode<RSS , 0> MeshDistanceTraversalNodeRSS;
typedef MeshDistanceTraversalNode<kIOS , 0> MeshDistanceTraversalNodekIOS;
typedef MeshDistanceTraversalNode<OBBRSS, 0> MeshDistanceTraversalNodeOBBRSS;
/// @}
......
......@@ -40,6 +40,7 @@
/// @cond INTERNAL
#include <hpp/fcl/internal/tools.h>
#include <hpp/fcl/internal/traversal_node_bvhs.h>
#include <hpp/fcl/internal/traversal_node_shapes.h>
#include <hpp/fcl/internal/traversal_node_bvh_shape.h>
......@@ -513,7 +514,7 @@ bool initialize(ShapeDistanceTraversalNode<S1, S2>& node,
/// @brief Initialize traversal node for distance computation between two meshes, given the current transforms
template<typename BV>
bool initialize(MeshDistanceTraversalNode<BV>& node,
bool initialize(MeshDistanceTraversalNode<BV, RelativeTransformationIsIdentity>& node,
BVHModel<BV>& model1, Transform3f& tf1,
BVHModel<BV>& model2, Transform3f& tf2,
const DistanceRequest& request,
......@@ -574,27 +575,37 @@ bool initialize(MeshDistanceTraversalNode<BV>& node,
return true;
}
/// @brief Initialize traversal node for distance computation between two meshes, specialized for RSS type
bool initialize(MeshDistanceTraversalNodeRSS& node,
const BVHModel<RSS>& model1, const Transform3f& tf1,
const BVHModel<RSS>& model2, const Transform3f& tf2,
/// @brief Initialize traversal node for distance computation between two meshes
template<typename BV>
bool initialize(MeshDistanceTraversalNode<BV, 0>& node,
const BVHModel<BV>& model1, const Transform3f& tf1,
const BVHModel<BV>& model2, const Transform3f& tf2,
const DistanceRequest& request,
DistanceResult& result);
DistanceResult& result)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
return false;
/// @brief Initialize traversal node for distance computation between two meshes, specialized for kIOS type
bool initialize(MeshDistanceTraversalNodekIOS& node,
const BVHModel<kIOS>& model1, const Transform3f& tf1,
const BVHModel<kIOS>& model2, const Transform3f& tf2,
const DistanceRequest& request,
DistanceResult& result);
node.request = request;
node.result = &result;
/// @brief Initialize traversal node for distance computation between two meshes, specialized for OBBRSS type
bool initialize(MeshDistanceTraversalNodeOBBRSS& node,
const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
const BVHModel<OBBRSS>& model2, const Transform3f& tf2,
const DistanceRequest& request,
DistanceResult& result);
node.model1 = &model1;
node.tf1 = tf1;
node.model2 = &model2;
node.tf2 = tf2;
node.vertices1 = model1.vertices;
node.vertices2 = model2.vertices;
node.tri_indices1 = model1.tri_indices;
node.tri_indices2 = model2.tri_indices;
relativeTransform(tf1.getRotation(), tf1.getTranslation(),
tf2.getRotation(), tf2.getTranslation(),
node.RT.R, node.RT.T);
return true;
}
/// @brief Initialize traversal node for distance computation between one mesh and one shape, given the current transforms
template<typename BV, typename S>
......
/*
* 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 */
#include <hpp/fcl/internal/traversal_node_bvhs.h>
namespace hpp
{
namespace fcl
{
namespace details
{
template<typename BV>
static inline void meshDistanceOrientedNodeleafComputeDistance(int b1, int b2,
const BVHModel<BV>* model1, const BVHModel<BV>* model2,
Vec3f* vertices1, Vec3f* vertices2,
Triangle* tri_indices1, Triangle* tri_indices2,
const Matrix3f& R, const Vec3f& T,
bool enable_statistics,
int& num_leaf_tests,
const DistanceRequest&,
DistanceResult& result)
{
if(enable_statistics) num_leaf_tests++;
const BVNode<BV>& node1 = model1->getBV(b1);
const BVNode<BV>& node2 = model2->getBV(b2);
int primitive_id1 = node1.primitiveId();
int primitive_id2 = node2.primitiveId();
const Triangle& tri_id1 = tri_indices1[primitive_id1];
const Triangle& tri_id2 = tri_indices2[primitive_id2];
const Vec3f& t11 = vertices1[tri_id1[0]];
const Vec3f& t12 = vertices1[tri_id1[1]];
const Vec3f& t13 = vertices1[tri_id1[2]];
const Vec3f& t21 = vertices2[tri_id2[0]];
const Vec3f& t22 = vertices2[tri_id2[1]];
const Vec3f& t23 = vertices2[tri_id2[2]];
// nearest point pair
Vec3f P1, P2, normal;
FCL_REAL d = sqrt (TriangleDistance::sqrTriDistance
(t11, t12, t13, t21, t22, t23, R, T, P1, P2));
result.update(d, model1, model2, primitive_id1, primitive_id2, P1, P2,
normal);
}
} // namespace details
namespace details
{
template<typename BV>
static inline void distancePreprocessOrientedNode(const BVHModel<BV>* model1, const BVHModel<BV>* model2,
const Vec3f* vertices1, Vec3f* vertices2,
Triangle* tri_indices1, Triangle* tri_indices2,
int init_tri_id1, int init_tri_id2,
const Matrix3f& R, const Vec3f& T,
const DistanceRequest&,
DistanceResult& result)
{
const Triangle& init_tri1 = tri_indices1[init_tri_id1];
const Triangle& init_tri2 = tri_indices2[init_tri_id2];
Vec3f init_tri1_points[3];
Vec3f init_tri2_points[3];
init_tri1_points[0] = vertices1[init_tri1[0]];
init_tri1_points[1] = vertices1[init_tri1[1]];
init_tri1_points[2] = vertices1[init_tri1[2]];
init_tri2_points[0] = vertices2[init_tri2[0]];
init_tri2_points[1] = vertices2[init_tri2[1]];
init_tri2_points[2] = vertices2[init_tri2[2]];
Vec3f p1, p2, normal;
FCL_REAL distance = sqrt (TriangleDistance::sqrTriDistance
(init_tri1_points[0], init_tri1_points[1],
init_tri1_points[2], init_tri2_points[0],
init_tri2_points[1], init_tri2_points[2],
R, T, p1, p2));
result.update(distance, model1, model2, init_tri_id1, init_tri_id2, p1, p2,
normal);
}
template<typename BV>
static inline void distancePostprocessOrientedNode(const BVHModel<BV>* model1, const BVHModel<BV>* model2,
const Transform3f& tf1, const DistanceRequest& request, DistanceResult& result)
{
/// the points obtained by triDistance are not in world space: both are in object1's local coordinate system, so we need to convert them into the world space.
if(request.enable_nearest_points && (result.o1 == model1) && (result.o2 == model2))
{
result.nearest_points[0] = tf1.transform(result.nearest_points[0]).eval();
result.nearest_points[1] = tf1.transform(result.nearest_points[1]).eval();
}
}
}
MeshDistanceTraversalNodeRSS::MeshDistanceTraversalNodeRSS() : MeshDistanceTraversalNode<RSS>()
{
R.setIdentity();
}
void MeshDistanceTraversalNodeRSS::preprocess()
{
details::distancePreprocessOrientedNode(model1, model2, vertices1, vertices2, tri_indices1, tri_indices2, 0, 0, R, T, request, *result);
}
void MeshDistanceTraversalNodeRSS::postprocess()
{
details::distancePostprocessOrientedNode(model1, model2, tf1, request, *result);
}
FCL_REAL MeshDistanceTraversalNodeRSS::BVDistanceLowerBound(int b1, int b2) const
{
if(enable_statistics) num_bv_tests++;
return distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
}
void MeshDistanceTraversalNodeRSS::leafComputeDistance(int b1, int b2) const
{
details::meshDistanceOrientedNodeleafComputeDistance(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2,
R, T, enable_statistics, num_leaf_tests,
request, *result);
}
MeshDistanceTraversalNodekIOS::MeshDistanceTraversalNodekIOS() : MeshDistanceTraversalNode<kIOS>()
{
R.setIdentity();
}
void MeshDistanceTraversalNodekIOS::preprocess()
{
details::distancePreprocessOrientedNode(model1, model2, vertices1, vertices2, tri_indices1, tri_indices2, 0, 0, R, T, request, *result);
}
void MeshDistanceTraversalNodekIOS::postprocess()
{
details::distancePostprocessOrientedNode(model1, model2, tf1, request, *result);
}
FCL_REAL MeshDistanceTraversalNodekIOS::BVDistanceLowerBound(int b1, int b2) const
{
if(enable_statistics) num_bv_tests++;
return distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
}
void MeshDistanceTraversalNodekIOS::leafComputeDistance(int b1, int b2) const
{
details::meshDistanceOrientedNodeleafComputeDistance(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2,
R, T, enable_statistics, num_leaf_tests,
request, *result);
}
MeshDistanceTraversalNodeOBBRSS::MeshDistanceTraversalNodeOBBRSS() : MeshDistanceTraversalNode<OBBRSS>()
{
R.setIdentity();
}
void MeshDistanceTraversalNodeOBBRSS::preprocess()
{
details::distancePreprocessOrientedNode(model1, model2, vertices1, vertices2, tri_indices1, tri_indices2, 0, 0, R, T, request, *result);
}
void MeshDistanceTraversalNodeOBBRSS::postprocess()
{
details::distancePostprocessOrientedNode(model1, model2, tf1, request, *result);
}
FCL_REAL MeshDistanceTraversalNodeOBBRSS::BVDistanceLowerBound(int b1, int b2) const
{
if(enable_statistics) num_bv_tests++;
return distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
}
void MeshDistanceTraversalNodeOBBRSS::leafComputeDistance(int b1, int b2) const
{
details::meshDistanceOrientedNodeleafComputeDistance(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2,
R, T, enable_statistics, num_leaf_tests,
request, *result);
}
}
} // namespace hpp
/*
* 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 */
#include <hpp/fcl/internal/traversal_node_setup.h>
#include <hpp/fcl/internal/tools.h>
namespace hpp
{
namespace fcl
{
namespace details
{
template<typename BV, typename OrientedNode>
static inline bool setupMeshDistanceOrientedNode(OrientedNode& node,
const BVHModel<BV>& model1, const Transform3f& tf1,
const BVHModel<BV>& model2, const Transform3f& tf2,
const DistanceRequest& request,
DistanceResult& result)