mesh_conservative_advancement_traversal_node.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2016, 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 
38 #ifndef FCL_TRAVERSAL_MESHCONSERVATIVEADVANCEMENTTRAVERSALNODE_H
39 #define FCL_TRAVERSAL_MESHCONSERVATIVEADVANCEMENTTRAVERSALNODE_H
40 
44 
45 namespace fcl
46 {
47 
48 namespace detail
49 {
50 
52 template <typename BV>
54  : public MeshDistanceTraversalNode<BV>
55 {
56 public:
57 
58  using S = typename BV::S;
59 
61 
63  S BVTesting(int b1, int b2) const;
64 
66  void leafTesting(int b1, int b2) const;
67 
69  bool canStop(S c) const;
70 
71  mutable S min_distance;
72 
73  mutable Vector3<S> closest_p1, closest_p2;
74 
75  mutable int last_tri_id1, last_tri_id2;
76 
78  S w;
79 
81  S toc;
83 
85  mutable S delta_t;
86 
90 
91  mutable std::vector<ConservativeAdvancementStackData<S>> stack;
92 
93  template <typename, typename>
94  friend struct CanStopImpl;
95 };
96 
99 template <typename BV>
100 FCL_EXPORT
101 bool initialize(
103  BVHModel<BV>& model1,
104  const Transform3<typename BV::S>& tf1,
105  BVHModel<BV>& model2,
106  const Transform3<typename BV::S>& tf2,
107  typename BV::S w = 1,
108  bool use_refit = false,
109  bool refit_bottomup = false);
110 
111 template <typename S>
114 {
115 public:
117 
118  S BVTesting(int b1, int b2) const
119  {
120  if (this->enable_statistics)
121  this->num_bv_tests++;
122 
123  Vector3<S> P1, P2;
124  S d = distance(
125  R,
126  T,
127  this->model1->getBV(b1).bv,
128  this->model2->getBV(b2).bv,
129  &P1,
130  &P2);
131 
132  this->stack.emplace_back(P1, P2, b1, b2, d);
133 
134  return d;
135  }
136 
137  void leafTesting(int b1, int b2) const;
138 
139  bool canStop(S c) const;
140 
143 
144  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
145 };
146 
149 
152 template <typename S>
153 FCL_EXPORT
154 bool initialize(
156  const BVHModel<RSS<S>>& model1,
157  const Transform3<S>& tf1,
158  const BVHModel<RSS<S>>& model2,
159  const Transform3<S>& tf2,
160  S w = 1);
161 
162 template <typename S>
164  : public MeshConservativeAdvancementTraversalNode<OBBRSS<S>>
165 {
166 public:
168 
169  S BVTesting(int b1, int b2) const
170  {
171  if (this->enable_statistics)
172  this->num_bv_tests++;
173 
174  Vector3<S> P1, P2;
175  S d = distance(
176  R,
177  T,
178  this->model1->getBV(b1).bv,
179  this->model2->getBV(b2).bv,
180  &P1,
181  &P2);
182 
183  this->stack.emplace_back(P1, P2, b1, b2, d);
184 
185  return d;
186  }
187 
188  void leafTesting(int b1, int b2) const;
189 
190  bool canStop(S c) const;
191 
194 
195  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
196 };
197 
200 
201 template <typename S>
202 FCL_EXPORT
203 bool initialize(
205  const BVHModel<OBBRSS<S>>& model1,
206  const Transform3<S>& tf1,
207  const BVHModel<OBBRSS<S>>& model2,
208  const Transform3<S>& tf2,
209  S w = 1);
210 
211 template <typename S, typename BV>
212 FCL_EXPORT
213 const Vector3<S> getBVAxis(const BV& bv, int i);
214 
215 template <typename BV>
216 FCL_EXPORT
218  typename BV::S c,
219  typename BV::S min_distance,
220  typename BV::S abs_err,
221  typename BV::S rel_err,
222  typename BV::S w,
223  const BVHModel<BV>* model1,
224  const BVHModel<BV>* model2,
225  const MotionBase<typename BV::S>* motion1,
226  const MotionBase<typename BV::S>* motion2,
228  typename BV::S& delta_t);
229 
230 template <typename BV>
231 FCL_EXPORT
233  typename BV::S c,
234  typename BV::S min_distance,
235  typename BV::S abs_err,
236  typename BV::S rel_err,
237  typename BV::S w,
238  const BVHModel<BV>* model1,
239  const BVHModel<BV>* model2,
240  const MotionBase<typename BV::S>* motion1,
241  const MotionBase<typename BV::S>* motion2,
243  typename BV::S& delta_t);
244 
245 template <typename BV>
246 FCL_EXPORT
248  int b1,
249  int b2,
250  const BVHModel<BV>* model1,
251  const BVHModel<BV>* model2,
252  const Triangle* tri_indices1,
253  const Triangle* tri_indices2,
254  const Vector3<typename BV::S>* vertices1,
255  const Vector3<typename BV::S>* vertices2,
256  const Matrix3<typename BV::S>& R,
257  const Vector3<typename BV::S>& T,
258  const MotionBase<typename BV::S>* motion1,
259  const MotionBase<typename BV::S>* motion2,
260  bool enable_statistics,
261  typename BV::S& min_distance,
264  int& last_tri_id1,
265  int& last_tri_id2,
266  typename BV::S& delta_t,
267  int& num_leaf_tests);
268 
269 } // namespace detail
270 } // namespace fcl
271 
273 
274 #endif
fcl::detail::MeshConservativeAdvancementTraversalNode::motion1
const MotionBase< S > * motion1
Motions for the two objects in query.
Definition: mesh_conservative_advancement_traversal_node.h:88
fcl::detail::meshConservativeAdvancementTraversalNodeCanStop
bool meshConservativeAdvancementTraversalNodeCanStop(typename BV::S c, typename BV::S min_distance, typename BV::S abs_err, typename BV::S rel_err, typename BV::S w, const BVHModel< BV > *model1, const BVHModel< BV > *model2, const MotionBase< typename BV::S > *motion1, const MotionBase< typename BV::S > *motion2, std::vector< ConservativeAdvancementStackData< typename BV::S >> &stack, typename BV::S &delta_t)
Definition: mesh_conservative_advancement_traversal_node-inl.h:503
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
mesh_distance_traversal_node.h
fcl::detail::BVHDistanceTraversalNode< RSS< S > >::S
typename RSS< S > ::S S
Definition: bvh_distance_traversal_node.h:58
conservative_advancement_stack_data.h
fcl::detail::MeshConservativeAdvancementTraversalNode::delta_t
S delta_t
The delta_t each step.
Definition: mesh_conservative_advancement_traversal_node.h:85
fcl::detail::MeshConservativeAdvancementTraversalNode::toc
S toc
The time from beginning point.
Definition: mesh_conservative_advancement_traversal_node.h:81
fcl::detail::ConservativeAdvancementStackData
Definition: conservative_advancement_stack_data.h:50
fcl::detail::MeshConservativeAdvancementTraversalNode::motion2
const MotionBase< S > * motion2
Definition: mesh_conservative_advancement_traversal_node.h:89
fcl::detail::MeshConservativeAdvancementTraversalNodeRSS
Definition: mesh_conservative_advancement_traversal_node.h:112
fcl::RSS< S >
fcl::Triangle
Triangle with 3 indices for points.
Definition: triangle.h:48
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
fcl::detail::MeshConservativeAdvancementTraversalNodeRSS::R
Matrix3< S > R
Definition: mesh_conservative_advancement_traversal_node.h:141
fcl::detail::MeshConservativeAdvancementTraversalNodeRSS::T
Vector3< S > T
Definition: mesh_conservative_advancement_traversal_node.h:142
fcl::Matrix3
Eigen::Matrix< S, 3, 3 > Matrix3
Definition: types.h:85
fcl::detail::MeshConservativeAdvancementTraversalNode::t_err
S t_err
Definition: mesh_conservative_advancement_traversal_node.h:82
fcl::detail::meshConservativeAdvancementOrientedNodeLeafTesting
void meshConservativeAdvancementOrientedNodeLeafTesting(int b1, int b2, const BVHModel< BV > *model1, const BVHModel< BV > *model2, const Triangle *tri_indices1, const Triangle *tri_indices2, const Vector3< typename BV::S > *vertices1, const Vector3< typename BV::S > *vertices2, const Matrix3< typename BV::S > &R, const Vector3< typename BV::S > &T, const MotionBase< typename BV::S > *motion1, const MotionBase< typename BV::S > *motion2, bool enable_statistics, typename BV::S &min_distance, Vector3< typename BV::S > &p1, Vector3< typename BV::S > &p2, int &last_tri_id1, int &last_tri_id2, typename BV::S &delta_t, int &num_leaf_tests)
Definition: mesh_conservative_advancement_traversal_node-inl.h:664
fcl::detail::getBVAxis
const Vector3< typename BV::S > getBVAxis(const BV &bv, int i)
Definition: mesh_conservative_advancement_traversal_node-inl.h:485
fcl::MotionBase
Definition: bv_motion_bound_visitor.h:45
fcl::detail::MeshConservativeAdvancementTraversalNode::closest_p2
Vector3< S > closest_p2
Definition: mesh_conservative_advancement_traversal_node.h:73
fcl::detail::MeshDistanceTraversalNode
Traversal node for distance computation between two meshes.
Definition: mesh_distance_traversal_node.h:55
fcl::detail::initialize
template bool initialize(MeshCollisionTraversalNodeOBB< double > &node, const BVHModel< OBB< double >> &model1, const Transform3< double > &tf1, const BVHModel< OBB< double >> &model2, const Transform3< double > &tf2, const CollisionRequest< double > &request, CollisionResult< double > &result)
fcl::detail::MeshConservativeAdvancementTraversalNode::stack
std::vector< ConservativeAdvancementStackData< S > > stack
Definition: mesh_conservative_advancement_traversal_node.h:91
fcl::detail::distance
template void distance(DistanceTraversalNodeBase< double > *node, BVHFrontList *front_list, int qsize)
fcl::OBBRSS< S >
fcl::detail::MeshConservativeAdvancementTraversalNodeOBBRSS::R
Matrix3< S > R
Definition: mesh_conservative_advancement_traversal_node.h:192
fcl::BVHModel
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH_model.h:57
fcl::detail::MeshConservativeAdvancementTraversalNodeOBBRSS::BVTesting
S BVTesting(int b1, int b2) const
BV test between b1 and b2.
Definition: mesh_conservative_advancement_traversal_node.h:169
fcl::detail::MeshConservativeAdvancementTraversalNodeOBBRSS::T
Vector3< S > T
Definition: mesh_conservative_advancement_traversal_node.h:193
fcl::detail::MeshConservativeAdvancementTraversalNode::w
S w
CA controlling variable: early stop for the early iterations of CA.
Definition: mesh_conservative_advancement_traversal_node.h:78
mesh_conservative_advancement_traversal_node-inl.h
fcl::detail::MeshConservativeAdvancementTraversalNodeRSS::BVTesting
S BVTesting(int b1, int b2) const
BV test between b1 and b2.
Definition: mesh_conservative_advancement_traversal_node.h:118
fcl::detail::MeshConservativeAdvancementTraversalNode
continuous collision node using conservative advancement. when using this default version,...
Definition: mesh_conservative_advancement_traversal_node.h:53
fcl::detail::MeshConservativeAdvancementTraversalNode::last_tri_id2
int last_tri_id2
Definition: mesh_conservative_advancement_traversal_node.h:75
fcl::detail::CanStopImpl
Definition: mesh_conservative_advancement_traversal_node-inl.h:169
tbv_motion_bound_visitor.h
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::detail::MeshConservativeAdvancementTraversalNodeOBBRSS
Definition: mesh_conservative_advancement_traversal_node.h:163
fcl::detail::meshConservativeAdvancementOrientedNodeCanStop
bool meshConservativeAdvancementOrientedNodeCanStop(typename BV::S c, typename BV::S min_distance, typename BV::S abs_err, typename BV::S rel_err, typename BV::S w, const BVHModel< BV > *model1, const BVHModel< BV > *model2, const MotionBase< typename BV::S > *motion1, const MotionBase< typename BV::S > *motion2, std::vector< ConservativeAdvancementStackData< typename BV::S >> &stack, typename BV::S &delta_t)
Definition: mesh_conservative_advancement_traversal_node-inl.h:581
fcl::detail::MeshConservativeAdvancementTraversalNode::min_distance
S min_distance
Definition: mesh_conservative_advancement_traversal_node.h:71


fcl
Author(s):
autogenerated on Fri Apr 2 2021 02:38:00