shape_conservative_advancement_traversal_node-inl.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_SHAPECONSERVATIVEADVANCEMENTTRAVERSALNODE_INL_H
39 #define FCL_TRAVERSAL_SHAPECONSERVATIVEADVANCEMENTTRAVERSALNODE_INL_H
40 
42 
43 namespace fcl
44 {
45 
46 namespace detail
47 {
48 
49 //==============================================================================
50 template <typename Shape1, typename Shape2, typename NarrowPhaseSolver>
53  : ShapeDistanceTraversalNode<Shape1, Shape2, NarrowPhaseSolver>()
54 {
55  delta_t = 1;
56  toc = 0;
57  t_err = (S)0.0001;
58 
59  motion1 = nullptr;
60  motion2 = nullptr;
61 }
62 
63 //==============================================================================
64 template <typename Shape1, typename Shape2, typename NarrowPhaseSolver>
66 leafTesting(int, int) const
67 {
68  S distance;
69  // NOTE(JS): The closest points are set to zeros in order to suppress the
70  // maybe-uninitialized warning. It seems the warnings occur since
71  // NarrowPhaseSolver::shapeDistance() conditionally set the closest points.
72  // If this wasn't intentional then please remove the initialization of the
73  // closest points, and change the function NarrowPhaseSolver::shapeDistance()
74  // to always set the closest points.
75  Vector3<S> closest_p1 = Vector3<S>::Zero();
76  Vector3<S> closest_p2 = Vector3<S>::Zero();
77  this->nsolver->shapeDistance(*(this->model1), this->tf1, *(this->model2), this->tf2, &distance, &closest_p1, &closest_p2);
78 
79  Vector3<S> n = closest_p2 - closest_p1;
80  const S norm_sq = n.squaredNorm();
81  if (norm_sq > 0.0)
82  n /= sqrt(norm_sq); // Normalize.
83  TBVMotionBoundVisitor<RSS<S>> mb_visitor1(model1_bv, n);
84  TBVMotionBoundVisitor<RSS<S>> mb_visitor2(model2_bv, -n);
85  S bound1 = motion1->computeMotionBound(mb_visitor1);
86  S bound2 = motion2->computeMotionBound(mb_visitor2);
87 
88  S bound = bound1 + bound2;
89 
90  S cur_delta_t;
91  if(bound <= distance) cur_delta_t = 1;
92  else cur_delta_t = distance / bound;
93 
94  if(cur_delta_t < delta_t)
95  delta_t = cur_delta_t;
96 }
97 
98 //==============================================================================
99 template <typename Shape1, typename Shape2, typename NarrowPhaseSolver>
102  const Shape1& shape1,
104  const Shape2& shape2,
106  const NarrowPhaseSolver* nsolver)
107 {
108  using S = typename Shape1::S;
109 
110  node.model1 = &shape1;
111  node.tf1 = tf1;
112  node.model2 = &shape2;
113  node.tf2 = tf2;
114  node.nsolver = nsolver;
115 
118 
119  return true;
120 }
121 
122 } // namespace detail
123 } // namespace fcl
124 
125 #endif
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
fcl::detail::ShapeConservativeAdvancementTraversalNode::motion1
const MotionBase< S > * motion1
Motions for the two objects in query.
Definition: shape_conservative_advancement_traversal_node.h:70
fcl::detail::ShapeDistanceTraversalNode::model2
const Shape2 * model2
Definition: shape_distance_traversal_node.h:67
fcl::detail::ShapeConservativeAdvancementTraversalNode::model2_bv
RSS< S > model2_bv
Definition: shape_conservative_advancement_traversal_node.h:73
fcl::detail::TraversalNodeBase< Shape1::S >::tf1
Transform3< Shape1::S > tf1
configuation of first object
Definition: traversal_node_base.h:85
fcl::computeBV
FCL_EXPORT void computeBV(const Shape &s, const Transform3< typename BV::S > &tf, BV &bv)
calculate a bounding volume for a shape in a specific configuration
Definition: geometry/shape/utility-inl.h:1056
fcl::detail::ShapeDistanceTraversalNode::nsolver
const NarrowPhaseSolver * nsolver
Definition: shape_distance_traversal_node.h:69
fcl::detail::ShapeConservativeAdvancementTraversalNode::ShapeConservativeAdvancementTraversalNode
ShapeConservativeAdvancementTraversalNode()
Definition: shape_conservative_advancement_traversal_node-inl.h:52
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
fcl::detail::ShapeConservativeAdvancementTraversalNode::motion2
const MotionBase< S > * motion2
Definition: shape_conservative_advancement_traversal_node.h:71
fcl::detail::ShapeConservativeAdvancementTraversalNode::model1_bv
RSS< S > model1_bv
Definition: shape_conservative_advancement_traversal_node.h:73
fcl::detail::ShapeConservativeAdvancementTraversalNode::delta_t
S delta_t
The delta_t each step.
Definition: shape_conservative_advancement_traversal_node.h:67
shape_conservative_advancement_traversal_node.h
fcl::detail::ShapeDistanceTraversalNode::model1
const Shape1 * model1
Definition: shape_distance_traversal_node.h:66
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::ShapeConservativeAdvancementTraversalNode::leafTesting
void leafTesting(int, int) const
Leaf test between node b1 and b2, if they are both leafs.
Definition: shape_conservative_advancement_traversal_node-inl.h:66
fcl::bound
template Interval< double > bound(const Interval< double > &i, double v)
fcl::detail::ShapeConservativeAdvancementTraversalNode::toc
S toc
The time from beginning point.
Definition: shape_conservative_advancement_traversal_node.h:63
fcl::detail::distance
template void distance(DistanceTraversalNodeBase< double > *node, BVHFrontList *front_list, int qsize)
fcl::TBVMotionBoundVisitor
Definition: tbv_motion_bound_visitor.h:65
fcl::detail::ShapeDistanceTraversalNode
Traversal node for distance between two shapes.
Definition: shape_distance_traversal_node.h:52
fcl::detail::ShapeConservativeAdvancementTraversalNode::S
typename Shape1::S S
Definition: shape_conservative_advancement_traversal_node.h:54
fcl::detail::TraversalNodeBase< Shape1::S >::tf2
Transform3< Shape1::S > tf2
configuration of second object
Definition: traversal_node_base.h:88
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::detail::ShapeConservativeAdvancementTraversalNode
Definition: shape_conservative_advancement_traversal_node.h:50
fcl::detail::ShapeConservativeAdvancementTraversalNode::t_err
S t_err
Definition: shape_conservative_advancement_traversal_node.h:64


fcl
Author(s):
autogenerated on Thu Mar 23 2023 02:38:06