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

fcl_catkin
Author(s):
autogenerated on Thu Mar 23 2023 03:00:17