traversal_node_base.h 5.85 KB
Newer Older
sachinc's avatar
sachinc committed
1
2
3
/*
 * Software License Agreement (BSD License)
 *
4
5
 *  Copyright (c) 2011-2014, Willow Garage, Inc.
 *  Copyright (c) 2014-2015, Open Source Robotics Foundation
sachinc's avatar
sachinc committed
6
7
8
9
10
11
12
13
14
15
16
17
 *  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.
18
 *   * Neither the name of Open Source Robotics Foundation nor the names of its
sachinc's avatar
sachinc committed
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
 *     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 */

Joseph Mirabel's avatar
Joseph Mirabel committed
38
39
#ifndef HPP_FCL_TRAVERSAL_NODE_BASE_H
#define HPP_FCL_TRAVERSAL_NODE_BASE_H
sachinc's avatar
sachinc committed
40

41
42
/// @cond INTERNAL

43
44
45
#include <hpp/fcl/data_types.h>
#include <hpp/fcl/math/transform.h>
#include <hpp/fcl/collision_data.h>
sachinc's avatar
sachinc committed
46

47
48
namespace hpp
{
sachinc's avatar
sachinc committed
49
50
51
namespace fcl
{

52
/// @brief Node structure encoding the information required for traversal.
Lucile Remigy's avatar
Lucile Remigy committed
53

54
class TraversalNodeBase
sachinc's avatar
sachinc committed
55
56
{
public:
57
58
59
  TraversalNodeBase () : enable_statistics(false) {}

  virtual ~TraversalNodeBase() {}
sachinc's avatar
sachinc committed
60

jpan's avatar
   
jpan committed
61
62
63
64
  virtual void preprocess() {}
  
  virtual void postprocess() {}

65
  /// @brief Whether b is a leaf node in the first BVH tree 
66
  virtual bool isFirstNodeLeaf(int /*b*/) const { return true; }
sachinc's avatar
sachinc committed
67

68
  /// @brief Whether b is a leaf node in the second BVH tree
69
  virtual bool isSecondNodeLeaf(int /*b*/) const { return true; }
sachinc's avatar
sachinc committed
70

71
  /// @brief Traverse the subtree of the node in the first tree first
72
  virtual bool firstOverSecond(int /*b1*/, int /*b2*/) const { return true; }
sachinc's avatar
sachinc committed
73

74
  /// @brief Get the left child of the node b in the first tree
75
  virtual int getFirstLeftChild(int b) const { return b; }
sachinc's avatar
sachinc committed
76

77
  /// @brief Get the right child of the node b in the first tree
78
  virtual int getFirstRightChild(int b) const { return b; }
sachinc's avatar
sachinc committed
79

80
  /// @brief Get the left child of the node b in the second tree
81
  virtual int getSecondLeftChild(int b) const { return b; }
sachinc's avatar
sachinc committed
82

83
  /// @brief Get the right child of the node b in the second tree
84
  virtual int getSecondRightChild(int b) const { return b; }
sachinc's avatar
sachinc committed
85

86
87
  /// @brief Whether store some statistics information during traversal
  void enableStatistics(bool enable) { enable_statistics = enable; }
88

89
  /// @brief configuation of first object
90
  Transform3f tf1;
91

92
  /// @brief configuration of second object
93
  Transform3f tf2;
94
95
96

  /// @brief Whether stores statistics
  bool enable_statistics;
sachinc's avatar
sachinc committed
97
98
};

99
100
101
/// @defgroup Traversal_For_Collision
/// regroup class about traversal for distance.
/// @{
Lucile Remigy's avatar
Lucile Remigy committed
102

103
/// @brief Node structure encoding the information required for collision traversal.
104
class CollisionTraversalNodeBase : public TraversalNodeBase
sachinc's avatar
sachinc committed
105
106
{
public:
107
  CollisionTraversalNodeBase (const CollisionRequest& request_) :
108
  request (request_), result(NULL) {}
sachinc's avatar
sachinc committed
109

110
  virtual ~CollisionTraversalNodeBase() {}
sachinc's avatar
sachinc committed
111

112
  /// @brief BV test between b1 and b2
113
  virtual bool BVDisjoints(int b1, int b2) const = 0;
sachinc's avatar
sachinc committed
114

115
  /// BV test between b1 and b2
Lucile Remigy's avatar
Lucile Remigy committed
116
117
  /// @param b1, b2 Bounding volumes to test,
  /// @retval sqrDistLowerBound square of a lower bound of the minimal
118
  ///         distance between bounding volumes.
119
  virtual bool BVDisjoints(int b1, int b2, FCL_REAL& sqrDistLowerBound) const = 0;
120

121
  /// @brief Leaf test between node b1 and b2, if they are both leafs
122
  virtual void leafCollides(int /*b1*/, int /*b2*/, FCL_REAL& /*sqrDistLowerBound*/) const
123
124
125
  {
    throw std::runtime_error ("Not implemented");
  }
sachinc's avatar
sachinc committed
126

127
  /// @brief Check whether the traversal can stop
128
  bool canStop() const { return this->request.isSatisfied(*(this->result)); }
jpan's avatar
jpan committed
129

130
  /// @brief request setting for collision
131
  const CollisionRequest& request;
jpan's avatar
jpan committed
132

133
  /// @brief collision result kept during the traversal iteration
jpan's avatar
   
jpan committed
134
135
  CollisionResult* result;

136
  /// @brief Whether stores statistics 
jpan's avatar
jpan committed
137
  bool enable_statistics;
sachinc's avatar
sachinc committed
138
139
};

140
141
142
143
144
145
/// @}

/// @defgroup Traversal_For_Distance
/// regroup class about traversal for distance.
/// @{

146
/// @brief Node structure encoding the information required for distance traversal.
147
class DistanceTraversalNodeBase : public TraversalNodeBase
sachinc's avatar
sachinc committed
148
149
{
public:
150
  DistanceTraversalNodeBase() : result(NULL) {}
sachinc's avatar
sachinc committed
151

152
  virtual ~DistanceTraversalNodeBase() {}
sachinc's avatar
sachinc committed
153

154
  /// @brief BV test between b1 and b2
Lucile Remigy's avatar
Lucile Remigy committed
155
156
  /// @return a lower bound of the distance between the two BV.
  /// @note except for OBB, this method returns the distance.
157
158
159
160
  virtual FCL_REAL BVDistanceLowerBound(int /*b1*/, int /*b2*/) const
  {
    return std::numeric_limits<FCL_REAL>::max();
  }
sachinc's avatar
sachinc committed
161

162
  /// @brief Leaf test between node b1 and b2, if they are both leafs
163
  virtual void leafComputeDistance(int b1, int b2) const = 0;
sachinc's avatar
sachinc committed
164

165
  /// @brief Check whether the traversal can stop
166
167
  virtual bool canStop(FCL_REAL /*c*/) const
  { return false; }
jpan's avatar
jpan committed
168

169
  /// @brief request setting for distance
170
  DistanceRequest request;
jpan's avatar
jpan committed
171

172
  /// @brief distance result kept during the traversal iteration
jpan's avatar
   
jpan committed
173
  DistanceResult* result;
sachinc's avatar
sachinc committed
174
};
175
176
177

///@}

sachinc's avatar
sachinc committed
178
179
}

180
181
} // namespace hpp

182
183
/// @endcond

Justin Carpentier's avatar
Justin Carpentier committed
184
185
#endif