collision_geometry-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_COLLISION_GEOMETRY_INL_H
39 #define FCL_COLLISION_GEOMETRY_INL_H
40 
42 
43 namespace fcl
44 {
45 
46 //==============================================================================
47 extern template
48 class FCL_EXPORT CollisionGeometry<double>;
49 
50 //==============================================================================
51 template <typename S>
53  : aabb_center(Vector3<S>::Zero()),
54  aabb_radius((S)0),
55  user_data(nullptr),
56  cost_density((S)1),
57  threshold_occupied((S)1),
58  threshold_free((S)0)
59 {
60  // Do nothing
61 }
62 
63 //==============================================================================
64 template <typename S>
66 {
67  // Do nothing
68 }
69 
70 //==============================================================================
71 template <typename S>
73 {
74  return OT_UNKNOWN;
75 }
76 
77 //==============================================================================
78 template <typename S>
80 {
81  return BV_UNKNOWN;
82 }
83 
84 //==============================================================================
85 template <typename S>
87 {
88  return user_data;
89 }
90 
91 //==============================================================================
92 template <typename S>
94 {
95  user_data = data;
96 }
97 
98 //==============================================================================
99 template <typename S>
101 {
102  return cost_density >= threshold_occupied;
103 }
104 
105 //==============================================================================
106 template <typename S>
108 {
109  return cost_density <= threshold_free;
110 }
111 
112 //==============================================================================
113 template <typename S>
115 {
116  return !isOccupied() && !isFree();
117 }
118 
119 //==============================================================================
120 template <typename S>
122 {
123  return Vector3<S>::Zero();
124 }
125 
126 //==============================================================================
127 template <typename S>
129 {
130  return Matrix3<S>::Zero();
131 }
132 
133 //==============================================================================
134 template <typename S>
136 {
137  return 0;
138 }
139 
140 //==============================================================================
141 template <typename S>
143 {
144  // TODO(SeanCurtis-TRI): This is *horribly* inefficient. In complex cases,
145  // this will require computing volume integrals three times. The
146  // CollisionGeometry class should have a single virtual function that will
147  // return all three quantities in one call so that particular sub-classes can
148  // override this to process this answer more efficiently. The default
149  // implementation can be exactly these three calls.
150  // See: https://github.com/flexible-collision-library/fcl/issues/327.
151  Matrix3<S> C = computeMomentofInertia();
152  Vector3<S> com = computeCOM();
153  S V = computeVolume();
154 
155  Matrix3<S> m;
156  m << C(0, 0) - V * (com[1] * com[1] + com[2] * com[2]),
157  C(0, 1) + V * com[0] * com[1],
158  C(0, 2) + V * com[0] * com[2],
159  C(1, 0) + V * com[1] * com[0],
160  C(1, 1) - V * (com[0] * com[0] + com[2] * com[2]),
161  C(1, 2) + V * com[1] * com[2],
162  C(2, 0) + V * com[2] * com[0],
163  C(2, 1) + V * com[2] * com[1],
164  C(2, 2) - V * (com[0] * com[0] + com[1] * com[1]);
165 
166  return m;
167 }
168 
169 } // namespace fcl
170 
171 #endif
fcl::CollisionGeometry< double >
template class FCL_EXPORT CollisionGeometry< double >
fcl::OT_UNKNOWN
@ OT_UNKNOWN
Definition: collision_geometry.h:50
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
fcl::Matrix3
Eigen::Matrix< S, 3, 3 > Matrix3
Definition: types.h:85
fcl::CollisionGeometry
The geometry for the object for collision or distance computation.
Definition: collision_geometry.h:58
fcl::BV_UNKNOWN
@ BV_UNKNOWN
Definition: collision_geometry.h:53
fcl::OBJECT_TYPE
OBJECT_TYPE
object type: BVH (mesh, points), basic geometry, octree
Definition: collision_geometry.h:50
fcl::CollisionGeometry::CollisionGeometry
CollisionGeometry()
Definition: collision_geometry-inl.h:52
fcl::NODE_TYPE
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18,...
Definition: collision_geometry.h:53
collision_geometry.h
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45


fcl
Author(s):
autogenerated on Sat Sep 11 2021 02:37:41