gjk.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_DETAIL_GJK_H
39 #define FCL_NARROWPHASE_DETAIL_GJK_H
40 
41 #include "fcl/common/types.h"
43 
44 namespace fcl
45 {
46 
47 namespace detail
48 {
49 
51 template <typename S>
52 struct FCL_EXPORT GJK
53 {
54  struct SimplexV
55  {
60  };
61 
62  struct Simplex
63  {
65  SimplexV* c[4];
67  S p[4];
69  size_t rank;
70 
71  Simplex();
72  };
73 
74  enum Status {Valid, Inside, Failed};
75 
79  Simplex simplices[2];
80 
81  GJK(unsigned int max_iterations_, S tolerance_);
82 
83  void initialize();
84 
86  Status evaluate(const MinkowskiDiff<S>& shape_, const Vector3<S>& guess);
87 
89  void getSupport(const Vector3<S>& d, SimplexV& sv) const;
90 
92  void getSupport(const Vector3<S>& d, const Vector3<S>& v, SimplexV& sv) const;
93 
95  void removeVertex(Simplex& simplex);
96 
98  void appendVertex(Simplex& simplex, const Vector3<S>& v);
99 
101  bool encloseOrigin();
102 
104  Simplex* getSimplex() const;
105 
107  Vector3<S> getGuessFromSimplex() const;
108 
109 private:
110  SimplexV store_v[4];
111  SimplexV* free_v[4];
112  size_t nfree;
113  size_t current;
116 
117  unsigned int max_iterations;
119 
120 };
121 
122 using GJKf = GJK<float>;
124 
125 } // namespace detail
126 } // namespace fcl
127 
129 
130 #endif
fcl::detail::GJK::current
size_t current
Definition: gjk.h:113
fcl::detail::GJK::ray
Vector3< S > ray
Definition: gjk.h:77
types.h
fcl::detail::GJK::Simplex
Definition: gjk.h:62
fcl::detail::GJK::Simplex::rank
size_t rank
size of simplex (number of vertices)
Definition: gjk.h:69
fcl::detail::GJK::shape
MinkowskiDiff< S > shape
Definition: gjk.h:76
fcl::detail::GJK::SimplexV::d
Vector3< S > d
support direction
Definition: gjk.h:57
fcl::detail::GJK::status
Status status
Definition: gjk.h:115
gjk-inl.h
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
fcl::detail::GJK::nfree
size_t nfree
Definition: gjk.h:112
fcl::detail::GJK::tolerance
S tolerance
Definition: gjk.h:118
fcl::detail::GJK::max_iterations
unsigned int max_iterations
Definition: gjk.h:117
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::GJK::SimplexV
Definition: gjk.h:54
minkowski_diff.h
fcl::detail::GJK::Valid
@ Valid
Definition: gjk.h:74
fcl::detail::GJK
class for GJK algorithm
Definition: gjk.h:52
fcl::detail::GJK::Status
Status
Definition: gjk.h:74
fcl::detail::GJK::SimplexV::w
Vector3< S > w
@brieg support vector (i.e., the furthest point on the shape along the support direction)
Definition: gjk.h:59
fcl::detail::getSupport
FCL_EXPORT Vector3< S > getSupport(const ShapeBase< S > *shape, const Eigen::MatrixBase< Derived > &dir)
the support function for shape
Definition: minkowski_diff-inl.h:67
fcl::detail::GJK::simplex
Simplex * simplex
Definition: gjk.h:114
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::detail::MinkowskiDiff
Minkowski difference class of two shapes.
Definition: minkowski_diff.h:58
fcl::detail::GJK::distance
S distance
Definition: gjk.h:78


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