gjk_solver_indep.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_NARROWPHASE_GJKSOLVERINDEP_H
39 #define FCL_NARROWPHASE_GJKSOLVERINDEP_H
40 
41 #include <iostream>
42 
43 #include "fcl/common/types.h"
45 
46 namespace fcl
47 {
48 
49 namespace detail
50 {
51 
53 template <typename S_>
54 struct FCL_EXPORT GJKSolver_indep
55 {
56  using S = S_;
57 
60  template<typename Shape1, typename Shape2>
61  FCL_DEPRECATED
62  bool shapeIntersect(
63  const Shape1& s1,
64  const Transform3<S>& tf1,
65  const Shape2& s2,
66  const Transform3<S>& tf2,
67  Vector3<S>* contact_points,
68  S* penetration_depth,
69  Vector3<S>* normal) const;
70 
72  template<typename Shape1, typename Shape2>
73  bool shapeIntersect(
74  const Shape1& s1,
75  const Transform3<S>& tf1,
76  const Shape2& s2,
77  const Transform3<S>& tf2,
78  std::vector<ContactPoint<S>>* contacts = nullptr) const;
79 
81  template<typename Shape>
82  bool shapeTriangleIntersect(
83  const Shape& s,
84  const Transform3<S>& tf,
85  const Vector3<S>& P1,
86  const Vector3<S>& P2,
87  const Vector3<S>& P3,
88  Vector3<S>* contact_points = nullptr,
89  S* penetration_depth = nullptr,
90  Vector3<S>* normal = nullptr) const;
91 
93  template<typename Shape>
94  bool shapeTriangleIntersect(
95  const Shape& s,
96  const Transform3<S>& tf1,
97  const Vector3<S>& P1,
98  const Vector3<S>& P2,
99  const Vector3<S>& P3,
100  const Transform3<S>& tf2,
101  Vector3<S>* contact_points = nullptr,
102  S* penetration_depth = nullptr,
103  Vector3<S>* normal = nullptr) const;
104 
106  template<typename Shape1, typename Shape2>
107  bool shapeDistance(
108  const Shape1& s1,
109  const Transform3<S>& tf1,
110  const Shape2& s2,
111  const Transform3<S>& tf2,
112  S* distance = nullptr,
113  Vector3<S>* p1 = nullptr,
114  Vector3<S>* p2 = nullptr) const;
115 
117  template<typename Shape1, typename Shape2>
118  bool shapeSignedDistance(
119  const Shape1& s1,
120  const Transform3<S>& tf1,
121  const Shape2& s2,
122  const Transform3<S>& tf2,
123  S* distance = nullptr,
124  Vector3<S>* p1 = nullptr,
125  Vector3<S>* p2 = nullptr) const;
126 
128  template<typename Shape>
129  bool shapeTriangleDistance(
130  const Shape& s,
131  const Transform3<S>& tf,
132  const Vector3<S>& P1,
133  const Vector3<S>& P2,
134  const Vector3<S>& P3,
135  S* distance = nullptr,
136  Vector3<S>* p1 = nullptr,
137  Vector3<S>* p2 = nullptr) const;
138 
140  template<typename Shape>
141  bool shapeTriangleDistance(
142  const Shape& s,
143  const Transform3<S>& tf1,
144  const Vector3<S>& P1,
145  const Vector3<S>& P2,
146  const Vector3<S>& P3,
147  const Transform3<S>& tf2,
148  S* distance = nullptr,
149  Vector3<S>* p1 = nullptr,
150  Vector3<S>* p2 = nullptr) const;
151 
153  GJKSolver_indep();
154 
155  void enableCachedGuess(bool if_enable) const;
156 
157  void setCachedGuess(const Vector3<S>& guess) const;
158 
159  Vector3<S> getCachedGuess() const;
160 
162  unsigned int epa_max_face_num;
163 
165  unsigned int epa_max_vertex_num;
166 
168  unsigned int epa_max_iterations;
169 
172 
175 
178 
180  mutable bool enable_cached_guess;
181 
184 
185  friend
186  std::ostream& operator<<(std::ostream& out, const GJKSolver_indep& solver) {
187  out << "GjkSolver_indep"
188  << "\n gjk tolerance: " << solver.gjk_tolerance
189  << "\n gjk max iterations: " << solver.gjk_max_iterations
190  << "\n epa tolerance: " << solver.epa_tolerance
191  << "\n epa max face num: " << solver.epa_max_face_num
192  << "\n epa max vertex num: " << solver.epa_max_vertex_num
193  << "\n epa max iterations: " << solver.epa_max_iterations
194  << "\n enable cached guess: " << solver.enable_cached_guess;
195  if (solver.enable_cached_guess) out << solver.cached_guess.transpose();
196  return out;
197  }
198 };
199 
202 
203 } // namespace detail
204 } // namespace fcl
205 
207 
208 #endif
fcl::detail::GJKSolver_indep::S
S_ S
Definition: gjk_solver_indep.h:56
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
fcl::ContactPoint
Minimal contact information returned by collision.
Definition: contact_point.h:48
types.h
fcl::detail::GJKSolver_indep::epa_tolerance
S epa_tolerance
the threshold used in EPA to stop iteration
Definition: gjk_solver_indep.h:171
fcl::detail::GJKSolver_indep::operator<<
friend std::ostream & operator<<(std::ostream &out, const GJKSolver_indep &solver)
Definition: gjk_solver_indep.h:186
gjk_solver_indep-inl.h
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
fcl::detail::GJKSolver_indep::epa_max_vertex_num
unsigned int epa_max_vertex_num
maximum number of simplex vertex used in EPA algorithm
Definition: gjk_solver_indep.h:165
fcl::detail::GJKSolver_indep::gjk_tolerance
S gjk_tolerance
the threshold used in GJK to stop iteration
Definition: gjk_solver_indep.h:174
fcl::detail::GJKSolver_indep
collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the ...
Definition: gjk_solver_indep.h:54
contact_point.h
fcl::detail::distance
template void distance(DistanceTraversalNodeBase< double > *node, BVHFrontList *front_list, int qsize)
fcl::detail::GJKSolver_indep::enable_cached_guess
bool enable_cached_guess
Whether smart guess can be provided.
Definition: gjk_solver_indep.h:180
fcl::detail::GJKSolver_indep::gjk_max_iterations
S gjk_max_iterations
maximum number of iterations used for GJK iterations
Definition: gjk_solver_indep.h:177
fcl::detail::GJKSolver_indep::cached_guess
Vector3< S > cached_guess
smart guess
Definition: gjk_solver_indep.h:183
fcl::detail::GJKSolver_indep::epa_max_face_num
unsigned int epa_max_face_num
maximum number of simplex face used in EPA algorithm
Definition: gjk_solver_indep.h:162
fcl::detail::GJKSolver_indep::epa_max_iterations
unsigned int epa_max_iterations
maximum number of iterations used for EPA iterations
Definition: gjk_solver_indep.h:168
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45


fcl
Author(s):
autogenerated on Tue Jun 22 2021 07:27:50