capsule-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_SHAPE_CAPSULE_INL_H
39 #define FCL_SHAPE_CAPSULE_INL_H
40 
42 
43 namespace fcl
44 {
45 
46 //==============================================================================
47 extern template
48 class FCL_EXPORT Capsule<double>;
49 
50 //==============================================================================
51 template <typename S>
52 Capsule<S>::Capsule(S radius, S lz)
53  : ShapeBase<S>(), radius(radius), lz(lz)
54 {
55  // Do nothing
56 }
57 
58 //==============================================================================
59 template <typename S>
61 {
62  const Vector3<S> v_delta(radius, radius, 0.5 * lz + radius);
63  this->aabb_local.max_ = v_delta;
64  this->aabb_local.min_ = -v_delta;
65 
66  this->aabb_center = this->aabb_local.center();
67  this->aabb_radius = (this->aabb_local.min_ - this->aabb_center).norm();
68 }
69 
70 //==============================================================================
71 template <typename S>
73 {
74  return GEOM_CAPSULE;
75 }
76 
77 //==============================================================================
78 template <typename S>
80 {
81  return constants<S>::pi() * radius * radius *(lz + radius * 4/3.0);
82 }
83 
84 //==============================================================================
85 // Compare https://www.gamedev.net/articles/programming/math-and-physics/capsule-inertia-tensor-r3856/
86 template <typename S>
88 {
89  S l2 = lz * lz;
90  S r2 = radius * radius;
91 
92  S v_cyl = r2 * lz * constants<S>::pi();
93  S v_sph = r2 * radius * constants<S>::pi() * 4 / 3.0;
94 
95  S ix = v_cyl * (l2 / 12. + r2 / 4.) + v_sph * (0.4 * r2 + 0.25 * l2 + 3. * radius * lz / 8.);
96  S iz = (0.5 * v_cyl + 0.4 * v_sph) * r2;
97 
98  return Vector3<S>(ix, ix, iz).asDiagonal();
99 }
100 
101 //==============================================================================
102 template <typename S>
103 std::vector<Vector3<S>> Capsule<S>::getBoundVertices(
104  const Transform3<S>& tf) const
105 {
106  std::vector<Vector3<S>> result(36);
107  const auto m = (1 + std::sqrt(5.0)) / 2.0;
108 
109  auto hl = lz * 0.5;
110  auto edge_size = radius * 6 / (std::sqrt(27.0) + std::sqrt(15.0));
111  auto a = edge_size;
112  auto b = m * edge_size;
113  auto r2 = radius * 2 / std::sqrt(3.0);
114 
115  result[0] = tf * Vector3<S>(0, a, b + hl);
116  result[1] = tf * Vector3<S>(0, -a, b + hl);
117  result[2] = tf * Vector3<S>(0, a, -b + hl);
118  result[3] = tf * Vector3<S>(0, -a, -b + hl);
119  result[4] = tf * Vector3<S>(a, b, hl);
120  result[5] = tf * Vector3<S>(-a, b, hl);
121  result[6] = tf * Vector3<S>(a, -b, hl);
122  result[7] = tf * Vector3<S>(-a, -b, hl);
123  result[8] = tf * Vector3<S>(b, 0, a + hl);
124  result[9] = tf * Vector3<S>(b, 0, -a + hl);
125  result[10] = tf * Vector3<S>(-b, 0, a + hl);
126  result[11] = tf * Vector3<S>(-b, 0, -a + hl);
127 
128  result[12] = tf * Vector3<S>(0, a, b - hl);
129  result[13] = tf * Vector3<S>(0, -a, b - hl);
130  result[14] = tf * Vector3<S>(0, a, -b - hl);
131  result[15] = tf * Vector3<S>(0, -a, -b - hl);
132  result[16] = tf * Vector3<S>(a, b, -hl);
133  result[17] = tf * Vector3<S>(-a, b, -hl);
134  result[18] = tf * Vector3<S>(a, -b, -hl);
135  result[19] = tf * Vector3<S>(-a, -b, -hl);
136  result[20] = tf * Vector3<S>(b, 0, a - hl);
137  result[21] = tf * Vector3<S>(b, 0, -a - hl);
138  result[22] = tf * Vector3<S>(-b, 0, a - hl);
139  result[23] = tf * Vector3<S>(-b, 0, -a - hl);
140 
141  auto c = 0.5 * r2;
142  auto d = radius;
143  result[24] = tf * Vector3<S>(r2, 0, hl);
144  result[25] = tf * Vector3<S>(c, d, hl);
145  result[26] = tf * Vector3<S>(-c, d, hl);
146  result[27] = tf * Vector3<S>(-r2, 0, hl);
147  result[28] = tf * Vector3<S>(-c, -d, hl);
148  result[29] = tf * Vector3<S>(c, -d, hl);
149 
150  result[30] = tf * Vector3<S>(r2, 0, -hl);
151  result[31] = tf * Vector3<S>(c, d, -hl);
152  result[32] = tf * Vector3<S>(-c, d, -hl);
153  result[33] = tf * Vector3<S>(-r2, 0, -hl);
154  result[34] = tf * Vector3<S>(-c, -d, -hl);
155  result[35] = tf * Vector3<S>(c, -d, -hl);
156 
157  return result;
158 }
159 
160 } // namespace fcl
161 
162 #endif
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
fcl::ShapeBase
Base class for all basic geometric shapes.
Definition: shape_base.h:48
fcl::Capsule< S >::S
S S
Definition: capsule.h:54
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
fcl::Matrix3
Eigen::Matrix< S, 3, 3 > Matrix3
Definition: types.h:85
capsule.h
fcl::Capsule< double >
template class FCL_EXPORT Capsule< double >
fcl::Capsule::Capsule
Capsule(S radius, S lz)
Constructor.
Definition: capsule-inl.h:52
fcl::constants
Definition: constants.h:129
fcl::NODE_TYPE
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18,...
Definition: collision_geometry.h:53
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::GEOM_CAPSULE
@ GEOM_CAPSULE
Definition: collision_geometry.h:54
fcl::Capsule
Center at zero point capsule.
Definition: capsule.h:50


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