Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/hpp/fcl/internal/traversal_node_bvhs.h	Lines:	179	186	96.2 %
Date:	2024-02-09 12:57:42	Branches:	73	134	54.5 %


/*
 * 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 HPP_FCL_TRAVERSAL_NODE_MESHES_H
#define HPP_FCL_TRAVERSAL_NODE_MESHES_H

/// @cond INTERNAL

#include <hpp/fcl/collision_data.h>
#include <hpp/fcl/internal/traversal_node_base.h>
#include <hpp/fcl/BV/BV_node.h>
#include <hpp/fcl/BV/BV.h>
#include <hpp/fcl/BVH/BVH_model.h>
#include <hpp/fcl/internal/intersect.h>
#include <hpp/fcl/shape/geometric_shapes.h>
#include <hpp/fcl/narrowphase/narrowphase.h>
#include <hpp/fcl/internal/traversal.h>

#include <limits>
#include <vector>
#include <cassert>

namespace hpp {
namespace fcl {

/// @addtogroup Traversal_For_Collision
/// @{

/// @brief Traversal node for collision between BVH models
template <typename BV>
class BVHCollisionTraversalNode : public CollisionTraversalNodeBase {
 public:
  BVHCollisionTraversalNode(const CollisionRequest& request)
      : CollisionTraversalNodeBase(request) {
    model1 = NULL;
    model2 = NULL;

    num_bv_tests = 0;
    num_leaf_tests = 0;
    query_time_seconds = 0.0;
  }

  /// @brief Whether the BV node in the first BVH tree is leaf
  bool isFirstNodeLeaf(unsigned int b) const {
    assert(model1 != NULL && "model1 is NULL");
    return model1->getBV(b).isLeaf();
  }

  /// @brief Whether the BV node in the second BVH tree is leaf
  bool isSecondNodeLeaf(unsigned int b) const {
    assert(model2 != NULL && "model2 is NULL");
    return model2->getBV(b).isLeaf();
  }

  /// @brief Determine the traversal order, is the first BVTT subtree better
  bool firstOverSecond(unsigned int b1, unsigned int b2) const {
    FCL_REAL sz1 = model1->getBV(b1).bv.size();
    FCL_REAL sz2 = model2->getBV(b2).bv.size();

    bool l1 = model1->getBV(b1).isLeaf();
    bool l2 = model2->getBV(b2).isLeaf();

    if (l2 || (!l1 && (sz1 > sz2))) return true;
    return false;
  }

  /// @brief Obtain the left child of BV node in the first BVH
  int getFirstLeftChild(unsigned int b) const {
    return model1->getBV(b).leftChild();
  }

  /// @brief Obtain the right child of BV node in the first BVH
  int getFirstRightChild(unsigned int b) const {
    return model1->getBV(b).rightChild();
  }

  /// @brief Obtain the left child of BV node in the second BVH
  int getSecondLeftChild(unsigned int b) const {
    return model2->getBV(b).leftChild();
  }

  /// @brief Obtain the right child of BV node in the second BVH
  int getSecondRightChild(unsigned int b) const {
    return model2->getBV(b).rightChild();
  }

  /// @brief The first BVH model
  const BVHModel<BV>* model1;
  /// @brief The second BVH model
  const BVHModel<BV>* model2;

  /// @brief statistical information
  mutable int num_bv_tests;
  mutable int num_leaf_tests;
  mutable FCL_REAL query_time_seconds;
};

/// @brief Traversal node for collision between two meshes
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) {
    vertices1 = NULL;
    vertices2 = NULL;
    tri_indices1 = NULL;
    tri_indices2 = NULL;
  }

  /// 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 BVDisjoints(unsigned int b1, unsigned int b2,
                   FCL_REAL& sqrDistLowerBound) const {
    if (this->enable_statistics) this->num_bv_tests++;
    bool disjoint;
    if (RTIsIdentity)
      disjoint = !this->model1->getBV(b1).overlap(
          this->model2->getBV(b2), this->request, sqrDistLowerBound);
    else {
      disjoint = !overlap(RT._R(), RT._T(), this->model2->getBV(b2).bv,
                          this->model1->getBV(b1).bv, this->request,
                          sqrDistLowerBound);
    }
    if (disjoint)
      internal::updateDistanceLowerBoundFromBV(this->request, *this->result,
                                               sqrDistLowerBound);
    return disjoint;
  }

  /// Intersection testing between leaves (two triangles)
  ///
  /// @param b1, b2 id of primitive in bounding volume hierarchy
  /// @retval sqrDistLowerBound squared lower bound of distance between
  ///         primitives if they are not in collision.
  ///
  /// This method supports a security margin. If the distance between
  /// the primitives is less than the security margin, the objects are
  /// considered as in collision. in this case a contact point is
  /// returned in the CollisionResult.
  ///
  /// @note If the distance between objects is less than the security margin,
  ///       and the object are not colliding, the penetration depth is
  ///       negative.
  void leafCollides(unsigned int b1, unsigned int b2,
                    FCL_REAL& sqrDistLowerBound) const {
    if (this->enable_statistics) this->num_leaf_tests++;

    const BVNode<BV>& node1 = this->model1->getBV(b1);
    const BVNode<BV>& node2 = this->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& P1 = vertices1[tri_id1[0]];
    const Vec3f& P2 = vertices1[tri_id1[1]];
    const Vec3f& P3 = vertices1[tri_id1[2]];
    const Vec3f& Q1 = vertices2[tri_id2[0]];
    const Vec3f& Q2 = vertices2[tri_id2[1]];
    const Vec3f& Q3 = vertices2[tri_id2[2]];

    TriangleP tri1(P1, P2, P3);
    TriangleP tri2(Q1, Q2, Q3);
    GJKSolver solver;
    Vec3f p1,
        p2;  // closest points if no collision contact points if collision.
    Vec3f normal;
    FCL_REAL distance;
    solver.shapeDistance(tri1, this->tf1, tri2, this->tf2, distance, p1, p2,
                         normal);

    const FCL_REAL distToCollision = distance - this->request.security_margin;
    if (distToCollision <=
        this->request.collision_distance_threshold) {  // collision
      sqrDistLowerBound = 0;
      Vec3f p(p1);  // contact point
      if (this->result->numContacts() < this->request.num_max_contacts) {
        // How much (Q1, Q2, Q3) should be moved so that all vertices are
        // above (P1, P2, P3).
        if (distance > 0) {
          normal = (p2 - p1).normalized();
          p = .5 * (p1 + p2);
        }
        this->result->addContact(Contact(this->model1, this->model2,
                                         primitive_id1, primitive_id2, p,
                                         normal, -distance));
      }
    } else
      sqrDistLowerBound = distToCollision * distToCollision;

    internal::updateDistanceLowerBoundFromLeaf(this->request, *this->result,
                                               distToCollision, p1, p2);
  }

  Vec3f* vertices1;
  Vec3f* vertices2;

  Triangle* tri_indices1;
  Triangle* tri_indices2;

  details::RelativeTransformation<!bool(RTIsIdentity)> RT;
};

/// @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;

/// @}

namespace details {
template <typename BV>
struct DistanceTraversalBVDistanceLowerBound_impl {
  static FCL_REAL run(const BVNode<BV>& b1, const BVNode<BV>& b2) {
    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> {
  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);
  }
  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

/// @addtogroup Traversal_For_Distance
/// @{

/// @brief Traversal node for distance computation between BVH models
template <typename BV>
class BVHDistanceTraversalNode : public DistanceTraversalNodeBase {
 public:
  BVHDistanceTraversalNode() : DistanceTraversalNodeBase() {
    model1 = NULL;
    model2 = NULL;

    num_bv_tests = 0;
    num_leaf_tests = 0;
    query_time_seconds = 0.0;
  }

  /// @brief Whether the BV node in the first BVH tree is leaf
  bool isFirstNodeLeaf(unsigned int b) const {
    return model1->getBV(b).isLeaf();
  }

  /// @brief Whether the BV node in the second BVH tree is leaf
  bool isSecondNodeLeaf(unsigned int b) const {
    return model2->getBV(b).isLeaf();
  }

  /// @brief Determine the traversal order, is the first BVTT subtree better
  bool firstOverSecond(unsigned int b1, unsigned int b2) const {
    FCL_REAL sz1 = model1->getBV(b1).bv.size();
    FCL_REAL sz2 = model2->getBV(b2).bv.size();

    bool l1 = model1->getBV(b1).isLeaf();
    bool l2 = model2->getBV(b2).isLeaf();

    if (l2 || (!l1 && (sz1 > sz2))) return true;
    return false;
  }

  /// @brief Obtain the left child of BV node in the first BVH
  int getFirstLeftChild(unsigned int b) const {
    return model1->getBV(b).leftChild();
  }

  /// @brief Obtain the right child of BV node in the first BVH
  int getFirstRightChild(unsigned int b) const {
    return model1->getBV(b).rightChild();
  }

  /// @brief Obtain the left child of BV node in the second BVH
  int getSecondLeftChild(unsigned int b) const {
    return model2->getBV(b).leftChild();
  }

  /// @brief Obtain the right child of BV node in the second BVH
  int getSecondRightChild(unsigned int b) const {
    return model2->getBV(b).rightChild();
  }

  /// @brief The first BVH model
  const BVHModel<BV>* model1;
  /// @brief The second BVH model
  const BVHModel<BV>* model2;

  /// @brief statistical information
  mutable int num_bv_tests;
  mutable int num_leaf_tests;
  mutable FCL_REAL query_time_seconds;
};

/// @brief Traversal node for distance computation between two meshes
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;
    vertices2 = NULL;
    tri_indices1 = NULL;
    tri_indices2 = NULL;

    rel_err = this->request.rel_err;
    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(unsigned int b1, unsigned 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(unsigned int b1, unsigned int b2) const {
    if (this->enable_statistics) this->num_leaf_tests++;

    const BVNode<BV>& node1 = this->model1->getBV(b1);
    const BVNode<BV>& node2 = this->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 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);
  }

  /// @brief Whether the traversal process can stop early
  bool canStop(FCL_REAL c) const {
    if ((c >= this->result->min_distance - abs_err) &&
        (c * (1 + rel_err) >= this->result->min_distance))
      return true;
    return false;
  }

  Vec3f* vertices1;
  Vec3f* vertices2;

  Triangle* tri_indices1;
  Triangle* tri_indices2;

  /// @brief relative and absolute error, default value is 0.01 for both terms
  FCL_REAL rel_err;
  FCL_REAL abs_err;

  details::RelativeTransformation<!bool(RTIsIdentity)> RT;

 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]);
    }
  }
};

/// @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;

/// @}

/// @brief for OBB and RSS, there is local coordinate of BV, so normal need to
/// be transformed
namespace details {

template <typename BV>
inline const Matrix3f& getBVAxes(const BV& bv) {
  return bv.axes;
}

template <>
inline const Matrix3f& getBVAxes<OBBRSS>(const OBBRSS& bv) {
  return bv.obb.axes;
}

}  // namespace details

}  // namespace fcl

}  // namespace hpp

/// @endcond

#endif


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			/*
2			* Software License Agreement (BSD License)
3			*
4			* Copyright (c) 2011-2014, Willow Garage, Inc.
5			* Copyright (c) 2014-2015, Open Source Robotics Foundation
6			* All rights reserved.
7			*
8			* Redistribution and use in source and binary forms, with or without
9			* modification, are permitted provided that the following conditions
10			* are met:
11			*
12			* * Redistributions of source code must retain the above copyright
13			* notice, this list of conditions and the following disclaimer.
14			* * Redistributions in binary form must reproduce the above
15			* copyright notice, this list of conditions and the following
16			* disclaimer in the documentation and/or other materials provided
17			* with the distribution.
18			* * Neither the name of Open Source Robotics Foundation nor the names of its
19			* contributors may be used to endorse or promote products derived
20			* from this software without specific prior written permission.
21			*
22			* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23			* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24			* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25			* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26			* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27			* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28			* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29			* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30			* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31			* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32			* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33			* POSSIBILITY OF SUCH DAMAGE.
34			*/
35
36			/** \author Jia Pan */
37
38			#ifndef HPP_FCL_TRAVERSAL_NODE_MESHES_H
39			#define HPP_FCL_TRAVERSAL_NODE_MESHES_H
40
41			/// @cond INTERNAL
42
43			#include <hpp/fcl/collision_data.h>
44			#include <hpp/fcl/internal/traversal_node_base.h>
45			#include <hpp/fcl/BV/BV_node.h>
46			#include <hpp/fcl/BV/BV.h>
47			#include <hpp/fcl/BVH/BVH_model.h>
48			#include <hpp/fcl/internal/intersect.h>
49			#include <hpp/fcl/shape/geometric_shapes.h>
50			#include <hpp/fcl/narrowphase/narrowphase.h>
51			#include <hpp/fcl/internal/traversal.h>
52
53			#include <limits>
54			#include <vector>
55			#include <cassert>
56
57			namespace hpp {
58			namespace fcl {
59
60			/// @addtogroup Traversal_For_Collision
61			/// @{
62
63			/// @brief Traversal node for collision between BVH models
64			template <typename BV>
65			class BVHCollisionTraversalNode : public CollisionTraversalNodeBase {
66			public:
67		20631	BVHCollisionTraversalNode(const CollisionRequest& request)
68		20631	: CollisionTraversalNodeBase(request) {
69		20631	model1 = NULL;
70		20631	model2 = NULL;
71
72		20631	num_bv_tests = 0;
73		20631	num_leaf_tests = 0;
74		20631	query_time_seconds = 0.0;
75		20631	}
76
77			/// @brief Whether the BV node in the first BVH tree is leaf
78		116958527	bool isFirstNodeLeaf(unsigned int b) const {
79	✗✓	116958527	assert(model1 != NULL && "model1 is NULL");
80		116958527	return model1->getBV(b).isLeaf();
81			}
82
83			/// @brief Whether the BV node in the second BVH tree is leaf
84		116958527	bool isSecondNodeLeaf(unsigned int b) const {
85	✗✓	116958527	assert(model2 != NULL && "model2 is NULL");
86		116958527	return model2->getBV(b).isLeaf();
87			}
88
89			/// @brief Determine the traversal order, is the first BVTT subtree better
90		41563421	bool firstOverSecond(unsigned int b1, unsigned int b2) const {
91		41563421	FCL_REAL sz1 = model1->getBV(b1).bv.size();
92		41563421	FCL_REAL sz2 = model2->getBV(b2).bv.size();
93
94		41563421	bool l1 = model1->getBV(b1).isLeaf();
95		41563421	bool l2 = model2->getBV(b2).isLeaf();
96
97	✓✓✓✓ ✓✓	41563421	if (l2 \|\| (!l1 && (sz1 > sz2))) return true;
98		16060292	return false;
99			}
100
101			/// @brief Obtain the left child of BV node in the first BVH
102		25503129	int getFirstLeftChild(unsigned int b) const {
103		25503129	return model1->getBV(b).leftChild();
104			}
105
106			/// @brief Obtain the right child of BV node in the first BVH
107		25503129	int getFirstRightChild(unsigned int b) const {
108		25503129	return model1->getBV(b).rightChild();
109			}
110
111			/// @brief Obtain the left child of BV node in the second BVH
112		16060292	int getSecondLeftChild(unsigned int b) const {
113		16060292	return model2->getBV(b).leftChild();
114			}
115
116			/// @brief Obtain the right child of BV node in the second BVH
117		16060292	int getSecondRightChild(unsigned int b) const {
118		16060292	return model2->getBV(b).rightChild();
119			}
120
121			/// @brief The first BVH model
122			const BVHModel<BV>* model1;
123			/// @brief The second BVH model
124			const BVHModel<BV>* model2;
125
126			/// @brief statistical information
127			mutable int num_bv_tests;
128			mutable int num_leaf_tests;
129			mutable FCL_REAL query_time_seconds;
130			};
131
132			/// @brief Traversal node for collision between two meshes
133			template <typename BV, int _Options = RelativeTransformationIsIdentity>
134			class MeshCollisionTraversalNode : public BVHCollisionTraversalNode<BV> {
135			public:
136			enum {
137			Options = _Options,
138			RTIsIdentity = _Options & RelativeTransformationIsIdentity
139			};
140
141		20631	MeshCollisionTraversalNode(const CollisionRequest& request)
142	✓✗	20631	: BVHCollisionTraversalNode<BV>(request) {
143		20631	vertices1 = NULL;
144		20631	vertices2 = NULL;
145		20631	tri_indices1 = NULL;
146		20631	tri_indices2 = NULL;
147		20631	}
148
149			/// BV test between b1 and b2
150			/// @param b1, b2 Bounding volumes to test,
151			/// @retval sqrDistLowerBound square of a lower bound of the minimal
152			/// distance between bounding volumes.
153		41708925	bool BVDisjoints(unsigned int b1, unsigned int b2,
154			FCL_REAL& sqrDistLowerBound) const {
155	✓✓	41708925	if (this->enable_statistics) this->num_bv_tests++;
156			bool disjoint;
157			if (RTIsIdentity)
158		83175980	disjoint = !this->model1->getBV(b1).overlap(
159		41587990	this->model2->getBV(b2), this->request, sqrDistLowerBound);
160			else {
161		120935	disjoint = !overlap(RT._R(), RT._T(), this->model2->getBV(b2).bv,
162		120935	this->model1->getBV(b1).bv, this->request,
163			sqrDistLowerBound);
164			}
165	✓✓	41708925	if (disjoint)
166		145504	internal::updateDistanceLowerBoundFromBV(this->request, *this->result,
167			sqrDistLowerBound);
168		41708925	return disjoint;
169			}
170
171			/// Intersection testing between leaves (two triangles)
172			///
173			/// @param b1, b2 id of primitive in bounding volume hierarchy
174			/// @retval sqrDistLowerBound squared lower bound of distance between
175			/// primitives if they are not in collision.
176			///
177			/// This method supports a security margin. If the distance between
178			/// the primitives is less than the security margin, the objects are
179			/// considered as in collision. in this case a contact point is
180			/// returned in the CollisionResult.
181			///
182			/// @note If the distance between objects is less than the security margin,
183			/// and the object are not colliding, the penetration depth is
184			/// negative.
185		58356268	void leafCollides(unsigned int b1, unsigned int b2,
186			FCL_REAL& sqrDistLowerBound) const {
187	✓✓	58356268	if (this->enable_statistics) this->num_leaf_tests++;
188
189		58356268	const BVNode<BV>& node1 = this->model1->getBV(b1);
190		58356268	const BVNode<BV>& node2 = this->model2->getBV(b2);
191
192		58356268	int primitive_id1 = node1.primitiveId();
193		58356268	int primitive_id2 = node2.primitiveId();
194
195		58356268	const Triangle& tri_id1 = tri_indices1[primitive_id1];
196		58356268	const Triangle& tri_id2 = tri_indices2[primitive_id2];
197
198		58356268	const Vec3f& P1 = vertices1[tri_id1[0]];
199		58356268	const Vec3f& P2 = vertices1[tri_id1[1]];
200		58356268	const Vec3f& P3 = vertices1[tri_id1[2]];
201		58356268	const Vec3f& Q1 = vertices2[tri_id2[0]];
202		58356268	const Vec3f& Q2 = vertices2[tri_id2[1]];
203		58356268	const Vec3f& Q3 = vertices2[tri_id2[2]];
204
205	✓✗	116712536	TriangleP tri1(P1, P2, P3);
206	✓✗	116712536	TriangleP tri2(Q1, Q2, Q3);
207	✓✗	58356268	GJKSolver solver;
208	✓✗	58356268	Vec3f p1,
209	✓✗	58356268	p2; // closest points if no collision contact points if collision.
210	✓✗	58356268	Vec3f normal;
211			FCL_REAL distance;
212	✓✗	58356268	solver.shapeDistance(tri1, this->tf1, tri2, this->tf2, distance, p1, p2,
213			normal);
214
215		58356268	const FCL_REAL distToCollision = distance - this->request.security_margin;
216		58356268	if (distToCollision <=
217	✓✓	58356268	this->request.collision_distance_threshold) { // collision
218		30705	sqrDistLowerBound = 0;
219	✓✗	30705	Vec3f p(p1); // contact point
220	✓✗	30705	if (this->result->numContacts() < this->request.num_max_contacts) {
221			// How much (Q1, Q2, Q3) should be moved so that all vertices are
222			// above (P1, P2, P3).
223	✗✓	30705	if (distance > 0) {
224			normal = (p2 - p1).normalized();
225			p = .5 * (p1 + p2);
226			}
227	✓✗✓✗	30705	this->result->addContact(Contact(this->model1, this->model2,
228			primitive_id1, primitive_id2, p,
229			normal, -distance));
230			}
231			} else
232		58325563	sqrDistLowerBound = distToCollision * distToCollision;
233
234	✓✗	58356268	internal::updateDistanceLowerBoundFromLeaf(this->request, *this->result,
235			distToCollision, p1, p2);
236		58356268	}
237
238			Vec3f* vertices1;
239			Vec3f* vertices2;
240
241			Triangle* tri_indices1;
242			Triangle* tri_indices2;
243
244			details::RelativeTransformation<!bool(RTIsIdentity)> RT;
245			};
246
247			/// @brief Traversal node for collision between two meshes if their underlying
248			/// BVH node is oriented node (OBB, RSS, OBBRSS, kIOS)
249			typedef MeshCollisionTraversalNode<OBB, 0> MeshCollisionTraversalNodeOBB;
250			typedef MeshCollisionTraversalNode<RSS, 0> MeshCollisionTraversalNodeRSS;
251			typedef MeshCollisionTraversalNode<kIOS, 0> MeshCollisionTraversalNodekIOS;
252			typedef MeshCollisionTraversalNode<OBBRSS, 0> MeshCollisionTraversalNodeOBBRSS;
253
254			/// @}
255
256			namespace details {
257			template <typename BV>
258			struct DistanceTraversalBVDistanceLowerBound_impl {
259		5556	static FCL_REAL run(const BVNode<BV>& b1, const BVNode<BV>& b2) {
260		5556	return b1.distance(b2);
261			}
262		2962522	static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<BV>& b1,
263			const BVNode<BV>& b2) {
264		2965116	return distance(R, T, b1.bv, b2.bv);
265			}
266			};
267
268			template <>
269			struct DistanceTraversalBVDistanceLowerBound_impl<OBB> {
270		15564	static FCL_REAL run(const BVNode<OBB>& b1, const BVNode<OBB>& b2) {
271			FCL_REAL sqrDistLowerBound;
272	✓✗	15564	CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
273			// request.break_distance = ?
274	✓✗✓✓	15564	if (b1.overlap(b2, request, sqrDistLowerBound)) {
275			// TODO A penetration upper bound should be computed.
276		10494	return -1;
277			}
278		5070	return sqrt(sqrDistLowerBound);
279			}
280			static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<OBB>& b1,
281			const BVNode<OBB>& b2) {
282			FCL_REAL sqrDistLowerBound;
283			CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
284			// request.break_distance = ?
285			if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) {
286			// TODO A penetration upper bound should be computed.
287			return -1;
288			}
289			return sqrt(sqrDistLowerBound);
290			}
291			};
292
293			template <>
294			struct DistanceTraversalBVDistanceLowerBound_impl<AABB> {
295			static FCL_REAL run(const BVNode<AABB>& b1, const BVNode<AABB>& b2) {
296			FCL_REAL sqrDistLowerBound;
297			CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
298			// request.break_distance = ?
299			if (b1.overlap(b2, request, sqrDistLowerBound)) {
300			// TODO A penetration upper bound should be computed.
301			return -1;
302			}
303			return sqrt(sqrDistLowerBound);
304			}
305			static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<AABB>& b1,
306			const BVNode<AABB>& b2) {
307			FCL_REAL sqrDistLowerBound;
308			CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
309			// request.break_distance = ?
310			if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) {
311			// TODO A penetration upper bound should be computed.
312			return -1;
313			}
314			return sqrt(sqrDistLowerBound);
315			}
316			};
317			} // namespace details
318
319			/// @addtogroup Traversal_For_Distance
320			/// @{
321
322			/// @brief Traversal node for distance computation between BVH models
323			template <typename BV>
324			class BVHDistanceTraversalNode : public DistanceTraversalNodeBase {
325			public:
326		12946	BVHDistanceTraversalNode() : DistanceTraversalNodeBase() {
327		12946	model1 = NULL;
328		12946	model2 = NULL;
329
330		12946	num_bv_tests = 0;
331		12946	num_leaf_tests = 0;
332		12946	query_time_seconds = 0.0;
333		12946	}
334
335			/// @brief Whether the BV node in the first BVH tree is leaf
336		2152944	bool isFirstNodeLeaf(unsigned int b) const {
337		2152944	return model1->getBV(b).isLeaf();
338			}
339
340			/// @brief Whether the BV node in the second BVH tree is leaf
341		2152944	bool isSecondNodeLeaf(unsigned int b) const {
342		2152944	return model2->getBV(b).isLeaf();
343			}
344
345			/// @brief Determine the traversal order, is the first BVTT subtree better
346		1500900	bool firstOverSecond(unsigned int b1, unsigned int b2) const {
347		1500900	FCL_REAL sz1 = model1->getBV(b1).bv.size();
348		1500900	FCL_REAL sz2 = model2->getBV(b2).bv.size();
349
350		1500900	bool l1 = model1->getBV(b1).isLeaf();
351		1500900	bool l2 = model2->getBV(b2).isLeaf();
352
353	✓✓✓✓ ✓✓	1500900	if (l2 \|\| (!l1 && (sz1 > sz2))) return true;
354		422418	return false;
355			}
356
357			/// @brief Obtain the left child of BV node in the first BVH
358		1078482	int getFirstLeftChild(unsigned int b) const {
359		1078482	return model1->getBV(b).leftChild();
360			}
361
362			/// @brief Obtain the right child of BV node in the first BVH
363		1078482	int getFirstRightChild(unsigned int b) const {
364		1078482	return model1->getBV(b).rightChild();
365			}
366
367			/// @brief Obtain the left child of BV node in the second BVH
368		422418	int getSecondLeftChild(unsigned int b) const {
369		422418	return model2->getBV(b).leftChild();
370			}
371
372			/// @brief Obtain the right child of BV node in the second BVH
373		422418	int getSecondRightChild(unsigned int b) const {
374		422418	return model2->getBV(b).rightChild();
375			}
376
377			/// @brief The first BVH model
378			const BVHModel<BV>* model1;
379			/// @brief The second BVH model
380			const BVHModel<BV>* model2;
381
382			/// @brief statistical information
383			mutable int num_bv_tests;
384			mutable int num_leaf_tests;
385			mutable FCL_REAL query_time_seconds;
386			};
387
388			/// @brief Traversal node for distance computation between two meshes
389			template <typename BV, int _Options = RelativeTransformationIsIdentity>
390			class MeshDistanceTraversalNode : public BVHDistanceTraversalNode<BV> {
391			public:
392			enum {
393			Options = _Options,
394			RTIsIdentity = _Options & RelativeTransformationIsIdentity
395			};
396
397			using BVHDistanceTraversalNode<BV>::enable_statistics;
398			using BVHDistanceTraversalNode<BV>::request;
399			using BVHDistanceTraversalNode<BV>::result;
400			using BVHDistanceTraversalNode<BV>::tf1;
401			using BVHDistanceTraversalNode<BV>::model1;
402			using BVHDistanceTraversalNode<BV>::model2;
403			using BVHDistanceTraversalNode<BV>::num_bv_tests;
404			using BVHDistanceTraversalNode<BV>::num_leaf_tests;
405
406	✓✗	12946	MeshDistanceTraversalNode() : BVHDistanceTraversalNode<BV>() {
407		12946	vertices1 = NULL;
408		12946	vertices2 = NULL;
409		12946	tri_indices1 = NULL;
410		12946	tri_indices2 = NULL;
411
412		12946	rel_err = this->request.rel_err;
413		12946	abs_err = this->request.abs_err;
414		12946	}
415
416		12946	void preprocess() {
417		12898	if (!RTIsIdentity) preprocessOrientedNode();
418		12946	}
419
420		12946	void postprocess() {
421		12898	if (!RTIsIdentity) postprocessOrientedNode();
422		12946	}
423
424			/// @brief BV culling test in one BVTT node
425		3001800	FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int b2) const {
426	✗✓	3001800	if (enable_statistics) num_bv_tests++;
427			if (RTIsIdentity)
428		36684	return details::DistanceTraversalBVDistanceLowerBound_impl<BV>::run(
429		73368	model1->getBV(b1), model2->getBV(b2));
430			else
431		2965116	return details::DistanceTraversalBVDistanceLowerBound_impl<BV>::run(
432		5930232	RT._R(), RT._T(), model1->getBV(b1), model2->getBV(b2));
433			}
434
435			/// @brief Distance testing between leaves (two triangles)
436		650868	void leafComputeDistance(unsigned int b1, unsigned int b2) const {
437	✗✓	650868	if (this->enable_statistics) this->num_leaf_tests++;
438
439		650868	const BVNode<BV>& node1 = this->model1->getBV(b1);
440		650868	const BVNode<BV>& node2 = this->model2->getBV(b2);
441
442		650868	int primitive_id1 = node1.primitiveId();
443		650868	int primitive_id2 = node2.primitiveId();
444
445		650868	const Triangle& tri_id1 = tri_indices1[primitive_id1];
446		650868	const Triangle& tri_id2 = tri_indices2[primitive_id2];
447
448		650868	const Vec3f& t11 = vertices1[tri_id1[0]];
449		650868	const Vec3f& t12 = vertices1[tri_id1[1]];
450		650868	const Vec3f& t13 = vertices1[tri_id1[2]];
451
452		650868	const Vec3f& t21 = vertices2[tri_id2[0]];
453		650868	const Vec3f& t22 = vertices2[tri_id2[1]];
454		650868	const Vec3f& t23 = vertices2[tri_id2[2]];
455
456			// nearest point pair
457	✓✗✓✗ ✓✗	650868	Vec3f P1, P2, normal;
458
459			FCL_REAL d2;
460			if (RTIsIdentity)
461	✓✗	7202	d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23, P1,
462			P2);
463			else
464	✓✗	643666	d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23,
465			RT._R(), RT._T(), P1, P2);
466		650868	FCL_REAL d = sqrt(d2);
467
468	✓✗	650868	this->result->update(d, this->model1, this->model2, primitive_id1,
469			primitive_id2, P1, P2, normal);
470		650868	}
471
472			/// @brief Whether the traversal process can stop early
473		2995702	bool canStop(FCL_REAL c) const {
474	✓✓	2995702	if ((c >= this->result->min_distance - abs_err) &&
475	✓✗	856880	(c * (1 + rel_err) >= this->result->min_distance))
476		856880	return true;
477		2138822	return false;
478			}
479
480			Vec3f* vertices1;
481			Vec3f* vertices2;
482
483			Triangle* tri_indices1;
484			Triangle* tri_indices2;
485
486			/// @brief relative and absolute error, default value is 0.01 for both terms
487			FCL_REAL rel_err;
488			FCL_REAL abs_err;
489
490			details::RelativeTransformation<!bool(RTIsIdentity)> RT;
491
492			private:
493		12898	void preprocessOrientedNode() {
494		12898	const int init_tri_id1 = 0, init_tri_id2 = 0;
495		12898	const Triangle& init_tri1 = tri_indices1[init_tri_id1];
496		12898	const Triangle& init_tri2 = tri_indices2[init_tri_id2];
497
498	✓✓✓✗	51592	Vec3f init_tri1_points[3];
499	✓✓✓✗	51592	Vec3f init_tri2_points[3];
500
501	✓✗	12898	init_tri1_points[0] = vertices1[init_tri1[0]];
502	✓✗	12898	init_tri1_points[1] = vertices1[init_tri1[1]];
503	✓✗	12898	init_tri1_points[2] = vertices1[init_tri1[2]];
504
505	✓✗	12898	init_tri2_points[0] = vertices2[init_tri2[0]];
506	✓✗	12898	init_tri2_points[1] = vertices2[init_tri2[1]];
507	✓✗	12898	init_tri2_points[2] = vertices2[init_tri2[2]];
508
509	✓✗✓✗ ✓✗	12898	Vec3f p1, p2, normal;
510	✓✗	12898	FCL_REAL distance = sqrt(TriangleDistance::sqrTriDistance(
511			init_tri1_points[0], init_tri1_points[1], init_tri1_points[2],
512			init_tri2_points[0], init_tri2_points[1], init_tri2_points[2], RT._R(),
513			RT._T(), p1, p2));
514
515	✓✗	12898	result->update(distance, model1, model2, init_tri_id1, init_tri_id2, p1, p2,
516			normal);
517		12898	}
518		6449	void postprocessOrientedNode() {
519			/// the points obtained by triDistance are not in world space: both are in
520			/// object1's local coordinate system, so we need to convert them into the
521			/// world space.
522	✓✓✓✗	12898	if (request.enable_nearest_points && (result->o1 == model1) &&
523	✓✗	36	(result->o2 == model2)) {
524		36	result->nearest_points[0] = tf1.transform(result->nearest_points[0]);
525		36	result->nearest_points[1] = tf1.transform(result->nearest_points[1]);
526			}
527		12898	}
528			};
529
530			/// @brief Traversal node for distance computation between two meshes if their
531			/// underlying BVH node is oriented node (RSS, OBBRSS, kIOS)
532			typedef MeshDistanceTraversalNode<RSS, 0> MeshDistanceTraversalNodeRSS;
533			typedef MeshDistanceTraversalNode<kIOS, 0> MeshDistanceTraversalNodekIOS;
534			typedef MeshDistanceTraversalNode<OBBRSS, 0> MeshDistanceTraversalNodeOBBRSS;
535
536			/// @}
537
538			/// @brief for OBB and RSS, there is local coordinate of BV, so normal need to
539			/// be transformed
540			namespace details {
541
542			template <typename BV>
543			inline const Matrix3f& getBVAxes(const BV& bv) {
544			return bv.axes;
545			}
546
547			template <>
548			inline const Matrix3f& getBVAxes<OBBRSS>(const OBBRSS& bv) {
549			return bv.obb.axes;
550			}
551
552			} // namespace details
553
554			} // namespace fcl
555
556			} // namespace hpp
557
558			/// @endcond
559
560			#endif