FCL  0.6.0
Flexible Collision Library
mesh_shape_conservative_advancement_traversal_node.h
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_MESHSHAPECONSERVATIVEADVANCEMENTTRAVERSALNODE_H
39 #define FCL_TRAVERSAL_MESHSHAPECONSERVATIVEADVANCEMENTTRAVERSALNODE_H
40 
41 #include "fcl/narrowphase/detail/traversal/distance/conservative_advancement_stack_data.h"
42 #include "fcl/narrowphase/detail/traversal/distance/mesh_shape_distance_traversal_node.h"
43 
44 namespace fcl
45 {
46 
47 namespace detail
48 {
49 
51 template <typename BV, typename Shape, typename NarrowPhaseSolver>
53  : public MeshShapeDistanceTraversalNode<BV, Shape, NarrowPhaseSolver>
54 {
55 public:
56 
57  using S = typename BV::S;
58 
60 
62  S BVTesting(int b1, int b2) const;
63 
65  void leafTesting(int b1, int b2) const;
66 
68  bool canStop(S c) const;
69 
70  mutable S min_distance;
71 
72  mutable Vector3<S> closest_p1, closest_p2;
73 
74  mutable int last_tri_id;
75 
77  S w;
78 
80  S toc;
81  S t_err;
82 
84  mutable S delta_t;
85 
88  const MotionBase<S>* motion2;
89 
90  mutable std::vector<ConservativeAdvancementStackData<S>> stack;
91 };
92 
93 template <typename BV, typename Shape, typename NarrowPhaseSolver>
94 bool initialize(
96  BVHModel<BV>& model1,
97  const Transform3<typename BV::S>& tf1,
98  const Shape& model2,
99  const Transform3<typename BV::S>& tf2,
100  const NarrowPhaseSolver* nsolver,
101  typename BV::S w = 1,
102  bool use_refit = false,
103  bool refit_bottomup = false);
104 
105 template <typename BV, typename Shape, typename NarrowPhaseSolver>
106 void meshShapeConservativeAdvancementOrientedNodeLeafTesting(
107  int b1,
108  int /* b2 */,
109  const BVHModel<BV>* model1,
110  const Shape& model2,
111  const BV& model2_bv,
112  Vector3<typename BV::S>* vertices,
113  Triangle* tri_indices,
114  const Transform3<typename BV::S>& tf1,
115  const Transform3<typename BV::S>& tf2,
117  const MotionBase<typename BV::S>* motion2,
118  const NarrowPhaseSolver* nsolver,
119  bool enable_statistics,
120  typename BV::S& min_distance,
121  Vector3<typename BV::S>& p1,
122  Vector3<typename BV::S>& p2,
123  int& last_tri_id,
124  typename BV::S& delta_t,
125  int& num_leaf_tests);
126 
127 template <typename BV, typename Shape>
128 bool meshShapeConservativeAdvancementOrientedNodeCanStop(
129  typename BV::S c,
130  typename BV::S min_distance,
131  typename BV::S abs_err,
132  typename BV::S rel_err,
133  typename BV::S w,
134  const BVHModel<BV>* model1,
135  const Shape& model2,
136  const BV& model2_bv,
138  const MotionBase<typename BV::S>* motion2,
140  typename BV::S& delta_t);
141 
142 template <typename Shape, typename NarrowPhaseSolver>
145  RSS<typename Shape::S>, Shape, NarrowPhaseSolver>
146 {
147 public:
148 
149  using S = typename Shape::S;
150 
152 
153  S BVTesting(int b1, int b2) const;
154 
155  void leafTesting(int b1, int b2) const;
156 
157  bool canStop(S c) const;
158 };
159 
160 template <typename Shape, typename NarrowPhaseSolver>
161 bool initialize(
163  const BVHModel<RSS<typename Shape::S>>& model1,
164  const Transform3<typename Shape::S>& tf1,
165  const Shape& model2,
166  const Transform3<typename Shape::S>& tf2,
167  const NarrowPhaseSolver* nsolver,
168  typename Shape::S w = 1);
169 
170 template <typename Shape, typename NarrowPhaseSolver>
173  OBBRSS<typename Shape::S>, Shape, NarrowPhaseSolver>
174 {
175 public:
176 
177  using S = typename Shape::S;
178 
180 
181  S BVTesting(int b1, int b2) const;
182 
183  void leafTesting(int b1, int b2) const;
184 
185  bool canStop(S c) const;
186 };
187 
188 template <typename Shape, typename NarrowPhaseSolver>
189 bool initialize(
191  const BVHModel<OBBRSS<typename Shape::S>>& model1,
192  const Transform3<typename Shape::S>& tf1,
193  const Shape& model2,
194  const Transform3<typename Shape::S>& tf2,
195  const NarrowPhaseSolver* nsolver,
196  typename Shape::S w = 1);
197 
198 } // namespace detail
199 } // namespace fcl
200 
201 #include "fcl/narrowphase/detail/traversal/distance/mesh_shape_conservative_advancement_traversal_node-inl.h"
202 
203 #endif
S delta_t
The delta_t each step.
Definition: mesh_shape_conservative_advancement_traversal_node.h:84
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
Definition: mesh_shape_conservative_advancement_traversal_node.h:171
A class for rectangle sphere-swept bounding volume.
Definition: RSS.h:49
Traversal node for distance between mesh and shape.
Definition: mesh_shape_distance_traversal_node.h:52
S BVTesting(int b1, int b2) const
BV culling test in one BVTT node.
Definition: mesh_shape_conservative_advancement_traversal_node-inl.h:69
S toc
The time from beginning point.
Definition: mesh_shape_conservative_advancement_traversal_node.h:80
Transform3< BV::S > tf2
configuration of second object
Definition: traversal_node_base.h:88
Traversal node for conservative advancement computation between BVH and shape.
Definition: mesh_shape_conservative_advancement_traversal_node.h:52
Triangle with 3 indices for points.
Definition: triangle.h:47
bool canStop(S c) const
Whether the traversal process can stop early.
Definition: mesh_shape_conservative_advancement_traversal_node-inl.h:131
void leafTesting(int b1, int b2) const
Conservative advancement testing between leaves (one triangle and one shape)
Definition: mesh_shape_conservative_advancement_traversal_node-inl.h:83
Definition: bv_motion_bound_visitor.h:45
Definition: conservative_advancement_stack_data.h:50
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH_model.h:57
Transform3< BV::S > tf1
configuation of first object
Definition: traversal_node_base.h:85
S w
CA controlling variable: early stop for the early iterations of CA.
Definition: mesh_shape_conservative_advancement_traversal_node.h:77
const MotionBase< S > * motion1
Motions for the two objects in query.
Definition: mesh_shape_conservative_advancement_traversal_node.h:87
Definition: mesh_shape_conservative_advancement_traversal_node.h:143
Class merging the OBB and RSS, can handle collision and distance simultaneously.
Definition: OBBRSS.h:50
bool enable_statistics
Whether stores statistics.
Definition: distance_traversal_node_base.h:79